SLAM基础|基于误差状态的卡尔曼滤波ESKF SLAM|误差状态卡尔曼滤波|ESKF

本文主要参考高博的知乎：https://zhuanlan.zhihu.com/p/441182819
ESKF的优点在现在的大多数惯性系统中，人们往往更倾向于使用误差状态卡尔曼滤波器（Error state Kalman filter, ESKF）而非原始状态的卡尔曼滤波器。相比于传统KF，ESKF的优点可以总结如下：

在旋转的处理上，ESKF的状态变量可以采用最小化的参数表达，也就是使用三维变量来表达旋转的增量。而传统KF需要用到四元数（4维）或者更高维的表达（旋转矩阵，9维），要不就得采用带有奇异性的表达方式（欧拉角）。
ESKF总是在原点附近，离奇异点较远，并且也不会由于离工作点太远而导致线性化近似不够的问题。
ESKF的状态量为小量，其二阶变量相对来说可以忽略。同时大多数雅可比矩阵在小量情况下变得非常简单，甚至可以用单位阵代替。误差状态的运动学也相比原状态变量要来得更小，因为我们可以把大量更新部分放到原状态变量中。

ESKF流程在ESKF中，我们通常把原状态变量称为名义状态变量（nominal state），然后把ESKF里的状态变量称为误差状态变量（error state）。
ESKF整体流程如下：当IMU测量数据到达时，我们把它积分后，放入名义状态变量中。**由于这种做法没有考虑噪声，其结果自然会快速漂移，于是我们希望把误差部分作为误差变量放在ESKF中。**ESKF内部会考虑各种噪声和零偏的影响，并且给出误差状态的一个高斯分布描述。同时，ESKF本身作为一种卡尔曼滤波器，也具有预测过程和修正过程，其中修正过程需要依赖IMU以外的传感器观测。当然，在修正之后，ESKF可以给出后验的误差高斯分布，随后我们可以把这部分误差放入名义状态变量中，并把ESKF置零，这样就完成了一次循环。
连续时间的ESKF推导我们设ESKF的真值状态为： x t = [ p t , v t , R t , b a t , b g t , g t ] T \boldsymbol{x}_{t}=\left[\boldsymbol{p}_{t}, \boldsymbol{v}_{t}, \boldsymbol{R}_{t}, \boldsymbol{b}_{a_t}, \boldsymbol{b}_{g_t}, \boldsymbol{g}_{t}\right]^{\mathrm{T}} xt?=[pt?,vt?,Rt?,bat??,bgt??,gt?]T。这个状态随着时间而改变，可以记为x ( t ) t \boldsymbol{x}(t)_{t} x(t)t?。在连续时间上，我们记 IMU 的读数为ω ~ , a ~ \tilde{\boldsymbol{\omega}}, \tilde{\boldsymbol{a}} ω~,a~，那么可以写出状态变量导数相对于观测量之间的关系式：
p ˙ t = v t v ˙ t = R t ( a ~ ? b a t ? η a ) + g t R ˙ t = R t ( ω ~ ? b g t ? η g ) ∧ b ˙ g t = η b g b ˙ a t = η b a g ˙ t = 0 (1) \begin{aligned}\dot{\boldsymbol{p}}_{t} &=\boldsymbol{v}_{t} \\\dot{\boldsymbol{v}}_{t} &=\boldsymbol{R}_{t}\left(\tilde{\boldsymbol{a}}-\boldsymbol{b}_{a_t}-\boldsymbol{\eta}_{a}\right)+\boldsymbol{g}_t \\\dot{\boldsymbol{R}}_{t} &=\boldsymbol{R}_{t}\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g_t}-\boldsymbol{\eta}_{g}\right)^{\wedge} \\\dot{\boldsymbol{b}}_{g_t} &=\boldsymbol{\eta}_{b_g} \\\dot{\boldsymbol{b}}_{a_t} &=\boldsymbol{\eta}_{b_a} \\\dot{\boldsymbol{g}}_t &=\mathbf{0}\end{aligned}\tag{1} p˙?t?v˙t?R˙t?b˙gt??b˙at??g˙?t??=vt?=Rt?(a~?bat???ηa?)+gt?=Rt?(ω~?bgt???ηg?)∧=ηbg??=ηba??=0?(1)
其中带下标t t t 的表示真值。这里把重力g g g 考虑进来的主要原因是方便确定IMU的初始姿态。如果我们不在状态方程里写出重力变量，那么必须事先确定初始时刻的IMU朝向R ( 0 ) \boldsymbol{R}(0) R(0)，才可以执行后续的计算。此时 IMU 的姿态就是相对于初始的水平面来描述的。而如果把重力写出来，就可以设IMU的初始姿态为单位矩阵R = I \boldsymbol{R} = \boldsymbol{I} R=I ，而把重力方向作为IMU当前姿态相比于水平面的一个度量。二种方法都是可行的，不过将重力方向单独表达出来会使得初始姿态表达更加简单，同时还可以增加一些线性性。
如果把观测量和噪声量整理的一个向量，我们也可以把上式整理成矩阵形式。不过这里的矩阵形式将含有很多的零项，相比上式并不会有明显简化，所以我们就先使用这种散开的公式。下面我们来推导误差状态方程。首先定义误差状态变量为：
p t = p + δ p v t = v + δ v R t = R δ R或q t = q δ q b g t = b g + δ b g b a t = b a + δ b a g t = g + δ g (2) \begin{aligned}\boldsymbol{p}_{t} &=\boldsymbol{p}+\delta \boldsymbol{p} \\\boldsymbol{v}_{t} &=\boldsymbol{v}+\delta \boldsymbol{v} \\\boldsymbol{R}_{t} &=\boldsymbol{R} \delta \boldsymbol{R} \text { 或 } \boldsymbol{q}_{t}=\boldsymbol{q} \delta \boldsymbol{q} \\\boldsymbol{b}_{g t} &=\boldsymbol{b}_{g}+\delta \boldsymbol{b}_{g} \\\boldsymbol{b}_{a t} &=\boldsymbol{b}_{a}+\delta \boldsymbol{b}_{a} \\\boldsymbol{g}_{t} &=\boldsymbol{g}+\delta \boldsymbol{g}\end{aligned}\tag{2} pt?vt?Rt?bgt?bat?gt??=p+δp=v+δv=RδR 或 qt?=qδq=bg?+δbg?=ba?+δba?=g+δg?(2)
不带下标的就是名义状态变量。名义状态变量的运动学方程式与真值相同，只是不必考虑噪声（因为噪声在误差状态方程中考虑了）。其中旋转部分的δ R \delta \boldsymbol{R} δR 可以用它的李代数Exp ? ( δ θ ) \operatorname{Exp}(\delta \boldsymbol{\theta}) Exp(δθ) 来表示，此时旋转公式也需要改成用指数形式来表达。关于误差变量的平移、零偏和重力公式，都很容易得出对应的时间导数表达式，只需在等式两侧分别对时间求导即可：
δ p ˙ = δ v δ b g ˙ = η b g δ b a ˙ = η b a δ g = 0 (3) \begin{aligned}\delta \dot{\boldsymbol{p}} &=\delta \boldsymbol{v} \\\delta \dot{\boldsymbol{b}_{g}} &=\boldsymbol{\eta}_{b_g} \\\delta \dot{\boldsymbol{b}_{a}} &=\boldsymbol{\eta}_{b_a} \\\delta \boldsymbol{g} &=\mathbf{0}\end{aligned}\tag{3} δp˙?δbg?˙?δba?˙?δg?=δv=ηbg??=ηba??=0?(3)
而速度、旋转两式由于和δ R \delta \boldsymbol{R} δR 有关系，所以要单独推导。
误差状态的旋转项对旋转式两侧求时间导数，可得：
R ˙ t = R ˙ Exp ? ( δ θ ) + R Exp ? ( δ θ ˙ ) = R t ( ω ~ ? b g t ? η g ) ∧ (4) \dot{\boldsymbol{R}}_{t} =\dot{\boldsymbol{R}} \operatorname{Exp}(\delta \boldsymbol{\theta})+\boldsymbol{R} \dot{\operatorname{Exp}(\delta \boldsymbol{\theta}}) =\boldsymbol{R}_{t}\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g t}-\boldsymbol{\eta}_{g}\right)^{\wedge}\tag{4} R˙t?=R˙Exp(δθ)+RExp(δθ˙?)=Rt?(ω~?bgt??ηg?)∧(4)
由于Exp ? ( δ θ ) = Exp ? ( δ θ ) δ θ ˙ ∧ \operatorname{Exp}(\delta \boldsymbol{\theta})=\operatorname{Exp}(\delta \boldsymbol{\theta}) \delta \dot{\boldsymbol{\theta}}^{\wedge} Exp(δθ)=Exp(δθ)δθ˙∧，因此公式（4）中间的式子可以写成：
R ˙ Exp ? ( δ θ ) + R Exp ? ( δ θ ˙ ) = R ( ω ~ ? b g ) ∧ Exp ? ( δ θ ) + R Exp ? ( δ θ ) δ θ ˙ ∧ (5) \dot{\boldsymbol{R}} \operatorname{Exp}(\delta \boldsymbol{\theta})+\boldsymbol{R} \dot{\operatorname{Exp}(\delta \boldsymbol{\theta}}) =\boldsymbol{R}\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g}\right)^{\wedge} \operatorname{Exp}(\delta \boldsymbol{\theta})+\boldsymbol{R} \operatorname{Exp}(\delta \boldsymbol{\theta}) \delta \dot{\boldsymbol{\theta}}^{\wedge}\tag{5} R˙Exp(δθ)+RExp(δθ˙?)=R(ω~?bg?)∧Exp(δθ)+RExp(δθ)δθ˙∧(5)
而公式（4）最右边的式子可以写成：
R t ( ω ~ ? b g t ? η g ) ∧ = R Exp ? ( δ θ ) ( ω ~ ? b g t ? η g ) ∧ (6) \boldsymbol{R}_{t}\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g t}-\boldsymbol{\eta}_{g}\right)^{\wedge}=\boldsymbol{R} \operatorname{Exp}(\delta \boldsymbol{\theta})\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g t}-\boldsymbol{\eta}_{g}\right)^{\wedge}\tag{6} Rt?(ω~?bgt??ηg?)∧=RExp(δθ)(ω~?bgt??ηg?)∧(6)
比较公式（5）和公式（6），将δ θ ˙ \dot{\delta \boldsymbol{\theta}} δθ˙ 移到等号左边，并约掉R \boldsymbol{R} R ，整理得：
Exp ? ( δ θ ) δ θ ˙ ∧ = Exp ? ( δ θ ) ( ω ~ ? b g t ? η g ) ∧ ? ( ω ~ ? b g ) ∧ Exp ? ( δ θ ) (7) \operatorname{Exp}(\delta \boldsymbol{\theta}) \delta \dot{\boldsymbol{\theta}}^{\wedge}=\operatorname{Exp}(\delta \boldsymbol{\theta})\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g t}-\boldsymbol{\eta}_{g}\right)^{\wedge}-\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g}\right)^{\wedge} \operatorname{Exp}(\delta \boldsymbol{\theta})\tag{7} Exp(δθ)δθ˙∧=Exp(δθ)(ω~?bgt??ηg?)∧?(ω~?bg?)∧Exp(δθ)(7)
由于Exp ? ( δ θ ) \operatorname{Exp}(\delta \boldsymbol{\theta}) Exp(δθ) 本身是一个S O ( 3 ) SO(3) SO(3) 矩阵，利用S O ( 3 ) SO(3) SO(3) 上的伴随性质：
? ∧ R = R ( R T ? ) ∧ \boldsymbol{\phi}^{\wedge} \boldsymbol{R}=\boldsymbol{R}\left(\boldsymbol{R}^{\mathrm{T}} \boldsymbol{\phi}\right)^{\wedge} ?∧R=R(RT?)∧
用来交换公式（7）中的Exp ? ( δ θ ) \operatorname{Exp}(\delta \boldsymbol{\theta}) Exp(δθ)，即：
Exp ? ( δ θ ) δ θ ˙ ∧ = Exp ? ( δ θ ) ( ω ~ ? b g t ? η g ) ∧ ? Exp ? ( δ θ ) ( Exp ? ( ? δ θ ) ( ω ~ ? b g ) ) ∧ = Exp ? ( δ θ ) [ ( ω ~ ? b g t ? η g ) ∧ ? ( Exp ? ( ? δ θ ) ( ω ~ ? b g ) ) ∧ ] ≈ Exp ? ( δ θ ) [ ( ω ~ ? b g t ? η g ) ∧ ? ( ( I ? δ θ ∧ ) ( ω ~ ? b g ) ) ∧ ] = Exp ? ( δ θ ) [ b g ? b g t ? η g + δ θ ∧ ω ~ ? δ θ ∧ b g ] ∧ = Exp ? ( δ θ ) [ ( ? ω ~ + b g ) ∧ δ θ ? δ b g ? η g ] ∧ (8) \begin{aligned}\operatorname{Exp}(\delta \boldsymbol{\theta}) \delta \dot{\boldsymbol{\theta}}^{\wedge} &=\operatorname{Exp}(\delta \boldsymbol{\theta})\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g t}-\boldsymbol{\eta}_{g}\right)^{\wedge}-\operatorname{Exp}(\delta \boldsymbol{\theta})\left(\operatorname{Exp}(-\delta \boldsymbol{\theta})\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g}\right)\right)^{\wedge} \\&=\operatorname{Exp}(\delta \boldsymbol{\theta})\left[\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g t}-\boldsymbol{\eta}_{g}\right)^{\wedge}-\left(\operatorname{Exp}(-\delta \boldsymbol{\theta})\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g}\right)\right)^{\wedge}\right] \\& \approx \operatorname{Exp}(\delta \boldsymbol{\theta})\left[\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g t}-\boldsymbol{\eta}_{g}\right)^{\wedge}-\left(\left(\boldsymbol{I}-\delta \boldsymbol{\theta}^{\wedge}\right)\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g}\right)\right)^{\wedge}\right] \\&=\operatorname{Exp}(\delta \boldsymbol{\theta})\left[\boldsymbol{b}_{g}-\boldsymbol{b}_{g t}-\boldsymbol{\eta}_{g}+\delta \boldsymbol{\theta}^{\wedge} \tilde{\boldsymbol{\omega}}-\delta \boldsymbol{\theta}^{\wedge} \boldsymbol{b}_{g}\right]^{\wedge} \\&=\operatorname{Exp}(\delta \boldsymbol{\theta})\left[\left(-\tilde{\boldsymbol{\omega}}+\boldsymbol{b}_{g}\right)^{\wedge} \delta \boldsymbol{\theta}-\delta \boldsymbol{b}_{g}-\boldsymbol{\eta}_{g}\right]^{\wedge}\end{aligned} \tag{8} Exp(δθ)δθ˙∧?=Exp(δθ)(ω~?bgt??ηg?)∧?Exp(δθ)(Exp(?δθ)(ω~?bg?))∧=Exp(δθ)[(ω~?bgt??ηg?)∧?(Exp(?δθ)(ω~?bg?))∧]≈Exp(δθ)[(ω~?bgt??ηg?)∧?((I?δθ∧)(ω~?bg?))∧]=Exp(δθ)[bg??bgt??ηg?+δθ∧ω~?δθ∧bg?]∧=Exp(δθ)[(?ω~+bg?)∧δθ?δbg??ηg?]∧?(8)
约掉等式左侧的系数，可得：
δ θ ˙ ≈ ? ( ω ~ ? b g ) ∧ δ θ ? δ b g ? η g (9) \delta \dot{\boldsymbol{\theta}} \approx-\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g}\right)^{\wedge} \delta \boldsymbol{\theta}-\delta \boldsymbol{b}_{g}-\boldsymbol{\eta}_{g}\tag{9} δθ˙≈?(ω~?bg?)∧δθ?δbg??ηg?(9)
误差状态的速度项接下来考虑速度方程的误差形式。同样地，对公式（2）中的速度相关的方程两侧求时间导数，就可以得到δ v ˙ \delta \dot{\boldsymbol{v}} δv˙ 的表达式。等式左侧为：
v ˙ t = R t ( a ~ ? b a t ? η a ) + g t = R Exp ? ( δ θ ) ( a ~ ? b a ? δ b a ? η a ) + g + δ g ≈ R ( I + δ θ ∧ ) ( a ~ ? b a ? δ b a ? η a ) + g + δ g ≈ R a ~ ? R b a ? R δ b a ? R η a + R δ θ ∧ a ? R δ θ ∧ b a + g + δ g = R a ~ ? R b a ? R δ b a ? R a ? R a ~ ∧ δ θ + R b a ∧ δ θ + g + δ g (10) \begin{aligned}\dot{\boldsymbol{v}}_{t} &=\boldsymbol{R}_{t}\left(\tilde{\boldsymbol{a}}-\boldsymbol{b}_{a t}-\boldsymbol{\eta}_{a}\right)+\boldsymbol{g}_{t} \\&=\boldsymbol{R} \operatorname{Exp}(\delta \boldsymbol{\theta})\left(\tilde{\boldsymbol{a}}-\boldsymbol{b}_{a}-\delta \boldsymbol{b}_{a}-\boldsymbol{\eta}_{a}\right)+\boldsymbol{g}+\delta \boldsymbol{g} \\& \approx \boldsymbol{R}\left(\boldsymbol{I}+\delta \boldsymbol{\theta}^{\wedge}\right)\left(\tilde{\boldsymbol{a}}-\boldsymbol{b}_{a}-\delta \boldsymbol{b}_{a}-\boldsymbol{\eta}_{a}\right)+\boldsymbol{g}+\delta \boldsymbol{g} \\& \approx \boldsymbol{R} \tilde{\boldsymbol{a}}-\boldsymbol{R} \boldsymbol{b}_{a}-\boldsymbol{R} \delta \boldsymbol{b}_{a}-\boldsymbol{R} \boldsymbol{\eta}_{a}+\boldsymbol{R} \delta \boldsymbol{\theta}^{\wedge} \boldsymbol{a}-\boldsymbol{R} \delta \boldsymbol{\theta}^{\wedge} \boldsymbol{b}_{a}+\boldsymbol{g}+\delta \boldsymbol{g} \\&=\boldsymbol{R} \tilde{\boldsymbol{a}}-\boldsymbol{R} \boldsymbol{b}_{a}-\boldsymbol{R} \delta \boldsymbol{b}_{a}-\boldsymbol{R}_{a}-\boldsymbol{R} \tilde{\boldsymbol{a}}^{\wedge} \delta \boldsymbol{\theta}+\boldsymbol{R} \boldsymbol{b}_{a}^{\wedge} \delta \boldsymbol{\theta}+\boldsymbol{g}+\delta \boldsymbol{g}\end{aligned}\tag{10} v˙t??=Rt?(a~?bat??ηa?)+gt?=RExp(δθ)(a~?ba??δba??ηa?)+g+δg≈R(I+δθ∧)(a~?ba??δba??ηa?)+g+δg≈Ra~?Rba??Rδba??Rηa?+Rδθ∧a?Rδθ∧ba?+g+δg=Ra~?Rba??Rδba??Ra??Ra~∧δθ+Rba∧?δθ+g+δg?(10)
其中，上式从第三行推向第四行时，需要忽略δ θ ∧ \delta \boldsymbol{\theta}^{\wedge} δθ∧ 与δ b a , η a \delta \boldsymbol{b}_{a}, \boldsymbol{\eta}_{a} δba?,ηa? 相乘的二阶小量。从第四行推第五行则用到了叉乘符号交换顺序之后需要加负号的性质。
另一方面，等式右侧为：
v ˙ + δ v ˙ = R ( a ~ ? b a ) + g + δ v ˙ (11) \dot{\boldsymbol{v}}+\delta \dot{\boldsymbol{v}}=\boldsymbol{R}\left(\tilde{\boldsymbol{a}}-\boldsymbol{b}_{a}\right)+\boldsymbol{g}+\delta \dot{\boldsymbol{v}}\tag{11} v˙+δv˙=R(a~?ba?)+g+δv˙(11)
因为上面两式相等，可以得到：
δ v ˙ = ? R ( a ~ ? b a ) ∧ δ θ ? R δ b a ? R η a + δ g (12) \delta \dot{\boldsymbol{v}}=-\boldsymbol{R}\left(\tilde{\boldsymbol{a}}-\boldsymbol{b}_{a}\right)^{\wedge} \delta \boldsymbol{\theta}-\boldsymbol{R} \delta \boldsymbol{b}_{a}-\boldsymbol{R}\boldsymbol{\eta}_{a}+\delta \boldsymbol{g}\tag{12} δv˙=?R(a~?ba?)∧δθ?Rδba??Rηa?+δg(12)
这样我们就得到了δ v \delta \boldsymbol{v} δv 的运动学模型。需要补充一句，由于上式中的η a \boldsymbol{\eta}_{a} ηa? 是一个零均值白噪声，它乘上任意选择矩阵之后仍然是一个零均值白噪声，而且由于R T R = I \boldsymbol{R}^{\mathrm{T}} \boldsymbol{R}=\boldsymbol{I} RTR=I，其协方差矩阵也不变。所以，也可以把上式简化为：
δ v ˙ = ? R ( a ~ ? b a ) ∧ δ θ ? R δ b a ? η a + δ g (13) \delta \dot{\boldsymbol{v}}=-\boldsymbol{R}\left(\tilde{\boldsymbol{a}}-\boldsymbol{b}_{a}\right)^{\wedge} \delta \boldsymbol{\theta}-\boldsymbol{R} \delta \boldsymbol{b}_{a}-\boldsymbol{\eta}_{a}+\delta \boldsymbol{g}\tag{13} δv˙=?R(a~?ba?)∧δθ?Rδba??ηa?+δg(13)
至此，我们可以把误差变量的运动学方程整理如下：
δ p ˙ = δ v δ v ˙ = ? R ( a ~ ? b a ) ∧ δ θ ? R δ b a ? η a + δ g δ θ ˙ = ? ( ω ~ ? b g ) ∧ δ θ ? δ b g ? η g δ b g ˙ = η b g δ b ˙ a = η b a δ g ˙ = 0 (14) \begin{aligned}\delta \dot{\boldsymbol{p}} &=\delta \boldsymbol{v} \\\delta \dot{\boldsymbol{v}} &=-\boldsymbol{R}\left(\tilde{\boldsymbol{a}}-\boldsymbol{b}_{a}\right)^{\wedge} \delta \boldsymbol{\theta}-\boldsymbol{R} \delta \boldsymbol{b}_{a}-\boldsymbol{\eta}_{a}+\delta \boldsymbol{g} \\\delta \dot{\boldsymbol{\theta}} &=-\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g}\right)^{\wedge} \delta \boldsymbol{\theta}-\delta \boldsymbol{b}_{g}-\boldsymbol{\eta}_{g} \\\delta \dot{\boldsymbol{b}_{g}} &=\boldsymbol{\eta}_{b g} \\\delta \dot{\boldsymbol{b}}_{a} &=\boldsymbol{\eta}_{b a} \\\delta \dot{\boldsymbol{g}} &=\mathbf{0}\end{aligned}\tag{14} δp˙?δv˙δθ˙δbg?˙?δb˙a?δg˙??=δv=?R(a~?ba?)∧δθ?Rδba??ηa?+δg=?(ω~?bg?)∧δθ?δbg??ηg?=ηbg?=ηba?=0?(14)
离散时间的ESKF运动学方程从连续时间状态方程推出离散时间的状态方程并不困难，不妨直接来列写它们。名义状态变量的离散时间运动学方程可以写为：
p ( t + Δ t ) = p ( t ) + v Δ t + 1 2 ( R ( a ~ ? b a ) ) Δ t 2 + 1 2 g Δ t 2 v ( t + Δ t ) = v ( t ) + R ( a ~ ? b a ) Δ t + g Δ t R ( t + Δ t ) = R ( t ) Exp ? ( ( ω ~ ? b g ) Δ t ) b g ( t + Δ t ) = b g ( t ) b a ( t + Δ t ) = b a ( t ) g ( t + Δ t ) = g ( t ) (15) \begin{aligned}\boldsymbol{p}(t+\Delta t) &=\boldsymbol{p}(t)+\boldsymbol{v} \Delta t+\frac{1}{2}\left(\boldsymbol{R}\left(\tilde{\boldsymbol{a}}-\boldsymbol{b}_{a}\right)\right) \Delta t^{2}+\frac{1}{2} \boldsymbol{g} \Delta t^{2} \\\boldsymbol{v}(t+\Delta t) &=\boldsymbol{v}(t)+\boldsymbol{R}\left(\tilde{\boldsymbol{a}}-\boldsymbol{b}_{a}\right) \Delta t+\boldsymbol{g} \Delta t \\\boldsymbol{R}(t+\Delta t) &=\boldsymbol{R}(t) \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g}\right) \Delta t\right) \\\boldsymbol{b}_{g}(t+\Delta t) &=\boldsymbol{b}_{g}(t) \\\boldsymbol{b}_{a}(t+\Delta t) &=\boldsymbol{b}_{a}(t) \\\boldsymbol{g}(t+\Delta t) &=\boldsymbol{g}(t)\end{aligned}\tag{15} p(t+Δt)v(t+Δt)R(t+Δt)bg?(t+Δt)ba?(t+Δt)g(t+Δt)?=p(t)+vΔt+21?(R(a~?ba?))Δt2+21?gΔt2=v(t)+R(a~?ba?)Δt+gΔt=R(t)Exp((ω~?bg?)Δt)=bg?(t)=ba?(t)=g(t)?(15)
该式只需在上面的基础上添加零偏项与重力项即可。而误差状态的离散形式则只需要处理连续形式中的旋转部分。参考角速度的积分公式，可以将误差状态方程写为：
δ p ( t + Δ t ) = δ p + δ v Δ t δ v ( t + Δ t ) = δ v + ( ? R ( a ~ ? b a ) ∧ δ θ ? R δ b a + δ g ) Δ t + η v δ θ ( t + Δ t ) = Exp ? ( ? ( ω ~ ? b g ) Δ t ) δ θ ? δ b g Δ t ? η θ δ b g ( t + Δ t ) = δ b g + η b g δ b a ( t + Δ t ) = δ b a + η b a δ g ( t + Δ t ) = δ g (16) \begin{aligned}\delta \boldsymbol{p}(t+\Delta t) &=\delta \boldsymbol{p}+\delta \boldsymbol{v} \Delta t \\\delta \boldsymbol{v}(t+\Delta t) &=\delta \boldsymbol{v}+\left(-\boldsymbol{R}\left(\tilde{\boldsymbol{a}}-\boldsymbol{b}_{a}\right)^{\wedge} \delta \boldsymbol{\theta}-\boldsymbol{R} \delta \boldsymbol{b}_{a}+\delta \boldsymbol{g}\right) \Delta t+\boldsymbol{\eta}_{v} \\\delta \boldsymbol{\theta}(t+\Delta t) &=\operatorname{Exp}\left(-\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g}\right) \Delta t\right) \delta \boldsymbol{\theta}-\delta \boldsymbol{b}_{g} \Delta t-\boldsymbol{\eta}_{\theta} \\\delta \boldsymbol{b}_{g}(t+\Delta t) &=\delta \boldsymbol{b}_{g}+\boldsymbol{\eta}_{b_g} \\\delta \boldsymbol{b}_{a}(t+\Delta t) &=\delta \boldsymbol{b}_{a}+\boldsymbol{\eta}_{b_a} \\\delta \boldsymbol{g}(t+\Delta t) &=\delta \boldsymbol{g}\end{aligned} \tag{16} δp(t+Δt)δv(t+Δt)δθ(t+Δt)δbg?(t+Δt)δba?(t+Δt)δg(t+Δt)?=δp+δvΔt=δv+(?R(a~?ba?)∧δθ?Rδba?+δg)Δt+ηv?=Exp(?(ω~?bg?)Δt)δθ?δbg?Δt?ηθ?=δbg?+ηbg??=δba?+ηba??=δg?(16)
注意：

