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

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

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

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

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

使用ESKF的优势：

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

[ a ] × ≜ [ 0 − a z a y a z 0 − a x − a y a x 0 ] [\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] [a]×​≜⎣⎡​0az​−ay​​−az​0ax​​ay​−ax​0​⎦⎤​

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

p ˙ = v v ˙ = R ( a m − a b ) + g q ˙ = 1 2 q ⊗ ( ω m − ω b ) a ˙ b = 0 ω ˙ b = 0 g ˙ = 0 \begin{aligned} \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(\bm\omega_{m}-\bm\omega_{b}\right) \\ \dot{\mathbf{a}}_{b} &=0 \\ \dot{\bm\omega}_{b} &=0 \\ \dot{\mathbf{g}} &=0 \end{aligned} p˙​v˙q˙​a˙b​ω˙b​g˙​​=v=R(am​−ab​)+g=21​q⊗(ωm​−ωb​)=0=0=0​

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

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)​] 其中， θ = ∣ ∣ ( ω m − ω b ) Δ t ∣ ∣ , u = ( ω m − ω b ) Δ t θ \theta=||\left(\bm\omega_{m}-\bm\omega_{b}\right) \Delta t||,\mathbf{u}=\frac{\left(\bm\omega_{m}-\bm\omega_{b}\right) \Delta t}{\theta} θ=∣∣(ωm​−ωb​)Δt∣∣,u=θ(ωm​−ωb​)Δt​

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

