【自动驾驶中的SLAM技术】第2讲：基础数学知识回顾

第二讲：基础数学回顾

1 几何学

1.1 坐标系

  车辆上会装有各式各样的传感器，下面得顺序都是XYZ。车辆本体一般使用前左上(XYZ)或者右前上的顺序来定义它的坐标系，相机坐标系则普遍取右下前的顺序，激光雷达和GNSS通常采用前左上(XYZ)得顺序。

1.2 坐标变换

  这里定义的是坐标之间的变换关系。$R_{wb}$ 和 $t_{wb}$ 都用于处理向量之间的坐标变换。而有些资料处理的是坐标轴（或者坐标基底）之间的变换关系，把旋转和平移解释成某个坐标轴从一处变换到了另一处。那样的定义方式是与这里相反
$$p_w=R_{wb}{p}_b+t_{wb}$$
  这里相反得本质就是，基变换与坐标变换得关系(矩阵论第一讲)

① 空间向量

  对于一个向量来讲，它不会随空间基向量变化而变化，变化的只是其在不同基下的空间坐标！向量$c$在两组空间基$({\alpha }_1,{\alpha }_2,\dots ,{\alpha }_n)$和$({\beta }_1,{\beta }_2,\dots ,{\beta }_n)$下的坐标分别为 [ a 1 a 2 . . . a n ] \begin{bmatrix}a_1\\a_2\\.\\.\\.\\a_n\end{bmatrix} ​a1​a2​...an​​ ​和 [ b 1 b 2 . . . b n ] \begin{bmatrix}b_1\\b_2\\.\\.\\.\\b_n\end{bmatrix} ​b1​b2​...bn​​ ​。通过下面这一关系，我们可以得到基变换和坐标变换公式！

c = ( α 1 , α 2 , … , α n ) [ a 1 a 2 . . . a n ] = ( β 1 , β 2 , … , β n ) [ b 1 b 2 . . . b n ] ( 1 ) c=(\alpha_1,\alpha_2,\ldots,\alpha_n)\begin{bmatrix}a_1\\a_2\\.\\.\\.\\a_n\end{bmatrix}=(\beta_1,\beta_2,\ldots,\beta_n)\begin{bmatrix}b_1\\b_2\\.\\.\\.\\b_n\end{bmatrix}\quad(1) c=(α1​,α2​,…,αn​) ​a1​a2​...an​​ ​=(β1​,β2​,…,βn​) ​b1​b2​...bn​​ ​(1)

② 基变换

  以三维空间为例，两组空间基存在以下的变换
$$({\beta }_1{\beta }_2{\beta }_3{)}_{1\times 3}=({\alpha }_1{\alpha }_2{\alpha }_3{)}_{1\times 3}\cdot A_{3\times 3}(2)$$

③ 坐标变换

  把基变换公式(2)带入公式(1)，即可得到坐标变换公式
[ a 1 a 2 a 3 ] = A 3 × 3 [ b 1 b 2 b 3 ] ( 3 ) \begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix} = A_{3\times3}\begin{bmatrix}b_1\\b_2\\b_3\end{bmatrix} \quad(3) ​a1​a2​a3​​ ​=A3×3​ ​b1​b2​b3​​ ​(3)

[ b 1 b 2 b 3 ] = A 3 × 3 − 1 [ a 1 a 2 a 3 ] ( 4 ) \begin{bmatrix}b_1\\b_2\\b_3\end{bmatrix}=A_{3\times3}^{-1}\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix} \quad(4) ​b1​b2​b3​​ ​=A3×3−1​ ​a1​a2​a3​​ ​(4)

④ 总结

  从公式(2)和公式(4)可以看出坐标变换和基变换的关系，互为逆矩阵！

坐标变换：左乘
$$p_w=R_{wb}{p}_b+t_{wb}$$
基(坐标轴)变换：右乘转置（逆）
$$P_w=P_b{R}_{wb}^T+t_{wb}$$
旋转不影响平移，平移都是以原点来看的(不论是坐标变换，还是基变换)。

1.3 四元数与旋转向量

  利用单位四元数$\mathbf{q}=q_0+q_1\mathrm{i}+{\mathrm{q}}_2\mathrm{j}+{\mathrm{q}}_3\mathrm{k}$表示旋转，实部表示了旋转角，虚部表示了旋转轴。下面公式可知，实部为0时，虚部刚好为转轴。，

$$\mathbf{q}=[\cos \frac{\theta }{2},n\sin \frac{\theta }{2}{]}^{\mathrm{T}}.$$

