当前位置: 首页 > news >正文

简明误差卡尔曼滤波器(ESKF)及其推导过程

文章目录

  • 1. 简明`ESKF`
    • 简介
    • `ESKF`基本过程及优点
    • `ESKF`参数含义
    • 连续时间上的 `ESKF`状态方程
      • 误差状态方程推导
      • 误差状态的旋转项
      • 误差状态的速度项
      • 完整误差变量的运动学方程
    • 离散时间上的`ESKF`运动学方程
    • `ESKF`的运动过程
    • `ESKF`的更新过程
    • `ESKF`的误差状态后续处理
    • 小结

1. 简明ESKF

简介

本文主要介绍一种特殊正交群 SO(3) \text{SO(3)} SO(3) 上的ESKF(Error State Kalman Filter, 误差卡尔曼滤波器)(有时也叫做流形上的ESKF)推导过程。

ESKF基本过程及优点

在现代的大多数IMU系统中,人们往往使用误差状态卡尔曼滤波器(Error State Kalman Filter, ESKF),而非原始状态的卡尔曼滤波器。大部分基于滤波器的LIOVIO实现,都使用ESKF作为状态估计方法。

相比于传统KFESKF优点如下:

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

ESKF中,我们通常把原状态变量称为名义状态变量(Norminal State),把ESKF中的状态变量称为误差状态变量(Error State)

ESKF整体流程如下:

  • IMU测量数据到达时,首先将其积分后,放入名义状态变量中。
  • 由于这种做法没有考虑噪声,其结果自然会快速漂移。于是我们希望把误差部分作为误差变量,放入ESKF中。
  • ESKF内部会考虑各种噪声零偏的影响,并且给出误差状态的一个 高斯分布 描述。同时ESKF本身作为一种卡尔曼滤波器,也具有预测过程修正过程。其中 修正过程 需要依赖 IMU 以外的传感器观测。在修正后,ESKF可以给出 后验的误差高斯分布
  • 随后,我们将这部分误差放入名义状态变量中,并把ESKF置零,这样就完成了一次循环。

ESKF参数含义

在这里插入图片描述

连续时间上的 ESKF状态方程

我们设ESKF的真值状态为:
x t = [ p t , v t , R t , b a t , b g t , g t ] T x_t = [p_t, v_t,R_t,b_{at},b_{gt},g_t]^\text{T} xt=[pt,vt,Rt,bat,bgt,gt]T
这个状态随时间改变,可以记为 x ( t ) t x(t)_t x(t)t

在连续时间上,我们记IMU读数为 ω ~ , a ~ \tilde{\omega}, \tilde{a} ω~,a~,那么可以写出状态变量导数 相对于 观测量之间的关系式:
p t ˙ = v t (1 a) \dot{p_t} = v_t \tag{1 a} pt˙=vt(1 a)

v ˙ t = R t ( a ~ − b a t − η a ) + g (1 b) \dot{v}_t = R_t(\tilde{a} - b_{at} -\eta_a) + g \tag {1 b} v˙t=Rt(a~batηa)+g(1 b)

R ˙ t = R t ( ω ~ − b g t − η g ) ∧ (1 c) \dot{R}_t = R_t(\tilde{\omega} - b_{gt} - \eta_g)^{\wedge} \tag{1 c} R˙t=Rt(ω~bgtηg)(1 c)

b ˙ g t = η b g (1 d) \dot{b}_{gt} = \eta_{bg} \tag{1 d} b˙gt=ηbg(1 d)

b ˙ a t = η b a (1 e) \dot{b}_{at} = \eta_{ba} \tag{1 e} b˙at=ηba(1 e)

g ˙ = 0 (1 f) \dot{g} = 0 \tag{1 f} g˙=0(1 f)

其中,带下标 t t t 的表示真值。

这里把重力 g g g 考虑进来的主要理由是方便确定IMU的初始姿态。

  • 如果我们不在状态方程里写出重力变量,那么就必须事先确定初始时刻的IMU朝向 R ( 0 ) R(0) R(0),才能执行后续的计算。此时IMU的姿态就是相对于初始的水平面来描述的。

  • 而如果把重力写出来,就可以设IMU的初始姿态为单位矩阵 R = I R = I R=I,而把重力方向作为IMU当前姿态相对于水平面的一个度量。

两种方法都是可行的,不过将重力方向单独表达出来会使初始状态表达更为简单,同时还可以增加一些线性性

如果把观测量噪声量 整理成一个向量,我们也可以把上式整理成矩阵形式。不过这里的矩阵形式将含有很多的 零项,相比于上式不会有明显的简化,所以这里采用散开的公式表示。

误差状态方程推导

下面我们推导误差状态方程。首先定义误差状态变量为:
p t = p + δ p (2 a) p_t = p + \delta p \tag{2 a} pt=p+δp(2 a)

v t = v + δ v (2 b) v_t = v + \delta v \tag{2 b} vt=v+δv(2 b)

