fast-lio原理解析

---

一、fast-lio原理解析

相关解析：知乎
相关解析：csdn
相关连接，强烈推荐：知乎

1. 误差状态方程

在ESKF中，我们通常把原状态变量称为名义状态变量（nominal state），然后把ESKF里的状态变量称为误差状态变量（error state）。我们设ESKF的真值状态为：
$${\mathbf{x}}_t=\left[\begin{array}{c}{\mathbf{p}}_t\\ {\mathbf{v}}_t\\ {\mathbf{R}}_t\\ {\mathbf{b}}_{at}\\ {\mathbf{b}}_{gt}\\ {\mathbf{g}}_t\end{array}\right]$$
这个状态随时间改变，可以记$\mathbf{x}(t{)}_t$。在连续时间上，我们记IMU读数为$\tilde{\mathbf{\omega }},\tilde{\mathbf{a}}$，那么可以写出状态变量导数相对于观测量之间的关系式：

$$\begin{array}{rl}{\dot{\mathbf{p}}}_t& ={\mathbf{v}}_t\\ {\dot{\mathbf{v}}}_t& ={\mathbf{R}}_t(\tilde{\mathbf{a}}-{\mathbf{b}}_{at}-{\mathbf{\eta }}_a)+\mathbf{g}\\ {\dot{\mathbf{R}}}_t& ={\mathbf{R}}_t(\tilde{\mathbf{\omega }}-{\mathbf{b}}_{gt}-{\mathbf{\eta }}_g{)}^{\wedge }\\ {\dot{\mathbf{b}}}_{gt}& ={\mathbf{\eta }}_{bg}\\ {\dot{\mathbf{b}}}_{at}& ={\mathbf{\eta }}_{ba}\\ \dot{\mathbf{g}}& =0\end{array}$$

其中带下标 $t$的表示真值。下面我们来推导误差状态方程。首先定义误差状态变量为：

$$\begin{array}{rl}{\mathbf{p}}_t& =\mathbf{p}+\delta \mathbf{p}\\ {\mathbf{v}}_t& =\mathbf{v}+\delta \mathbf{v}\\ {\mathbf{R}}_t& =\mathbf{R}\delta \mathbf{R}\text{或}{\mathbf{q}}_t=\mathbf{q}\delta \mathbf{q}\\ {\mathbf{b}}_{gt}& ={\mathbf{b}}_g+\delta {\mathbf{b}}_g\\ {\mathbf{b}}_{at}& ={\mathbf{b}}_a+\delta {\mathbf{b}}_a\\ {\mathbf{g}}_t& =\mathbf{g}+\delta \mathbf{g}\end{array}$$

不带下标的就是名义状态变量。名义状态变量的运动学方程式与真值相同，只是不必考虑噪声（因为噪声在误差状态方程中考虑了）。其中旋转部分的$\delta \mathbf{R}$ 可以用它的李代数 $\mathrm{Exp}(\delta \mathbf{\theta })$ 来表示，此时旋转公式也需要改成用指数形式来表达。

(1)、平移、零偏和重力公式误差推导

关于误差变量的平移、零偏和重力公式，都很容易得出对应的时间导数表达式，只需在等式两侧分别对时间求导即可：

$$\begin{array}{rl}\delta \dot{\mathbf{p}}& =\delta \mathbf{v}\\ \delta \dot{{\mathbf{b}}_g}& ={\mathbf{\eta }}_g\\ \delta \dot{{\mathbf{b}}_a}& ={\mathbf{\eta }}_a\\ \delta \dot{\mathbf{g}}& =0\end{array}$$

(2)、旋转误差推导

而速度、旋转两式由于和 $\delta \mathbf{R}$ 有关系，所以要单独推导。对旋转式两侧求时间导数，可得：

