机器人运动估计——IMU运动方程与ESKF原理介绍（下）

Hello! 欢迎来到我的博客
今天的内容关于机器人中常用的传感器IMU，我们用它来实现机器人姿态、速度、位置的估计。
今天将会介绍使用低成本IMU进行机器人运动估计的一个常用方法——ESKF。

3 Error State Kalman Filter（ESKF介绍）

3.1 动机与流程

使用IMU的初衷是为了求解载体的位置、速度、姿态等系统状态，然而由于IMU测量数据（包括加速度计和陀螺仪的测量数据）都带有大量噪声，直接利用IMU运动方程进行积分会随着时间发生显著的漂移。因此，为了避免漂移我们需要融合其他传感器的信息（如GPS、视觉），对IMU运动积分的结果进行修正。

Error-state Kalman Filter（ESKF）就是一种传感器融合的算法，它的基础仍然是卡尔曼滤波。它的核心思想是把系统的状态分为三类：

• true state：实际状态，系统实际的运行状态
• nominal state：名义状态，描述了运动状态的主要趋势，主导成分。（large-signal，非线性）
• error state：误差状态，实际状态与名义状态之间的差值（small-signal，近似线性，适合线性高斯滤波）

基于以上状态分类，我们可以将关心的true-state，分为两部分，分别进行估计，即nominal state和error state，然后再进行二者叠加。

ESKF的全过程可以描述为如下步骤：

• 对高频率的IMU数据${\mathbf{u}}_m$进行积分，获得nomimal state $\mathbf{x}$。（注意：nominal state里面不考虑噪声，因此势必会累积误差。这部分误差可依据error state进行修正）
• 利用Kalman Filter估计error state，这里同样也包括时间更新和量测更新两部分。（注意：这个过程考虑了噪声，并且由于误差状态方程是近似线性的，所以可以直接用卡尔曼滤波）
• 利用error state修正nominal state，获取true state
• 重置error state

使用ESKF的优势：

1. error state 中的参数数量与运动自由度是相等的，避免了过参数化（over-parameterization or redundancy）引起的协方差矩阵奇异的风险。
2. error state 总是接近于0，Kalman Filter工作在原点附近。因此，远离奇异值、万向节锁，并且保证了线性化的合理性和有效性。
3. error state 总是很小，因此二阶项都可以忽略，因此雅可比矩阵的计算会很简单，很迅速。
4. error state 的变化平缓，因此KF修正的频率不需要太高。

3.2 状态定义

所有在ESKF中用到的变量定义如下：

图片中定义了所以状态，包括true state，nominal state以及error state。以及error state和nominal state组合获得true state的方式。
另外，图中的各个运算符在上一篇文章中均已有了定义。除了一个叉乘矩阵的符号，其定义如下：

$$[\mathbf{a}{]}_{\times }\triangleq \left[\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right]$$

图片来自于参考文献[1]的P31。

3.3 Nominal State 积分更新（每个时间步都要执行）

3.3.1 Nominal state 方程（连续时间）：

$$\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}$$

其中， R = R { q } \mathbf{R}=\mathbf{R}\{\mathbf{q}\} R=R{q}是基于$\mathbf{q}$生成的旋转矩阵，表示从IMU系到惯性系的旋转。
注意到，这组方程完全没有考虑噪声。这组方程的推导是显而意见的，只需要在IMU运动方程的基础上，假设噪声项全部为0即可。

