Error State Kalman Filter

Posted on In Waline:

大部分基于滤波器的 LIO 或 VIO 实现中，都使用 ESKF 作为状态估计方法。本文则结合网络资源对 ESKF 进行整理。

Error State Kalman Filter

1　ESKF 原理

大部分基于滤波器的 LIO 或 VIO 实现中，都使用 ESKF 作为状态估计方法。相比于传统 KF，ESKF 的优点可以如下：

  1. 在旋转的处理上，ESKF 的状态变量可以采用三维变量来表达旋转的增量。而传统 KF 需要用到四元数或者更高维的表达。
  2. ESKF 总是在原点附近，离奇异点较远。
  3. ESKF 的状态量为小量，其二阶变量相对来说可以忽略。同时，大多数雅可比矩阵在小量情况下变得非常简单，甚至可以用单位阵代替。
  4. 误差状态的运动学也相比原状态变量要来得更小，因为可以把大量更新部分放到原状态变量中。

在 ESKF 中，把原状态变量称为名义状态变量（nominal state），然后把 ESKF 里的状态变量称为误差状态变量（error state）。ESKF 整体流程如下：

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

2　直接法和间接法

假设存在待优化目标函数：

min⁡xF(x)(2-1)\min_x F(x) \tag{2-1}

xx 为 nn 维向量。使用直接法，求解方式为直接求导：

dFdx=0(2-2)\frac{d F}{d x} = 0 \tag{2-2}

求该式的所有解，并依次带入 F(x)F(x)，其中最小的值对应的 xx 即为解。

当 F(x)F(x) 比较简单时，可以很快速得到最优解，但当 F(x)F(x) 很复杂时，直接对状态量的一阶导数表达式可能同样很复杂，对上式的求解将变得极为困难甚至不可能，为了在复杂系统中解决这个问题，就诞生牛顿法这样的间接法，假设存在初值 x0x_0，可以在该值附近对 F(x)F(x) 进行一到二阶泰勒展开：

F(x0+Δx)=F(x0)+dFdΔx∣x0Δx+d2FdΔx2∣x0Δx2F(x_0 + \Delta x) = F(x_0) + \left. \frac{dF}{d \Delta x} \right|_{x_0} \Delta x + \left. \frac{d^2 F}{d \Delta x^2} \right|_{x_0} \Delta x^2

此时的问题变成了，求解使 F(x)F(x) 为极值的 Δx\Delta x，并更新 xx，再继续上述过程，直到收敛。

直接法中，直接求解目标函数对状态量的导数；间接法中，求解目标函数对于一个小值 Δx\Delta x 的导数。正是基于 Δx\Delta x 很小的假设，可以认为其高阶导数值都非常小，可以忽略，因此可以在局部区域对目标函数线性化，由此简化计算。

3　ESKF 公式推导

ESKF 核心在于将系统状态分解为两个部分：

Xt=X⊕δX(3-1)X_t = X \oplus \delta X \tag{3-1}

其中 XtX_t 为系统状态真值，XX 为名义状态变量，δX\delta X 为误差状态变量。

3.1　ESKF Predict

根据 IMU 中值积分模型，可直接得到 XtX_t 的一阶导数：

X˙t=Ut(Xt,um,i)(3-2)\dot{X}_t = U_t(X_t, u_m, i) \tag{3-2}

um,iu_m, i 为 IMU 的测量值和各类噪声。

提取所有状态量主成分，构建名义状态的动力学模型：

X˙=U(X,um)(3-3)\dot{X} = U(X, u_m) \tag{3-3}

这个模型是完全不受噪声分量影响的，因为噪声分量的影响都放到了误差状态的动力学模型中。

根据 (3-1, 2, 3) 可以推导得到误差状态的动力学模型：是对 (3-1) 进行求导，将 (3-2, 3) 带入求解。此时得到：

δX˙=Uδ(X,δx,um,i)(3-4)\delta \dot{X} = U_{\delta} (X, \delta x, u_m, i) \tag{3-4}

对 (3-3, 4) 积分得到离散时间下的递推方程：

Xk+1=f(Xk,um)δXk+1=fδ(Xk,δXk,um,i)(3-5)\begin{aligned} & X_{k+1} = f(X_k, u_m) \\ & \delta X_{k+1} = f_{\delta}(X_k, \delta X_k, u_m, i) \end{aligned} \tag{3-5}

有了系统递推方程后，剩下的部分与 EKF 基本相似，将误差状态方程线性化为：