$$\begin{array}{rl}第一个式子：{\dot{\mathbf{R}}}_t& =\dot{\mathbf{R}}\mathrm{Exp}(\delta \mathbf{\theta })+\mathbf{R}\dot{\mathrm{Exp}(\delta \mathbf{\theta })}\\ 第二个式子：{\dot{\mathbf{R}}}_t& ={\mathbf{R}}_t(\tilde{\mathbf{\omega }}-{\mathbf{b}}_{gt}-{\mathbf{\eta }}_g{)}^{\wedge }\end{array}$$
该式右边的$\dot{\mathrm{Exp}(\delta \mathbf{\theta })}$满足：
$$\dot{\mathrm{Exp}(\delta \mathbf{\theta })}=\mathrm{Exp}(\delta \mathbf{\theta })\delta {\dot{\mathbf{\theta }}}^{\wedge }.$$

因此第一个式子可写成（公式3）：

$$\begin{array}{rl}\dot{\mathbf{R}}\mathrm{Exp}(\delta \mathbf{\theta })+\mathbf{R}\dot{\mathrm{Exp}(\delta \mathbf{\theta })}& =\mathbf{R}(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g{)}^{\wedge }\mathrm{Exp}(\delta \mathbf{\theta })+\mathbf{R}\mathrm{Exp}(\delta \mathbf{\theta })\delta {\dot{\mathbf{\theta }}}^{\wedge }\end{array}$$

而第二个式子可以写成（公式4）：

$$\begin{array}{rl}{\mathbf{R}}_t{\left(\tilde{\mathbf{\omega }}-{\mathbf{b}}_{gt}-{\mathbf{\eta }}_g\right)}^{\wedge }& =\mathbf{R}\mathrm{Exp}(\delta \mathbf{\theta }){\left(\tilde{\mathbf{\omega }}-{\mathbf{b}}_{gt}-{\mathbf{\eta }}_g\right)}^{\wedge }\end{array}$$
将公式3 和4结合则（公式5）：
$$\mathbf{R}(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g{)}^{\wedge }\mathrm{Exp}(\delta \mathbf{\theta })+\mathbf{R}\mathrm{Exp}(\delta \mathbf{\theta })\delta {\dot{\mathbf{\theta }}}^{\wedge }=\mathbf{R}\mathrm{Exp}(\delta \mathbf{\theta }){\left(\tilde{\mathbf{\omega }}-{\mathbf{b}}_{gt}-{\mathbf{\eta }}_g\right)}^{\wedge }$$
公式（5），将 $\delta \dot{\mathbf{\theta }}$ 移到一侧，约掉两侧左边的 $\mathbf{R}$，整理类似项，不难得到：

$$\mathrm{Exp}(\delta \mathbf{\theta })\delta {\dot{\mathbf{\theta }}}^{\wedge }=\mathrm{Exp}(\delta \mathbf{\theta }){\left(\tilde{\mathbf{\omega }}-{\mathbf{b}}_{gt}-{\mathbf{\eta }}_g\right)}^{\wedge }-(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g{)}^{\wedge }\mathrm{Exp}(\delta \mathbf{\theta })$$

注意到 $\mathrm{Exp}(\delta \mathbf{\theta })$ 本身是一个 $\mathrm{SO}(3)$ 矩阵，我们利用 $\mathrm{SO}(3)$ 上的伴随性质：

$${\mathbf{\varphi }}^{\wedge }\mathbf{R}=\mathbf{R}({\mathbf{R}}^{\mathrm{T}}\mathbf{\varphi }{)}^{\wedge },$$

用来交换上面的 $\mathrm{Exp}(\delta \mathbf{\theta })$：

$$\mathrm{Exp}(\delta \mathbf{\theta })\delta {\dot{\mathbf{\theta }}}^{\wedge }=\mathrm{Exp}(\delta \mathbf{\theta })\left[{\left(\tilde{\mathbf{\omega }}-{\mathbf{b}}_{gt}-{\mathbf{\eta }}_g\right)}^{\wedge }-{\left(\mathrm{Exp}(-\delta \mathbf{\theta })(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g)\right)}^{\wedge }\right]$$

