目录
[2.1 几何学](#2.1 几何学)
[SO(3)上的 BCH 线性近似式](#SO(3)上的 BCH 线性近似式)
[2.2 运动学](#2.2 运动学)
[SO(3) + t 上的运动学](#SO(3) + t 上的运动学)
[2.3 滤波器和最优化理论](#2.3 滤波器和最优化理论)
[KF 卡尔曼滤波(线性系统)](#KF 卡尔曼滤波(线性系统))
[EKF 扩展卡尔曼 (非线性系统)](#EKF 扩展卡尔曼 (非线性系统))
2.1 几何学
向量的内积和外积

旋转矩阵

旋转向量

四元数
三维旋转也可以由单位四元数 表示。注意:单位四元数的逆等于其共轭 。即 。任意的旋转都可以由两个互为相反数的四元数表示。

旋转向量和四元数的转换关系如下:

李群和李代数

SO(3)上的 BCH 线性近似式
和
的括号里面只能是
,或者
,或者
。如果是
或者没有括号,表示省略。

2.2 运动学
李群视角下的运动学

SO(3) + t 上的运动学
其中 t 为平移向量。

线速度和加速度
注意:能被各种传感器(车速传感器,轮速计)测量到的速度是车体系速度,。
线速度的变换式:

加速度的变换式:

在实际的处理中,由于测量传感器只能测量离散化的值,在精度不高的应用场景中,我们通常会选择忽略后面三项,只保留最简单的转换关系。
扰动模型和雅可比矩阵
典型算例:对向量进行旋转

设扰动 对应的李代数为
:
对 进行泰勒展开并保留一阶项:
右扰动:
左扰动:
典型算例:旋转的复合


的一阶线性近似式 (视觉SLAM十四讲,p82):
① 对
求导,对
进行右扰动:
其中第 3 行的 ,根据
的一阶线性近似式得:
② 对
求导,对
进行右扰动:
其中第 2 行的 ,根据
的一阶线性近似式得:
2.3 滤波器和最优化理论
状态估计问题与最小二乘
注意:这里的运动噪声为 ,观测噪声为
,后续噪声的符号会变化,但表示的意义不变。

KF 卡尔曼滤波(线性系统)

EKF 扩展卡尔曼 (非线性系统)

矢量函数 在
点处进行线性化 。
在某一点
进行线性化的意思是:矢量函数
对状态
的雅可比矩阵,代入状态
的具体值。

为运动方程在上一时刻状态
进行线性化得到的雅可比矩阵,即运动方程对状态
的雅可比矩阵
,代入上一时刻状态
的具体值:
为观测方程在当前时刻预测状态
进行线性化得到的雅可比矩阵,即观测方程对状态
的雅可比矩阵
,代入当前时刻预测状态
的具体值:
这一块内容可以参考《机器人学中的状态估计》p89页,内容如下:

最优化方法和图优化
优化和滤波