δXk+1=Fδ(X,um)δXk+Fii(3-6)\delta X_{k+1} = F_{\delta}(X, u_m) \delta X_k + F_i i \tag{3-6}

Fδ,FiF_{\delta}, F_i 为 fδf_{\delta} 对 δX,i\delta X, i 的雅可比矩阵。有了这个线性模型，predict 部分就退化为一般针对误差状态的 EKF，套用 EKF 的 predict 公式即可：

δX^x+1=Fδ(X,um)δX^kP^k+1=FδPˇkFδT+FiQiFiT(3-7)\begin{aligned} & \delta \hat{X}_{x+1}=F_\delta\left(X, u_m\right) \delta \hat{X}_k \\ & \hat{P}_{k+1}=F_\delta \check{P}_k F_\delta^T+F_i Q_i F_i^T \end{aligned} \tag{3-7}

3.2　ESKF Update

假设观测方程：

Y=h(Xt)+v(3-8)Y = h(X_t) + v \tag{3-8}

套用 EKF 的 update 公式：

K=P^k+1HT(HP^k+1HT+V)−1δXˇk+1=H(Y−h(X^t,k+1))Pˇk+1=(I−KH)P^k+1(3-9)\begin{aligned} & K = \hat{P}_{k+1} H^T (H \hat{P}_{k+1} H^T + V)^{-1} \\ & \delta \check{X}_{k+1} = H(Y - h(\hat{X}_{t, k+1})) \\ & \check{P}_{k+1} = (I - KH) \hat{P}_{k+1} \end{aligned} \tag{3-9}

整个过程其实跟 EKF 完全一样，唯一不同是，式子中的 HH 矩阵是 hh 相对于误差状态的雅可比矩阵。对于 HH 的求解则采用链式法则：

H≜dhdXt∣X=dXtdδX∣X(3-10)H \triangleq \left. \frac{d h}{d X_t} \right|_X = \left. \frac{d X_t}{d \delta X} \right|_X \tag{3-10}

3.3　ESKF Reset

之后将融合得到的误差状态注入名义状态用于修正误差：

Xk+1←Xk+1+δXˇk+1(3-11)X_{k+1} \leftarrow X_{k+1} + \delta \check{X}_{k+1} \tag{3-11}

因为误差状态主成分已经注入名义状态了，最后需要执行一个针对误差状态的充值操作：

δXk+1=g(δXk+1)=δXk+1⊖δXˇk+1(3-12)\delta X_{k+1} = g(\delta X_{k+1}) = \delta X_{k+1} \ominus \delta \check{X}_{k+1} \tag{3-12}

4　ESKF 在 SLAM 中的使用

设真值状态为 xt=[pt,vt,Rt,bat,bgt,gt]T\mathbf{x}_t = [\mathbf{p}_t, \mathbf{v}_t, \mathbf{R}_t, \mathbf{b}_{at}, \mathbf{b}_{gt}, \mathbf{g}_t]^T，状态随时间变化，可以记为 x(t)\mathbf{x}(t)。在连续时间上记 IMU 读数 ω~,a~\tilde{\boldsymbol{\omega}}, \tilde{\mathbf{a}}，则状态变量导数相对于观测量之间的关系：

p˙t=vtv˙t=Rt(a~−bat−ηa)+gR˙t=Rt(ω~−bgt−ηg)∧b˙gt=ηbgb˙at=ηbag˙t=0(4-1)\begin{aligned} \dot{\mathbf{p}}_t &= \mathbf{v}_t \\ \dot{\mathbf{v}}_t &= \mathbf{R}_t(\tilde{\mathbf{a}} - \mathbf{b}_{at} - \boldsymbol{\eta}_a) + \mathbf{g} \\ \dot{\mathbf{R}}_t &= \mathbf{R}_t (\tilde{\boldsymbol{\omega}} - \mathbf{b}_{gt} - \boldsymbol{\eta}_g)^{\wedge} \\ \dot{\mathbf{b}}_{gt} &= \boldsymbol{\eta}_{bg} \\ \dot{\mathbf{b}}_{at} &= \boldsymbol{\eta}_{ba} \\ \dot{\mathbf{g}}_t &= \mathbf{0} \end{aligned} \tag{4-1}

定义误差状态变量为：

