slam中的eskf观测矩阵推导

在之前的《slam中的eskf推导》一文中,没有写观测矩阵 H 矩阵的过程,现在补上这部分。

前置

列举几个等下推导需要用到的一些点:

平面特征点构造观测矩阵

例如在 fastlio 中,是利用平面特征点到拟合平面的距离来构造观测方程,利用平面特征点到拟合平面的距离应该趋向于0来列方程。假设拟合后的法向量为 u = [A, B, C]',截距为 D,点到平面的距离为 d = Ax + By + Cz + D

以距离来列观测方程:

对于 q 点的解释

对于平面方程,可以写作:

u'(x - q) = 0

u 是单位法向量,q 是平面上的任意已知点,点到平面的距离可以写作:

d = u'(x - q)

如果 x 也在平面上,则 d = 0

q 的选取不影响距离值的计算,因为:

u'(x - q1) = u'(x - q2)

推导观测方程

添加姿态偏差 δR,平移偏差 δt,以及噪声 V:

变换式子,可以得到:

等式两边对 δx 求偏导,得:

之后对 δx 中的角度 δθ 和 位移 δt 分别求偏导,得到:

对 δt 求偏导就比较简单了:

那么对于点 Pi 以及整体的观测矩阵 H:

相关推荐
放羊郎16 小时前
基于三维点云图的路径规划
人工智能·动态规划·slam·点云·路径规划·激光slam
求索小沈1 天前
ubuntu22.04 ros2 fast_lio2 复现
slam·ros2·重定位·建图·mid360·fast_lio2
WWZZ20252 天前
快速上手大模型:深度学习2(实践:深度学习基础、线性回归)
人工智能·深度学习·算法·计算机视觉·机器人·大模型·slam
WWZZ20257 天前
快速上手大模型:机器学习6(过拟合、正则化)
人工智能·算法·机器学习·计算机视觉·机器人·slam·具身感知
WWZZ20257 天前
快速上手大模型:机器学习5(逻辑回归及其代价函数)
人工智能·算法·机器学习·计算机视觉·机器人·slam·具身感知
WWZZ20257 天前
快速上手大模型:机器学习3(多元线性回归及梯度、向量化、正规方程)
人工智能·算法·机器学习·机器人·slam·具身感知
放羊郎9 天前
面向不同障碍物的自主导航策略
yolo·slam·识别·平滑·语义地图
WWZZ20259 天前
快速上手大模型:机器学习2(一元线性回归、代价函数、梯度下降法)
人工智能·算法·机器学习·计算机视觉·机器人·大模型·slam
小猫挖掘机(绝版)11 天前
kalibr进行相机内参以及相机imu的融合标定
ubuntu·无人机·slam·标定·vinsfusion
自然数e12 天前
视觉Slam14讲笔记第6讲非线性优化
slam·g2o