$$=\mathrm{Exp}(\delta \mathbf{\theta })\left[{\left(\tilde{\mathbf{\omega }}-{\mathbf{b}}_{gt}-{\mathbf{\eta }}_g\right)}^{\wedge }-{\left((\mathbf{I}-\delta {\mathbf{\theta }}^{\wedge })(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g)\right)}^{\wedge }\right]$$

$$=\mathrm{Exp}(\delta \mathbf{\theta }){\left[{\mathbf{b}}_g-{\mathbf{b}}_{gt}-{\mathbf{\eta }}_g+\delta {\mathbf{\theta }}^{\wedge }\tilde{\mathbf{\omega }}-\delta {\mathbf{\theta }}^{\wedge }{\mathbf{b}}_g\right]}^{\wedge }$$

$$=\mathrm{Exp}(\delta \mathbf{\theta }){\left[(-\tilde{\mathbf{\omega }}+{\mathbf{b}}_g{)}^{\wedge }\delta \mathbf{\theta }-\delta {\mathbf{b}}_g-{\mathbf{\eta }}_g\right]}^{\wedge }$$

约掉等式左侧的系数，可得：

$$\delta \dot{\mathbf{\theta }}\approx -(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g{)}^{\wedge }\delta \mathbf{\theta }-\delta {\mathbf{b}}_g-{\mathbf{\eta }}_g$$

(3)、旋转速度项推导

接下来考虑速度方程的误差形式。同样地，对两侧求时间导数，就可以得到 $\delta \dot{v}$ 的表达式。等式左侧为:

$$\begin{array}{rl}{\dot{v}}_t& =R_t(\tilde{a}-b_{at}-{\eta }_a)+g_t\\ & =R\mathrm{Exp}(\delta \theta )(\tilde{a}-b_a-\delta b_a-{\eta }_a)+g+\delta g\\ & \approx R(I+\delta {\theta }^{\wedge })(\tilde{a}-b_a-\delta b_a-{\eta }_a)+g+\delta g\\ & \approx R\tilde{a}-R{b}_a-R\delta b_a-R{\eta }_a+R\delta {\theta }^{\wedge }\tilde{a}-R\delta {\theta }^{\wedge }{b}_a+g+\delta g\\ & =R\tilde{a}-R{b}_a-R\delta b_a-R{\eta }_a-R{\tilde{a}}^{\wedge }\delta \theta +R{b}_a^{\wedge }\delta \theta +g+\delta g\end{array}$$

从第三行推向第四行时，需要忽略 $\delta {\theta }^{\wedge }$ 与 $\delta b_a,{\eta }_a$ 相乘的二阶小量。从第四行推第五行则用到了又乘符号交换顺序之后需加负号的性质。另一方面，等式右侧为:

$$\dot{v}+\delta \dot{v}=R(\tilde{a}-b_a)+g+\delta \dot{v}$$

因为上面两式相等，可以得到:

$$\delta \dot{v}=-R(\tilde{a}-b_a{)}^{\wedge }\delta \theta -R\delta b_a-R{\eta }_a+\delta g$$

这样我们就得到了 $\delta v$ 的运动学模型。需要补充一句，由于上式中 ${\eta }_a$ 是一个零均值白噪声，它乘上任意旋转矩阵之后仍然是一个零均值白噪声，而且由于 $R^{T}R=I$，其协方差矩阵也不变（留作习题）。所以，也可以把上式简化为:

$$\delta \dot{v}=-R(\tilde{a}-b_a{)}^{\wedge }\delta \theta -R\delta b_a-{\eta }_a+\delta g$$

把误差变量的运动学方程整理如下:

