IEKF协方差矩阵的投影问题

[1344618323]

1. 卡尔曼增益后协方差的投影问题：

   slam_in_autonomous_driving/src/ch8/lio-iekf/iekf.hpp

   Line 254 in fe9c79b

   Vec3d dtheta = (R_.inverse() * start_R).log();

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

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

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

[ZYXiao-g]

同样发现这个问题了， 第三章eskf代码就是采用dx_, 到第8章代码就是用的R_.inverse() * start_R.从公式看确实是第8章代码有问题，在迭代的时候也是采用的R_.inverse() * start_R也是有问题的

[ZYXiao-g]

并且最后一步应该也是有问题的，更新完对P重新投影的时候，应该是切空间k->k+1的投影，而代码里面是0->k+1时刻的投影。所以迭代收敛完成的时候fast_lio里面直接是忽略掉这一步也是有道理的，此时的值非常小。

[gaoxiang12]

这算是一个trick。
本来每一次IEKF迭代的时候都要做一次切空间投影，这样能够实时得到投影后的P，也就是k->k+1的投影。
但实际上并没有这个必要，因为每次迭代重点是更新均值x并重新计算最近邻，而P并不参与这部分计算。所以可以在迭代结束时再计算P的投影，于是就像现在第8章实现那样，算完整个迭代再进行投影了。

[ZYXiao-g]

这里的疑问不是迭代过程中疑问，是最后迭代结束后切空间投影存在疑问，具体在图片中展示

[dongbo19]

I think it should be the transformation from the 0th iteration to the kth iteration, rather than the transformation from the kth to the (k+1)th iteration. You can refer to the paper 'Notes on Kalman Filter (KF, EKF, ESKF, IEKF, IESKF)', which provides a detailed introduction to the theory of IESKF.

[zubingtan]

这里的疑问不是迭代过程中疑问，是最后迭代结束后切空间投影存在疑问，具体在图片中展示

《自动驾驶与机器人中的SLAM技术》书上的是 dx，也就是k到k+1的投影；

@dongbo19 您好，想问一下 'Notes on Kalman Filter (KF, EKF, ESKF, IEKF, IESKF)' 书中何处指明 “it should be the transformation from the 0th iteration to the kth iteration” 呢？希望指明一下方向。我看到的是最后甚至都没有整体（cc @gaoxiang12 ）