δ p ˙ = δ v δ v ˙ = − R [ a m − a b ] × δ θ − R δ a b + δ g − R a n δ θ ˙ = − [ ω m − ω b ] × δ θ − δ ω b − ω n δ a ˙ b = a w δ ω ˙ b = ω w δ g ˙ = 0 \begin{aligned} \delta \dot\mathbf{p} &=\delta \mathbf{v} \\ \dot{\delta \mathbf{v}} &=-\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{a}_{b}+\delta \mathbf{g}-\mathbf{R} \mathbf{a}_{n} \\ {\delta} \dot\boldsymbol{\theta} &=-\left[\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\delta \boldsymbol{\omega}_{b}-\boldsymbol{\omega}_{n} \\ \delta\dot \mathbf{a}_{b} &=\mathbf{a}_{w} \\ \delta \dot{\boldsymbol{\omega}}_{b} &=\boldsymbol{\omega}_{w} \\ \dot{\delta \mathbf{g}} &=0 \end{aligned} δp˙​δv˙δθ˙δa˙b​δω˙b​δg˙​​=δv=−R[am​−ab​]×​δθ−Rδab​+δg−Ran​=−[ωm​−ωb​]×​δθ−δωb​−ωn​=aw​=ωw​=0​ 这个误差方程，考虑了噪声等不确定性。其形式看起来似乎不太直观，但实际上推导也很简单。 比如，以位置为例： δ p = p t − p \delta \mathbf{p}=\mathbf{p}_t-\mathbf{p} δp=pt​−p 所以其导数为： δ p ˙ = p ˙ t − p ˙ = v t − v = δ v \delta \dot\mathbf{p}=\dot\mathbf{p}_t-\dot\mathbf{p}=\mathbf{v}_t-\mathbf{v}=\delta \mathbf{v} δp˙​=p˙​t​−p˙​=vt​−v=δv 再比如以速度为例： δ v = v t − v \delta \mathbf{v}=\mathbf{v}_t-\mathbf{v} δv=vt​−v 所以其导数为： δ v ˙ = v ˙ t − v ˙ = R t ( a m − a b t − a n ) + g t − ( R ( a m − a b ) + g ) = ( R δ R − R ) ( a m − a b ) − R δ R ( δ a b + a m ) + ( g t − g ) ≈ [ R ( I + [ δ θ ] × ) − R ] ( a m − a b ) − R ( I + [ δ θ ] × ) ( δ a b + a m ) + δ g ≈ R [ δ θ ] × ( a m − a b ) − R ( δ a b + a m ) + δ g = − R [ a m − a b ] × δ θ − R δ a b + δ g − R a n \begin{aligned} \delta \dot\mathbf{v}&=\dot\mathbf{v}_t-\dot\mathbf{v}\\ &=\mathbf{R}_{t}\left(\mathbf{a}_{m}-\mathbf{a}_{b t}-\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\bm{\theta}]_\times)-\mathbf{R}](\mathbf{a}_{m}-\mathbf{a}_{b})-\mathbf{R}(\mathbf{I}+[\delta\bm{\theta}]_\times)(\delta\mathbf{a}_b+\mathbf{a}_m)+\delta\mathbf{g}\\ &\approx\mathbf{R}[\delta\bm{\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 \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{a}_{b}+\delta \mathbf{g}-\mathbf{R} \mathbf{a}_{n} \\ \end{aligned} δv˙​=v˙t​−v˙=Rt​(am​−abt​−an​)+gt​−(R(am​−ab​)+g)=(RδR−R)(am​−ab​)−RδR(δab​+am​)+(gt​−g)≈[R(I+[δθ]×​)−R](am​−ab​)−R(I+[δθ]×​)(δab​+am​)+δg≈R[δθ]×​(am​−ab​)−R(δab​+am​)+δg=−R[am​−ab​]×​δθ−Rδab​+δg−Ran​​ 其中，用到了 δ R = e [ δ θ ] × ≈ I + [ δ θ ] × \delta\mathbf{R}=e^{[\delta\bm{\theta}]_\times} \approx \mathbf{I}+[\delta\bm{\theta}]_\times δR=e[δθ]×​≈I+[δθ]×​。此外，还用到了两个小量相乘可以忽略的假设。

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

定义状态向量： x = [ p v q a b ω b g ] ∈ R 19 × 1 , δ x = [ δ p δ v δ θ δ a b δ ω b δ g ] ∈ R 18 × 1 , u m = [ a m ω m ] ∈ R 6 × 1 , i = [ v i θ i a i ω i ] ∈ R 12 × 1 \mathbf{x}=\left[\begin{array}{l} \mathbf{p} \\ \mathbf{v} \\ \mathbf{q} \\ \mathbf{a}_{b} \\ \boldsymbol{\omega}_{b} \\ \mathbf{g} \end{array}\right] \in \mathbb{R}_{19\times 1}, \quad \delta \mathbf{x}=\left[\begin{array}{c} \delta \mathbf{p} \\ \delta \mathbf{v} \\ \delta \boldsymbol{\theta} \\ \delta \mathbf{a}_{b} \\ \delta \boldsymbol{\omega}_{b} \\ \delta \mathbf{g} \end{array}\right] \in \mathbb{R}_{18\times 1}\quad, \quad \mathbf{u}_{m}=\left[\begin{array}{c} \mathbf{a}_{m} \\ \boldsymbol{\omega}_{m} \end{array}\right] \in \mathbb{R}_{6\times 1}, \quad \mathbf{i}=\left[\begin{array}{c} \mathbf{v}_{\mathbf{i}} \\ \boldsymbol{\theta}_{\mathbf{i}} \\ \mathbf{a}_{\mathbf{i}} \\ \boldsymbol{\omega}_{\mathbf{i}} \end{array}\right] \in \mathbb{R}_{12\times 1} x=⎣⎢⎢⎢⎢⎢⎢⎡​pvqab​ωb​g​⎦⎥⎥⎥⎥⎥⎥⎤​∈R19×1​,δx=⎣⎢⎢⎢⎢⎢⎢⎡​δpδvδθδab​δωb​δg​⎦⎥⎥⎥⎥⎥⎥⎤​∈R18×1​,um​=[am​ωm​​]∈R6×1​,i=⎣⎢⎢⎡​vi​θi​ai​ωi​​⎦⎥⎥⎤​∈R12×1​ 于是误差状态方程可以写作： δ x ← f ( x , δ x , u m , i ) = F x ( x , u m ) ⋅ δ x + F i ⋅ i \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} δx←f(x,δx,um​,i)=Fx​(x,um​)⋅δx+Fi​⋅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​

F i = ∂ f ∂ i ∣ x , u m = [ 0 0 0 0 I 0 0 0 0 I 0 0 0 0 I 0 0 0 0 I 0 0 0 0 ] ∈ R 18 × 12 , Q i = [ V i 0 0 0 0 Θ i 0 0 0 0 A i 0 0 0 0 Ω i ] ∈ R 12 × 12 \mathbf{F}_{\mathbf{i}}=\left.\frac{\partial f}{\partial \mathbf{i}}\right|_{\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] \quad \in \mathbb{R}_{18\times 12}, \quad \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} Fi​=∂i∂f​∣∣∣∣​x,um​​=⎣⎢⎢⎢⎢⎢⎢⎡​0I0000​00I000​000I00​0000I0​⎦⎥⎥⎥⎥⎥⎥⎤​∈R18×12​,Qi​=⎣⎢⎢⎡​Vi​000​0Θi​00​00Ai​0​000Ωi​​⎦⎥⎥⎤​∈R12×12​