R t = R δ R    或    q t = q δ q (2 c) R_t = R\delta R \space \space 或 \space\space q_t = q \delta q \tag{2 c} Rt=RδR    qt=qδq(2 c)

b g t = b g + δ b g (2 d) b_{gt} = b_g + \delta b_g \tag{2 d} bgt=bg+δbg(2 d)

b a t = b a + δ b a (2 e) b_{at} = b_a + \delta b_a \tag{2 e} bat=ba+δba(2 e)

g t = g + δ g (2 f) g_t = g + \delta g \tag{2 f} gt=g+δg(2 f)

其中,不带下标的就是名义状态变量名义状态变量 的运动学方程 与真值相同,只是 不需要考虑噪声(因为噪声在误差状态方程中考虑了) 。 其中旋转部分的 δ R \delta R δR 可以用它的李代数 Exp ( δ θ ) \text{Exp}(\delta \theta) Exp(δθ)来表示,此时旋转公式也需要改成用指数形式来表达。

关于误差变量的平移、零偏和重力公式,都很容易的而出对应的时间导数表达式,只需要在等式两侧分别对时间求导即可:
δ p ˙ = δ v (3 a) \delta \dot{p} = \delta v \tag{3 a} δp˙=δv(3 a)

δ b g ˙ = η g (3 b) \delta \dot{b_g} = \eta_g \tag{3 b} δbg˙=ηg(3 b)

δ b a ˙ = η a (3 c) \delta \dot{b_a} = \eta_a \tag{3 c} δba˙=ηa(3 c)

δ g = 0 (3 d) \delta g = 0 \tag{3 d} δg=0(3 d)

速度、旋转两式由于和 δ R \delta R δR有关系,所以需要单独推导。

误差状态的旋转项

对旋转式两侧求时间导数,可得:
R ˙ t = R ˙ Exp ( δ θ ) + R Exp ( δ θ ) ˙ = R t ( ω ~ − b g t − η g ) ∧ (4) \begin{aligned} \dot{R}_t &= \dot{R}\text{Exp}(\delta \theta) + R \dot{\text{Exp}(\delta \theta)} \\ &=R_t(\tilde{\omega} - b_{gt} -\eta_g)^{\wedge } \end{aligned} \tag{4} R˙t=R˙Exp(δθ)+RExp(δθ)˙=Rt(ω~bgtηg)(4)
该式右侧的 Exp ( δ θ ) ˙ \dot{\text{Exp}(\delta \theta)} Exp(δθ)˙ 满足:
Exp ( δ θ ) ˙ = Exp ( δ θ ) δ θ ˙ ∧ (5) \dot{\text{Exp}(\delta \theta)} = \text{Exp}(\delta \theta) \delta \dot{\theta}^{\wedge } \tag{5} Exp(δθ)˙=Exp(δθ)δθ˙(5)
因此,公式(4)中第一个式子可以写成:
R ˙ Exp ( δ θ ) + R Exp ( δ θ ) ˙ = R ( ω ~ − b g ) ∧ Exp ( δ θ ) + R Exp ( δ θ ) δ θ ˙ ∧ (6) \dot{R}\text{Exp}(\delta \theta) + R \dot{\text{Exp}(\delta \theta)} = R(\tilde{\omega} - b_{g})^{\wedge} \text{Exp}(\delta \theta) + R\text{Exp}(\delta \theta) \delta\dot{\theta} ^{\wedge } \tag{6} R˙Exp(δθ)+RExp(δθ)˙=R(ω~bg)Exp(δθ)+RExp(δθ)δθ˙(6)

而 第二个式子可以写成:

R t ( ω ~ − b g t − η g ) ∧ = R Exp ( δ θ ) ( ω ~ − b g t − η g ) ∧ (7) R_t(\tilde{\omega} - b_{gt} -\eta_g)^{\wedge } = R\text{Exp}(\delta \theta)(\tilde{\omega} - b_{gt} -\eta_g)^{\wedge } \tag{7} Rt(ω~bgtηg)=RExp(δθ)(ω~bgtηg)(7)

比较公式(6)、(7),将 δ θ ˙ \delta \dot{\theta} δθ˙ 移到一侧,约掉两侧左边的 R R R ,整理类似项,不难得到:

Exp ( δ θ ) δ θ ˙ ∧ = Exp ( δ θ ) ( ω ~ − b g t − η g ) ∧ − ( ω ~ − b g ) ∧ Exp ( δ θ ) (8) \text{Exp}(\delta \theta) \delta\dot{\theta} ^{\wedge } = \text{Exp}(\delta \theta)(\tilde{\omega} - b_{gt} -\eta_g)^{\wedge }- (\tilde{\omega} - b_{g})^{\wedge} \text{Exp}(\delta \theta) \tag{8} Exp(δθ)δθ˙=Exp(δθ)(ω~bgtηg)(ω~bg)Exp(δθ)(8)
注意到 Exp ( δ θ ) \text{Exp}(\delta \theta) Exp(δθ) 本身是一个 SO(3) \text{SO(3)} SO(3) 矩阵,我们利用 SO(3) \text{SO(3)} SO(3) 上的伴随性质:
ϕ ∧ R = R ( R T ϕ ) ∧ (9) \phi ^{\wedge} R = R (R^{\text{T}} \phi )^{\wedge } \tag{9} ϕR=R(RTϕ)(9)