$$\{\begin{array}{l}\theta =2\arccos q_0\\ {\left[n_x,n_y,n_z\right]}^{\mathrm{T}}={\left[q_1,q_2,q_3\right]}^{\mathrm{T}}/\sin \frac{\theta }{2}\end{array}.$$

  不是是旋转向量，还是四元数，它们只能旋转向量，不可以改变向量的尺度！所以，对于四元数，只可使用单位四元数，对尺度无影响；对于旋转向量，转轴是单位向量，长度只能是转角！

2 运动学

  用 ω 来表达角速度，用 ϕ 来表达旋转向量

2.1 李群视角

  当时间连续变化时，旋转和平移就成为了随时间变化的函数 $R(t) $和$ t(t)$。平移变量本质就是一个在不断变化的量，这里忽略。
$$R^{\top }R=I$$
  上式对时间的求导：
$$\frac{\mathrm{d}}{\mathrm{d}t}\left(R^{\top }R\right)={\dot{R}}^{\top }R+R^{\top }\dot{R}=0$$

$$R^{\top }\dot{R}=-(R^{\top }\dot{R}{)}^{\top }$$

  然后，注意到反对称矩阵经过转置运算后再取负保持不变，即$-(R^{\top }\dot{R}{)}^{\top }=R^{\top }\dot{R}$，为了简化表达形式，令${\omega }^{\wedge }\in {\mathbb{R}}^{3\times 3}=R^{\top }\dot{R}$，则得泊松方程：
$$\begin{gathered} \dot{R}=-R(R^{\top }\dot{R}{)}^{\top } \\ =R{\omega }^{\wedge } \end{gathered}$$
  求导后是一种特殊的微分方程，这种方程都有类似得通解，比如$y^{\prime }=a{e}^{ax},y(0)=1$，则$y=e^{ax}$。

  考虑瞬时变化，那么在固定得时刻$t$，$\omega$就是一个确定的数，即角速度。微分方程初解：$t_0$ 时刻旋转矩阵为 $R(t_0)$。类比可以得到旋转矩阵微分方程得通解：
$$\mathbf{R}(t)=\mathbf{R}(t_0)\exp ({\mathbf{\omega }}^{\wedge }(t-t_0))$$
  计$\Delta t=t-t_0$，则：
$$\mathbf{R}(t)=\mathbf{R}(t_0)\mathrm{Exp}(\mathbf{\omega }\Delta t)$$
  在$t=t_0$处进行泰勒展开：
$$\begin{array}{rl}\mathbf{R}(t_0+\Delta t)& \approx \mathbf{R}(t_0)+\dot{\mathbf{R}}(t_0)\Delta t\\ & =\mathbf{R}(t_0)+\mathbf{R}(t_0){\mathbf{\omega }}^{\wedge }\Delta t\\ & =\mathbf{R}(t_0)(\mathbf{I}+{\mathbf{\omega }}^{\wedge }\Delta t).\end{array}$$
  侧面反映了指数映射形式，注意这里时大写的Exp，所以没有加反对称符号！
$$\mathrm{Exp}(\mathbf{\omega }\Delta t)=\mathbf{I}+{\mathbf{\omega }}^{\wedge }\Delta t+\frac{1}{2}({\mathbf{\omega }}^{\wedge }\Delta t{)}^2+...$$

关于这里微分方程求解的补充，可以用定积分求解的方法直接求解这个微分方程的通解，这个解在IMU的运动学方程之离散旋转矩阵中也会用到。

2.2 四元数视角

  在旋转矩阵R的求导中，利用$R^{\top }R=I$这一性质。只能使用单位四元数表示旋转，$q{q}^{\ast }=q^{\ast }q=1$，$q{q}^{\ast }$是两个四元数的模长乘积，仍然是1。非常类似旋转矩阵形式。

  对时间求导：
$$\dot{q^{\ast }q}+q^{\ast }\dot{q}=0$$

$$q^{\ast }\dot{q}=-\dot{q^{\ast }q}=-({\mathbf{q}}^{\ast }\dot{\mathbf{q}}{)}^{\ast }$$

  从上面这个式子中可以得到一个信息，$q^{\ast }\dot{q}$是一个纯虚四元数(实部为0)！这是因为负号的存在，共轭只是虚部不一样，但实部一致，那么就只有0满足条件了。

  记一个纯虚四元数$\varpi =[0,\underset{\omega }{{\omega }_1,{\omega }_2,{\omega }_3}{]}^{\top }\in \mathcal{Q}$，然后令：
$$q^{\ast }\dot{q}=\varpi$$
  通过下面推导，得到四元数关于时间的导数：
$$\begin{array}{rl}q{q}^{\ast }\dot{q}& =q\varpi st:q{q}^{\ast }=q^{\ast }q=1\\ \dot{q}& =q\varpi \end{array}$$
  类比于 SO(3) 的情况，我们也可以讨论在 t 时刻附近的瞬时角速度、李代数、指数映射。在考虑瞬时变化时，可以认为 $\varpi$ 为固定值，于是上述微分方程给出解为：
$$\mathbf{q}(t)=\mathbf{q}(t_0)\exp (\varpi \Delta t)$$
  指数映射：
$$\exp \left(\varpi \right)=\sum _{k=0}^{\infty }\frac{1}{k!}{\varpi }^k$$
  四元数可以表示为角轴，即方向与长度$\varpi =\mathbf{u}\theta$，$\theta$是长度，$u$是纯虚单位四元数。纯虚四元数有以下性质：
$$u^2=-1,u^3=-u$$
  代入指数映射(会得到一个非常类似于欧拉公式的式子)：
$$\begin{array}{rl}\exp \left(\mathbf{u}\theta \right)& =1+\mathbf{u}\theta -\frac{1}{2!}{\theta }^2-\frac{1}{3!}{\theta }^3\mathbf{u}+\frac{1}{4!}{\theta }^4+\dots \\ & =\underset{\cos \theta }{\left(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-\dots \right)}+\underset{\sin \theta }{\left(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-\dots \right)}\mathbf{u}\\ & =\cos \theta +u\sin \theta .\end{array}$$
  即
$$\exp (\varpi )=[\cos \theta ,u\sin \theta {]}^{\top }$$
  求模长后发现，纯虚四元数的指数映射结果是一个单位四元数，反过来想，一个单位四元数的李代数就是一个纯虚四元数！
$$\Vert \exp (\varpi )\Vert ={\cos }^2\theta +{\sin }^2\theta \Vert \mathbf{u}{\Vert }^2=1$$

2.3 四元数的李代数与旋转向量间的转换

  首先，一个单位四元数$q$对于的李代数为一个纯虚四元数$\varpi =\mathbf{u}\theta$，李代数通过指数映射得到的四元数为：
$$q=\exp (\varpi )=[\cos \theta ,u\sin \theta {]}^{\top }(5)$$
  其次，寻找一个旋转矩阵R对应的单位四元数的李代数！
$$R=\mathrm{Exp}(\varphi )=\mathrm{Exp}(\theta {}^{\prime }\mathbf{n})$$
  这时候旋转向量$\varphi$对应的四元数为：
$$q=[\cos \frac{{\theta }^{\prime }}{2},n\sin \frac{{\theta }^{\prime }}{2}](6)$$
​

  注意公式5和6中纯虚四元数$\varpi [0,\underset{\omega }{{\omega }_1,{\omega }_2,{\omega }_3}{]}^{\top }=\mathbf{u}\theta$和$\varphi ={\theta }^{\prime }n$中的角度差了一倍！旋转矩阵中的角度是纯虚四元数的2倍，即$\frac{{\theta }^{\prime }}{2}=\theta$！因为旋转矩阵一半角度和其全部角度等价！

  也就是说，可得：
$$\varpi =[0,\frac{1}{2}\varphi {]}^{\top },\text{或}\omega =\frac{1}{2}\varphi .$$
  四元数表达的角速度正好是 SO(3) 李代数的一半！这与四元数在旋转一个矢量时，要乘两遍相对应。

• 总结

  对于一个三维瞬时角速度（或者优化函数的目标更新量）$\omega \in {\mathbb{R}}^3$, 定义在SO(3)上的运动形式为：
$$\dot{R}=R{\omega }^{\wedge }$$

$$R=Exp(\omega )=\exp ({\omega }^{\wedge })$$
  如果这个量作为纯虚四元数的更新量（通常由优化函数求解得到），那么对应的四元数应该只更新它的一半
$$\dot{q}=\frac{1}{2}q[0,\omega {]}^{\top }$$
  上面$\omega$是三维，消除括号后已经变成了一个四元数：
$$\dot{q}=\frac{1}{2}q\omega$$
  四元数指数映射也可以类似也写作
$$q=\exp (\frac{1}{2}[0,\omega {]}^{\top })\stackrel{\Delta }{=}\exp (\omega )$$
  若$\omega$是一个较小量，$\cos (\frac{\theta }{2})\approx 1,n\sin \frac{\theta }{2}\approx n\frac{\theta }{2}$,简化
$$\mathrm{Exp}(\omega )\approx [1,\frac{1}{2}\omega ]$$
  四元数更新公式：
$$q\mathrm{Exp}(\omega )\approx \mathbf{q}[1,\frac{1}{2}\omega ]$$

2.4 SO(3)+t 上的运动学

  用 ω 来表达角速度,t是平移量，v表示线速度
$$\dot{R}=R{\omega }^{\wedge },\dot{t}=v$$

2.5 线速度与加速度

  考虑线速度和加速度在不同坐标系之间的变换关系。这里考虑只带旋转关系的两个坐标系之间的线速度和加速度的变换关系。

  考虑坐标系1和2,某个向量p在两个系下坐标为$p_1,p_2$ ，它们满足关系$p_1=R_{12}{p}_2$

对上述关系进行求导：
$$\begin{array}{rl}{\dot{p}}_1& ={\dot{R}}_{12}{\mathbf{p}}_2+{\mathbf{R}}_{12}{\dot{\mathbf{p}}}_2\\ & =R_{12}{\omega }^{\wedge }{p}_2+R_{12}{\dot{p}}_2\\ & ={\mathbf{R}}_{12}({\mathbf{\omega }}^{\wedge }{\mathbf{p}}_2+{\dot{\mathbf{p}}}_2).\end{array}$$
  我们发现，$p$ 在两个系下的速度矢量是不同的，不是同一个矢量在不同坐标系中的表达。

  一般来讲，位置的导数是速度，即${\dot{p}}_1={\mathbf{v}}_1,{\dot{p}}_2={\mathbf{v}}_2$，代入上式：
$$v_1=R_{12}({\omega }^{\wedge }{p}_2+v_2)$$
  速度信息对时间求导：
$$\begin{array}{c}{\dot{v}}_1={\dot{R}}_{12}\left({\mathbf{\omega }}^{\wedge }{\mathbf{p}}_2+{\mathbf{v}}_2\right)+{\mathbf{R}}_{12}\left({\dot{\mathbf{\omega }}}^{\wedge }{\mathbf{p}}_2+{\mathbf{\omega }}^{\wedge }{\dot{\mathbf{p}}}_2+{\dot{\mathbf{v}}}_2\right)\\ =R_{12}\left({\omega }^{\wedge }{\omega }^{\wedge }{p}_2+{\omega }^{\wedge }{v}_2+{\dot{\omega }}^{\wedge }{p}_2+{\omega }^{\wedge }{\dot{p}}_2+{\dot{v}}_2\right)\\ ={\mathbf{R}}_{12}\left({\dot{\mathbf{v}}}_2+2{\mathbf{\omega }}^{\wedge }{\mathbf{v}}_2+{\dot{\mathbf{\omega }}}^{\wedge }{\mathbf{p}}_2+{\mathbf{\omega }}^{\wedge }{\mathbf{\omega }}^{\wedge }{\mathbf{p}}_2\right)\end{array}$$
  定义加速度$a_1={\dot{v}}_1,a_2={\dot{v}}_2$，则得到
$$a_1={\mathbf{R}}_{12}(\underset{\text{加速度}}{a_2}+\underset{\text{科氏加速度}}{2{\mathbf{\omega }}^{\wedge }{\mathbf{v}}_2}+\underset{\text{角加速度}}{{\dot{\mathbf{\omega }}}^{\wedge }{\mathbf{p}}_2}+\underset{\text{向心加速度}}{{\mathbf{\omega }}^{\wedge }{\mathbf{\omega }}^{\wedge }{\mathbf{p}}_2})$$
  在精度不高的应用场景中，通常会选择忽略后面三项，只保留最简单的转换关系

  车辆本身的速度，也就是车体坐标系原点在世界系下的速度（车体原点在车辆系下速度一直是零，没有实际意义）。这个速度是定义在世界系中的，记为$v_w$ 。如果左乘 $R_{bw}$ ，也可以将这个矢量转换到车辆坐标系下，记作$v_b$,称其为车体系速度。

2.6 扰动模型

• 对$R_1$进行右扰动求导

$$\begin{array}{rl}\frac{\partial \mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}{\partial {\mathbf{R}}_1}& =\underset{\varphi \to 0}{\lim }\frac{\mathrm{Log}\left({\mathbf{R}}_1\mathrm{Exp}\left(\varphi \right){\mathbf{R}}_2\right)-\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2\mathrm{Exp}\left({\mathbf{R}}_2^{\top }\mathbf{\varphi }\right)\right)-\mathrm{Log}\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}{\varphi }\\ & ={\mathbf{J}}_r^{-1}(\mathrm{Log}({\mathbf{R}}_1{\mathbf{R}}_2)){\mathbf{R}}_2^{\top }.\end{array}$$