其中， v i , θ i , a i , ω i \mathbf{v}_{\mathbf{i}},\boldsymbol{\theta}_{\mathbf{i}},\mathbf{a}_{\mathbf{i}},\boldsymbol{\omega}_{\mathbf{i}} vi​,θi​,ai​,ω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−... 这是一个微分方程的闭合解，利用这个解进行递推会更精确。当然，也可以只取前面两项。

误差状态更新 δ x ^ ← F x ( x , u m ) ⋅ δ x ^ \hat{\delta \mathbf{x}} \leftarrow \mathbf{F}_{\mathbf{x}}\left(\mathbf{x}, \mathbf{u}_{m}\right) \cdot \hat{\delta \mathbf{x}} δx^←Fx​(x,um​)⋅δx^ 协方差矩阵更新 P ← F x P F x ⊤ + F i Q i F i ⊤ \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} P←Fx​PFx⊤​+Fi​Qi​Fi⊤​ 这个就是很基础的卡尔曼滤波方程了，不了解的可以移步我的博客： 机器人运动估计系列（番外篇）——从贝叶斯滤波到卡尔曼（上）

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

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

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

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

H ≡ ∂ h ∂ δ x ∣ x = ∂ h ∂ x t ∣ x ∂ x t ∂ δ x ∣ x = H x X δ x \left.\mathbf{H} \equiv \frac{\partial h}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}}=\left.\left.\frac{\partial h}{\partial \mathbf{x}_{t}}\right|_{\mathbf{x}} \frac{\partial \mathbf{x}_{t}}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}}=\mathbf{H}_{\mathbf{x}} \mathbf{X}_{\delta \mathbf{x}} H≡∂δx∂h​∣∣∣∣​x​=∂xt​∂h​∣∣∣∣​x​∂δx∂xt​​∣∣∣∣​x​=Hx​Xδx​ 其中 H x ≜ ∂ h ∂ x t ∣ x \left.\mathbf{H}_{\mathbf{x}} \triangleq \frac{\partial h}{\partial \mathbf{x}_{t}}\right|_{\mathbf{x}} Hx​≜∂xt​∂h​∣∣∣​x​是量测函数的雅可比矩阵，根据传感器的不同而具有不同形式。

而 X δ x ≜ ∂ x t ∂ δ x ∣ x \left.\mathbf{X}_{\delta \mathbf{x}} \triangleq \frac{\partial \mathbf{x}_{t}}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}} Xδx​≜∂δx∂xt​​∣∣∣​x​则可以表示为： X δ x ≜ ∂ x t ∂ δ x ∣ x = [ I 6 0 0 0 Q δ θ 0 0 0 I 9 ] , Q δ θ = 1 2 [ − q x − q y − q z q w − q z q y q z q w − q x − q y q x q w ] \left.\mathbf{X}_{\delta \mathbf{x}} \triangleq \frac{\partial \mathbf{x}_{t}}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}}=\left[\begin{array}{ccc} \mathbf{I}_{6} & 0 & 0 \\ 0 & \mathbf{Q}_{\delta \boldsymbol{\theta}} & 0 \\ 0 & 0 & \mathbf{I}_{9} \end{array}\right], \mathbf{Q}_{\delta \boldsymbol{\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] Xδx​≜∂δx∂xt​​∣∣∣∣​x​=⎣⎡​I6​00​0Qδθ​0​00I9​​⎦⎤​,Qδθ​=21​⎣⎢⎢⎡​−qx​qw​qz​−qy​​−qy​−qz​qw​qx​​−qz​qy​−qx​qw​​⎦⎥⎥⎤​

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

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

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

x ← x ⊕ δ x ^ \mathbf{x} \leftarrow \mathbf{x} \oplus \hat{\delta \mathbf{x}} x←x⊕δ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^​​

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

通常来说，重置的方法如下： δ x ^ ← 0 P ← G P G ⊤ \begin{aligned} &\hat{\delta \mathbf{x}} \leftarrow 0\\ &\mathbf{P} \leftarrow \mathbf{G} \mathbf{P} \mathbf{G}^{\top} \end{aligned} ​δx^←0P←GPG⊤​ 其中： G = I 18 \mathbf{G}= \mathbf{I}_{18} G=I18​。

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

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

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