来交换公式(8)中的 Exp ( δ θ ) \text{Exp}(\delta \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 ] ∧ (10) \begin{aligned} \text{Exp}(\delta \theta) \delta\dot{\theta} ^{\wedge } &= \text{Exp}(\delta \theta)(\tilde{\omega} - b_{gt} -\eta_g)^{\wedge } - \text{Exp}(\delta \theta) \text{Exp}(-\delta \theta) (\tilde{\omega} - b_{g})^{\wedge} \\ &= \text{Exp}(\delta \theta) \left [ (\tilde{\omega} - b_{gt} -\eta_g)^{\wedge } - \text{Exp}(-\delta \theta) (\tilde{\omega} - b_{g})^{\wedge} \right] \\ &\approx \text{Exp}(\delta \theta) \left [ (\tilde{\omega} - b_{gt} -\eta_g)^{\wedge } - \left ((I-\delta \theta ^{\wedge}) (\tilde{\omega} - b_{g}) \right )^{\wedge} \right] \\ &= \text{Exp}(\delta \theta) \left [ b_{g} - b_{gt} - \eta_g + \delta \theta ^{\wedge}\tilde{\omega} - \delta \theta ^{\wedge}b_{g} \right] ^{\wedge } \\ &= \text{Exp}(\delta \theta) \left [ (-\tilde{\omega} + b_{g})^{\wedge} \delta \theta - \delta b_{g} - \eta_g \right] ^{\wedge } \end{aligned} \tag{10} Exp(δθ)δθ˙=Exp(δθ)(ω~bgtηg)Exp(δθ)Exp(δθ)(ω~bg)=Exp(δθ)[(ω~bgtηg)Exp(δθ)(ω~bg)]Exp(δθ)[(ω~bgtηg)((Iδθ)(ω~bg))]=Exp(δθ)[bgbgtηg+δθω~δθbg]=Exp(δθ)[(ω~+bg)δθδbgηg](10)

约掉公式(10)等式左侧的系数,可得:

δ θ ˙ ≈ − ( ω ~ − b g ) ∧ δ θ − δ b g − η g (11) \delta\dot{\theta} \approx -(\tilde{\omega} - b_{g})^{\wedge} \delta \theta - \delta b_{g} - \eta_g \tag{11} δθ˙(ω~bg)δθδbgηg(11)

误差状态的速度项

