search

gaoxiang12 / slam_in_autonomous_driving

《自动驾驶中的SLAM技术》对应开源代码
1.76k stars 430 forks source link

IEKF协方差矩阵的投影问题 #138

Open 1344618323 opened 5 months ago

1344618323 commented 5 months ago
  1. 卡尔曼增益后协方差的投影问题： https://github.com/gaoxiang12/slam_in_autonomous_driving/blob/fe9c79bf03cffb4b9e6388015a42c747c318a182/src/ch8/lio-iekf/iekf.hpp#L254

在update P那一步，cov_ = (Mat18T::Identity() - Qk * HTVH) * Pk 用得是上一步迭代（最后一个线性化点）后的协方差矩阵Pk，那么project P中计算 dtheta时应该用最后一次 dx_ 里的 $\theta$ 而不该是 (R_.inverse() * start_R).log()？

  1. 另外，我觉得公式8.5写错了，代码也有问题：书里写的是 $$J_{\theta}=I-\frac{1}{2} \delta \theta_k^{\wedge}, \delta \theta_k=Log(R_k^TR0)$$ 我觉得应该为： $$J{\theta}=I-\frac{1}{2} \delta \theta_k^{\wedge}, \delta \theta_k=Log(R_0^TR_k)$$

否则和公式3.61相反了。 希望高博能看到，哈哈