pt=p+δpvt=v+δvRt=RδRorqt=qδqbgt=bg+δbgbat=ba+δbagt=g+δg(4-2)\begin{aligned} \mathbf{p}_t & =\mathbf{p}+\delta \mathbf{p} \\ \mathbf{v}_t & =\mathbf{v}+\delta \mathbf{v} \\ \mathbf{R}_t & =\mathbf{R} \delta \mathbf{R} \quad \text{or} \quad \mathbf{q}_t=\mathbf{q} \delta \mathbf{q} \\ \mathbf{b}_{g t} & =\mathbf{b}_g+\delta \mathbf{b}_g \\ \mathbf{b}_{a t} & =\mathbf{b}_a+\delta \mathbf{b}_a \\ \mathbf{g}_t & =\mathbf{g}+\delta \mathbf{g} \end{aligned} \tag{4-2}

不带下标的是名义状态变量。名义状态变量的运动学方程式与真值相同，只是必考虑噪声（因为噪声在误差状态方程中考虑了）。其中旋转部分的 δR\delta \mathbf{R} 可以用它的李代数 Exp(δθ)\text{Exp}(\delta \boldsymbol{\theta}) 来表示。

关于误差变量的平移、零偏和重力公式，都很容易得出对应的时间导数表达式，只需在等式两侧分别对时间求导即可：

δp˙=δvδbg˙=ηgδb˙a=ηaδg=0(4-3)\begin{aligned} \delta \dot{\mathbf{p}} & =\delta \mathbf{v} \\ \delta \dot{\mathbf{b}_g} & =\mathbf{\eta}_g \\ \delta \dot{\mathbf{b}}_a & =\mathbf{\eta}_a \\ \delta \mathbf{g} & =\mathbf{0} \end{aligned} \tag{4-3}

4.1　旋转和速度的求导展开

（1）对旋转的求导

对旋转式两侧对时间求导：

R˙t=R˙Exp⁡(δθ)+RExp⁡(δθ)˙=Rt(ω~−bgt−ηg)∧(4-4)\begin{aligned} \dot{\mathbf{R}}_t &= \dot{\mathbf{R}} \operatorname{Exp}(\delta \boldsymbol{\theta}) + \mathbf{R} \dot{\operatorname{Exp}(\delta \boldsymbol{\theta})} \\ &= \mathbf{R}_t (\tilde{\boldsymbol{\omega}} - \mathbf{b}_{gt} - \boldsymbol{\eta}_g)^{\wedge} \end{aligned} \tag{4-4}

其中，式子右边满足：

Exp⁡(δθ)˙=Exp⁡(δθ)δθ˙∧(4-5)\dot{\operatorname{Exp}(\delta \boldsymbol{\theta})} = \operatorname{Exp}(\delta \boldsymbol{\theta}) \dot{\delta \boldsymbol{\theta}}^{\wedge} \tag{4-5}

从而 (4-4) 式改写为：

R˙t=R˙Exp⁡(δθ)+RExp⁡(δθ)˙=R(ω~−bg)∧Exp⁡(δθ)+RExp⁡(δθ)δθ˙∧=Rt(ω~−bgt−ηg)∧=RExp⁡(δθ)(ω~−bgt−ηg)∧(4-6)\begin{aligned} \dot{\mathbf{R}}_t &= \dot{\mathbf{R}} \operatorname{Exp}(\delta \boldsymbol{\theta}) + \mathbf{R} \dot{\operatorname{Exp}(\delta \boldsymbol{\theta})} \\ &= \mathbf{R}(\tilde{\boldsymbol{\omega}} - \mathbf{b}_g)^{\wedge} \operatorname{Exp}(\delta \boldsymbol{\theta}) + \mathbf{R} \operatorname{Exp}(\delta \boldsymbol{\theta}) \dot{\delta \boldsymbol{\theta}}^{\wedge} \\ &= \mathbf{R}_t (\tilde{\boldsymbol{\omega}} - \mathbf{b}_{gt} - \boldsymbol{\eta}_g)^{\wedge} \\ &= \mathbf{R} \operatorname{Exp}(\delta \boldsymbol{\theta}) (\tilde{\boldsymbol{\omega}} - \mathbf{b}_{gt} - \boldsymbol{\eta}_g)^{\wedge} \end{aligned} \tag{4-6}

对于 (4-6)，整理式子，得到：