$$\begin{array}{rl}\delta \dot{p}& =\delta v\\ \delta \dot{v}& =-R(\tilde{a}-b_a{)}^{\wedge }\delta \theta -R\delta b_a-{\eta }_a+\delta g\\ \delta \dot{\theta }& =-(\tilde{\omega }-b_g{)}^{\wedge }\delta \theta -\delta b_g-{\eta }_g\\ \delta \dot{b_g}& ={\eta }_{bg}\\ \delta \dot{b_a}& ={\eta }_{ba}\\ \delta \dot{g}& =0\end{array}$$

2. 离散时间的误差状态方程（预测过程）

从连续时间状态方程推出离散时间的状态方程并不困难，不妨直接来列写它们。名义状态变量的离散时间运动学方程可以写为：

$$\begin{array}{rl}p(t+\Delta t)& =p(t)+v\Delta t+\frac{1}{2}\left(R(\tilde{a}-b_a)\right)\Delta t^2+\frac{1}{2}g\Delta t^2\\ v(t+\Delta t)& =v(t)+R(\tilde{a}-b_a)\Delta t+g\Delta t\\ R(t+\Delta t)& =R(t)\mathrm{Exp}\left((\tilde{\omega }-b_g)\Delta t\right)\\ b_g(t+\Delta t)& =b_g(t)\\ b_a(t+\Delta t)& =b_a(t)\\ g(t+\Delta t)& =g(t)\end{array}$$

该式只需在上面的基础上添加零偏项与重力项即可。而误差状态的离散形式则只需要处理连续形式中的旋转部分。参考角速度的积分公式，可以将误差状态方程写为：

$$\begin{array}{rl}\delta p(t+\Delta t)& =\delta p+\delta v\Delta t\\ \delta v(t+\Delta t)& =\delta v+\left(-R(\tilde{a}-b_a{)}^{\wedge }\delta \theta -R\delta b_a+\delta g\right)\Delta t+{\eta }_v\\ \delta \theta (t+\Delta t)& =\mathrm{Exp}\left(-(\tilde{\omega }-b_g)\Delta t\right)\delta \theta -\delta b_g\Delta t-{\eta }_{\theta }\\ \delta b_g(t+\Delta t)& =\delta b_g+{\eta }_g\\ \delta b_a(t+\Delta t)& =\delta b_a+{\eta }_a\\ \delta g(t+\Delta t)& =\delta g\end{array}$$

注意：

1. 右侧部分我们省略了括号里的 (t) 以简化公式；
2. 关于旋转部分的积分，我们可以将连续形式看成关于 $\delta \theta$ 的微分方程然后求解。求解过程类似于对角速度进行积分；
3. 噪声项并不参与递推，需要把它们单独归入噪声部分中。连续时间的噪声项可以视为随机过程的能量谱密度，而离散时间下的噪声变量就是我们日常看到的随机变量了。这些噪声随机变量的标准差可以列写如下：

$$\sigma ({\eta }_v)=\Delta t{\sigma }_a,\sigma ({\eta }_{\theta })=\Delta t{\sigma }_g,\sigma ({\eta }_g)=\sqrt{\Delta t}{\sigma }_{bg},\sigma ({\eta }_a)=\sqrt{\Delta t}{\sigma }_{ba}$$

其中前两式的 $\Delta t$ 是由积分关系导致的。整体地记为：

$$\delta x=f(\delta x)+w,w\sim \mathcal{N}(0,Q)$$

其中 $w$ 为噪声。按照前面的定义，$Q$ 应该为：

$$Q=\text{diag}(0_3,\text{Cov}({\eta }_v),\text{Cov}({\eta }_{\theta }),\text{Cov}({\eta }_g),\text{Cov}({\eta }_a),0_3)$$

两侧的零是由于第一个和最后一个方程本身没有噪声导致的。
$$\delta x=F\delta x+w$$

其中 $F$ 为线性化后的雅可比矩阵。由于我们列写的运动方程已经是线性化的了，只需把它们的线性系统拿出来即可：

