[MSCKF-VIO]零空间投影：消除特征位置不确定性

零空间投影：消除特征位置不确定性文章简介本文是MSCKF VIO系列文章的第九篇，详细介绍零空间投影的实现。零空间投影是MSCKF的核心技术之一，通过消除特征位置不确定性的影响，实现高效的状态更新。目录文章目录零空间投影：消除特征位置不确定性文章简介目录1. 零空间投影概述1.1 问题背景1.2 核心思想1.3 数学原理2. 代码实现2.1 零空间投影主函数2.2 SVD分解2.3 投影操作3. 数学推导3.1 测量方程3.2 零空间性质3.3 投影后的测量方程3.4 噪声统计特性3.5 维度分析4. 实现细节4.1 稀疏雅可比矩阵4.2 内存效率4.3 数值稳定性5. 与其他方法对比5.1 直接包含特征位置5.2 MSCKF方法（零空间投影）5.3 边缘化6. 性能分析6.1 计算复杂度6.2 内存使用6.3 最大观测数7. 调试技巧7.1 验证零空间性质7.2 检查投影后残差7.3 监控奇异值8. 常见问题8.1 观测数不足8.2 退化情况8.3 数值精度问题9. 扩展应用9.1 多特征批量处理9.2 卡方检验10. 总结下一篇预告参考文献1. 零空间投影概述1.1 问题背景在MSCKF中，特征位置不是状态向量的一部分。当我们想要使用特征观测来更新状态时，需要消除特征位置不确定性的影响。1.2 核心思想对于一个特征在多个相机帧中的观测，测量方程为：z = h ( x , p ) + n \mathbf{z} = h(\mathbf{x}, \mathbf{p}) + \mathbf{n}z=h(x,p)+n其中：x \mathbf{x}x：系统状态p \mathbf{p}p：特征3D位置n \mathbf{n}n：测量噪声线性化后：δ z ≈ H x δ x + H p δ p + n \delta \mathbf{z} \approx H_x \delta \mathbf{x} + H_p \delta \mathbf{p} + \mathbf{n}δz≈Hx​δx+Hp​δp+n我们的目标是只更新系统状态x \mathbf{x}x，而不估计特征位置p \mathbf{p}p。通过投影到H p H_pHp​的零空间，可以消除δ p \delta \mathbf{p}δp的影响。1.3 数学原理对H p H_pHp​进行SVD分解：H p = U Σ V T H_p = U \Sigma V^THp​=UΣVT其中U UU是左奇异向量矩阵。取U UU的后m − 3 m-3m−3列（对应零奇异值）构成零空间基A AA：A = U : , 4 : m A = U_{:, 4:m}A=U:,4:m​投影后的测量方程：A T δ z = A T H x δ x + A T n A^T \delta \mathbf{z} = A^T H_x \delta \mathbf{x} + A^T \mathbf{n}ATδz=ATHx​δx+ATn由于A T H p = 0 A^T H_p = 0ATHp​=0，特征位置的影响被消除。2. 代码实现2.1 零空间投影主函数// src/msckf_vio.cpp:862-916voidMsckfVio::featureJacobian(constFeatureIDTypefeature_id,conststd::vectorStateIDTypecam_state_ids,MatrixXdH_x,VectorXdr){constautofeature=map_server[feature_id];// 1. 找到有效的相机状态vectorStateIDTypevalid_cam_state_ids(0);for(constautocam_id:cam_state_ids){if(feature.observations.find(cam_id)==feature.observations.end())continue;valid_cam_state_ids.push_back(cam_id);}// 2. 计算雅可比矩阵大小intjacobian_row_size=4*valid_cam_state_ids.size();// 3. 初始化雅可比矩阵MatrixXd H_xj=MatrixXd::Zero(jacobian_row_size,21+state_server.cam_states.size()*6);MatrixXd H_fj=MatrixXd::Zero(jacobian_row_size,3);VectorXd r_j=VectorXd::Zero(jacobian_row_size);intstack_cntr=0;// 4. 堆叠每个观测的雅可比for(constautocam_id:valid_cam_state_ids){Matrixdouble,4,6H_xi=Matrixdouble,4,6::Zero();Matrixdouble,4,3H_fi=Matrixdouble,4,3::Zero();Vector4d r_i=Vector4d::Zero();measurementJacobian(cam_id,feature.id,H_xi,H_fi,r_i);// 找到相机状态在状态向量中的位置autocam_state_iter=state_server.cam_states.find(cam_id);intcam_state_cntr=std::distance(state_server.cam_states.begin(),cam_state_iter);// 填入对应的列H_xj.block4,6(stack_cntr,21+6*cam_state_cntr)=H_xi;H_fj.block4,3(stack_cntr,0)=H_fi;r_j.segment4(stack_cntr)=r_i;stack_cntr+=4;}// 5. 零空间投影JacobiSVDMatrixXdsvd_helper(H_fj,ComputeFullU|ComputeThinV);MatrixXd A=svd_helper.matrixU().rightCols(jacobian_row_size-3);H_x=A.transpose()*H_xj;r=A.transpose()*r_j;}2.2 SVD分解// 对H_fj进行SVD分解JacobiSVDMatrixXdsvd_helper(H_fj,ComputeFullU|ComputeThinV);// 获取左奇异向量矩阵UMatrixXd U=svd_helper.matrixU();// 取后(m-3)列作为零空间基MatrixXd A=U.rightCols(jacobian_row_size-3);