其中第二行的转换使用到$SO(3)$的伴随性质：
$$R^{\top }\mathrm{Exp}(\varphi )R=\mathrm{Exp}({\mathbf{R}}^{\top }\varphi )$$
第三行使用了BCH近似公式：
$$\mathrm{Log}\left(R_1{R}_2\mathrm{Exp}\left(R_2^{\top }\varphi \right)\right)=\mathrm{Log}(R_1{R}_2)+J_r^{-1}(R_1{R}_2)\mathrm{Log}(\mathrm{Exp}(R_2^{\top }\varphi ))$$

• 对$R_2$进行右扰动求导

$$\frac{\partial \mathrm{Log}\left(R_1{R}_2\right)}{\partial R_2}=\underset{\varphi \to 0}{\lim }\frac{\mathrm{Log}\left(R_1{R}_2\mathrm{Exp}\left(\varphi \right)\right)-\mathrm{Log}\left(R_1{R}_2\right)}{\varphi }=J_r^{-1}(\mathrm{Log}(R_1{R}_2))$$

• 不能直接说$R1R2$对 $R1$ 或 $R2$ 的导数，那样就变成矩阵对向量求导，无法使用矩阵来描述了。在不引入张量的前提下，我们最多只能求向量到向量的导数。因此分子部分必须加上 Log 符号，取为矢量