3.3.2 Nominal state 方程（离散时间，用于写程序）：

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(\bm\omega_{m}-\bm\omega_{b}\right) \Delta t\right\} \\ \mathbf{a}_{b} \leftarrow \mathbf{a}_{b} \\ \bm\omega_{b} \leftarrow \bm\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​
其中，定义 q { v } = e v / 2 \mathbf{q}\left\{\mathbf{v}\right\}=e^{\mathbf{v} / 2} q{v}=ev/2，所以：
q { ( ω m − ω b ) Δ t } = e ( ω m − ω b ) Δ t / 2 = e u θ / 2 = [ cos ⁡ ( θ / 2 ) u sin ⁡ ( θ / 2 ) ] \mathbf{q}\left\{\left(\bm\omega_{m}-\bm\omega_{b}\right) \Delta t\right\}=e^{\mathbf{\left(\bm\omega_{m}-\bm\omega_{b}\right) \Delta t} / 2}=e^{\mathbf{u}\theta/ 2}= \left[\begin{array}{c} \cos (\theta/ 2) \\ \mathbf{u} \sin (\theta/ 2) \end{array}\right] q{(ωm​−ωb​)Δt}=e(ωm​−ωb​)Δt/2=euθ/2=[cos(θ/2)usin(θ/2)​]
其中，$\theta =||\left({\mathbf{\omega }}_m-{\mathbf{\omega }}_b\right)\Delta t||,\mathbf{u}=\frac{\left({\mathbf{\omega }}_m-{\mathbf{\omega }}_b\right)\Delta t}{\theta }$

实际上，上面这个操作就是把角度增量改造成单位四元数，两个单位四元数相乘仍然是单位四元数，这样才能保证$\mathbf{q}$始终为单位四元数，保证迭代是收敛的。

3.4 Error State 时间更新（每个时间步都要执行）

3.4.1 Error state 方程（连续时间）：

$$\begin{array}{rl}\delta \dot{\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\\ \delta \dot{\mathbf{\theta }}& =-{\left[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b\right]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n\\ \delta {\dot{\mathbf{a}}}_b& ={\mathbf{a}}_w\\ \delta {\dot{\mathbf{\omega }}}_b& ={\mathbf{\omega }}_w\\ \dot{\delta \mathbf{g}}& =0\end{array}$$
这个误差方程，考虑了噪声等不确定性。其形式看起来似乎不太直观，但实际上推导也很简单。
比如，以位置为例：
$$\delta \mathbf{p}={\mathbf{p}}_t-\mathbf{p}$$
所以其导数为：
$$\delta \dot{\mathbf{p}}={\dot{\mathbf{p}}}_t-\dot{\mathbf{p}}={\mathbf{v}}_t-\mathbf{v}=\delta \mathbf{v}$$
再比如以速度为例：
$$\delta \mathbf{v}={\mathbf{v}}_t-\mathbf{v}$$
所以其导数为：
$$\begin{array}{rl}\delta \dot{\mathbf{v}}& ={\dot{\mathbf{v}}}_t-\dot{\mathbf{v}}\\ & ={\mathbf{R}}_t\left({\mathbf{a}}_m-{\mathbf{a}}_{bt}-{\mathbf{a}}_n\right)+{\mathbf{g}}_t-(\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{a}}_b\right)+\mathbf{g})\\ & =(\mathbf{R}\delta \mathbf{R}-\mathbf{R})({\mathbf{a}}_m-{\mathbf{a}}_b)-\mathbf{R}\delta \mathbf{R}(\delta {\mathbf{a}}_b+{\mathbf{a}}_m)+({\mathbf{g}}_t-\mathbf{g})\\ & \approx [\mathbf{R}(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times })-\mathbf{R}]({\mathbf{a}}_m-{\mathbf{a}}_b)-\mathbf{R}(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times })(\delta {\mathbf{a}}_b+{\mathbf{a}}_m)+\delta \mathbf{g}\\ & \approx \mathbf{R}[\delta \mathbf{\theta }{]}_{\times }({\mathbf{a}}_m-{\mathbf{a}}_b)-\mathbf{R}(\delta {\mathbf{a}}_b+{\mathbf{a}}_m)+\delta \mathbf{g}\\ & =-\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\end{array}$$
其中，用到了$\delta \mathbf{R}=e^{[\delta \mathbf{\theta }{]}_{\times }}\approx \mathbf{I}+[\delta \mathbf{\theta }{]}_{\times }$。此外，还用到了两个小量相乘可以忽略的假设。

其它方程的推导，就不在此说明了，可以参考文献[1]的P33页。

3.4.2 Error state 方程（离散时间，用于写程序）：