Exp⁡(δθ)δθ˙∧=Exp⁡(δθ)(ω~−bgt−ηg)∧−(ω~−bg)∧Exp⁡(δθ)(4-7)\operatorname{Exp}(\delta \boldsymbol{\theta}) \dot{\delta \boldsymbol{\theta}}^{\wedge}=\operatorname{Exp}(\delta \boldsymbol{\theta})\left(\tilde{\boldsymbol{\omega}}-\mathbf{b}_{g t}-\boldsymbol{\eta}_g\right)^{\wedge}-\left(\tilde{\boldsymbol{\omega}}-\mathbf{b}_g\right)^{\wedge} \operatorname{Exp}(\delta \boldsymbol{\theta}) \tag{4-7}

由于 Exp⁡(δθ)∈SO(3)\operatorname{Exp}(\delta \boldsymbol{\theta}) \in SO(3)，利用伴随性质 ϕ∧R=R(RTϕ)T\boldsymbol{\phi}^{\wedge} \mathbf{R} = \mathbf{R} (\mathbf{R}^T \boldsymbol{\phi})^T，替换 (4-7)：

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]∧(4-8)\begin{aligned} \operatorname{Exp}(\delta \boldsymbol{\theta}) \dot{\delta \boldsymbol{\theta}}^{\wedge} & =\operatorname{Exp}(\delta \boldsymbol{\theta})\left(\tilde{\boldsymbol{\omega}}-\mathbf{b}_{g t}-\boldsymbol{\eta}_g\right)^{\wedge}-\operatorname{Exp}(\delta \boldsymbol{\theta})\left(\operatorname{Exp}(-\delta \boldsymbol{\theta})\left(\tilde{\boldsymbol{\omega}}-\mathbf{b}_g\right)\right)^{\wedge} \\ & =\operatorname{Exp}(\delta \boldsymbol{\theta})\left[\left(\tilde{\boldsymbol{\omega}}-\mathbf{b}_{g t}-\boldsymbol{\eta}_g\right)^{\wedge}-\left(\operatorname{Exp}(-\delta \boldsymbol{\theta})\left(\tilde{\boldsymbol{\omega}}-\mathbf{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(\mathbf{I}-\delta \boldsymbol{\theta}^{\wedge}\right)\left(\tilde{\boldsymbol{\omega}}-\mathbf{b}_g\right)\right)^{\wedge}\right] \\ & =\operatorname{Exp}(\delta \boldsymbol{\theta})\left[\mathbf{b}_g-\mathbf{b}_{g t}-\boldsymbol{\eta}_g+\delta \boldsymbol{\theta}^{\wedge} \tilde{\boldsymbol{\omega}}-\delta \boldsymbol{\theta}^{\wedge} \mathbf{b}_g\right]^{\wedge} \\ & =\operatorname{Exp}(\delta \boldsymbol{\theta})\left[\left(-\tilde{\boldsymbol{\omega}}+\mathbf{b}_g\right)^{\wedge} \delta \boldsymbol{\theta}-\delta \mathbf{b}_g-\boldsymbol{\eta}_g\right]^{\wedge} \end{aligned} \tag{4-8}

约分得到：

δθ˙≈−(ω~−bg)∧δθ−δbg−ηg(4-9)\delta \dot{\boldsymbol{\theta}} \approx - (\tilde{\boldsymbol{\omega}} - \mathbf{b}_g)^{\wedge} \delta \boldsymbol{\theta} - \delta \mathbf{b}_g - \boldsymbol{\eta}_g \tag{4-9}

（2）对速度的求导

对 (4-2) 速度式子两侧求时间导数，得到 δv˙\dot{\delta \mathbf{v}} 对表达式。(4-2) 左侧为：

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−Rηa−Ra~∧δθ+Rba∧δθ+g+δg(4-10)\begin{aligned} \dot{\mathbf{v}}_t & =\mathbf{R}_t\left(\tilde{\mathbf{a}}-\mathbf{b}_{a t}-\boldsymbol{\eta}_a\right)+\mathbf{g}_t \\ & =\mathbf{R} \operatorname{Exp}(\delta \boldsymbol{\theta})\left(\tilde{\mathbf{a}}-\mathbf{b}_a-\delta \mathbf{b}_a-\boldsymbol{\eta}_a\right)+\mathbf{g}+\delta \mathbf{g} \\ & \approx \mathbf{R}\left(\mathbf{I}+\delta \boldsymbol{\theta}^{\wedge}\right)\left(\tilde{\mathbf{a}}-\mathbf{b}_a-\delta \mathbf{b}_a-\boldsymbol{\eta}_a\right)+\mathbf{g}+\delta \mathbf{g} \\ & \approx \mathbf{R} \tilde{\mathbf{a}}-\mathbf{R} \mathbf{b}_a-\mathbf{R} \delta \mathbf{b}_a-\mathbf{R} \boldsymbol{\eta}_a+\mathbf{R} \delta \boldsymbol{\theta}^{\wedge} \mathbf{a}-\mathbf{R} \delta \boldsymbol{\theta}^{\wedge} \mathbf{b}_a+\mathbf{g}+\delta \mathbf{g} \\ & =\mathbf{R} \tilde{\mathbf{a}}-\mathbf{R} \mathbf{b}_a-\mathbf{R} \delta \mathbf{b}_a-\mathbf{R} \mathbf{\eta}_a-\mathbf{R} \tilde{\mathbf{a}}^{\wedge} \delta \boldsymbol{\theta}+\mathbf{R} \mathbf{b}_a^{\wedge} \delta \boldsymbol{\theta}+\mathbf{g}+\delta \mathbf{g} \end{aligned} \tag{4-10}

(4-2) 右侧为：

v˙+δv˙=R(a~−ba)+g+δv˙(4-11)\dot{\mathbf{v}} + \dot{\delta \mathbf{v}} = \mathbf{R} (\tilde{\mathbf{a}} - \mathbf{b}_a) + \mathbf{g} + \dot{\delta \mathbf{v}} \tag{4-11}

由于 (4-10) 和 (4-11) 相等，联合两式得到：

δv˙=−R(a~−ba)∧δθ−Rδba−Rηa+δg=−R(a~−ba)∧δθ−Rδba−ηa+δg(4-12)\begin{aligned} \dot{\delta \mathbf{v}} &= -\mathbf{R}(\tilde{\mathbf{a}} - \mathbf{b}_a)^{\wedge} \delta \boldsymbol{\theta} - \mathbf{R} \delta \mathbf{b}_a - \mathbf{R} \boldsymbol{\eta}_a + \delta \mathbf{g} \\ &= -\mathbf{R}(\tilde{\mathbf{a}} - \mathbf{b}_a)^{\wedge} \delta \boldsymbol{\theta} - \mathbf{R} \delta \mathbf{b}_a - \boldsymbol{\eta}_a + \delta \mathbf{g} \end{aligned} \tag{4-12}

由于 ηa\boldsymbol{\eta}_a 为零均值白噪声，其乘于旋转矩阵仍然为零均值白噪声，且 RTR=I\mathbf{R}^T \mathbf{R} = \mathbf{I}，其协方差矩阵也不变，因此有 (4-12) 的第二行。

至此，把误差变量的运动学方程整理如下：

δp˙=δvδv˙=−R(a~−ba)∧δθ−δba−ηa+δgδθ˙=−(ω~−bg)∧δθ−δbg−ηgδbg˙=ηbgδba˙=ηbaδg˙=0(4-13)\begin{aligned} \dot{\delta \mathbf{p}} & =\delta \mathbf{v} \\ \dot{\delta \mathbf{v}} & =-\mathbf{R}\left(\tilde{\mathbf{a}}-\mathbf{b}_a\right)^{\wedge} \delta \boldsymbol{\theta}-\mathbf \delta \mathbf{b}_a-\boldsymbol{\eta}_a+\delta \mathbf{g} \\ \dot{\delta \boldsymbol{\theta}} & =-\left(\tilde{\boldsymbol{\omega}}-\mathbf{b}_g\right)^{\wedge} \delta \boldsymbol{\theta}-\delta \mathbf{b}_g-\boldsymbol{\eta}_g \\ \dot{\delta \mathbf{b}_g} & =\boldsymbol{\eta}_{b g} \\ \dot{\delta \mathbf{b}_a} & =\boldsymbol{\eta}_{b a} \\ \dot{\delta \mathbf{g}} & =\mathbf{0} \end{aligned} \tag{4-13}

4.2　离散时间下的运动学方程

名义状态变量的离散时间运动学方程可以写为：

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)(4-14)\begin{aligned} \mathbf{p}(t+\Delta t) & =\mathbf{p}(t)+\mathbf{v} \Delta t+\frac{1}{2}\left(\mathbf{R}\left(\tilde{\mathbf{a}}-\mathbf{b}_a\right)\right) \Delta t^2+\frac{1}{2} \mathbf{g} \Delta t^2 \\ \mathbf{v}(t+\Delta t) & =\mathbf{v}(t)+\mathbf{R}\left(\tilde{\mathbf{a}}-\mathbf{b}_a\right) \Delta t+\mathbf{g} \Delta t \\ \mathbf{R}(t+\Delta t) & =\mathbf{R}(t) \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}-\mathbf{b}_g\right) \Delta t\right) \\ \mathbf{b}_g(t+\Delta t) & =\mathbf{b}_g(t) \\ \mathbf{b}_a(t+\Delta t) & =\mathbf{b}_a(t) \\ \mathbf{g}(t+\Delta t) & =\mathbf{g}(t) \end{aligned} \tag{4-14}