2.7 关于左扰动和右扰动的选择

左右扰动由状态定义和误差状态定义形式确定

2.7.1 第一种形式

1. 定义状态：定义系统旋转状态为Globally形式：$R_L^G$ ，即pose表达在Global坐标系（平移量在Global坐标系下）

2. 定义误差状态：旋转的误差状态为：${\theta }_{L^{\prime }}^L$

3. 确认状态的叠加方式：根据旋转的链式法则，叠加了扰动的旋转为:

$$R_{L^{\prime }}^G=R_L^G{R}_{L^{\prime }}^L=R_L^G(I+[{\theta }_{L^{\prime }}^L{]}^{\times })$$

  扰动的叠加是右扰动形式

2.7.2 第二种形式

1. 定义状态：定义系统旋转状态为Locally形式：$R_G^L$ ，即pose表达在Local坐标系（平移量在local坐标系下）
2. 定义误差状态：旋转的误差状态为：${\theta }_{L^{\prime }}^L$
3. 确认状态的叠加方式：根据旋转的链式法则，叠加了扰动的旋转为:

$$R_G^{L^{\prime }}=R_L^{L^{\prime }}{R}_G^L=(I+[{\theta }_L^{L^{\prime }}{]}^{\times })R_G^L=(I-[{\theta }_{L^{\prime }}^L{]}^{\times })R_G^L$$