右侧部分我们省略了括号里的( t ) (t) (t) 以简化公式;
关于旋转部分的积分，我们可以将连续形式看成关于δ θ \delta \boldsymbol{\theta} δθ 的微分方程然后求解。求解过程类似于对角速度进行积分。
噪声项并不参与递推，需要把它们单独归入噪声部分中。连续时间的噪声项可以视为随机过程的能量谱密度，而离散时间下的噪声变量就是我们日常看到的随机变量了。这些噪声随机变量的标准差可以列写如下：
σ ( η v ) = Δ t σ a , σ ( η θ ) = Δ t σ g , σ ( η b g ) = Δ t σ b g , σ ( η b a ) = Δ t σ b a \sigma(\boldsymbol{\eta}_v) = \Delta t \sigma_a, \sigma(\boldsymbol{\eta}_{\theta}) = \Delta t \sigma_{g}, \sigma(\boldsymbol{\eta}_{b_g}) = \sqrt{\Delta t} \sigma_{bg}, \sigma(\boldsymbol{\eta}_{b_a}) = \sqrt{\Delta t} \sigma_{ba} σ(ηv?)=Δtσa?,σ(ηθ?)=Δtσg?,σ(ηbg??)=Δt ?σbg?,σ(ηba??)=Δt ?σba?
其中前两式的Δ t \Delta t Δt 是由积分关系导致的。
至此，我们给出了如何在ESKF中进行IMU递推的过程，对应于卡尔曼滤波器中的状态方程。为了让滤波器收敛，我们通常需要外部的观测来对卡尔曼滤波器进行修正，也就是所谓的组合导航。当然，组合导航的方法有很多，从传统的EKF，到本节介绍的ESKF，以及后续章节将要介绍预积分和图优化技术，都可以应用于组合导航中。
ESKF的运动过程根据上述讨论，我们可以写出ESKF的运动过程。误差状态变量δ x \delta \boldsymbol{x} δx 的离散时间运动方程已经在上式给出，我们可以整体地记为：
δ x = f ( δ x ) + w , w ～ N ( 0 , Q ) (17) \delta \boldsymbol{x}=f(\delta \boldsymbol{x})+\boldsymbol{w}, \boldsymbol{w} \sim \mathcal{N}(0, \boldsymbol{Q}) \tag{17} δx=f(δx)+w,w～N(0,Q)(17)
其中， w \boldsymbol{w} w 为噪声。按照前面的定义， Q \boldsymbol{Q} Q 应该为：
Q = diag ? ( 0 3 , Cov ? ( η v ) , Cov ? ( η θ ) , Cov ? ( η g ) , Cov ? ( η a ) , 0 3 ) (18) \boldsymbol{Q}=\operatorname{diag}\left(\mathbf{0}_{3}, \operatorname{Cov}\left(\boldsymbol{\eta}_{v}\right), \operatorname{Cov}\left(\boldsymbol{\eta}_{\theta}\right), \operatorname{Cov}\left(\boldsymbol{\eta}_{g}\right), \operatorname{Cov}\left(\boldsymbol{\eta}_{a}\right), \mathbf{0}_{3}\right) \tag{18} Q=diag(03?,Cov(ηv?),Cov(ηθ?),Cov(ηg?),Cov(ηa?),03?)(18)
两侧的零是由于第一个和最后一个方程本身没有噪声导致的。
为了保持与EKF的符号统一，我们计算运动方程的线性化形式：
δ x = F δ x + w (19) \delta \boldsymbol{x}=\boldsymbol{F} \delta \boldsymbol{x}+\boldsymbol{w} \tag{19} δx=Fδx+w(19)
其中F \boldsymbol{F} F 为线性化后的雅可比矩阵。由于我们列写的运动方程已经是线性化的了，只需把它们的线性系统拿出来即可：
F = [ I I Δ t 0 0 0 0 0 I ? R ( a ~ ? b a ) ∧ Δ t ? R Δ t 0 I Δ t 0 0 Exp ? ( ? ( ω ~ ? b g ) Δ t ) 0 ? I Δ t 0 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I ] (20) \boldsymbol{F}=\left[\begin{array}{cccccc}\boldsymbol{I} & \boldsymbol{I} \Delta t & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\\mathbf{0} & \boldsymbol{I} & -\boldsymbol{R}\left(\tilde{\boldsymbol{a}}-\boldsymbol{b}_{a}\right)^{\wedge} \Delta t & -\boldsymbol{R} \Delta t & \mathbf{0} & \boldsymbol{I} \Delta t \\\mathbf{0} & \mathbf{0} & \operatorname{Exp}\left(-\left(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_{g}\right) \Delta t\right) & \mathbf{0} & -\boldsymbol{I} \Delta t & \mathbf{0} \\\mathbf{0} & \mathbf{0} & \mathbf{0} & \boldsymbol{I} & \mathbf{0} & \mathbf{0} \\\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \boldsymbol{I} & \mathbf{0} \\\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \boldsymbol{I}\end{array}\right] \tag{20} F=? ??I00000?IΔtI0000?0?R(a~?ba?)∧ΔtExp(?(ω~?bg?)Δt)000?0?RΔt0I00?00?IΔt0I0?0IΔt000I?? ??(20)
在此基础上，我们执行ESKF的预测过程。预测过程包括对名义状态的预测（IMU积分）以及对误差状态的预测：
δ x pred= F δ x (21) \delta \boldsymbol{x}_{\text {pred }}=\boldsymbol{F} \delta \boldsymbol{x} \tag{21} δxpred ?=Fδx(21)
P p r e d = F P F T + Q (22) \boldsymbol{P}_{\mathrm{pred}}=\boldsymbol{F} \boldsymbol{P} \boldsymbol{F}^{\mathrm{T}}+\boldsymbol{Q} \tag{22} Ppred?=FPFT+Q(22)
不过由于ESKF的误差状态在每次更新以后会被重置，因此运动方程的均值部分没有太大意义，而方差部分则可以指导整个误差估计的分布情况。
ESKF的更新过程前面介绍的是ESKF的运动过程，现在我们来考虑更新过程。假设一个抽象的传感器能够对状态变量产生观测，其观测方程为抽象的h h h，那么可以写为：
z = h ( x ) + v , v ～ N ( 0 , V ) (23) \boldsymbol{z}=h(\boldsymbol{x})+\boldsymbol{v}, \boldsymbol{v} \sim \mathcal{N}(0, \boldsymbol{V}) \tag{23} z=h(x)+v,v～N(0,V)(23)
其中， z \boldsymbol{z} z 为观测数据， v \boldsymbol{v} v 为观测噪声， V \boldsymbol{V} V 为该噪声的协方差矩阵。
在传统EKF中，我们可以直观对观测方程线性化，求出观测方程相对于状态变量的雅可比矩阵，进而更新卡尔曼滤波器。而在ESKF中，我们当前拥有名义状态x \boldsymbol{x} x 的估计以及误差状态δ x \delta \boldsymbol{x} δx 的估计，且希望更新的是误差状态，因此要计算观测方程相比于误差状态的雅可比矩阵：
H = ? h ? δ x (24) \boldsymbol{H}=\frac{\partial h}{\partial \delta \boldsymbol{x}} \tag{24} H=?δx?h?(24)
然后再计算卡尔曼增益，进而计算误差状态的更新过程：
K = P predH T ( H P p r e d H T + V ) ? 1 δ x = K ( z ? h ( x t ) ) P = ( I ? K H ) P p r e d (25) \begin{aligned}\boldsymbol{K} &=\boldsymbol{P}_{\text {pred }} \boldsymbol{H}^{\mathrm{T}}\left(\boldsymbol{H} \boldsymbol{P}_{\mathrm{pred}} \boldsymbol{H}^{\mathrm{T}}+\boldsymbol{V}\right)^{-1} \\\delta \boldsymbol{x} &=\boldsymbol{K}\left(\boldsymbol{z}-h\left(\boldsymbol{x}_{t}\right)\right) \\\boldsymbol{P} &=(\boldsymbol{I}-\boldsymbol{K} \boldsymbol{H}) \boldsymbol{P}_{\mathrm{pred}}\end{aligned} \tag{25} KδxP?=Ppred ?HT(HPpred?HT+V)?1=K(z?h(xt?))=(I?KH)Ppred??(25)
其中， K \boldsymbol{K} K 为卡尔曼增益， P p r e d \boldsymbol{P}_{pred} Ppred? 为预测的协方差矩阵，最后的P \boldsymbol{P} P 为修正后的协方差矩阵。这里的H \boldsymbol{H} H 可以通过链式法则来计算：
H = ? h ? x ? x ? δ x (26) \boldsymbol{H}=\frac{\partial h}{\partial \boldsymbol{x}} \frac{\partial \boldsymbol{x}}{\partial \delta \boldsymbol{x}} \tag{26} H=?x?h??δx?x?(26)
第一项只需要对观测方程进行线性化，第二项，根据我们之前对状态变量的定义，可以得到：
? x ? δ x = d i a g ( I 3 , I 3 , ? L o g ( R ( E x p ( δ θ ) ) ) ? δ θ , I 3 , I 3 , I 3 ) (27) \frac{\partial\bm{x}}{\partial \delta\bm{x}} = \mathrm{diag}(\bm{I}_3, \bm{I}_3, \frac{\partial \mathrm{Log} (\bm{R}(\mathrm{Exp}(\delta \boldsymbol{\theta})))}{\partial \delta \boldsymbol{\theta}}, \bm{I}_3, \bm{I}_3, \bm{I}_3) \tag{27} ?δx?x?=diag(I3?,I3?,?δθ?Log(R(Exp(δθ)))?,I3?,I3?,I3?)(27)
其他几种都是平凡的，只有旋转部分，因为δ θ \delta \boldsymbol{\theta} δθ 定义为R \boldsymbol{R} R 的右乘，我们用右乘的BCH即可：
? L o g ( R ( E x p ( δ θ ) ) ) ? δ θ = J r ? 1 ( R ) (28) \frac{\partial \mathrm{Log} (\bm{R}(\mathrm{Exp}(\delta\boldsymbol{\theta})))}{\partial \delta \boldsymbol{\theta}} = \bm{J}_r^{-1} (\bm{R}) \tag{28} ?δθ?Log(R(Exp(δθ)))?=Jr?1?(R)(28)
最后，我们可以给每个变量加下标k k k，表示在k k k 时刻进行状态估计。
ESKF的误差状态后续处理在经过预测和更新过程之后，我们修正了误差状态的估计。接下来，只需把误差状态归入名义状态，然后重置ESKF即可。归入部分可以简单地写为：
p k + 1 = p k + δ p k v k + 1 = v k + δ v k R k + 1 = R k Exp ? ( δ θ k ) b g , k + 1 = b g , k + δ b g , k b a , k + 1 = b a , k + δ b a , k g k + 1 = g k + δ g k (29) \begin{aligned}\boldsymbol{p}_{k+1} &=\boldsymbol{p}_{k}+\delta \boldsymbol{p}_{k} \\\boldsymbol{v}_{k+1} &=\boldsymbol{v}_{k}+\delta \boldsymbol{v}_{k} \\\boldsymbol{R}_{k+1} &=\boldsymbol{R}_{k} \operatorname{Exp}\left(\delta \boldsymbol{\theta}_{k}\right) \\\boldsymbol{b}_{g, k+1} &=\boldsymbol{b}_{g, k}+\delta \boldsymbol{b}_{g, k} \\\boldsymbol{b}_{a, k+1} &=\boldsymbol{b}_{a, k}+\delta \boldsymbol{b}_{a, k} \\\boldsymbol{g}_{k+1} &=\boldsymbol{g}_{k}+\delta \boldsymbol{g}_{k}\end{aligned} \tag{29} pk+1?vk+1?Rk+1?bg,k+1?ba,k+1?gk+1??=pk?+δpk?=vk?+δvk?=Rk?Exp(δθk?)=bg,k?+δbg,k?=ba,k?+δba,k?=gk?+δgk??(29)
有些文献里也会定义为广义的状态变量加法：
x k + 1 = x k ⊕ δ x k (30) \boldsymbol{x}_{k+1}=\boldsymbol{x}_{k} \oplus \delta \boldsymbol{x}_{k} \tag{30} xk+1?=xk?⊕δxk?(30)
这种写法可以简化整体的表达式。不过，如果公式里出现太多的广义加减法，可能让人不好马上辨认它们的具体含义，所以本书还是倾向于将各状态分别写开，或者直接用加法而非广义加法符号。
ESKF的重置可以简单地实现为：
δ x = 0 (31) \delta\bm{x} = \bm{0} \tag{31} δx=0(31)
小结 【SLAM基础|基于误差状态的卡尔曼滤波ESKF】本节向大家介绍了SO(3)流形上的ESKF，相比于四元数形式或欧拉角形式，更为简单，无需自定义太多符号。