接下来考虑速度方程的误差形式。同样地,对两侧求时间导数,就可以得到 δ v ˙ \delta \dot{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 (12) \begin{aligned} \dot{v}_t &= R_t(\tilde{a} - b_{at} -\eta_a) + g_t \\ &= R \text{Exp}(\delta \theta) (\tilde{a} - b_{a} - \delta b_a -\eta_a) + g + \delta g \\ &\approx R(I + \delta \theta ^{\wedge})(\tilde{a} - b_{a} - \delta b_a -\eta_a) + g + \delta g \\ &\approx R\tilde{a} - Rb_a - R\delta b_a - R\eta_a + R\delta \theta ^{\wedge} a - R\delta \theta ^{\wedge} b_a + g + \delta g \\ &= R\tilde{a} - Rb_a - R\delta b_a - R\eta_a - R\tilde{a}^{\wedge} \delta \theta + Rb_a^{\wedge} \delta \theta + g + \delta g \end{aligned} \tag{12} v˙t=Rt(a~batηa)+gt=RExp(δθ)(a~baδbaηa)+g+δgR(I+δθ)(a~baδbaηa)+g+δgRa~RbaRδbaRηa+RδθaRδθba+g+δg=Ra~RbaRδbaRηaRa~δθ+Rbaδθ+g+δg(12)
在公式(12)中,由第三行向第四行推导时,需要忽略 δ θ ∧ \delta \theta ^{\wedge} δθ δ b a \delta b_a δba η a \eta_a ηa 相乘的二阶小量。

从第四行推第五行则用到了叉乘符号交换顺序后,需要加负号的性质

另一方面,等式右侧为:
v ˙ + θ v ˙ = R ( a ~ − b a ) + g + δ v ˙ (13) \dot{v} + \theta\dot{v} =R(\tilde{a} - b_a) + g + \delta \dot{v} \tag{13} v˙+θv˙=R(a~ba)+g+δv˙(13)
因为上面两式相等,可以得到:
δ v ˙ = − R ( a ~ − b a ) ∧ δ θ − R δ b a − R η a + δ g (14) \delta \dot{v} = -R(\tilde{a} - b_a)^{\wedge} \delta \theta - R \delta b_a - R \eta_a + \delta g \tag{14} δv˙=R(a~ba)δθRδbaRηa+δg(14)
这样我们就得到了 δ v \delta v δv的运动学模型。

需要补充一句,由于上式中 η a \eta_a ηa 是一个零均值白噪声,它乘上任意旋转矩阵之后,仍然是一个零均值白噪声,而且由于 R T R = I R^{\text{T}}R = I RTR=I,其协方差矩阵也不变

所以,公式(14)可以简化为:
δ v ˙ = − R ( a ~ − b a ) ∧ δ θ − R δ b a − η a + δ g (15) \delta \dot{v} = -R(\tilde{a} - b_a)^{\wedge} \delta \theta - R \delta b_a - \eta_a + \delta g \tag{15} δv˙=R(a~ba)δθRδbaηa+δg(15)

完整误差变量的运动学方程

至此,我们可以把误差变量的运动学方程整理如下:
δ p ˙ = δ v (16 a) \delta \dot{p} = \delta v \tag{16 a} δp˙=δv(16 a)

δ v ˙ = − R ( a ~ − b a ) ∧ δ θ − R δ b a − η a + δ g (16 b) \delta \dot{v} = -R(\tilde{a} - b_a)^{\wedge} \delta \theta - R \delta b_a - \eta_a + \delta g \tag{16 b} δv˙=R(a~ba)δθRδbaηa+δg(16 b)

δ θ ˙ ≈ − ( ω ~ − b g ) ∧ δ θ − δ b g − η g (16 c) \delta\dot{\theta} \approx -(\tilde{\omega} - b_{g})^{\wedge} \delta \theta - \delta b_{g} - \eta_g \tag{16 c} δθ˙(ω~bg)δθδbgηg(16 c)

δ b g ˙ = η g (16 d) \delta \dot{b_g} = \eta_g \tag{16 d} δbg˙=ηg(16 d)

δ b a ˙ = η a (16 e) \delta \dot{b_a} = \eta_a \tag{16 e} δba˙=ηa(16 e)

δ g = 0 (16 f) \delta g = 0 \tag{16 f} δg=0(16 f)


离散时间上的ESKF运动学方程

从连续时间状态方程,推出离散时间的状态方程并不困难,不妨直接来列写它们。

名义状态变量的离散时间运动学方程可以写为:
p ( t + Δ t ) = p ( t ) + v Δ t + 1 2 ( R ( a ~ − b a ) ) Δ t 2 + 1 2 g Δ t 2 (17 a) p(t + \Delta t) = p(t) + v \Delta t + \frac{1}{2}(R(\tilde{a} - b_a)) \Delta t^2 + \frac{1}{2}g \Delta t^2 \tag{17 a} p(t+Δt)=p(t)+vΔt+21(R(a~ba))Δt2+21gΔt2(17 a)

v ( t + Δ t ) = v ( t ) + R ( a ~ − b a ) ) Δ t + g Δ t (17 b) v(t + \Delta t) = v(t) + R(\tilde{a} - b_a)) \Delta t + g \Delta t \tag{17 b} v(t+Δt)=v(t)+R(a~ba))Δt+gΔt(17 b)

R ( t + Δ t ) = R ( t ) Exp ( ( ω ~ − b g ) Δ t ) (17 c) R(t + \Delta t) = R(t)\text{Exp}((\tilde{\omega} - b_g) \Delta t) \tag{17 c} R(t+Δt)=R(t)Exp((ω~bg)Δt)(17 c)

b g ( t + Δ t ) = b g ( t ) (17 d) b_g(t + \Delta t) = b_g(t) \tag{17 d} bg(t+Δt)=bg(t)(17 d)

b a ( t + Δ t ) = b a ( t ) (17 e) b_a(t + \Delta t) = b_a(t) \tag{17 e} ba(t+Δt)=ba(t)(17 e)

g ( t + Δ t ) = g ( t ) (17 f) g(t + \Delta t) = g(t) \tag{17 f} g(t+Δt)=g(t)(17 f)

公式(17) 只需要在上面基础上,添加零偏项和重力项即可。

误差状态的离散形式,只需要处理连续形态中的旋转部分。参考角速度的积分公式,可以将误差状态方程写为:
δ p ( t + Δ t ) = δ p + δ v Δ t (18 a) \delta p(t + \Delta t) = \delta p + \delta v \Delta t \tag{18 a} δp(t+Δt)=δp+δvΔt(18 a)

δ v ( t + Δ t ) = δ v + ( − R ( a ~ − b a ) ∧ δ θ − R δ b a + δ g ) Δ t + η v (18 b) \delta v (t + \Delta t) = \delta v + (-R (\tilde{a} - b_a)^{\wedge} \delta \theta - R \delta b_a + \delta g) \Delta t + \eta_v \tag{18 b} δv(t+Δt)=δv+(R(a~ba)δθRδba+δg)Δt+ηv(18 b)

