[voxel_map.cpp] About StateEstimation formulas of measurement jacobian matrix H and the solution update

[gyyc233]

@xuankuzcr 郑博你好

StateEstimation 函数中关于IEFK measurement jacobian matrix H 以及 solution 计算时时涉及的一些变量不理解

• Hsub.row(i) << VEC_FROM_ARRAY(A), ptpl_list_[i].normal_[0], ptpl_list_[i].normal_[1], ptpl_list_[i].normal_[2]; Hsub矩阵的计算过程没看懂，方便提供H矩阵更详细的计算过程说明吗

https://github.com/hku-mars/FAST-LIVO2/blob/7e884601bcc193e3eabaf1958d4c14cdcff80212/src/voxel_map.cpp#L430-L457

• 关于 solution 计算公式后面添加vec.block<DIM_STATE, 1>(0, 0) - G.block<DIM_STATE, 6>(0, 0) * vec.block<6, 1>(0, 0); 的这两项的理解

https://github.com/hku-mars/FAST-LIVO2/blob/7e884601bcc193e3eabaf1958d4c14cdcff80212/src/voxel_map.cpp#L472

[thetengda]

分别可以参考VoxelMap、fast-lio2论文里的公式

[gyyc233]

分别可以参考VoxelMap、fast-lio2论文里的公式

谢谢。通过FAST-LIO，H矩阵已经了解

error state 增量的计算，对比程序，整体漏乘个-1 还在找原因

[yqmy0814]

这样够清楚吗

[gyyc233]

这样够清楚吗

thanks 我没有认真看到 meas_vec(i) = -ptpl_list_[i].dis_to_plane_; & auto vec = state_propagat - state_; // x_pred - x_k 的含义