$$F=\left[\begin{array}{cccccc}I& I\Delta t& 0& 0& 0& 0\\ 0& I& -R(\tilde{a}-b_a{)}^{\wedge }\Delta t& -R\Delta t& 0& I\Delta t\\ 0& 0& \text{Exp}\left(-(\tilde{\omega }-b_g)\Delta t\right)& 0& -I\Delta t& 0\\ 0& 0& 0& I& 0& 0\\ 0& 0& 0& 0& I& 0\\ 0& 0& 0& 0& 0& I\end{array}\right]$$

在此基础上，我们执行ESKF的预测过程。预测过程包括对名义状态的预测（IMU积分）以及对误差状态的预测：

$$\begin{array}{rl}\delta x_{\text{pred}}& =F\delta x\\ P_{\text{pred}}& =FP{F}^T+Q\end{array}$$

不过由于ESKF的误差状态在每次更新以后会被重置，因此运动方程的均值部分没有太大意义，而方差部分则可以指导整个误差估计的分布情况。

3. 更新过程

前面介绍的是ESKF的预测过程，来考虑更新过程。假设一个抽象的传感器能够对状态变量产生观测，其观测方程为抽象的 ( h )，那么可以写为：

$$z=h(x)+v,v\sim \mathcal{N}(0,V)$$

其中$z$ 为观测数据，$v$为观测噪声，$V$ 为该噪声的协方差矩阵。由于状态变量里已经有$\mathbb{R}$了，这里我们换个符号。

在传统EKF中，我们可以直观对观测方程线性化，求出观测方程相对于状态变量的雅可比矩阵，进而更新卡尔曼滤波器。而在ESKF中，我们当前拥有名义状态$x$ 的估计以及误差状态 $\delta x$ 的估计，且希望更新的是误差状态，因此要计算观测方程相比于误差状态的雅可比矩阵：

$$H=\frac{\partial h}{\partial \delta x},$$

然后再计算卡尔曼增益，进而计算误差状态的更新过程：

$$\begin{array}{rl}K& =P_{\text{pred}}{H}^T(H{P}_{\text{pred}}{H}^T+V{)}^{-1}\\ \delta x& =K(z-h(x_{\text{t}}))\\ P& =(I-KH)P_{\text{pred}}\end{array}$$

其中 $K$ 为卡尔曼增益，$P_{\text{pred}}$ 为预测的协方差矩阵，最后的 $P$ 为修正后的协方差矩阵。这里的 $H$的计算可以通过链式法则来生成：

$$H=\frac{\partial h}{\partial x}\frac{\partial x}{\partial \delta x}$$

其中第一项只需对观测方程进行线性化，第二项，根据我们之前对状态变量的定义，可以得到：

$$\frac{\partial x}{\partial \delta x}=\text{diag}(I_3,I_3,\frac{\partial \text{Log}(R(\text{Exp}(\delta \theta )))}{\partial \delta \theta },I_3,I_3,I_3)$$

其他几种都是平只有旋转部分，因为 $\delta \theta$定义为$R$的右乘，我们用右乘的BCH即可：

$$\frac{\partial \text{Log}(R(\text{Exp}(\delta \theta )))}{\partial \delta \theta }=J_r^{-1}(R)$$

最后，我们可以给每个变量加下标$k$，表示在$k$ 时刻进行状态估计。

ESKF的误差状态后续处理

在经过预测和更新过程之后，我们修正了误差状态的估计。接下来，只需把误差状态归入名义状态，然后重置ESKF即可。归入部分可以简单地写为：

$$\begin{array}{rl}{p}_{k+1}& =p_k+\delta p_k\\ v_{k+1}& =v_k+\delta v_k\\ R_{k+1}& =R_k\text{Exp}(\delta {\theta }_k)\\ b_{g,k+1}& =b_{g,k}+\delta b_{g,k}\\ b_{a,k+1}& =b_{a,k}+\delta b_{a,k}\\ g_{k+1}& =g_k+\delta g_k\end{array}$$

