MSCKF里面的两个实用算法策略

滤波框架中融合相对观测【欢迎讨论】

以在无人车定位系统中融合GNSS，IMU，轮速，camera/lidar frame to map pose为例，想要在这个绝对定位系统中继续融合LO、VIO的relative pose该怎么做呢？（这里只考虑松耦合）

• 首先维护一个sliding window，类似MSCKF做状态增广
• 使用lidar 3d points ($P_{f_i}$)约束imu pose($T_{b_i}$)， lidar坐标系 $L$， imu坐标系 $b$
  • 回顾MSCKF算法中里面使用三维feature点约束cam pose的经验。具体做法是将残差函数线性化之后，求出feature points对cam pose的导数H，求其left null space matrix，然后两边都乘这个left null space matrix，可以把这个不想优化的feature points量从残差函数中消灭掉
  • 观测方程 $z=D({}^{L_m}{P}_{f_i},{}^{L_n}{P}_{f_i},{}^{L_n}{P}_{f_j},{}^{L_n}{P}_{f_k})+n_i$，这个表示 $L_m$ frame的一个点和 $L_n$ frame中的三个点构成点到面的距离，观测值真值是0，其中
    $${}^{L_m}{P}_{f_i}=T_{bl}^{-1}{T}_{wb}^{-1}{}^w{P}_{f_i}+n_i$$
    线性化残差函数
    $$r=\check{z}-z=H_x{\delta }_X+H_{L_m{P}_{f_i},{}^{L_n}{P}_{f_i},{}^{L_n}{P}_{f_j},{}^{L_n}{P}_{f_k}}\delta \left[L_m{P}_{f_i},{}^{L_n}{P}_{f_i},{}^{L_n}{P}_{f_j},{}^{L_n}{P}_{f_k}\right]$$
    这里 ${\delta }_X$ 表示系统状态如imu pose，lidar imu外参。然后按照上述MSCKF中对于feature的处理即可将LO紧耦合融合到定位系统中（重点是做好特征点的数据关联）。这个里面的第二项我认为也可以直接当做0，也就是认为lidar points没有测量误差。因为Lidar features points相比VO 三角化测量得到的feature points是准确的，这里我们认为lidar points没有测量误差的话是可以直接忽略掉而不用进行nullspace变换的。

解决观测维度过大计算K耗时增加

• 当观测量维度很大的时候，求解 $$(HP{H}^T+R{)}^{-1}$$计算量大，可以使用QR分解来重写残差函数降低计算耗时，重写之后计算量正比于系统的状态维度，详细可见文章