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);
}
/**
* Set Covariance
*/
bool setCovariance(const Covariance<StateType>& covariance)
{
S.compute(covariance);
return (S.info() == Eigen::Success);
}