​ 扰动的叠加是左扰动形式，注意上面括号里面+-号，转换为四元数的话，+表示了常见的Hamilton四元数，-表示了JPL四元数。两个微小四元数表示的旋转是相反的。

2.8 运动学示例：圆周运动

bin/motion −−use_quaternion=true −−angular_velocity=15   #  

搞清楚body系与世界系
1 body系：有恒定的线速度和恒定的角速度，那么就一定可以表示为圆周运动
2 世界系：body系下速度不发生变化，但旋转到世界系就会变化。我们求的位移是世界系的位移，所以要转换速度
3 pose.so3()就相当于$R_{wb}$，角速度omega就不需要转换到世界系，这里位姿的变换是右扰动（全局坐标系）。$R_{w{b}^{\prime }}=R_{wb}{R}_{b{b}^{\prime }}$

​

#include 
#include 

#include "common/eigen_types.h"
#include "common/math_utils.h"
#include "tools/ui/pangolin_window.h"

/// 本节程序演示一个正在作圆周运动的1车辆
/// 车辆的角速度与线速度可以在flags中设置

DEFINE_double(angular_velocity, 10.0, "角速度（角度）制");		// 这里都是宏变量
DEFINE_double(linear_velocity, 5.0, "车辆前进线速度 m/s");
DEFINE_bool(use_quaternion, false, "是否使用四元数计算");

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);// 这行代码用于初始化Google日志系统。它将日志输出到标准错误输出（stderr）
    FLAGS_stderrthreshold = google::INFO;	// 这行代码设置了在stderr中显示的日志级别。在这里，将日志级别设置为INFO，这意味着只有INFO级别及以上的日志会被输出到stderr
    FLAGS_colorlogtostderr = true;	// 在stderr中显示彩色日志输出
    google::ParseCommandLineFlags(&argc, &argv, true);	// 解析命令行参数。它会检查命令行中的标志参数（例如--flag_name=value）并根据相应的定义来设置相应的标志值
		
    /// 可视化
    sad::ui::PangolinWindow ui;
    if (ui.Init() == false) {
        return -1;
    }

    double angular_velocity_rad = FLAGS_angular_velocity * sad::math::kDEG2RAD;  // 弧度制角速度
    SE3 pose;                                                                    // TWB表示的位姿
    Vec3d omega(0, 0, angular_velocity_rad);    // 角速度矢量   00z  如果z=1是角轴，这样是旋转向量，值代表角度
    Vec3d v_body(FLAGS_linear_velocity, 0, 0);  // 本体系速度  所以只有x方向有值
    const double dt = 0.05;                                                      // 每次更新的时间

    while (ui.ShouldQuit() == false) {
        // 更新自身位置
        Vec3d v_world = pose.so3() * v_body;
        pose.translation() += v_world * dt;

        // 更新自身旋转
        if (FLAGS_use_quaternion) {
            Quatd q = pose.unit_quaternion() * Quatd(1, 0.5 * omega[0] * dt, 0.5 * omega[1] * dt, 0.5 * omega[2] * dt);
            q.normalize();
            pose.so3() = SO3(q);
        } else {
            pose.so3() = pose.so3() * SO3::exp(omega * dt);
        }

        LOG(INFO) << "pose: " << pose.translation().transpose();
        ui.UpdateNavState(sad::NavStated(0, pose, v_world));

        usleep(dt * 1e6);
    }

    ui.Quit();
    return 0;
}