δ θ ( t + Δ t ) = Exp ( − ( ω − b a ~ ) Δ t ) δ θ − δ b g Δ t − η θ (18 c) \delta \theta(t + \Delta t) = \text{Exp}(-(\tilde{\omega - b_a}) \Delta t) \delta \theta - \delta b_g \Delta t - \eta_\theta \tag{18 c} δθ(t+Δt)=Exp((ωba~)Δt)δθδbgΔtηθ(18 c)

δ b g ( t + Δ t ) = δ b g + η g (18 d) \delta b_g (t + \Delta t) = \delta b_g + \eta_g \tag{18 d} δbg(t+Δt)=δbg+ηg(18 d)

δ b a ( t + Δ t ) = δ b a + η a (18 e) \delta b_a (t + \Delta t) = \delta b_a + \eta_a \tag{18 e} δba(t+Δt)=δba+ηa(18 e)

δ g ( t + Δ t ) = δ g (18 f) \delta g (t + \Delta t) = \delta g \tag{18 f} δg(t+Δt)=δg(18 f)

公式(18)中,需要注意的是:

  • 右侧部分省略了括号里的 ( t ) (t) (t) 以简化公式;
  • 关于旋转部分的积分,我们可以将连续形式 看成关于 δ θ \delta \theta δθ 的微分方程,然后求解。求解过程类似于对角速度进行积分;
  • 噪声项不参与递推,需要把它们单独归入噪声部分中。连续时间的噪声项可以视为随机过程中的能量谱密度,而离散时间下的噪声变量就是我们日常看到的随机变量了。这些噪声随机变量的标准差可以列写如下:

σ ( η v ) = Δ t σ a ,    σ ( η θ ) = Δ t σ g ,    σ ( η g ) = Δ t η b g ,    σ ( η a ) = Δ t σ b a (19) \sigma (\eta_v) = \Delta t \sigma_a , \space \space \sigma (\eta _\theta) = \Delta t \sigma _g, \space \space \sigma (\eta _g) = \sqrt{\Delta t} \eta _{bg}, \space \space \sigma (\eta _a) = \sqrt{\Delta t} \sigma _{ba} \tag{19} σ(ηv)=Δtσa,  σ(ηθ)=Δtσg,  σ(ηg)=Δt ηbg,  σ(ηa)=Δt σba(19)

其中,前两式中的 Δ t \Delta t Δt是由积分关系导致的。

至此,我们给出了如何在ESKF中进行IMU递推的过程,对应于卡尔曼滤波器中的状态方程。

为了让滤波器收敛,我们通常需要外部的观测来对卡尔曼滤波器进行修正,也就是所谓的组合导航。当然,组合导航的方法有很多,从传统的EKF,到本节介绍的ESKF,以及预积分图优化技术,都可以应用于组合导航中。


ESKF的运动过程

根据上述讨论,我们可以写出ESKF的运动过程。

误差状态变量 δ x \delta x δx 的离散时间运动方程已经在上式给出,我们可以整体地记为:
δ x = f ( δ x ) + w ,   w ∼ N ( 0 , Q ) (20) \delta x = f (\delta x) + w, \space w \sim \mathcal{N}(0, Q) \tag {20} δx=f(δx)+w, wN(0,Q)(20)
其中,

w w w为噪声,

按照之前定义, Q Q Q应该为:
Q = diag ( 0 3 , Cov ( η v ) , Cov ( η θ ) , Cov ( η g ) , Cov ( η a ) , 0 3 ) (21) Q = \text{diag}\left ( 0_3, \text{Cov}(\eta _v), \text{Cov}(\eta_\theta), \text{Cov}(\eta_g), \text{Cov}(\eta_a), 0_3 \right) \tag{21} Q=diag(03,Cov(ηv),Cov(ηθ),Cov(ηg),Cov(ηa),03)(21)
两侧的零是由于第一个和最后一个方程,本身没有噪声导致的

为了保持与EKF的符号通一,我们计算运动方程的线性化形式:
δ x = F δ x + w (22) \delta x = \boldsymbol{F} \delta x + w \tag{22} δx=Fδx+w(22)
其中, 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 ] (23) \boldsymbol{F} = \begin{bmatrix} I & I\Delta t & 0 & 0 & 0 & 0 \\ 0 & I & -R(\tilde{a} - b_a)^{\wedge} \Delta t & -R \Delta t & 0 & I \Delta t \\ 0 & 0 & \text{Exp}(-(\tilde{\omega} - b_g) \Delta t) & 0 & -I\Delta t & 0 \\ 0 & 0 & 0 & I & 0 & 0 \\ 0 & 0 & 0 & 0 & I & 0 \\ 0 & 0 & 0 & 0 & 0 & I \end{bmatrix} \tag{23} F=I00000IΔtI00000R(a~ba)ΔtExp((ω~bg)Δt)0000RΔt0I0000IΔt0I00IΔt000I(23)
在此基础上,我们执行ESKF的预测过程。预测过程包括对名义状态的预测(IMU积分),以及对误差状态的预测:
δ x pred = F δ x (24 a) \delta x_{\text{pred}} = \boldsymbol{F} \delta x \tag{24 a} δxpred=Fδx(24 a)

