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