误差状态的离散形式只需要处理连续形式中的旋转部分。参考角速度的积分公式，可以将误差状态方程写为：

δ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+ηgδba(t+Δt)=δba+ηaδg(t+Δt)=δg(4-15)\begin{aligned} \delta \mathbf{p}(t+\Delta t) & =\delta \mathbf{p}+\delta \mathbf{v} \Delta t \\ \delta \mathbf{v}(t+\Delta t) & =\delta \mathbf{v}+\left(-\mathbf{R}\left(\tilde{\mathbf{a}}-\mathbf{b}_a\right)^{\wedge} \delta \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{b}_a+\delta \mathbf{g}\right) \Delta t+\boldsymbol{\eta}_v \\ \delta \boldsymbol{\theta}(t+\Delta t) & =\operatorname{Exp}\left(-\left(\tilde{\boldsymbol{\omega}}-\mathbf{b}_g\right) \Delta t\right) \delta \boldsymbol{\theta}-\delta \mathbf{b}_g \Delta t-\boldsymbol{\eta}_\theta \\ \delta \mathbf{b}_g(t+\Delta t) & =\delta \mathbf{b}_g+\boldsymbol{\eta}_g \\ \delta \mathbf{b}_a(t+\Delta t) & =\delta \mathbf{b}_a+\boldsymbol{\eta}_a \\ \delta \mathbf{g}(t+\Delta t) & =\delta \mathbf{g} \end{aligned} \tag{4-15}

