基于误差状态的卡尔曼滤波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的真值状态为：xt=[pt,vt,Rt,bat,bgt,gt]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=vtv˙t=Rt(a~−bat−ηa)+gtR˙t=Rt(ω~−bgt−ηg)∧b˙gt=ηbgb˙at=ηbag˙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˙tv˙tR˙tb˙gtb˙atg˙t=vt=Rt(a~−bat−ηa)+gt=Rt(ω~−bgt−ηg)∧=ηbg=ηba=0(1)
其中带下标 ttt 的表示真值。这里把重力 ggg 考虑进来的主要原因是方便确定IMU的初始姿态。如果我们不在状态方程里写出重力变量，那么必须事先确定初始时刻的IMU朝向 R(0)\boldsymbol{R}(0)R(0)，才可以执行后续的计算。此时 IMU 的姿态就是相对于初始的水平面来描述的。而如果把重力写出来，就可以设IMU的初始姿态为单位矩阵 R=I\boldsymbol{R} = \boldsymbol{I}R=I ，而把重力方向作为IMU当前姿态相比于水平面的一个度量。二种方法都是可行的，不过将重力方向单独表达出来会使得初始姿态表达更加简单，同时还可以增加一些线性性。
如果把观测量和噪声量整理的一个向量，我们也可以把上式整理成矩阵形式。不过这里的矩阵形式将含有很多的零项，相比上式并不会有明显简化，所以我们就先使用这种散开的公式。下面我们来推导误差状态方程。首先定义误差状态变量为：
pt=p+δpvt=v+δvRt=RδR或 qt=qδqbgt=bg+δbgbat=ba+δbagt=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} ptvtRtbgtbatgt=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δbg˙=ηbgδba˙=ηbaδ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(δθ)+RExp(δθ˙)=Rt(ω~−bgt−η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(δθ)+RExp(δθ˙)=R(ω~−bg)∧Exp(δθ)+RExp(δθ)δθ˙∧(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）最右边的式子可以写成：
Rt(ω~−bgt−ηg)∧=RExp(δθ)(ω~−bgt−η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(δθ)(ω~−bgt−ηg)∧−(ω~−bg)∧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(δθ) 本身是一个 SO(3)SO(3)SO(3) 矩阵，利用 SO(3)SO(3)SO(3) 上的伴随性质：
ϕ∧R=R(RTϕ)∧\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(δθ)(ω~−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)\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)
约掉等式左侧的系数，可得：
δθ˙≈−(ω~−bg)∧δθ−δbg−η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=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)\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}δθ∧ 与 δba,ηa\delta \boldsymbol{b}_{a}, \boldsymbol{\eta}_{a}δba,ηa 相乘的二阶小量。从第四行推第五行则用到了叉乘符号交换顺序之后需要加负号的性质。
另一方面，等式右侧为：
v˙+δv˙=R(a~−ba)+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~−ba)∧δθ−Rδba−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 是一个零均值白噪声，它乘上任意选择矩阵之后仍然是一个零均值白噪声，而且由于 RTR=I\boldsymbol{R}^{\mathrm{T}} \boldsymbol{R}=\boldsymbol{I}RTR=I，其协方差矩阵也不变。所以，也可以把上式简化为：
δv˙=−R(a~−ba)∧δθ−Rδba−η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~−ba)∧δθ−Rδba−ηa+δgδθ˙=−(ω~−bg)∧δθ−δbg−ηgδbg˙=ηbgδb˙a=ηbaδ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+12(R(a~−ba))Δt2+12gΔt2v(t+Δt)=v(t)+R(a~−ba)Δt+gΔtR(t+Δt)=R(t)Exp((ω~−bg)Δt)bg(t+Δt)=bg(t)ba(t+Δt)=ba(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+21gΔ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~−ba)∧δθ−Rδba+δg)Δt+ηvδθ(t+Δt)=Exp(−(ω~−bg)Δt)δθ−δbgΔt−ηθδbg(t+Δt)=δbg+ηbgδba(t+Δt)=δba+ηbaδ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,σ(ηbg)=Δtσbg,σ(ηba)=Δtσba\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(03,Cov(ηv),Cov(ηθ),Cov(ηg),Cov(ηa),03)(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=[IIΔt00000I−R(a~−ba)∧Δt−RΔt0IΔt00Exp(−(ω~−bg)Δt)0−IΔt0000I000000I000000I](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=⎣⎡I00000IΔtI00000−R(a~−ba)∧ΔtExp(−(ω~−bg)Δt)0000−RΔt0I0000−IΔt0I00IΔt000I⎦⎤(20)
在此基础上，我们执行ESKF的预测过程。预测过程包括对名义状态的预测（IMU积分）以及对误差状态的预测：
δxpred =Fδx(21)\delta \boldsymbol{x}_{\text {pred }}=\boldsymbol{F} \delta \boldsymbol{x} \tag{21} δxpred =Fδx(21)
Ppred=FPFT+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的运动过程，现在我们来考虑更新过程。假设一个抽象的传感器能够对状态变量产生观测，其观测方程为抽象的 hhh，那么可以写为：
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=Ppred HT(HPpredHT+V)−1δx=K(z−h(xt))P=(I−KH)Ppred(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(HPpredHT+V)−1=K(z−h(xt))=(I−KH)Ppred(25)
其中，K\boldsymbol{K}K 为卡尔曼增益，Ppred\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=diag(I3,I3,∂Log(R(Exp(δθ)))∂δθ,I3,I3,I3)(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即可：
∂Log(R(Exp(δθ)))∂δθ=Jr−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)
最后，我们可以给每个变量加下标 kkk，表示在 kkk 时刻进行状态估计。
ESKF的误差状态后续处理
在经过预测和更新过程之后，我们修正了误差状态的估计。接下来，只需把误差状态归入名义状态，然后重置ESKF即可。归入部分可以简单地写为：
pk+1=pk+δpkvk+1=vk+δvkRk+1=RkExp(δθk)bg,k+1=bg,k+δbg,kba,k+1=ba,k+δba,kgk+1=gk+δgk(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+1vk+1Rk+1bg,k+1ba,k+1gk+1=pk+δpk=vk+δvk=RkExp(δθk)=bg,k+δbg,k=ba,k+δba,k=gk+δgk(29)
有些文献里也会定义为广义的状态变量加法：
xk+1=xk⊕δxk(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)
小结
本节向大家介绍了SO(3)流形上的ESKF，相比于四元数形式或欧拉角形式，更为简单，无需自定义太多符号。
基于误差状态的卡尔曼滤波ESKF相关推荐
- 基于误差状态卡尔曼滤波惯性导航理论
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 作者丨擦擦擦大侠 编辑丨古月居 点击进入->3D视觉工坊学习交流群 惯性导航备注 1.坐标系之间 ...
- 文章翻译—基于误差状态卡尔曼滤波器的四元数运动学—前言
基于误差状态卡尔曼滤波(ESKF)的四元数运动学 文章作者:Joan Sol`a 发表时间:October 12, 2017(注:Quaternion kinematics for the error ...
- 文章翻译—基于误差状态卡尔曼滤波器的四元数运动学—第4章
文章目录 基于误差状态卡尔曼滤波器的四元数运动学 4. 扰动,导数和积分 4.1 SO(3)SO(3)中的加法和减法运算符 4.2 四个可能的导数定义 4.2.1 从向量空间到向量空间的函数 4.2. ...
- 学习MSCKF笔记——真实状态、标称状态、误差状态
学习MSCKF笔记--真实状态.标称状态.误差状态 学习MSCKF笔记--真实状态.标称状态.误差状态 1. 连续时间系统 1.1 真实状态运动学公式 1.2 标称状态运动学公式 1.3 误差状态运动 ...
- 1. 简明误差卡尔曼滤波器（ESKF）及其推导过程
文章目录 1. 简明误差卡尔曼滤波器(`ESKF`)及其推导过程 简介 `ESKF`基本过程及优点 `ESKF`参数含义 连续时间上的 `ESKF`状态方程 误差状态方程推导 误差状态的旋转项 误差状 ...
- 基于绝缘状态的煤矿电缆绝缘可视化在线检测系统
摘要:针对供电系统绝缘问题检测技术限制煤炭产量效率的问题,以某煤炭企业6kV井下供电系统为研究对象,开展了在线监测系统设计与应用工作.结果表明,系统工作稳定,满足井下电力电缆绝缘在线监要求,降低了井下 ...
- matlab plv,一种基于微状态的脑功能网络构建方法与流程
本发明涉及脑功能网络研究技术领域,更具体而言,涉及一种基于微状态的脑功能网络构建方法. 背景技术: 复杂网络作为近年来一种新兴的数据分析方法,被应用于各个方面.由于大脑是一个十分复杂的系统,不同神经元 ...
- Semi-supervised Semantic Segmentation with Error Localization Network(基于误差定位网络的半监督语义分割 )
Semi-supervised Semantic Segmentation with Error Localization Network(基于误差定位网络的半监督语义分割 ) Abstract 本文 ...
- 基于simulink的拓展卡尔曼滤波估计路面附着系数识别EKF，内含道夫轮胎模型
基于simulink的拓展卡尔曼滤波估计路面附着系数识别EKF,内含道夫轮胎模型,七自由度整车仿真模型,输出结果收敛,如下图,并可以在多种附着系数工况下收敛识别,验证模型合理性. ID:6914966 ...
最新文章
- mysql2013年8月怎么打出来_2020年8月31日，上周完成了一个查询接口来检查mysql的数据，速度很慢,20200831,从,MySQL,中查,贼...
- linux 设备树调试必须知道的几个路径
- 无人值守数据中心这一次真的能“大势所趋”吗？
- gc可视化分析_GC内存可视化器教程–第一部分
- java ee 的使用方法_改善Java EE生产支持技能的8种方法
- Django 学习笔记第一课
- TensorFlow2.0 —— 模型保存与加载
- emacs latex_使用Emacs Org模式轻松创建LaTeX文档
- 数据采集技术的难点在于哪里
- apollo本地启动调方式
- Windows10下安装MySQL8.0
- ios 中h5网页跳到第三方后回到项目字体变大
- 从GitHub火到了CSDN，共计1658页的《Java岗面试核心MCA版》
- scala成长之路（1）基本语法和数据类型
- 【Excel】在单元格中插入换行符
- Linux:更新 /usr/share/glib-2.0/schemas 目录
- MySQL被黑客攻击勒索5000美元，幸好有备份
- 选项不属于HTML语言特点,JavaScript选择题
- web端上传图片的几种方式
- [HTTPD] Linux（Apache）Httpd服务器安装，启动及httpd.conf配置详解