定义状态向量：
$$\mathbf{x}=\left[\begin{array}{l}\mathbf{p}\\ \mathbf{v}\\ \mathbf{q}\\ {\mathbf{a}}_b\\ {\mathbf{\omega }}_b\\ \mathbf{g}\end{array}\right]\in {\mathbb{R}}_{19\times 1},\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]\in {\mathbb{R}}_{18\times 1},{\mathbf{u}}_m=\left[\begin{array}{c}{\mathbf{a}}_m\\ {\mathbf{\omega }}_m\end{array}\right]\in {\mathbb{R}}_{6\times 1},\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]\in {\mathbb{R}}_{12\times 1}$$
于是误差状态方程可以写作：
$$\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 ] ∈ R 18 × 18 \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(\omega_{m}-\omega_{b}\right) \Delta t\right\} & 0 & -\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] \in \mathbb{R}_{18\times 18} 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​⎦⎥⎥⎥⎥⎥⎥⎤​∈R18×18​

$${\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]\in {\mathbb{R}}_{18\times 12},{\mathbf{Q}}_{\mathbf{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]\in {\mathbb{R}}_{12\times 12}$$

其中，${\mathbf{v}}_{\mathbf{i}},{\mathbf{\theta }}_{\mathbf{i}},{\mathbf{a}}_{\mathbf{i}},{\mathbf{\omega }}_{\mathbf{i}}$代表随机脉冲，他们的均值为0，协方差矩阵为对角阵。
另外，定义：
R ⊤ { ( ω m − ω b ) Δ t } = e − [ ω m − ω b ] × Δ t = I − [ ω m − ω b ] × Δ t + 1 2 [ ω m − ω b ] × 2 Δ t 2 − . . . \mathbf{R}^{\top}\left\{\left(\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right) \Delta t\right\}=e^{-[\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}]_\times \Delta t}=\mathbf{I}-[\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}]_\times \Delta t+\frac{1}{2}[\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}]_\times^2 \Delta t^2-... R⊤{(ωm​−ωb​)Δt}=e−[ωm​−ωb​]×​Δt=I−[ωm​−ωb​]×​Δt+21​[ωm​−ωb​]×2​Δt2−...
这是一个微分方程的闭合解，利用这个解进行递推会更精确。当然，也可以只取前面两项。

3.4.3 对于Error state的卡尔曼滤波器的时间更新（用于写程序）

误差状态更新
$$\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 }$$
这个就是很基础的卡尔曼滤波方程了，不了解的可以移步我的博客：
机器人运动估计系列（番外篇）——从贝叶斯滤波到卡尔曼（上）

3.5 Error State 量测更新（只有测量值到来的时间步需要执行）

由于IMU的数据中夹带了大量噪声，我们需要利用更多的传感器的信息，来对状态进行修正。

而这些传感器中常用的组合包括：GPS+IMU，单目视觉+IMU，双目视觉+IMU等。

3.5.1 量测方程

通常，量测方程的基本形式如下：
$$\mathbf{y}=h\left({\mathbf{x}}_t\right)+v$$
其中$h()$是量测函数，与传感器有关。$v$是高斯白噪声，为 v ∼ N { 0 , V } v \sim \mathcal{N}\{0, \mathbf{V}\} v∼N{0,V}。

为了实现量测更新，我们需要求$h$对$\delta \mathbf{x}$的导数，所以有：

$${\mathbf{H}\equiv \frac{\partial h}{\partial \delta \mathbf{x}}|}_{\mathbf{x}}={{\frac{\partial h}{\partial {\mathbf{x}}_t}|}_{\mathbf{x}}\frac{\partial {\mathbf{x}}_t}{\partial \delta \mathbf{x}}|}_{\mathbf{x}}={\mathbf{H}}_{\mathbf{x}}{\mathbf{X}}_{\delta \mathbf{x}}$$
其中${{\mathbf{H}}_{\mathbf{x}}\triangleq \frac{\partial h}{\partial {\mathbf{x}}_t}|}_{\mathbf{x}}$是量测函数的雅可比矩阵，根据传感器的不同而具有不同形式。