噪声项并不参与递推，需要把它们单独归入噪声部分中。这些噪声随机变量的标准差可以列写如下：

σ(ηv)=Δtσa,σ(ηθ)=Δtσg,σ(ηg)=Δtσbg,σ(ηa)=Δtσba(4-16)\sigma(\boldsymbol{\eta}_v) = \Delta t \sigma_a, \sigma(\boldsymbol{\eta}_{\theta}) = \Delta t \sigma_g, \sigma(\boldsymbol{\eta}_g) = \sqrt{\Delta t} \sigma_{bg}, \sigma(\boldsymbol{\eta}_a) = \sqrt{\Delta t} \sigma_{ba} \tag{4-16}

4.3　ESKF 预测过程

误差状态变量 δx\delta \mathbf{x} 的式子可以写为：

δx=f(δx)+w,w∼N(0,Q)(4-17)\delta \mathbf{x} = f(\delta \mathbf{x}) + \mathbf{w}, \mathbf{w} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}) \tag{4-17}

其中有：

Q=diag⁡(03,Cov⁡(ηv),Cov⁡(ηθ),Cov⁡(ηg),Cov⁡(ηa),03)(4-18)\mathbf{Q} = \operatorname{diag}(\mathbf{0}_3, \operatorname{Cov}(\boldsymbol{\eta}_v), \operatorname{Cov}(\boldsymbol{\eta}_{\theta}), \operatorname{Cov}(\boldsymbol{\eta}_g), \operatorname{Cov}(\boldsymbol{\eta}_a), \mathbf{0}_3) \tag{4-18}

为了保持与 EKF 的符号统一，计算运动方程的线性化形式：

δx=Fδx+w(4-19)\delta \mathbf{x} = \mathbf{F} \delta \mathbf{x} + \mathbf{w} \tag{4-19}

其中为线性化后的雅可比矩阵：

F=[IIΔt00000I−R(a~−ba)∧Δt−RΔt0IΔt00Exp⁡(−(ω~−bg)Δt)0−IΔt0000I000000I000000I](4-20)\mathbf{F}= \left[\begin{array}{cccccc} \mathbf{I} & \mathbf{I} \Delta t & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{I} & -\mathbf{R}\left(\tilde{\mathbf{a}}-\mathbf{b}_a\right)^{\wedge} \Delta t & -\mathbf{R} \Delta t & \mathbf{0} & \mathbf{I} \Delta t \\ \mathbf{0} & \mathbf{0} & \operatorname{Exp}\left(-\left(\tilde{\boldsymbol{\omega}}-\mathbf{b}_g\right) \Delta t\right) & \mathbf{0} & -\mathbf{I} \Delta t & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \end{array}\right] \tag{4-20}

