标签：LIO mathbf 定义 测量 论文 噪声 Fast widehat kappa
@
目录一、系统流程与符号说明
1、定义流形中的＋－
从上面的定义，很容易验证
IEKF
| 符号 | 含义 |
|---|---|
| \(\mathbf{z}_{j}^{\kappa}=\mathbf{G}_{j}\left({ }^{G} \widehat{\mathbf{p}}_{f_{j}}^{\kappa}-{ }^{G} \mathbf{q}_{j}\right)\) | \(\mathbf{z}_{j}^{\kappa}\) LOAM的点到线、 点到面误差作为残差 |
| G() | 计算点到线、点到面残差的函数 |
| \({ }^{G} \mathbf{q}_{j}\) | 真值 |
| \(\widehat{x}\) | IMU积分的当前位姿 |
| \(\widehat{\mathbf{P}}_{k}\) | IMU协方差 |
| \({ }^{L_{j}} \mathbf{n}_{f_{j}}\) | 雷达测量噪声 |
| \({ }^{L_{j}} \mathbf{p}_{f_{j}}^{\mathrm{gt}}={ }^{L_{j}} \mathbf{p}_{f_{j}}-{ }^{L_{j}} \mathbf{n}_{f_{j}}\) | 雷达真值 = 测量值 - 噪声 |
| T | 变换矩阵 |
观察方程：
用它在\(\widehat{\mathbf{x}}_{k}^{\kappa}\)处的一阶近似来逼近上面的方程会得到
\(\mathbf{H}_{j}^{\kappa}\) 是\(h_j()\)关于 \(\widetilde{\mathbf{x}}_{k}^{\kappa}\)的雅克比
\(v_j\) 测量噪声，这里定义为了一个白噪声
\(J^k\)是\(\left(\widehat{\mathbf{x}}_{k}^{\kappa} \mathbb{\square{}} \widetilde{\mathbf{x}}_{k}^{\kappa}\right) \square \widehat{\mathbf{x}}_{k}\) 的雅克比
迭代初值为： \(\widehat{\mathbf{x}}_{k}^{\kappa}=\widehat{\mathbf{x}}_{k}\), then \(\mathbf{J}^{\kappa}=\mathbf{I}\)
将(15)中的先验与(14)中的后验分布相结合，得到最大后验估计(MAP)：
得到迭代卡尔曼公式
R是雷达测量噪声矩阵
重复上面的步骤直至收敛,得到
其中卡尔曼增益公式（18）可以用下面的公式替换，他们是等价的。这避免了对测量矩阵H求逆
算法步骤
标签：LIO,mathbf,定义,测量,论文,噪声,Fast,widehat,kappa 来源： https://www.cnblogs.com/long5683/p/15084799.html
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。