bool computeCovarianceSquareRootFromSigmaPoints(const Type& mean, const SigmaPoints<Type>& sigmaPoints,
const CovarianceSquareRoot<Type>& noiseCov, CovarianceSquareRoot<Type>& cov)
{
// Compute QR decomposition of (transposed) augmented matrix
Matrix<T, 2*State::length + Type::length, Type::length> tmp;
tmp.template topRows<2*State::length>() = std::sqrt(this->sigmaWeights_c[1]) * ( sigmaPoints.template rightCols<SigmaPointCount-1>().colwise() - mean).transpose();
tmp.template bottomRows<Type::length>() = noiseCov.matrixU().toDenseMatrix();
// TODO: Use ColPivHouseholderQR
Eigen::HouseholderQR<decltype(tmp)> qr( tmp );
// Set R matrix as upper triangular square root
cov.setU(qr.matrixQR().template topRightCorner<Type::length, Type::length>());
// Perform additional rank 1 update
// TODO: According to the paper (Section 3, "Cholesky factor updating") the update
// is defined using the square root of the scalar, however the correct result
// is obtained when using the weight directly rather than using the square root
// It should be checked whether this is a bug in Eigen or in the Paper
// T nu = std::copysign( std::sqrt(std::abs(sigmaWeights_c[0])), sigmaWeights_c[0]);
T nu = this->sigmaWeights_c[0];
cov.rankUpdate( sigmaPoints.template leftCols<1>() - mean, nu );
return (cov.info() == Eigen::Success);
}
bool computeKalmanGain( const Measurement& y,
const SigmaPoints<Measurement>& sigmaMeasurementPoints,
const CovarianceSquareRoot<Measurement>& S_y,
KalmanGain<Measurement>& K)
{
// Note: The intermediate eval() is needed here (for now) due to a bug in Eigen that occurs
// when Measurement::length == 1 AND State::length >= 8
auto W = this->sigmaWeights_c.transpose().template replicate<State::length,1>();
auto P = (sigmaStatePoints.colwise() - x).cwiseProduct( W ).eval()
* (sigmaMeasurementPoints.colwise() - y).transpose();
K = S_y.solve(P.transpose()).transpose();
return true;
}
bool updateStateCovariance(const KalmanGain<Measurement>& K, const CovarianceSquareRoot<Measurement>& S_y)
{
auto U = K * S_y.matrixL();
for(int i = 0; i < U.cols(); ++i)
{
S.rankUpdate( U.col(i), -1 );
if( S.info() == Eigen::NumericalIssue )
{
// TODO: handle numerical issues in some sensible way
return false;
}
}
return true;
}
SquareRootBase()
{
S.setIdentity();
}
/**
* @brief Set Covariance using Square Root
*
* @param covSquareRoot Lower triangular Matrix representing the covariance
* square root (i.e. P = LLˆT)
*/
bool setCovarianceSquareRoot(const Covariance<StateType>& covSquareRoot)
{
S.setL(covSquareRoot);
return true;
}
/**
* Set Covariance
*/
bool setCovariance(const Covariance<StateType>& covariance)
{
S.compute(covariance);
return (S.info() == Eigen::Success);
}
/**
* Get covariance reconstructed from square root
*/
Covariance<StateType> getCovariance() const
{
return S.reconstructedMatrix();
}