学习MSCKF笔记——真实状态、标称状态、误差状态

之前在学习《概率机器人》时总结过扩展卡尔曼滤波概率机器人总结——(扩展)卡尔曼滤波先实践再推导，在本书中介绍的滤波对象通常是各种和机器人相关的状态，包括位置、速度等，而MSCKF后端使用的滤波方法是误差状态的卡尔曼滤波，本文主要是总结《Quaternion Kinematics for the Error-state Kalman Filter》文献中对误差状态的推导，首先，为啥要使用误差状态呢？主要有如下四点：
（1）旋转的误差状态可以避免过参数化问题，由于我们通常使用四元数和旋转矩阵来表示旋转状态，这是一种过参数化的表达方式，带来的问题就是在滤波之后的旋转状态可能就不满足四元数为单位四元数或者旋转矩阵为正交矩阵这样的要求，而旋转的误差状态我们可以使用轴角来表示，轴角通过三个参数来表示一个三自由度的误差状态，这样就避免了过参数化问题。
（2）误差状态系统始终在原点伏击运行，远离了参数奇异性、万向节锁等问题，同时保证了扩展卡尔曼线性化有效性始终保持不变。
（3）误差状态的值通常都很小，这意味着二阶阶乘可以忽略不计，这意味着雅克比的计算变得非常容易和快速，部分雅克比甚至可以设置为常量。
（3）误差状态变化比较慢，这样可以使得我们使用比预测更低的速率进行更新。

误差状态卡尔曼系统一共由三部分组成，真实状态（true state）、标称状态（norminal state）和误差状态（error state），其中，真实状态为考虑噪声影响的状态，标称状态为不考虑噪声影响的状态，而噪声影响的部分都被考虑进误差状态，因此真实状态由标称状态和误差状态两者共同描述。以IMU和视觉系统为例，标称状态通过IMU测量获得的加速度和角速度基于运动学公式进行积分，而误差状态通过IMU测量的噪声基于扩展卡尔曼的预测方程进行预测，当获得视觉观测后，系统基于扩展卡尔曼的更新方程对误差状态进行更新。更新后将误差状态的均值合入标称状态，然后再被重置为零，而误差状态的协方差矩阵就不断更新来反应这个状态更新和重置的过程。下面进行各个状态的公式推导。

1. 连续时间系统

真实状态、标称状态和误差状态的状态变量如下表所示：

1.1 真实状态运动学公式

真实状态的运动学公式如下：$$\begin{array}{rl}{\dot{\mathbf{p}}}_t& ={\mathbf{v}}_t\\ {\dot{\mathbf{v}}}_t& ={\mathbf{R}}_t\left({\mathbf{a}}_m-{\mathbf{a}}_{bt}-{\mathbf{a}}_n\right)+{\mathbf{g}}_t\\ {\dot{\mathbf{q}}}_t& =\frac{1}{2}{\mathbf{q}}_t\otimes \left({\mathbf{\omega }}_m-{\mathbf{\omega }}_{bt}-{\mathbf{\omega }}_n\right)\\ {\dot{\mathbf{a}}}_{bt}& ={\mathbf{a}}_w\\ {\dot{\mathbf{\omega }}}_{bt}& ={\mathbf{\omega }}_w\\ {\dot{\mathbf{g}}}_t& =0\end{array}$$通过公式我们可以看到，真实状态同时收到高斯白噪声${\mathbf{a}}_n$和${\mathbf{\omega }}_n$和随机游走噪声${\mathbf{a}}_n$和${\mathbf{\omega }}_n$的影响。

1.2 标称状态运动学公式

标称状态运动学公式如下：$$\begin{array}{rl}\dot{\mathbf{p}}& =\mathbf{v}\\ \dot{\mathbf{v}}& =\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{a}}_b\right)+\mathbf{g}\\ \dot{\mathbf{q}}& =\frac{1}{2}\mathbf{q}\otimes \left({\mathbf{\omega }}_m-{\mathbf{\omega }}_b\right)\\ {\dot{\mathbf{a}}}_b& =0\\ {\dot{\mathbf{\omega }}}_b& =0\\ \dot{\mathbf{g}}& =0\end{array}$$可以看到，标称状态的运动学公式中完全不考虑以上高斯噪声和随机游走噪声，模型默认我们获得的测量值就是没有噪声。

1.3 误差状态运动学公式

