一、理论
(1)、系统观测值是均值为0,协方差为单位矩阵
(2)、系统观测值是均值为0,协方差不是单位矩阵
注意:这个S是观测值的协方差矩阵;比如对于GPS/INS组合的导航系统,S应该是GPS位置观测值的协方差矩阵;
二、SVD分解求矩阵伪逆
C++代码
#include <iostream>
#include <Eigen/SVD>
#include <Eigen/Core>
using namespace std;
MatrixXd Estimator::pinv(MatrixXd A)
{
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);//M=USV*
double pinvtoler = 1.e-8; //tolerance
int row = A.rows();
int col = A.cols();
int k = min(row,col);
MatrixXd X = MatrixXd::Zero(col,row);
MatrixXd singularValues_inv = svd.singularValues();//奇异值
MatrixXd singularValues_inv_mat = MatrixXd::Zero(col, row);
for (long i = 0; i<k; ++i)
{
if (singularValues_inv(i) > pinvtoler)
singularValues_inv(i) = 1.0 / singularValues_inv(i);
else singularValues_inv(i) = 0;
}
for (long i = 0; i < k; ++i)
{
singularValues_inv_mat(i, i) = singularValues_inv(i);
}
//X=VS+U*
X=(svd.matrixV())*(singularValues_inv_mat)*(svd.matrixU().transpose());
return X;
}
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(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;
for(int i = 0; i <= WINDOW_SIZE; ++i)
covariance_blocks.push_back(std::make_pair(para_Pose[i], para_Pose[i]));
// 有时候出现 rank问题,忽略一下
Ceres_cov_pose.setZero();
Eigen::Matrix<double,6,6, Eigen::RowMajor> Ceres_cov_mid = Eigen::Matrix<double,6,6, Eigen::RowMajor>::Zero();
if( covariance.Compute(covariance_blocks, &problem) )
{
for(int i = 0; i <= WINDOW_SIZE; ++i)
{
covariance.GetCovarianceBlockInTangentSpace(para_Pose[i],para_Pose[i], Ceres_cov_mid.data());
Ceres_cov_pose += Ceres_cov_mid;
}
}
std::cout <<"cov_pose.diagonal() ="<< cov_pose.diagonal() << std::endl;
别人的思路
1. ceres 官网
官网简介链接
2. 案例
double x[3];
double y[2];
Problem problem;
problem.AddParameterBlock(x, 3);
problem.AddParameterBlock(y, 2);
<Build Problem>
<Solve Problem>
Covariance::Options options;
Covariance covariance(options);
vector<pair<const double*, const double*> > covariance_blocks;
covariance_blocks.push_back(make_pair(x, x));
covariance_blocks.push_back(make_pair(y, y));
covariance_blocks.push_back(make_pair(x, y));
CHECK(covariance.Compute(covariance_blocks, &problem));
double covariance_xx[3 * 3];
double covariance_yy[2 * 2];
double covariance_xy[3 * 2];
covariance.GetCovarianceBlock(x, x, covariance_xx)
covariance.GetCovarianceBlock(y, y, covariance_yy)
covariance.GetCovarianceBlock(x, y, covariance_xy)
备注:
微信:j15212774116, 欢迎讨论 !