有些文献里也会定义为广义的状态变量加法：

$$x_{k+1}=x_k\oplus \delta x_k$$

这种写法可以简化整体的表达式。不过，如果公式里出现太多的广义加减法，可能让人不好马上辨认它们的具体含义，所以本书还是倾向于将各状态分别写开，或者直接用加法而非广义加法符号。

ESKF的重置分为均值部分和协方差部分。均值部分可以简单地实现为：

$$\delta x=0$$

由于均值被重置了，之前我们描述的是关于 $x_k$ 切空间中的协方差，而现在描述的是 $x_{k+1}$ 中的协方差。这次重置会带来一些微小的差异，主要影响旋转部分。事实上，在重置前，卡尔曼滤波器刻画了 $x_{\text{pred}}$ 切空间处的一个高斯分布 $\mathcal{N}(\delta x,P)$，而重置之后，应该刻画 $x_{\text{pred}}⊞\delta x$ 处的一个 $\mathcal{N}(0,P_{\text{reset}})$。

我们设重置前的名义旋转估计为 $R_k$，误差状态为 $\delta \theta$，卡尔曼滤波器的增量计算结果为 $\delta {\theta }_k$，注意此处 $\delta {\theta }_k$ 是已知的，而 $\delta \theta$ 是一个随机变量。重置之后的名义旋转部分为

$$R_k\text{Exp}(\delta {\theta }_k)=R^{+}$$

误差状态为 $\delta {\theta }^{+}$。由于误差状态被重置了，显然此时 $\delta {\theta }^{+}=0$。但我们关心的并不是它们直接的取值，而是 $\delta {\theta }^{+}$ 与 $\delta \theta$ 的线性化关系。把实际的重置过程写出来：

$$R^{+}\text{Exp}(\delta {\theta }^{+})=R_k\text{Exp}(\delta {\theta }_k)\text{Exp}(\delta {\theta }^{+})=R_k\text{Exp}(\delta \theta )$$

不难得到：

$$\text{Exp}(\delta {\theta }^{+})=\text{Exp}(-\delta {\theta }_k)\text{Exp}(\delta \theta )$$

注意这里 $\delta \theta$ 为小量，利用线性化后的BCH公式，可以得到：

$$\delta {\theta }^{+}=-\delta {\theta }_k+\delta \theta -\frac{1}{2}\delta {\theta }_k^{\wedge }\delta \theta +o((\delta \theta {)}^2)$$

于是有：

$$\frac{\partial \delta {\theta }^{+}}{\partial \delta \theta }\approx I-\frac{1}{2}\delta {\theta }_k^{\wedge }$$

该式表明重置前后的误差状态相差一个旋转方面的小雅可比矩阵，我们记作 $J_{\theta }=I-\frac{1}{2}\delta {\theta }_k^{\wedge }$。把这个小雅可比阵放到整个状态变量维度下，并保持其他部分为单位矩阵，可以得到一个完整的雅可比阵：

$$J_k=\text{diag}(I_3,I_3,J_{\theta },I_3,I_3,I_3),$$
因此，在把误差状态的均值归零同时，它们的协方差矩阵也应该进行线性变换：

$$P_{\text{reset}}=J_{k}P{J}_k^T.$$

不过，由于 $\delta {\theta }_k$ 并不大，这里的 $J_k$ 仍然十分接近于单位矩阵，所以大部分材料里并不处理这一项，而是直接把前面估计的 $P$ 阵作为下一时刻的起点。但本书仍然要介绍这一点，并且会在后面第9章中继续讨论这个问题。该问题实际意义是做了切空间投影，即把一个切空间中的高斯分布投影到另一个切空间中。在ESKF中，两者没有明显差异，但后文的迭代卡尔曼滤波器还牵扯到多次切空间的变换，我们必须在此加以介绍。
更新中…!