P pred = F P F T + Q (24 b) \boldsymbol{P}_{\text{pred}} = \boldsymbol{FPF}^{\text{T}} + \boldsymbol{Q} \tag{24 b} Ppred=FPFT+Q(24 b)

不过,由于ESKF的误差状态,在每次更新以后都会被重置,因此运动方程的均值部分没有太大意义,而方差部分则可以指导整个误差估计的分布情况


ESKF的更新过程

前面介绍的是ESKF的运动过程,现在我们来考虑更新过程。

假设一个抽象的传感器能够对状态变量产生观测,其观测方式为抽象的 h h h ,那么可以写为:
z = h ( x ) + v ,   v ∼ N ( 0 , V ) (25) z = h(x) + v, \space v \sim \mathcal{N}(0, \boldsymbol{V}) \tag {25} z=h(x)+v, vN(0,V)(25)
其中,

z z z是观测数据 ,

v v v 是观测噪声,

V \boldsymbol{V} V 为该噪声的协方差矩阵。由于变量里已经有 R R R了,这里我们换个符号。

在传统的EKF中,我们可以直接对观测方程线性化,求出预测方程相对于状态变量雅可比矩阵 ,进而更新卡尔曼滤波器

而在ESKF中,我们当前拥有名义状态 x x x 以及误差状态 δ x \delta x δx 的估计,且希望更新的是误差状态,因此要计算观测方程,相比于误差状态的雅可比矩阵
H = ∂ h ∂ δ x (26) \boldsymbol{H} = \frac{\partial h}{\partial \delta x} \tag{26} H=δxh(26)
然后再计算卡尔曼增益,进而计算误差状态的更新过程:
K = P pred H T ( H P pred H T + V ) − 1 (27 a) \boldsymbol{K} = \boldsymbol{P}_{\text{pred}} \boldsymbol{H}^{\text{T}} (\boldsymbol{H} \boldsymbol{P}_{\text{pred}} \boldsymbol{H}^{\text{T}} + \boldsymbol{V})^{-1} \tag{27 a} K=PpredHT(HPpredHT+V)1(27 a)

δ x = K ( z − h ( x t ) ) (27 b) \delta x = \boldsymbol{K}(z - h(x_t)) \tag{27 b} δx=K(zh(xt))(27 b)

P = ( I − K H ) P pred (27 c) \boldsymbol{P} = (\boldsymbol{I} - \boldsymbol{KH}) \boldsymbol{P}_{\text{pred}} \tag{27 c} P=(IKH)Ppred(27 c)

其中,

K \boldsymbol{K} K卡尔曼增益

P pred \boldsymbol{P}_{\text{pred}} Ppred 为预测的协方差矩阵

最后的 P \boldsymbol{P} P 为修正后的 协方差矩阵

这里 H \boldsymbol{H} H 的计算可以通过链式法则来生成:
H = ∂ h ∂ x ∂ x ∂ δ x (28) \boldsymbol{H} = \frac{\partial h}{\partial x} \frac{\partial x}{\partial \delta x} \tag{28} H=xhδxx(28)
其中,

第一项,只需要对观测方程进行线性化,

第二项,根据我们之前对状态变量的定义,可以得到:
∂ x ∂ δ x = diag ( I 3 , I 3 , ∂ Log ( R ( Exp ( δ θ ) ) ) ∂ δ θ , I 3 , I 3 , I 3 ) (29) \frac{\partial x}{\partial \delta x} = \text{diag} \left ( \boldsymbol{I}_3, \boldsymbol{I}_3, \frac{\partial \text{Log}(R(\text{Exp}(\delta \theta ) )) }{\partial \delta \theta } , \boldsymbol{I}_3, \boldsymbol{I}_3, \boldsymbol{I}_3 \right ) \tag {29} δxx=diag(I3,I3,δθLog(R(Exp(δθ))),I3,I3,I3)(29)
其他几种都是平凡的,只有旋转部分,因为 δ θ \delta \theta δθ 定义为 R R R得到右称,我们用右称的BCH即可:
∂ Log ( R ( Exp ( δ θ ) ) ) ∂ δ θ = J r − 1 ( R ) (30) \frac{\partial \text{Log}(R(\text{Exp}(\delta \theta ) )) }{\partial \delta \theta } = \boldsymbol{J}_{r}^{-1}(\boldsymbol{R}) \tag{30} δθLog(R(Exp(δθ)))=Jr1(R)(30)
最后,我们可以给每个变量加下标 k k k ,表示在 k k k 时刻进行状态估计。

