目录

2.1 几何学

        向量的内积和外积

        旋转矩阵

        旋转向量

        四元数 

         李群和李代数

        SO(3)上的 BCH 线性近似式 

 2.2 运动学

        李群视角下的运动学

        SO(3) + t 上的运动学

         线速度和加速度

        扰动模型和雅可比矩阵

         典型算例：对向量进行旋转

        典型算例：旋转的复合

2.3 滤波器和最优化理论 

        状态估计问题与最小二乘

           KF 卡尔曼滤波（线性系统）

        EKF 扩展卡尔曼 （非线性系统）

         最优化方法和图优化​编辑

        优化和滤波

---

2.1 几何学

        向量的内积和外积

        旋转矩阵

        旋转向量

        四元数 

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

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

         李群和李代数

        SO(3)上的 BCH 线性近似式 

         和  的括号里面只能是  ，或者  ，或者 。如果是  或者没有括号，表示省略。

 2.2 运动学

        李群视角下的运动学

        SO(3) + t 上的运动学

        其中 t 为平移向量。

         线速度和加速度

        注意：能被各种传感器（车速传感器，轮速计）测量到的速度是车体系速度，。

        线速度的变换式：

        加速度的变换式：

        在实际的处理中，由于测量传感器只能测量离散化的值，在精度不高的应用场景中，我们通常会选择忽略后面三项，只保留最简单的转换关系。

        扰动模型和雅可比矩阵

         典型算例：对向量进行旋转

        设扰动  对应的李代数为  ：

  

       对  进行泰勒展开并保留一阶项：

         右扰动：

        左扰动：

 

        典型算例：旋转的复合

         的一阶线性近似式 （视觉SLAM十四讲，p82）：

      ①   对  求导，对  进行右扰动：

     

         其中第 3 行的 ，根据  的一阶线性近似式得：

        ②   对  求导，对  进行右扰动：

     

         其中第 2 行的 ，根据  的一阶线性近似式得：

2.3 滤波器和最优化理论 

        状态估计问题与最小二乘

        注意：这里的运动噪声为  ，观测噪声为  ，后续噪声的符号会变化，但表示的意义不变。

           KF 卡尔曼滤波（线性系统）

        EKF 扩展卡尔曼 （非线性系统）

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

         

 

         为运动方程在上一时刻状态  进行线性化得到的雅可比矩阵，即运动方程对状态  的雅可比矩阵，代入上一时刻状态  的具体值：

         为观测方程在当前时刻预测状态  进行线性化得到的雅可比矩阵，即观测方程对状态  的雅可比矩阵，代入当前时刻预测状态  的具体值：

        这一块内容可以参考《机器人学中的状态估计》p89页，内容如下：

         最优化方法和图优化

        优化和滤波