执行ESKF的预测过程。预测过程包括对名义状态的预测（IMU 积分）以及对误差状态的预测：

δxpred=FδxPpred=FPFT+Q(4-21)\begin{aligned} & \delta \mathbf{x}_{\text{pred}} = \mathbf{F} \delta \mathbf{x} \\ & \mathbf{P}_{\text{pred}} = \mathbf{FPF}^T + \mathbf{Q} \end{aligned} \tag{4-21}

4.4　ESKF 更新过程

假设观测方程为：

z=h(x)+v,v∼N(0,V)(4-22)\mathbf{z} = h(\mathbf{x}) + \mathbf{v}, \mathbf{v} \sim \mathcal{N}(\mathbf{0}, \mathbf{V}) \tag{4-22}

在 ESKF 中，当前拥有名义状态 x\mathbf{x} 的估计以及误差状态 δx\delta \mathbf{x} 的估计，且希望更新的是误差状态，因此要计算观测方程相比于误差状态的雅可比矩阵：

H=∂h∂δx=∂h∂x∂x∂δx(4-23)\mathbf{H} = \frac{\partial h}{\partial \delta \mathbf{x}} = \frac{\partial h}{\partial \mathbf{x}} \frac{\partial \mathbf{x}}{\partial \delta \mathbf{x}} \tag{4-23}

其中第一项只需对观测方程进行线性化，第二项根据之前对状态变量的定义，可以得到：

∂x∂δx=diag⁡(I3,I3,∂Log⁡(R(Exp⁡(δθ)))∂δθ,I3,I3,I3)(4-24)\frac{\partial \mathbf{x}}{\partial \delta \mathbf{x}}=\operatorname{diag}\left(\mathbf{I}_3, \mathbf{I}_3, \frac{\partial \operatorname{Log} (\mathbf{R}(\operatorname{Exp}(\delta \mathbf{\theta})))}{\partial \delta \mathbf{\theta}}, \mathbf{I}_3, \mathbf{I}_3, \mathbf{I}_3\right) \tag{4-24}

其他部分是平凡的，对于旋转部分，使用 BCH 得到：

∂Log⁡(R(Exp⁡(δθ)))∂δθ=Jr−1(R)(4-25)\frac{\partial \operatorname{Log} (\mathbf{R}(\operatorname{Exp}(\delta \boldsymbol{\theta})))}{\partial \delta \boldsymbol{\theta}}=\mathbf{J}_r^{-1}(\mathbf{R}) \tag{4-25}

最后再计算卡尔曼增益，进而计算误差状态的更新过程：

K=Ppred HT(HPpred HT+V)−1δx=K(z−h(xt))P=(I−KH)Ppred (4-26)\begin{aligned} \mathbf{K} & =\mathbf{P}_{\text {pred }} \mathbf{H}^{T}\left(\mathbf{H} \mathbf{P}_{\text {pred }} \mathbf{H}^{T}+\mathbf{V}\right)^{-1} \\ \delta \mathbf{x} & =\mathbf{K}\left(\mathbf{z}-h\left(\mathbf{x}_t\right)\right) \\ \mathbf{P} & =(\mathbf{I}-\mathbf{K} \mathbf{H}) \mathbf{P}_{\text {pred }} \end{aligned} \tag{4-26}

4.5　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(4-27)\begin{aligned} \mathbf{p}_{k+1} & =\mathbf{p}_k+\delta \mathbf{p}_k \\ \mathbf{v}_{k+1} & =\mathbf{v}_k+\delta \mathbf{v}_k \\ \mathbf{R}_{k+1} & =\mathbf{R}_k \operatorname{Exp}\left(\delta \boldsymbol{\theta}_k\right) \\ \mathbf{b}_{g, k+1} & =\mathbf{b}_{g, k}+\delta \mathbf{b}_{g, k} \\ \mathbf{b}_{a, k+1} & =\mathbf{b}_{a, k}+\delta \mathbf{b}_{a, k} \\ \mathbf{g}_{k+1} & =\mathbf{g}_k+\delta \mathbf{g}_k \end{aligned} \tag{4-27}

ESKF 的重置分为均值部分和协方差部分。均值部分可以简单地表示为：

δx=0(4-28)\delta \mathbf{x} = \mathbf{0} \tag{4-28}