ESKF的误差状态后续处理

在经过预测和更新过程之后,我们修正了误差状态的估计。接下来,只需要把误差状态 归入名义状态,然后重置ESKF即可。

归入部分,可以简单地写为:
p k + 1 = p k + δ p k (30 a) \boldsymbol{p}_{k+1} = \boldsymbol{p}_{k} + \delta \boldsymbol{p}_{k} \tag{30 a} pk+1=pk+δpk(30 a)

v k + 1 = v k + δ v k (30 b) \boldsymbol{v}_{k+1} = \boldsymbol{v}_{k} + \delta \boldsymbol{v}_{k} \tag{30 b} vk+1=vk+δvk(30 b)

R k + 1 = R k Exp ( δ θ k ) (30 c) \boldsymbol{R}_{k+1} = \boldsymbol{R}_{k}\text{Exp}(\delta \boldsymbol{\theta}_{k}) \tag{30 c} Rk+1=RkExp(δθk)(30 c)

b g , k + 1 = b g , k + δ b g , k (30 d) \boldsymbol{b}_{g,k+1} = \boldsymbol{b}_{g,k} + \delta \boldsymbol{b}_{g,k} \tag{30 d} bg,k+1=bg,k+δbg,k(30 d)

b a , k + 1 = b a , k + δ b a , k (30 e) \boldsymbol{b}_{a,k+1} = \boldsymbol{b}_{a,k} + \delta \boldsymbol{b}_{a,k} \tag{30 e} ba,k+1=ba,k+δba,k(30 e)

g k + 1 = g k + δ g k (30 f) \boldsymbol{g}_{k+1} = \boldsymbol{g}_{k} + \delta \boldsymbol{g}_{k} \tag{30 f} gk+1=gk+δgk(30 f)

有些文献里,也会定义为广义的状态变量加法:
x k + 1 = x k ⊕ δ x k (31) \boldsymbol{x}_{k+1} = \boldsymbol{x}_{k} \oplus \delta \boldsymbol{x}_{k} \tag{31} xk+1=xkδxk(31)
这种写法,可以简化整体的表达式。不过,如果公式里出现太多的广义加减法,可能让人不好马上辨别它们的具体含义,所以本文倾向于将各状态分别写开,或者直接用加法而非广义加法符号。

ESKF的重置分为均值部分 和 协方差部分

均值部分可以简单地实现为:
δ x = 0 (32) \delta \boldsymbol{x} = \boldsymbol{0} \tag {32} δx=0(32)
由于均值被重置了,之前我们描述的是关于 x k \boldsymbol{x}_{k} xk 的切空间中的协方差,而现在描述的是 x k + 1 \boldsymbol{x}_{k+1} xk+1 中的协方差。

这次重置会来带来一些微小的差异,主要影响旋转部分。事实上,在重置前,卡尔曼滤波器刻画了 x pred \boldsymbol{x}_{\text{pred}} xpred 切空间处的一个高速分布 N ( δ x , P ) \mathcal{N}(\delta \boldsymbol{x}, \boldsymbol{P}) N(δx,P),而重置之后,应该刻画 x pred ⊞ δ x \boldsymbol{x}_{\text{pred}} \boxplus \delta \boldsymbol{x} xpredδx 处的一个 N ( δ x , P reset ) \mathcal{N}(\delta \boldsymbol{x}, \boldsymbol{P}_{\text{reset}}) N(δx,Preset)

我们设 重置前的名义旋转估计为 R k \boldsymbol{R}_k Rk误差状态 δ θ \delta \boldsymbol{\theta} δθ , 卡尔曼滤波器的增量计算结果为 δ θ k \delta \boldsymbol{\theta}_k δθk, 注意此处的 δ θ k \delta \boldsymbol{\theta}_k δθk 是已知的,而 δ θ \delta \boldsymbol{\theta} δθ是一个随机变量。

