紧耦合步骤
- 同步IMU与LIDAR数据
- IMU运动方程得到预测值
- 利预测值对点云去除畸变
- 预测值作为初始值计算残差
- 观测残差和先验残差迭代更新直至收敛
- 更新到卡尔曼滤波器
- 状态变量
- 运动方程
- 推算误差协方差
B.更新步 IEKF的观测过程是迭代的,从 x 0 x_0 x0, P 0 P_0 P0出发,不断迭代观测模型,计算出本次迭代的δx,进而得到下一次迭代的x和 P,最终收敛。 - 更新初始化收敛条件:状态增量的变化量小于某个阈值 考虑第k次迭代,每次迭代需要对P进行切空间的投影变换,记第k次迭代的那个切空间变换雅可比矩阵为
J
K
J_K
JK
计算残差
更新状态估计
上述迭代过程可被描述为:
从直观上看,这个公式平衡了两种信息: 测量信息:通过残差,反映了测量值与预测值之间的差异。 先验信息:反映了状态增量的大小,考虑了先验状态估计的不确定性