一、理论
(1)、系统观测值是均值为0，协方差为单位矩阵
(2)、系统观测值是均值为0，协方差不是单位矩阵
注意：这个S是观测值的协方差矩阵；比如对于GPS/INS组合的导航系统，S应该是GPS位置观测值的协方差矩阵；
二、SVD分解求矩阵伪逆
C++代码
#include <iostream>
#include <Eigen/SVD>
#include <Eigen/Core>
using namespace std;
// 利用Eigen库，采用SVD分解的方法求解矩阵伪逆，默认误差er为0
Eigen::MatrixXd pinv_eigen_based(Eigen::MatrixXd & origin, const float er = 0) {
// 进行svd分解
Eigen::JacobiSVD<Eigen::MatrixXd> svd_holder(origin,
Eigen::ComputeThinU |
Eigen::ComputeThinV);
// 构建SVD分解结果
Eigen::MatrixXd U = svd_holder.matrixU();
Eigen::MatrixXd V = svd_holder.matrixV();
Eigen::MatrixXd D = svd_holder.singularValues();
// 构建S矩阵
Eigen::MatrixXd S(V.cols(), U.cols());
S.setZero();
for (unsigned int i = 0; i < D.size(); ++i) {
if (D(i, 0) > er) {
S(i, i) = 1 / D(i, 0);
} else {
S(i, i) = 0;
}
}
// pinv_matrix = V * S * U^T
return V * S * U.transpose();
}
int main() {
// 设置矩阵行数、列数
const int ROW = 3;
const int COL = 4;
// 生成大小 ROW * COL 的随机矩阵
Eigen::MatrixXd A;
A = Eigen::MatrixXd::Random(ROW, COL);
// 打印矩阵A
cout << "矩阵A为：" << endl;
cout << A << endl;
// 打印矩阵A的伪逆矩阵
cout << "矩阵A的伪逆为：" << endl;
cout << pinv_eigen_based(A) << endl;
}
三、ceres-solver 求协方差
Eigen::Matrix<double,6,6, Eigen::RowMajor> cov_pose = Eigen::Matrix<double,6,6, Eigen::RowMajor>::Zero();
ceres::Covariance::Options cov_options;
ceres::Covariance covariance(cov_options);
std::vector<std::pair<const double*, const double*>> covariance_blocks;
covariance_blocks.push_back(std::make_pair(para_Pose[WINDOW_SIZE], para_Pose[WINDOW_SIZE]));
// 有时候出现 rank问题，忽略一下
if( covariance.Compute(covariance_blocks, &problem) )
{
covariance.GetCovarianceBlockInTangentSpace(para_Pose[WINDOW_SIZE],para_Pose[WINDOW_SIZE], cov_pose.data());
}
std::cout <<"cov_pose.diagonal() ="<< cov_pose.diagonal() << std::endl;备注：
微信：j15212774116， 欢迎讨论 ！