3 滤波器与最优化理论

3.1 状态估计问题与最小二乘

  首先要知道状态是什么？在多个传感器中，状态可以是旋转、平移、速度等等。

​  状态估计，就是我们要通过什么样的方式去估计状态量，得到的值是否正确，是否值得信赖。

  典型的离散时间状态估计由运动方程（如IMU/Wheel）和观测方程（cam/lidar/GNSS）组成
$$\{\begin{array}{l}{x}_k=f\left(x_{k-1},u_k\right)+{\mathbf{w}}_k,k=1,\dots ,N\\ z_k=\mathbf{h}\left(x_k\right)+{\mathbf{v}}_k,\end{array}$$

  上面是非线性函数，将其线性化，就可以得到线性高斯系统得状态估计问题。线性系统的无偏最优估计由KF给出。

$$\{\begin{array}{l}{x}_k=A_k{\mathbf{x}}_{k-1}+{\mathbf{u}}_k+{\mathbf{w}}_k\\ {\mathbf{z}}_k=C_k{\mathbf{x}}_k+{\mathbf{v}}_k\end{array}$$

​

3.2 卡尔曼滤波器

预测
$$x_{k,\text{pred}}=A_k{x}_{k-1}+u_k,P_{k,\text{pred}}=A_k{P}_{k-1}{A}_k^{\top }+R_k$$
更新
$$K_k=P_{k,\text{pred}}{C}_k^{\top }{\left(C_k{P}_{k,\text{pred}}{C}_k^{\top }+Q_k\right)}^{-1}$$
计算后验概率的分布
$$\begin{array}{rl}& {\mathbf{x}}_k={\mathbf{x}}_{k,\mathrm{pred}}+{\mathbf{K}}_k\left({\mathbf{z}}_k-{\mathbf{C}}_k{\mathbf{x}}_{k,\mathrm{pred}}\right),\\ & {\mathbf{P}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k\right){\mathbf{P}}_{k,\mathrm{pred}}.\end{array}$$

补充：关于最后那个协方差矩阵的更新，有三种形式

1 $\begin{array}{r}P=(I-KH)P\end{array}$ 最常见的卡尔曼滤波的形式，数值稳定性较差，计算的协方差不能保证对称正定。

2 $P=P-K(HP{H}^T+R)K^T$: 对称形式，计算的协方差仍是对称阵。

3 $P\leftarrow (I-KH)P(I-KH{)}^T+KR{K}^T$:对称正定形式，计算出的协方差是仍是对称正定阵

$$\begin{array}{rl}K=P{H}^T(HP{H}^T+R{)}^{-1}& \Rightarrow K(HP{H}^T+R)=P{H}^T\\ P-K(HP{H}^T+R)K^T& =P-P{H}^T{K}^T\\ & =P^T-KH{P}^T\\ & =(I-KH)P\\ (I-KH)P(I-KH{)}^T+KR{K}^T& =(I-KH)P-(I-KH)P{H}^T{K}^T+KR{K}^T\\ & =(I-KH)P-P{H}^T{K}^T+K(HP{H}^T+R)K^T\\ & =(I-KH)P-P{H}^T{K}^T+P{H}^T{K}^T\end{array}$$

3.3 非线性系统的处理方法-EKF

  假设一个高斯分布状态变量经过非线性函数后仍为高斯分布

$$x_{k,\mathrm{pred}}=f(x_{k-1},u_k),P_{k,\mathrm{pred}}=F_k{P}_{k-1}{F}_k^{\top }+R_k.$$

$$\begin{array}{rl}& K_k=P_{k,\mathrm{pred}}{H}_k^{\top }(H_k{P}_{k,\mathrm{pred}}{H}_k^{\top }+Q_k{)}^{-1},\\ & x_k=x_{k,\text{pred}}+K_k(z_k-H_k{x}_{k,\text{pred}}),\\ & P_k=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k){\mathbf{P}}_{k,\mathrm{pred}}.\end{array}$$

2.4 最优化方法与图优化

  运动学方程和观测方程都可以看成一个状态变量 x 与运动学输入、观测值之间的残差，这是一种批量最小二乘

$$\begin{array}{rl}{e}_{\text{motion}}& =x_k-f(x_{k-1},u)\sim \mathcal{N}(0,R_k),\\ e_{\text{obs}}& =z_k-h(x_k)\sim \mathcal{N}(0,Q_k).\end{array}$$

  对于上面提及的滤波器这种处理方法，其中的最优状态估计可以看成关于各种误差项的最小二乘问题（比较计算了协方差和预测，那么后验估计就非常类似这里的批量最小二乘）

$$x^{\ast }=\arg \underset{x}{\min }\sum _k\left(e_k^{\top }{\Omega }_k^{-1}{e}_k\right)$$

  如果是迭代最小二乘，处理方法按下面步骤。

关于滤波器与最优化的区别与联系,

  它们在线性系统中会得到同样的结果 ，但在非线性系统中通常不然。主要原因有以下几个

1. 最优化方法有迭代过程，而 EKF 则没有。

2. 迭代过程会不断在新的线性化点$x_i$上求取雅可比矩阵，而EKF的雅可比矩阵只在预测位置上求取一次。

3. EKF还会区分 $x_{k,pred}$ ,分开处理预测过程与观测过程。而最优化方法则没有$x_{k,pred}$，统一处理各处的状态变量。

​ 如果忽略上面差异，把卡尔曼滤波当作一种优化，那么它的优化变量和误差函数是什么（图优化里面最关键的地方）

2个优化变量：$x_{k-1}$和$x_k$，即上一个时刻与当前时刻状态。（图优化里面如滑动窗口，是一段时间内的状态；如果是全局优化，那么就是全部的状态）

3个误差函数：先验误差、运动误差、观测误差。（图优化：在纯视觉里面就是观测误差）

注意：普通的优化器没有计算协方差矩阵，需要边缘化Marginalization。

4 关于环境编译

基础环境：Ubuntu20.04

ROS环境：Noetic

​ 然后根据GitHub安装相应依赖

sudo apt install -y ros-noetic-pcl-ros ros-noetic-velodyne-msgs libopencv-dev libgoogle-glog-dev libeigen3-dev libsuitesparse-dev libpcl-dev libyaml-cpp-dev libbtbb-dev libgmock-dev

​ 额外安装Pangolin、g2o库，Sophus库作者把相应的文件提取出来了，无需安装。

mkdir build		# 进入到thirdparty/xxx  进行相应的编译安装
cd build
cmake ..
make -j8

​ 这本书的程序就是一个大的工程项目，所以直接编译

mkdir build
cd build
cmake ..
make -j8

​ 因为每一次编译都很费劲，所以可以先编译使用的章节，注释其它章节

add_subdirectory(common)
add_subdirectory(ch2)
add_subdirectory(ch3)
# add_subdirectory(ch4)
# add_subdirectory(ch5)
# add_subdirectory(ch6)
# add_subdirectory(ch7)
# add_subdirectory(ch8)
# add_subdirectory(ch9)
# add_subdirectory(ch10)

add_subdirectory(tools)

5 习题

5.1 计算

$$\frac{\partial R^{-1}p}{\partial R}.$$

右扰动

$$\begin{array}{rl}\begin{array}{r}\frac{\partial \left({\mathbf{R}}^{-1}\mathbf{p}\right)}{\partial \phi }\end{array}& \begin{array}{r}=\underset{\mathbf{\phi }\to 0}{\lim }\frac{(\mathbf{R}\exp {\left({\mathbf{\phi }}^{\wedge })\right)}^{-1}\mathbf{p}-{\mathbf{R}}^{-1}\mathbf{p}}{\phi }\end{array}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\left(\mathbf{I}-{\mathbf{\phi }}^{\wedge }\right){\mathbf{R}}^{-1}\mathbf{p}-{\mathbf{R}}^{-1}\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{({\mathbf{R}}^{-1}\mathbf{p}{)}^{\wedge }\mathbf{\phi }}{\phi }=({\mathbf{R}}^{-1}\mathbf{p}{)}^{\wedge }\end{array}$$

左扰动：和右扰动推导一致，最后结果应该是$-{\mathbf{R}}^{-1}{\mathbf{p}}^{\wedge }$

5.2 计算

$$\frac{\partial R_1{R}_2^{-1}}{\partial R_2}.$$

右扰动

  以右扰动为例，第一行到第二行求$\exp ({\varphi }^{\wedge }{)}^{-1}=\exp (-{\varphi }^{\wedge })\approx \mathbf{I}-{\varphi }^{\wedge }$，第三行到第四行利用SO(3)的伴随性质，第四行到第五行是对数形式的BCH近似公式。
$$\begin{array}{rl}\frac{d\ln ({\mathbf{R}}_1{\mathbf{R}}_2^{-1}{)}^{\vee }}{d{\mathbf{R}}_2}& =\underset{\psi \to 0}{\lim }\frac{\ln ({\mathbf{R}}_1({\mathbf{R}}_2\exp ({\psi }^{\wedge }){)}^{-1}{)}^{\vee }-\ln ({\mathbf{R}}_1{\mathbf{R}}_2^{-1}{)}^{\vee }}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{\ln ({\mathbf{R}}_1\exp (-{\psi }^{\wedge }){\mathbf{R}}_2^{-1}{)}^{\vee }-\ln ({\mathbf{R}}_1{\mathbf{R}}_2^{-1}{)}^{\vee }}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{\ln ({\mathbf{R}}_1{\mathbf{R}}_2^{-1}{\mathbf{R}}_2\exp (-{\psi }^{\wedge }){\mathbf{R}}_2^{-1}{)}^{\vee }-\ln ({\mathbf{R}}_1{\mathbf{R}}_2^{-1}{)}^{\vee }}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{\ln ({\mathbf{R}}_1{\mathbf{R}}_2^{-1}\exp (-{\mathbf{R}}_2{\psi }^{\wedge }){)}^{\vee }-\ln ({\mathbf{R}}_1{\mathbf{R}}_2^{-1}{)}^{\vee }}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{\ln ({\mathbf{R}}_1{\mathbf{R}}_2^{-1}{)}^{\vee }-{\mathbf{J}}_r^{-1}{\mathbf{R}}_2\psi -\ln ({\mathbf{R}}_1{\mathbf{R}}_2^{-1}{)}^{\vee }}{\psi }\\ & =\underset{\psi \to 0}{\lim }\frac{-{\mathbf{J}}_r^{-1}{\mathbf{R}}_2\psi }{\psi }\\ & =-{\mathbf{J}}_r^{-1}(\ln ({\mathbf{R}}_1{\mathbf{R}}_2^{-1}{)}^{\vee })\cdot {\mathbf{R}}_2\\ & =-{\mathbf{J}}_l^{-1}(\ln ({\mathbf{R}}_1{\mathbf{R}}_2^{-1}{)}^{\vee })\cdot {\mathbf{R}}_1\end{array}$$

左扰动：这个推导很简单，直接就类似于右扰动第4行，最后结果应该是$-{\mathbf{J}}_r^{-1}(\ln ({\mathbf{R}}_1{\mathbf{R}}_2^{-1}))$

5.3 设计程序

  将2.3节的实验修改成带旋转的抛物线运动。物体一方面沿 Z 轴自转，一方面存在水平的初始线速度，又受到 −Z 方向的重力加速度影响。请设计程序并完成动画演示。

注意几点

1 绕z轴自转，和原来程序一致----所以旋转不发生变化

2 水平初始线速度，设置为x方向，假设保持不变

3 重力加速度–自由落体，v(z)=-gt, x(z)=vt+0.5gt^2。

4 注意坐标系转换，搞清楚那个是body系，那个是世界系。比如位移是指世界系下位移，不能用body系下速度来更新；常见-9.8的重力加速度是世界系下量，如果要更新body系下速度，需要转换对应的加速度！

DEFINE_double(angular_velocity, 10.0, "角速度（角度）制");
DEFINE_double(linear_velocity, 5.0, "车辆前进线速度 m/s");
DEFINE_double(gravity, -9.8, "加速度 m/s2");
DEFINE_bool(use_quaternion, false, "是否使用四元数计算");

int main(int argc, char** argv) {

	....
	
    double angular_velocity_rad = FLAGS_angular_velocity * sad::math::kDEG2RAD;  // 弧度制角速度
    SE3 pose;                                                                    // TWB表示的位姿
    Vec3d omega(0, 0, angular_velocity_rad);                                     // 角速度矢量
    Vec3d v_body(FLAGS_linear_velocity, 0, 0);                                   // 本体系速度
    const double dt = 0.05;                                                      // 每次更新的时间
    const Vec3d a_w(0, 0, FLAGS_gravity);

    while (ui.ShouldQuit() == false) {
        // 1 v_w = R_wb * v_b;
        Vec3d v_world = pose.so3() * v_body;

        // 2 x_w = v_w * dt + 0.5 * a_w * dt * dt;
        pose.translation() += v_world * dt + 0.5 * a_w * dt * dt;

        // 3 a_b = (R_wb)^T * a_w
        Vec3d a_b = pose.so3().inverse() * a_w;

        // 4 v_b += a_b * dt
        v_body += a_b * dt; 

        // 更新自身旋转
        if (FLAGS_use_quaternion) {
            Quatd q = pose.unit_quaternion() * Quatd(1, 0.5 * omega[0] * dt, 0.5 * omega[1] * dt, 0.5 * omega[2] * dt);
            q.normalize();
            pose.so3() = SO3(q);
        } else {
            pose.so3() = pose.so3() * SO3::exp(omega * dt);
        }

        LOG(INFO) << "pose: " << pose.translation().transpose();
        ui.UpdateNavState(sad::NavStated(0, pose, v_world));

        usleep(dt * 1e6);
    }

    ui.Quit();
    return 0;
}

有了重力加速度，使得其不能在一个平面。右下角分别显示了世界系下的速度分量和body系（baselink）下的速度分量。body系下x分量不发生变换，z方向线性递减；世界系下两个速度像是随正弦函数变化。

5.4 GN和LM在处理非线性迭代时的差异

后续补充吧