重置之后的名义旋转部分为:
R + Exp ( δ θ + ) = R k Exp ( δ θ k ) Exp ( δ θ + ) = R k Exp ( δ θ ) (33) \boldsymbol{R}^{+} \text{Exp}(\delta \boldsymbol{\theta}^+) = \boldsymbol{R}_k \text{Exp}(\delta \boldsymbol{\theta}_k)\text{Exp}(\delta \boldsymbol{\theta}^+) = \boldsymbol{R}_k \text{Exp}(\delta \boldsymbol{\theta}) \tag {33} R+Exp(δθ+)=RkExp(δθk)Exp(δθ+)=RkExp(δθ)(33)
不难得到:
Exp ( δ θ + ) = Exp ( − δ θ k ) Exp ( δ θ ) (34) \text{Exp}(\delta \boldsymbol{\theta}^+) = \text{Exp}(-\delta \boldsymbol{\theta}_k) \text{Exp}(\delta \boldsymbol{\theta}) \tag {34} Exp(δθ+)=Exp(δθk)Exp(δθ)(34)
注意这里的 δ θ \delta \boldsymbol{\theta} δθ 为小量,利用线性化后的BCH公式,可以得到:
δ θ + = − δ θ k + δ θ − 1 2 δ θ k ∧ δ θ + o ( ( δ θ ) 2 ) (35) \delta \boldsymbol{\theta}^{+} = -\delta \boldsymbol{\theta}_k + \delta \boldsymbol{\theta} - \frac{1}{2}\delta \boldsymbol{\theta}_k^{\wedge}\delta \boldsymbol{\theta} + o((\delta \boldsymbol{\theta})^2) \tag{35} δθ+=δθk+δθ21δθkδθ+o((δθ)2)(35)
于是有:
∂ δ θ + ∂ δ θ ≈ I − 1 2 δ θ k ∧ (36) \frac{\partial \delta \boldsymbol{\theta}^+}{\partial \delta \boldsymbol{\theta}} \approx \boldsymbol{I} - \frac{1}{2} \delta \boldsymbol{\theta}_k^{\wedge } \tag{36} δθδθ+I21δθk(36)
公式(36)表明,重置前后的误差状态,相差一个旋转方面的小雅克比矩阵,我们记作:
J θ = I − 1 2 δ θ k ∧ (37) \boldsymbol{J_\theta} = \boldsymbol{I} - \frac{1}{2} \delta \boldsymbol{\theta}_k^{\wedge } \tag {37} Jθ=I21δθk(37)
把这个小雅可比矩阵 放到整个状态变量的维度下,并保持其他部分为单位矩阵,可以得到一个完整的雅可比矩阵
J k = diag ( I 3 , I 3 , I θ , I 3 , I 3 , I 3 ) (38) \boldsymbol{J_k} = \text{diag}( \boldsymbol{I}_3, \boldsymbol{I}_3, \boldsymbol{I_\theta}, \boldsymbol{I}_3, \boldsymbol{I}_3, \boldsymbol{I}_3 ) \tag{38} Jk=diag(I3,I3,Iθ,I3,I3,I3)(38)
因此,把误差状态的均值归零的同时,它们的协方差矩阵也应该进行线性变换:
P reset = J k P J k T (39) \boldsymbol{P}_{\text{reset}} = \boldsymbol{J}_k\boldsymbol{P}\boldsymbol{J}_k^{\text{T}} \tag{39} Preset=JkPJkT(39)
不过,由于 δ θ k \delta \boldsymbol{\theta}_k δθk 并不大,这里的 J k \boldsymbol{J}_k Jk 仍然十分接近于单位矩阵,所以大部分材料里并不处理这一项,而是直接把前面估计的 P \boldsymbol{P} P 矩阵作为下一个时刻的起点。

但是本文仍然要介绍这一点,该问题实际意义是做了 切空间投影,即:把一个切空间中的高斯分布投影到另一个切空间中。

小结

本文主要介绍了 SO ( 3 ) \text{SO}(3) SO(3) 流形上的ESKF,相比于四元数形式或者欧拉角形式,更为简单,无需自定义太多符号。

本文主要参考高博知乎

相关文章:

  • 计算机毕业设计Java高铁在线购票系统(源码+系统+mysql数据库+lw文档)
  • 你一定可以读懂的Linux中的变量、数组、和算数运算与测试看这篇就足够了
  • javaSE -类(class)和对象
  • 网络编程简单学习
  • Windows远程连接centos7图形化界面,安装xrdp
  • 【附源码】计算机毕业设计JAVA重工教师职称管理系统
  • Springboot毕业设计毕设作品,黑白图片和上色处理系统 开题报告
  • 马士兵-郑金维—并发编程—2.并发编程的三大特性
  • 遥感指数应用汇编
  • 智能合约学习资料
  • Vue3 - 路由 Vue-router 4.X(配置与使用教程)
  • 动态规划算法(2)最长回文子串详解
  • 链表之反转链表
  • MySQL存储引擎InnoDB架构
  • Transformer对接公司需求的调研报告
  • Leetcode DAY 15: 层序遍 and 翻转二叉树 and 对称二叉树
  • 永磁同步电机转子位置估算专题——正交锁相环
  • CppLib v1.1 和 pexports v4.7 的下载链接记录
  • Graalvm安装配置与springboot3.0尝鲜
  • 【Android笔记54】Android中几个常见的系统广播(分钟广播、网络广播、桌面和任务栏广播)
  • MFC无法进行添加/移除操作,因为代码是只读的
  • Android Java层UI渲染实现 四 Surface的创建和Vie绘制
  • MFC实现窗体初始隐藏
  • Android SurfaceView原理分析
  • LPCWSTR与LPWSTR
  • Android Retrofit源码分析
  • VS2005设置程序输出路径
  • JNI基本使用
  • VC++控制台程序隐藏窗口运行
  • 音视频相关基础知识与FFmpeg介绍