分享好友 资讯首页 频道列表

FAST-LIO2: 快速直接的激光雷达-惯性里程计

2021-09-12 21:48
（参见（4）和（5））。此外，通过在
处进行的一阶近似来近似测量方程(5)，得到：
 
 
其中
，
是
相对于
的雅可比矩阵,在零处计算，
是由于原始测量噪声
引起的，而
被称为残差：
 
 
3）迭代更新：来自第IV-B1节的传播状态
和协方差
对未知状态xk施加先验高斯分布。更具体地说，
表示以下误差状态的协方差：
 
 
其中
是
相对于评估为零的
的偏微分，在零处计算：
 
 
其中
在[22,25]中被定义，
和
分别是IMU姿态状态和旋转外参的误差。对于第一次迭代，
，
 
除了先验分布，我们还有一个由测量方程（8）引起的状态分布：
 
 
将 (10) 中的先验分布与 (12) 中的测量模型相结合，得到由
等价表示的状态xk的后验分布，并且
的最大后验估计为：
 
 
其中
。这个MAP问题可以通过迭代卡尔曼滤波器解决，如下所示（为了简化符号，让
，P=
，
）：
 
 
请注意，卡尔曼增益K计算需要反转状态维度的矩阵，而不是以前工作中使用的测量维度。
重复上述过程直到收敛（即
）。

来源：同济智能汽车研究所

« 上一页 4/8 下一页 »
评论 0
同类信息
理想汽车VCOS通信E2E实现分析

2025-06-0548