而${{\mathbf{X}}_{\delta \mathbf{x}}\triangleq \frac{\partial {\mathbf{x}}_t}{\partial \delta \mathbf{x}}|}_{\mathbf{x}}$则可以表示为：
$${{\mathbf{X}}_{\delta \mathbf{x}}\triangleq \frac{\partial {\mathbf{x}}_t}{\partial \delta \mathbf{x}}|}_{\mathbf{x}}=\left[\begin{array}{ccc}{\mathbf{I}}_6& 0& 0\\ 0& {\mathbf{Q}}_{\delta \mathbf{\theta }}& 0\\ 0& 0& {\mathbf{I}}_9\end{array}\right],{\mathbf{Q}}_{\delta \mathbf{\theta }}=\frac{1}{2}\left[\begin{array}{ccc}-q_x& -q_y& -q_z\\ q_w& -q_z& q_y\\ q_z& q_w& -q_x\\ -q_y& q_x& q_w\end{array}\right]$$

关于上式的详细推导，可以参考文献[1]的P39页。

3.5.2 对于Error state的卡尔曼滤波器的量测更新（用于写程序）

卡尔曼增益的计算
$$\mathbf{K}=\mathbf{P}{\mathbf{H}}^{\top }{\left(\mathbf{H}\mathbf{P}{\mathbf{H}}^{\top }+\mathbf{V}\right)}^{-1}$$
误差状态量测更新
$$\hat{\delta }\mathbf{x}\leftarrow \mathbf{K}\left(\mathbf{y}-h\left({\hat{\mathbf{x}}}_t\right)\right)$$
协方差矩阵更新
$$\mathbf{P}\leftarrow (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}$$
这是卡尔曼滤波的基础方程，就不做介绍了。

3.6 状态合并以及Error State 重置（在量测更新过后执行）

3.6.1 状态合并

这一步是要把error state与nominal state合并，即修正nominal state随时间的漂移。

$$\mathbf{x}\leftarrow \mathbf{x}\oplus \widehat{\delta \mathbf{x}}$$
即：
p ← p + δ p ^ v ← v + δ v ^ q ← q ⊗ q { δ θ ^ } a b ← a b + δ a ^ b ω b ← ω b + δ ω ^ b g ← g + δ g ^ \begin{aligned} &\begin{array}{l} \mathbf{p} \leftarrow \mathbf{p}+\hat{\delta \mathbf{p}} \\ \mathbf{v} \leftarrow \mathbf{v}+\hat{\delta \mathbf{v}} \end{array}\\ &\mathbf{q} \leftarrow \mathbf{q} \otimes \mathbf{q}\{\hat{\delta \boldsymbol{\theta}}\}\\ &\mathbf{a}_{b} \leftarrow \mathbf{a}_{b}+\delta \hat{\mathbf{a}}_{b}\\ &\bm\omega_{b} \leftarrow \bm\omega_{b}+\delta \hat{\bm\omega}_{b}\\ &\mathbf{g} \leftarrow \mathbf{g}+\hat{\delta \mathbf{g}} \end{aligned} ​p←p+δp^​v←v+δv^​q←q⊗q{δθ^}ab​←ab​+δa^b​ωb​←ωb​+δω^b​g←g+δg^​​

3.6.2 Error state 重置

由于nominal state的误差已经修正了，所以error state要置零，同时协方差矩阵也要相应的修改。

通常来说，重置的方法如下：
$$\begin{array}{rl}& \widehat{\delta \mathbf{x}}\leftarrow 0\\ & \mathbf{P}\leftarrow \mathbf{G}\mathbf{P}{\mathbf{G}}^{\top }\end{array}$$
其中：$\mathbf{G}={\mathbf{I}}_{18}$。

4 Matlab代码

https://github.com/CASIA-RoboticFish/FBUS-EKF

参考文献

[1] Joan Sol`a. Quaternion kinematics for the error-state KF. Sep 12, 2016.

关于<IMU运动方程与ESKF原理介绍>的上下两篇文章主要内容出自上面这篇参考文献，我只是进行了翻译和重新梳理逻辑顺序的工作。因此，大家如果使用到本博客内容，也烦请引用上述参考文献。谢谢~