误差状态运动学公式如下：$$\begin{array}{rl}\dot{\delta \mathbf{p}}& =\delta \mathbf{v}\\ \dot{\delta \mathbf{v}}& =-\mathbf{R}{\left[{\mathbf{a}}_m-{\mathbf{a}}_b\right]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g}-{\mathbf{R}\mathbf{a}}_n\\ \dot{\delta \mathbf{\theta }}& =-{\left[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b\right]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n\\ \dot{\delta {\mathbf{a}}_b}& ={\mathbf{a}}_w\\ {\dot{\delta \mathbf{\omega }}}_b& ={\mathbf{\omega }}_w\\ \dot{\delta \mathbf{g}}& =0\end{array}$$真实状态和标称状态运动学公式相对易于理解，而误差运动学公式中，$\delta \mathbf{v}$和$\delta \mathbf{\theta }$公式相对复杂，下面进行推导：
（1）速度误差状态$\dot{\delta \mathbf{v}}$：
从上面真实状态和标称状态的运动学公式中我们可以看到真实测量加速度${\mathbf{a}}_t$和测量角速度${\mathbf{a}}_m$满足：$${\mathbf{a}}_t={\mathbf{R}}_t\left({\mathbf{a}}_m-{\mathbf{a}}_{bt}-{\mathbf{a}}_n\right)+{\mathbf{g}}_t$$右侧公式中的真实状态我们使用标称状态和误差状态替换有：$${\mathbf{g}}_t=\mathbf{g}+\delta \mathbf{g}$$$${\mathbf{a}}_{bt}={\mathbf{a}}_b+\delta {\mathbf{a}}_b$$$${\mathbf{R}}_t=\mathbf{R}\left(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times }\right)+O\left(\Vert \delta \mathbf{\theta }{\Vert }^2\right)$$下面我们定义：$${\mathbf{a}}_{\mathcal{B}}\triangleq {\mathbf{a}}_m-{\mathbf{a}}_b$$$$\delta {\mathbf{a}}_{\mathcal{B}}\triangleq -\delta {\mathbf{a}}_b-{\mathbf{a}}_n$$那么上式就可以写为：$${\mathbf{a}}_t={\mathbf{R}}_t\left({\mathbf{a}}_{\mathcal{B}}+\delta {\mathbf{a}}_{\mathcal{B}}\right)+\mathbf{g}+\delta \mathbf{g}$$我们对$\dot{{\mathbf{v}}_t}$以两种不同的形式进行展开：$$\dot{\mathbf{v}}+\delta \mathbf{v}=\dot{{\mathbf{v}}_t}=\mathbf{R}\left(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times }\right)\left({\mathbf{a}}_{\mathcal{B}}+\delta {\mathbf{a}}_{\mathcal{B}}\right)+\mathbf{g}+\delta \mathbf{g}$$于是有$$\mathbf{R}{\mathbf{a}}_{\mathcal{B}}+\mathbf{g}+\delta \mathbf{v}=\mathbf{R}{\mathbf{a}}_{\mathcal{B}}+\mathbf{R}\delta {\mathbf{a}}_{\mathcal{B}}+\mathbf{R}[\delta \mathbf{\theta }{]}_{\times }{\mathbf{a}}_{\mathcal{B}}+\mathbf{R}[\delta \mathbf{\theta }{]}_{\times }\delta {\mathbf{a}}_{\mathcal{B}}+\mathbf{g}+\delta \mathbf{g}$$将$\mathbf{R}{\mathbf{a}}_{\mathcal{B}}+\mathbf{g}$从等式左侧移动到右侧有$$\dot{\delta \mathbf{v}}=\mathbf{R}\left(\delta {\mathbf{a}}_{\mathcal{B}}+[\delta \mathbf{\theta }{]}_{\times }{\mathbf{a}}_{\mathcal{B}}\right)+\mathbf{R}[\delta \mathbf{\theta }{]}_{\times }\delta {\mathbf{a}}_{\mathcal{B}}+\delta \mathbf{g}$$忽略二阶小项同时调整叉乘顺序有：$$\delta \mathbf{v}=\mathbf{R}\left(\delta {\mathbf{a}}_{\mathcal{B}}-{\left[{\mathbf{a}}_{\mathcal{B}}\right]}_{\times }\delta \mathbf{\theta }\right)+\delta \mathbf{g}$$因此有$$\delta \mathbf{v}=\mathbf{R}\left(-{\left[{\mathbf{a}}_m-{\mathbf{a}}_b\right]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{a}}_b-{\mathbf{a}}_n\right)+\delta \mathbf{g}$$展开获得最终的结论$$\delta \mathbf{v}=-\mathbf{R}{\left[{\mathbf{a}}_m-{\mathbf{a}}_b\right]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g}-\mathbf{R}{\mathbf{a}}_n$$
（2）角度误差状态$\dot{\delta \mathbf{\theta }}$：
首先根据真实状态和标称状态的角度运动学方程：$${\dot{\mathbf{q}}}_t=\frac{1}{2}{\mathbf{q}}_t\otimes {\mathbf{\omega }}_t$$$$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \mathbf{\omega }$$对于角速度真实状态有：$${\mathbf{\omega }}_t\triangleq {\mathbf{\omega }}_m-{\mathbf{\omega }}_{bt}-{\mathbf{\omega }}_n$$对于角速度标称状态有$$\mathbf{\omega }\triangleq {\mathbf{\omega }}_m-{\mathbf{\omega }}_b$$因此角速度误差状态有$$\delta \mathbf{\omega }\triangleq -\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n$$同理将角度真实状态进行展开有：$$\dot{(\mathbf{q}\otimes \delta \mathbf{q})}={\dot{\mathbf{q}}}_t=\frac{1}{2}{\mathbf{q}}_t\otimes {\mathbf{\omega }}_t$$于是有$$\dot{\mathbf{q}}\otimes \delta \mathbf{q}+\mathbf{q}\otimes \dot{\delta \mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \delta \mathbf{q}\otimes {\mathbf{\omega }}_t$$$$\frac{1}{2}\mathbf{q}\otimes \mathbf{\omega }\otimes \delta \mathbf{q}+\mathbf{q}\otimes \dot{\delta \mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \delta \mathbf{q}\otimes {\mathbf{\omega }}_t$$因此有$$\begin{array}{rl}2\dot{\delta \mathbf{q}}& =\delta \mathbf{q}\otimes {\mathbf{\omega }}_t-\mathbf{\omega }\otimes \delta \mathbf{q}\\ & =[\mathbf{q}{]}_R\left({\mathbf{\omega }}_t\right)\delta \mathbf{q}-[\mathbf{q}{]}_L(\mathbf{\omega })\delta \mathbf{q}\\ & =\left[\begin{array}{cc}0& -{\left({\mathbf{\omega }}_t-\mathbf{\omega }\right)}^{\top }\\ \left({\mathbf{\omega }}_t-\mathbf{\omega }\right)& -{\left[{\mathbf{\omega }}_t+\mathbf{\omega }\right]}_{\times }\end{array}\right]\left[\begin{array}{c}1\\ \delta \mathbf{\theta }/2\end{array}\right]+O\left(\Vert \delta \mathbf{\theta }{\Vert }^2\right)\\ & =\left[\begin{array}{cc}0& -\delta {\mathbf{\omega }}^{\top }\\ \delta \mathbf{\omega }& -[2\mathbf{\omega }+\delta \mathbf{\omega }{]}_{\times }\end{array}\right]\left[\begin{array}{c}1\\ \delta \mathbf{\theta }/2\end{array}\right]+O\left(\Vert \delta \mathbf{\theta }{\Vert }^2\right)\end{array}$$又$$2\dot{\delta \mathbf{q}}=\left[\begin{array}{c}0\\ \dot{\delta \mathbf{\theta }}\end{array}\right]$$因此有$$$\dot{\delta \mathbf{\theta }}=\delta \mathbf{\omega }-[\mathbf{\omega }{]}_{\times }\delta \mathbf{\theta }-\frac{1}{2}[\delta \mathbf{\omega }{]}_{\times }\delta \mathbf{\theta }+O\left(\Vert \delta \mathbf{\theta }{\Vert }^2\right)$$忽略二阶小项有$$\dot{\delta \mathbf{\theta }}=-[\mathbf{\omega }{]}_{\times }\delta \mathbf{\theta }+\delta \mathbf{\omega }$$展开得$$\dot{\delta \mathbf{\theta }}=-{\left[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b\right]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n$$

2. 离散时间系统

根据上面连续时间系统，我们可以推出离散时间系统公式，注意这里推导使用是近似离散法，也就是精确离散法的一阶泰勒展开，在MSCKF中采用的分别是龙格库塔（标称状态）和三阶泰勒展开（误差状态），所以这里的公式和MSCKF中实际应用的离散化方程是不相同的，由于我们实际使用的是标称状态和误差状态的离散时间系统，因此我们只对这两者进行推导，如下

2.1 标称状态

经过时间$\Delta t$系统状态更新为： p ← p + v Δ t + 1 2 ( R ( a m − a b ) + g ) Δ t 2 v ← v + ( R ( a m − a b ) + g ) Δ t q ← q ⊗ q { ( ω m − ω b ) Δ t } a b ← a b ω b ← ω b g ← g \begin{array}{l} \mathbf{p} \leftarrow \mathbf{p}+\mathbf{v} \Delta t+\frac{1}{2}\left(\mathbf{R}\left(\mathbf{a}_{m}-\mathbf{a}_{b}\right)+\mathbf{g}\right) \Delta t^{2} \\ \mathbf{v} \leftarrow \mathbf{v}+\left(\mathbf{R}\left(\mathbf{a}_{m}-\mathbf{a}_{b}\right)+\mathbf{g}\right) \Delta t \\ \mathbf{q} \leftarrow \mathbf{q} \otimes \mathbf{q}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\} \\ \mathbf{a}_{b} \leftarrow \mathbf{a}_{b} \\ \boldsymbol{\omega}_{b} \leftarrow \boldsymbol{\omega}_{b} \\ \mathbf{g} \leftarrow \mathbf{g} \end{array} p←p+vΔt+21​(R(am​−ab​)+g)Δt2v←v+(R(am​−ab​)+g)Δtq←q⊗q{(ωm​−ωb​)Δt}ab​←ab​ωb​←ωb​g←g​

2.2 误差状态

经过时间$\Delta t$系统状态更新为： δ p ← δ p + δ v Δ t δ v ← δ v + ( − R [ a m − a b ] × δ θ − R δ a b + δ g ) Δ t + v i δ θ ← R ⊤ { ( ω m − ω b ) Δ t } θ − δ ω b Δ t + θ i δ a b ← δ a b + a i δ ω b ← δ ω b + ω i δ g ← δ g \begin{aligned} \delta \mathbf{p} & \leftarrow \delta \mathbf{p}+\delta \mathbf{v} \Delta t \\ \delta \mathbf{v} & \leftarrow \delta \mathbf{v}+\left(-\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{a}_{b}+\delta \mathbf{g}\right) \Delta t+\mathbf{v}_{\mathbf{i}} \\ \delta \boldsymbol{\theta} & \leftarrow \mathbf{R}^{\top}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\} \boldsymbol{\theta}-\delta \boldsymbol{\omega}_{b} \Delta t+\boldsymbol{\theta}_{\mathbf{i}} \\ \delta \mathbf{a}_{b} & \leftarrow \delta \mathbf{a}_{b}+\mathbf{a}_{\mathbf{i}} \\ \delta \boldsymbol{\omega}_{b} & \leftarrow \delta \boldsymbol{\omega}_{b}+\boldsymbol{\omega}_{\mathbf{i}} \\ \delta \mathbf{g} & \leftarrow \delta \mathbf{g} \end{aligned} δpδvδθδab​δωb​δg​←δp+δvΔt←δv+(−R[am​−ab​]×​δθ−Rδab​+δg)Δt+vi​←R⊤{(ωm​−ωb​)Δt}θ−δωb​Δt+θi​←δab​+ai​←δωb​+ωi​←δg​其中${\mathbf{v}}_{\mathbf{i}},{\mathbf{\theta }}_{\mathbf{i}},{\mathbf{a}}_{\mathbf{i}},{\mathbf{\omega }}_{\mathbf{i}}$为高斯白噪声和随机游走造成的随机变量，其均值为零，协方差矩阵如下$$\begin{array}{ll}{\mathbf{V}}_{\mathbf{i}}={\sigma }_{{\mathbf{a}}_n}^2\Delta t^2\mathbf{I}& \left[m^2/s^2\right]\\ {\Theta }_{\mathbf{i}}={\sigma }_{{\tilde{\omega }}_n}^2\Delta t^2\mathbf{I}& \left[ra{d}^2\right]\\ {\mathbf{A}}_{\mathbf{i}}={\sigma }_{{\mathbf{a}}_w}^2\Delta t\mathbf{I}& \left[m^2/s^4\right]\\ {\Omega }_{\mathbf{i}}={\sigma }_{{\omega }_w}^2\Delta t\mathbf{I}& \left[ra{d}^2/s^2\right]\end{array}$$我们将上面的状态变量分别进行如下定义有：$$\delta \mathbf{x}=\left[\begin{array}{c}\delta \mathbf{p}\\ \delta \mathbf{v}\\ \delta \mathbf{\theta }\\ \delta {\mathbf{a}}_b\\ \delta {\mathbf{\omega }}_b\\ \delta \mathbf{g}\end{array}\right],{\mathbf{u}}_m=\left[\begin{array}{c}{\mathbf{a}}_m\\ {\mathbf{\omega }}_m\end{array}\right],\mathbf{i}=\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{i}}\\ {\mathbf{\theta }}_{\mathbf{i}}\\ {\mathbf{a}}_{\mathbf{i}}\\ {\mathbf{\omega }}_{\mathbf{i}}\end{array}\right]$$我们将误差状态的系统更新写成矩阵形式有：$$\delta \mathbf{x}\leftarrow f\left(\mathbf{x},\delta \mathbf{x},{\mathbf{u}}_m,\mathbf{i}\right)={\mathbf{F}}_{\mathbf{x}}\left(\mathbf{x},{\mathbf{u}}_m\right)\cdot \delta \mathbf{x}+{\mathbf{F}}_{\mathbf{i}}\cdot \mathbf{i}$$其中 F x = ∂ f ∂ δ x ∣ x , u m = [ I I Δ t 0 0 0 0 0 I − R [ a m − a b ] × Δ t − R Δ t 0 I Δ t 0 0 R ⊤ { ( ω m − ω b ) Δ t } 0 − I Δ t 0 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I ] \mathbf{F}_{\mathbf{x}}=\left.\frac{\partial f}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}, \mathbf{u}_{m}}=\left[\begin{array}{cccccc} \mathbf{I} & \mathbf{I} \Delta t & 0 & 0 & 0 & 0 \\ 0 & \mathbf{I} & -\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \Delta t & -\mathbf{R} \Delta t & 0 & \mathbf{I} \Delta t \\ 0 & 0 & \mathbf{R}^{\top}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\} & 0 & \mathbf{-} \mathbf{I} \Delta t & 0 \\ 0 & 0 & 0 & \mathbf{I} & 0 & 0 \\ 0 & 0 & 0 & 0 & \mathbf{I} & 0 \\ 0 & 0 & 0 & 0 & 0 & \mathbf{I} \end{array}\right] Fx​=∂δx∂f​∣∣∣∣​x,um​​=⎣⎢⎢⎢⎢⎢⎢⎡​I00000​IΔtI0000​0−R[am​−ab​]×​ΔtR⊤{(ωm​−ωb​)Δt}000​0−RΔt0I00​00−IΔt0I0​0IΔt000I​⎦⎥⎥⎥⎥⎥⎥⎤​$${\mathbf{F}}_{\mathbf{i}}={\frac{\partial f}{\partial \mathbf{i}}|}_{\mathbf{x},{\mathbf{u}}_m}=\left[\begin{array}{cccc}0& 0& 0& 0\\ \mathbf{I}& 0& 0& 0\\ 0& \mathbf{I}& 0& 0\\ 0& 0& \mathbf{I}& 0\\ 0& 0& 0& \mathbf{I}\\ 0& 0& 0& 0\end{array}\right]$$补充一句，如果按照扩展卡尔曼滤波的更新方程有：$$\widehat{\delta \mathbf{x}}\leftarrow {\mathbf{F}}_{\mathbf{x}}\left(\mathbf{x},{\mathbf{u}}_m\right)\cdot \widehat{\delta \mathbf{x}}$$$$\mathbf{P}\leftarrow {\mathbf{F}}_{\mathbf{x}}\mathbf{P}{\mathbf{F}}_{\mathbf{x}}^{\top }+{\mathbf{F}}_{\mathbf{i}}{\mathbf{Q}}_{\mathbf{i}}{\mathbf{F}}_{\mathbf{i}}^{\top }$$其中 δ x ∼ N { δ x ^ , P } \delta \mathbf{x} \sim \mathcal{N}\{\delta \hat{\mathbf{x}}, \mathbf{P}\} δx∼N{δx^,P}，并且$${\mathbf{Q}}_{\mathrm{i}}=\left[\begin{array}{cccc}{\mathbf{V}}_{\mathbf{i}}& 0& 0& 0\\ 0& {\mathbf{\Theta }}_{\mathbf{i}}& 0& 0\\ 0& 0& {\mathbf{A}}_{\mathbf{i}}& 0\\ 0& 0& 0& {\mathbf{\Omega }}_{\mathbf{i}}\end{array}\right]$$至此就完成了误差状态的一个初步推导，具体的误差状态扩展卡尔曼滤波如何应用到MSCKF中下篇博文中再进行总结，包括如何进行状态积分、如果进行状态变量的扩增、如何构建观测方程进行融合，有问题欢迎交流哈～

此外，对其他SLAM算法感兴趣的同学可以看考我的博客SLAM算法总结——经典SLAM算法框架总结