由于均值被重置了，之前描述的是关于 xk\mathbf{x}_k 切空间中的协方差，而现在描述的是 xk+1\mathbf{x}_{k+1} 中的协方差。重置会带来微小的差异，主要影响旋转部分。事实上，在重置前，卡尔曼滤波器刻画了 xpred\mathbf{x}_{\mathrm{pred}} 切空间处的一个高斯分布 N(δx,P)\mathcal{N}(\delta \mathbf{x}, \mathbf{P}), 而重置之后, 应该刻画 xpred⊞δx\mathbf{x}_{\mathrm{pred}} \boxplus \delta \mathbf{x} 处的一个 N(0,Preset )\mathcal{N}\left(0, \mathbf{P}_{\text {reset }}\right) 。

设重置前的名义旋转估计为 Rk\mathbf{R}_k，误差状态为 δθ\delta \boldsymbol{\theta}，卡尔曼滤波的增量计算结果为 δθk\delta \boldsymbol{\theta}_k，注意此处 δθk\delta \boldsymbol{\theta}_k 已知，而 δθ\delta \boldsymbol{\theta} 是随机变量。重置之后的名义旋转部分为
RkExp⁡(δθk)=R+\mathbf{R}_k \operatorname{Exp}\left(\delta \mathbf{\theta}_k\right)=\mathbf{R}^{+}，误差状态为 δθ+\delta \boldsymbol{\theta}^{+}。由于误差状态被重置了，显然此时 δθ+=0\delta \boldsymbol{\theta}^{+}=\mathbf{0}。但关心的并不是它们直接的取值，而是 δθ+\delta \boldsymbol{\theta}^{+}与 δθ\delta \boldsymbol{\theta} 的线性化关系。把实际的重置过程写出来:

R+Exp⁡(δθ+)=RkExp⁡(δθk)Exp⁡(δθ+)=RkExp⁡(δθ)(4-29)\mathbf{R}^{+} \operatorname{Exp}\left(\delta \boldsymbol{\theta}^{+}\right)=\mathbf{R}_k \operatorname{Exp}\left(\delta \boldsymbol{\theta}_k\right) \operatorname{Exp}\left(\delta \boldsymbol{\theta}^{+}\right)=\mathbf{R}_k \operatorname{Exp}(\delta \boldsymbol{\theta}) \tag{4-29}

不难得到：

Exp⁡(δθ+)=Exp⁡(−δθk)Exp⁡(δθ)(4-30)\operatorname{Exp}\left(\delta \boldsymbol{\theta}^{+}\right)=\operatorname{Exp}\left(-\delta \boldsymbol{\theta}_k\right) \operatorname{Exp}(\delta \boldsymbol{\theta}) \tag{4-30}

注意这里 δθ\delta \boldsymbol{\theta} 为小量, 利用线性化后的 BCH 公式, 可以得到:

δθ+=−δθk+δθ−12δθk∧δθ+o((δθ)2)(4-31)\delta \boldsymbol{\theta}^{+}=-\delta \boldsymbol{\theta}_k+\delta \boldsymbol{\theta}-\frac{1}{2} \delta \boldsymbol{\theta}_k^{\wedge} \delta \boldsymbol{\theta}+o\left((\delta \boldsymbol{\theta})^2\right) \tag{4-31}

从而：

Jθ=∂δθ+∂δθ≈I−12δθk∧(4-32)\mathbf{J}_{\theta} = \frac{\partial \delta \boldsymbol{\theta}^{+}}{\partial \delta \boldsymbol{\theta}} \approx \mathbf{I}-\frac{1}{2} \delta \boldsymbol{\theta}_k^{\wedge} \tag{4-32}

该式表明重置前后的误差状态相差一个旋转方面的小雅可比矩阵，记作 Jθ\mathbf{J}_{\theta}。 把这个小雅可比阵放到整个状态变量维度下，并保持其他部分为单位矩阵，可以得到一个完整的雅可比阵：

Jk=diag⁡(I3,I3,Jθ,I3,I3,I3)(4-34)\mathbf{J}_k=\operatorname{diag}\left(\mathbf{I}_3, \mathbf{I}_3, \mathbf{J}_{\boldsymbol{\theta}}, \mathbf{I}_3, \mathbf{I}_3, \mathbf{I}_3\right) \tag{4-34}

因此，在把误差状态的均值归零同时，它们的协方差矩阵也应该进行线性变换:

Preset=JkPJkT(4-35)\mathbf{P}_{\text {reset}}=\mathbf{J}_k \mathbf{P} \mathbf{J}_k^T \tag{4-35}

参考