ESKF学习笔记

IMU驱动系统的误差状态运动学

误差状态卡尔曼滤波器简介

  在误差状态滤波器的公式中，我们谈论真实状态、名义状态和误差状态值，其中真实状态被表示为名义状态和误差状态的适当组合（线性求和、四元数乘积或矩阵乘积）。这样做的想法是将名义状态视为大信号（可在非线性方式下积分），将误差状态视为小信号（因此可线性积分并适用于线性高斯滤波）。

  误差状态滤波器可以这样解释。一方面，高频IMU数据${\text{u}}_m$被集成到名义状态$\text{x}$中。该名义状态不考虑噪声项$\text{w}$和其他可能的模型缺陷。因此，它将累积误差。这些误差被收集到误差状态$\delta \text{x}$中，并且通过误差状态卡尔曼滤波器（ESKF）进行估计，这一次包括所有的噪声和扰动。误差状态由小信号量组成，其演变函数由正确定义的（时变）线性动态系统确定，其动态、控制和测量矩阵从名义状态的值计算而来。与名义状态的集成并行，ESKF预测误差状态的高斯估计。它只进行预测，因为现在还没有其他可用的测量来纠正这些估计。滤波器校正是在到达IMU以外的信息（例如GPS、视觉等）时进行的，这些信息能够使误差可观测，并且通常比集成阶段低得多。这种校正提供了误差状态的后验高斯估计。此后，误差状态的平均值被注入到名义状态中，然后重置为零。误差状态的协方差矩阵被方便地更新以反映此重置。系统会一直这样运行下去。

连续时间的系统运动学

误差状态卡尔曼滤波中的所有变量:

误差状态的幅值	真实值	名义值	误差	组合	测量值	噪声
状态变量集合	${\text{x}}_t$	$\text{x}$	$\delta \text{x}$	${\mathbf{x}}_t=\mathbf{x}\oplus \delta \mathbf{x}$
位置	${\mathbf{p}}_t$	$\mathbf{p}$	$\delta \mathbf{p}$	${\mathbf{p}}_t=\mathbf{p}+\delta \mathbf{p}$
速度	${\mathbf{v}}_t$	$\mathbf{v}$	$\delta \mathbf{v}$	${\mathbf{v}}_t=\mathbf{v}+\delta \mathbf{v}$
四元数	${\mathbf{q}}_t$	$\mathbf{q}$	$\delta \mathbf{q}$	${\mathbf{q}}_t=\mathbf{q}\otimes \delta \mathbf{q}$
旋转矩阵	${\mathbf{R}}_t$	$\mathbf{R}$	$\delta \mathbf{R}$	${\mathbf{R}}_t=\mathbf{R}\delta \mathbf{R}$
角度矢量			$\delta \mathbf{\theta }$	$\begin{array}{c}\delta \mathbf{q}=e^{\delta \mathbf{\theta }/2}\\ \delta \mathbf{R}=e^{[\delta \mathbf{\theta }{]}_{\times }}\end{array}$
加速度计偏置	${\mathbf{a}}_{bt}$	${\mathbf{a}}_b$	$\delta {\text{a}}_b$	${\mathbf{a}}_{bt}={\mathbf{a}}_b+\delta {\mathbf{a}}_b$		${\mathbf{a}}_w$
陀螺仪偏置	${\omega }_{bt}$	${\omega }_b$	$\delta {\omega }_b$	${\omega }_{bt}={\omega }_b+\delta {\omega }_b$		${\omega }_w$
重量矢量	${\mathbf{g}}_t$	$\mathbf{g}$	$\delta \text{g}$	${\mathbf{g}}_t=\mathbf{g}+\delta \mathbf{g}$
加速度	${\mathbf{a}}_t$				${\mathbf{a}}_m$	${\mathbf{a}}_n$
角速度	${\omega }_t$				${\omega }_m$	${\omega }_n$

真实状态运动学

真实值运动学方程：
$$\begin{array}{l}{\dot{\mathbf{p}}}_t={\mathbf{v}}_t\\ {\dot{\mathbf{v}}}_t={\mathbf{a}}_t\\ {\dot{\mathbf{q}}}_t=\frac{1}{2}{q}_t\otimes {\omega }_t\\ {\dot{\mathbf{a}}}_{bt}={\mathbf{a}}_w\\ {\dot{\omega }}_{bt}={\omega }_w\\ {\dot{\mathbf{g}}}_t=0\end{array}$$
  机身坐标系下的加速度传感器读数${\mathbf{a}}_m$和角速度传感器读数$w_m$被用来获取机器人在当前时刻的真实加速度${\mathbf{a}}_t$和角速率$w_t$。由于传感器本身存在噪声，因此${\mathbf{a}}_m$和$w_m$是带噪声的传感器读数。
$$\begin{array}{l}{\mathbf{a}}_m={\mathbf{R}}_t^{\top }({\mathbf{a}}_t-{\mathbf{g}}_t)+{\mathbf{a}}_{bt}+{\mathbf{a}}_n\\ {\omega }_m={\omega }_t+{\omega }_{bt}+{\omega }_n\end{array}$$
通过 R t ≜ R { q t } \mathbf{R}_{t}\triangleq\mathbf{R}\{\mathbf{q}_{t}\} Rt​≜R{qt​}分离出真实值。
$$\begin{array}{l}{\mathbf{a}}_t={\mathbf{R}}_t({\mathbf{a}}_m-{\mathbf{a}}_{bt}-{\mathbf{a}}_n)+{\mathbf{g}}_t\\ {\omega }_t={\omega }_m-{\omega }_{bt}-{\omega }_n\end{array}$$
代入
$$\begin{array}{l}{\dot{\text{p}}}_t={\text{v}}_t\\ {\text{v}}_t={\text{R}}_t({\text{a}}_m-{\text{a}}_{bt}-{\text{a}}_n)+{\text{g}}_t\\ {\dot{\text{q}}}_t=\frac{1}{2}{\text{q}}_t\otimes ({\omega }_m-{\omega }_{bt}-{\omega }_n)\\ {\dot{\text{a}}}_{bt}={\text{a}}_w\\ {\dot{\omega }}_t={\omega }_w\\ {\dot{\text{g}}}_t=0\end{array}$$
设${\dot{\mathbf{x}}}_t=f_t({\mathbf{x}}_t,\mathbf{u},\mathbf{w})$,该系统受IMU噪声读数${\mathbf{u}}_m$的控制，并受白高斯噪声$\text{w}$的扰动
$${\mathbf{x}}_t=\left[\begin{array}{c}{\mathbf{p}}_t\\ {\mathbf{v}}_t\\ {\mathbf{q}}_t\\ {\mathbf{a}}_d\\ {\mathbf{g}}_t\end{array}\right]\mathbf{u}=\left[\begin{array}{c}{\mathbf{a}}_m-{\mathbf{a}}_n\\ {\omega }_m-{\omega }_n\end{array}\right]\mathbf{v}=\left[\begin{array}{c}{\mathbf{a}}_w\\ {\omega }_w\end{array}\right]$$
​

  重力矢量${\mathbf{g}}_t$由滤波器估计。系统从一个固定且任意已知的初始方向开始，即${\mathbf{q}}_t(t=0)={\mathbf{q}}_0$，由于初始方向通常不在水平面上，因此初始重力向量通常是未知的，通常将初始方向设为${\mathbf{q}}_0=(1,0,0,0)$。因此， R 0 = R { q 0 } = I \mathbf{R}_0=\mathbf{R}\{\mathbf{q}_0\}=\mathbf{I} R0​=R{q0​}=I。还可以从系统中删除与重力相关的所有方程，并采用更传统的方法，即假设重力向量为$\mathbf{g}\stackrel{\Delta }{=}\left(0,0,-9.8xx\right)$，其中$xx$是实验现场重力向量的适当小数位数，同时假设初始方向${\mathbf{q}}_0$是不确定的。

名义状态运动学

  名义状态运动学对应于没有噪声或扰动的建模系统。
$$\begin{array}{l}\dot{\mathbf{p}}=\mathbf{v}\\ \dot{\mathbf{v}}=\mathbf{R}({\mathbf{a}}_m-{\mathbf{a}}_b)+\mathbf{g}\\ \dot{\mathbf{q}}=\frac{1}{2}\mathbf{a}\otimes ({\omega }_m-{\omega }_b)\\ {\dot{\mathbf{a}}}_b=0\\ {\dot{\omega }}_b=0\\ \dot{\mathbf{g}}=0\end{array}$$

误差状态运动学

  目标是确定误差状态的线性化动力学。
$$\begin{array}{rl}& \dot{\delta }\text{p}=\delta \text{v}\\ & \dot{\delta }\text{v}=-\text{R}{\left[{\text{a}}_m-{\text{a}}_b\right]}_{\times }\delta \mathbf{\theta }-\text{R}\delta {\text{a}}_b+\delta \text{g}-\text{R}{\text{a}}_n\\ & \dot{\delta }\mathbf{\theta }=-{\left[{\omega }_m-{\omega }_b\right]}_{\times }\delta \mathbf{\theta }-\delta {\omega }_b-{\omega }_n\\ & \dot{\delta }{\text{a}}_b={\text{a}}_w\\ & \dot{\delta }{\omega }_b={\omega }_w\\ & \delta \dot{\text{g}}=0\end{array}$$
其中，速度和方向误差的方程需要对角速度和重力矢量的误差方程进行一些操作，以获得线性化的动力学。

离散时间内的系统运动学

  上述微分方程需要被积分成差分方程，以考虑离散的时间间隔∆t>0，积分方法可以有所不同。在某些情况下，可以使用精确的闭式解决方案。在其他情况下，可以使用不同精度的数值积分方法。请参见附录中有关积分方法的相关细节。

需要对以下子系统进行积分：

1. 名义状态。
2. 误差状态。
   （a）确定性部分：状态动态和控制。
   （b）随机部分：噪声和扰动。

  对于名义状态和误差状态，需要对确定性部分和随机部分进行积分。确定性部分包括状态动态和控制，随机部分包括噪声和扰动。对于不同的子系统和积分方法，需要进行适当的选择和调整，以获得准确的结果。

名义状态的运动学

$$\begin{array}{l}\text{p}\leftarrow \text{p}+\text{v}\Delta t+\frac{1}{2}(\text{R}({\text{a}}_m-{\text{a}}_b)+\text{g})\Delta t^2\\ \text{v}\leftarrow \text{v}+(\text{R}({\text{a}}_m-{\text{a}}_b)+\text{g})\Delta t\\ \text{q}\leftarrow \text{q}\otimes \text{q}\{({\omega }_m-{\omega }_b)\Delta t\}\\ {\text{a}}_b\leftarrow {\text{a}}_b\\ \text{g}\leftarrow \text{g}\end{array}$$

其中，$x\leftarrow f(x,\bullet )$代表$x_{k+1}=f(x_k,{\bullet }_k)$类型的时间更新， R ≜ R { q } \mathbf{R} \triangleq\mathbf{R}\{\mathbf{q}\} R≜R{q}是与当前名义方向$\mathbf{q}$相关的旋转矩阵， q { v } \mathbf{q}\{v\} q{v}是与旋转$v$相关的四元数。

误差状态的运动学

δ p ← δ p + δ v Δ t δ v ← δ v + ( − R [ a m − a b ] × δ θ − R δ a b + δ g ) Δ t + v i δ θ ← R ⊤ { ( ω m − ω b ) Δ t } δ θ − δ ω b Δ t + θ i δ a b ← δ a b + a i δ ω b ← δ ω b + ω i δ g ← δ g \begin{array}{l}\delta\textbf{p}\leftarrow\delta\textbf{p}+\delta\textbf{v}\Delta t\\ \delta\textbf{v}\leftarrow\delta\textbf{v}+(-\textbf{R}\left[\textbf{a}_m-\textbf{a}_b\right]\times\delta\boldsymbol{\theta}-\textbf{R}\delta\textbf{a}_b+\delta\textbf{g})\Delta t+\textbf{v}_\textbf{i}\\ \delta\boldsymbol{\theta}\leftarrow\textbf{R}^\top\left\{(\omega_m-\omega_b)\Delta t\right\}\delta\boldsymbol{\theta}-\delta\omega_b\Delta t+\boldsymbol{\theta}_\textbf{i}\\ \delta\textbf{a}_b\leftarrow\delta\textbf{a}_b+\textbf{a}_\textbf{i}\\ \delta \omega_b\leftarrow\delta \omega_b+\omega_\textbf{i}\\ \delta\textbf{g}\leftarrow\delta\textbf{g}\end{array} δp←δp+δvΔtδv←δv+(−R[am​−ab​]×δθ−Rδab​+δg)Δt+vi​δθ←R⊤{(ωm​−ωb​)Δt}δθ−δωb​Δt+θi​δab​←δab​+ai​δωb​←δωb​+ωi​δg←δg​

其中,，${\text{v}}_{\text{i}}$、${\mathbf{\theta }}_{\text{i}}$、${\text{a}}_{\text{i}}$和${\omega }_{\text{i}}$是随机脉冲，作用于速度、方向和偏差估计上，由白高斯过程建模。它们的均值为零，协方差矩阵是通过将${\mathbf{a}}_n$、${\omega }_n$、${\mathbf{a}}_{\omega }$和${\omega }_{\omega }$的协方差积分到步长时间$\Delta t$上得到的。
$$\begin{array}{rlrl}{\text{V}}_{\text{i}}& ={\sigma }_{{\tilde{a}}_n}^2\Delta t^2\text{I}& & [m^2/s^2]\\ {\mathbf{\Theta }}_{\text{i}}& ={\sigma }_{{\tilde{\omega }}_n}^2\Delta t^2\text{I}& & [ra{d}^2]\\ {\text{A}}_{\text{i}}& ={\sigma }_{a_w}^2\Delta t\text{I}& & [m^2/s^4]\\ {\mathbf{\Omega }}_{\text{i}}& ={\sigma }_{{\omega }_w}^2\Delta t\text{I}& & [ra{d}^2/s^2]\end{array}$$

其中,${\sigma }_{{\tilde{a}}_n}[m^2/s^2]$, ${\sigma }_{{\tilde{\omega }}_n}[rad/s]$, ${\sigma }_{a_w}\left[m/s^2\sqrt{s}\right]$和${\sigma }_{{\omega }_w}\left[rad/s\sqrt{s}\right]$将根据IMU数据表中的信息或实验测量结果来确定。

误差状态的雅可比矩阵和扰动矩阵

  雅可比矩阵可以通过对前一节中的误差状态差分方程进行简单的推导得到。为了以简洁的形式书写这些方程，考虑名义状态向量$\mathbf{x}$、误差状态向量$\delta \mathbf{x}$、输入向量${\mathbf{u}}_m$和扰动脉冲向量$\mathbf{i}$，如下所示
$$\mathbf{x}=\left[\begin{array}{c}\mathbf{p}\\ \mathbf{v}\\ \mathbf{q}\\ {\mathbf{a}}_b\\ {\omega }_b\\ \mathbf{g}\end{array}\right],\delta \mathbf{x}=\left[\begin{array}{c}\delta \mathbf{p}\\ \delta \mathbf{v}\\ \delta \mathbf{\theta }\\ \delta {\mathbf{a}}_b\\ \delta {\omega }_b\\ \delta \mathbf{g}\end{array}\right],{\mathbf{u}}_m=\left[\begin{array}{c}{\mathbf{a}}_m\\ {\omega }_m\end{array}\right],\mathbf{i}=\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{i}}\\ {\mathbf{\theta }}_{\mathbf{i}}\\ {\mathbf{a}}_{\mathbf{i}}\\ {\omega }_{\mathbf{i}}\end{array}\right]$$
误差状态系统改写成：
$$\delta \text{x}\leftarrow f(\text{x},\delta \text{x},{\text{u}}_m,\text{i})={\text{F}}_{\text{x}}(\text{x},{\text{u}}_m)\cdot \delta \text{x}+{\text{F}}_{\text{i}}\cdot \text{i}$$
ESKF的预测方程:
$$\begin{array}{l}\widehat{\delta \mathbf{x}}\leftarrow {\mathbf{F}}_{\mathbf{x}}(\mathbf{x},{\mathbf{u}}_m)\cdot \widehat{\delta \mathbf{x}}\\ \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 }\end{array}$$
其中, δ x ∼ N { δ x ^ , P } \delta\mathbf{x}\sim\mathcal{N}\{\hat{\delta\mathbf{x}},\mathbf{P}\} δx∼N{δx^,P}；${\mathbf{F}}_{\mathbf{x}}$和${\mathbf{F}}_{\mathbf{i}}$是$f()$相对于误差和扰动矢量的雅可比矩阵；${\mathbf{Q}}_{\mathbf{i}}$是扰动脉冲的协方差矩阵。

  上述雅可比矩阵和协方差矩阵的表达方式详见下文。这里出现的所有与状态有关的值都是直接从名义状态中提取的。
$${\text{F}}_{\text{x}}=\frac{\partial f}{\partial \delta \text{x}}{|}_{\text{x},{\text{u}}_m}=\left[\begin{array}{cccccc}\text{I}& \text{I}\Delta t& 0& 0& 0& 0\\ 0& \text{I}& -\text{R}[{\text{a}}_m-{\text{a}}_b{]}_{\times }\Delta t& -\text{R}\Delta t& 0& \text{I}\Delta t\\ 0& 0& {\text{R}}^{\top }\{({\omega }_m-{\omega }_b)\Delta t\}& 0& -\text{I}\Delta t& 0\\ 0& 0& 0& \text{I}& 0& 0\\ 0& 0& 0& 0& \text{I}& 0\\ 0& 0& 0& 0& 0& \text{I}\end{array}\right]$$

$${\text{F}}_{\text{i}}=\frac{\partial f}{\partial \text{i}}{|}_{\text{x},{\text{u}}_m}=\left[\begin{array}{cccc}0& 0& 0& 0\\ \text{I}& 0& 0& 0\\ 0& \text{I}& 0& 0\\ 0& 0& \text{I}& 0\\ 0& 0& 0& \text{I}\\ 0& 0& 0& 0\end{array}\right],{\text{Q}}_{\text{i}}=\left[\begin{array}{cccc}{\text{V}}_{\text{i}}& 0& 0& 0\\ 0& {\mathbf{\Theta }}_{\text{i}}& 0& 0\\ 0& 0& {\text{A}}_{\text{i}}& 0\\ 0& 0& 0& {\mathbf{\Omega }}_{\text{i}}\end{array}\right]$$

将IMU与互补传感器数据融合

  当出现比IMU更多种类的信息，例如GPS或视觉信息时，会对ESKF进行纠正。在设计良好的系统中，这应该使IMU偏差可观测，并允许ESKF正确估计它们。有无数的可能性，最流行的是GPS + IMU，单眼视觉 + IMU和立体视觉 + IMU。

  虽然IMU信息到目前为止用于向ESKF进行预测，但此其他信息用于纠正滤波器，从而观测IMU偏差误差。纠正包括三个步骤：

1. 通过滤波器校正观测误差状态，
2. 注入观测到的误差到名义状态中，并
3. 重置误差状态。

通过滤波校正观测误差状态

假设像往常一样，传感器提供的信息取决于状态，例如：
$$\mathbf{y}=h({\mathbf{x}}_t)+v$$
其中$h()$是系统状态（真实状态）的一般非线性函数，$v$是具有协方差$\text{V}$的白高斯噪声，
v ∼ N { 0 , V } v\sim\mathcal{N}\{0,\mathbf{V}\} v∼N{0,V}
  滤波器是在估计错误状态，因此，滤波器的修正方程为:
$$\begin{array}{rl}\text{K}& ={\text{PH}}^{\top }(\text{H}{\text{PH}}^{\top }+\text{V}{)}^{-1}\\ & \widehat{\delta \text{x}}\leftarrow \text{K}(\text{y}-h({\hat{\text{x}}}_t))\\ & \text{P}\leftarrow (\text{I}-\text{K}\text{H})\text{P}\end{array}$$
要求雅可比矩阵$\text{H}$相对于误差状态$\delta \text{x}$定义，并在最佳真实状态估计${\hat{\mathbf{x}}}_t=\mathbf{x}\oplus \widehat{\delta \mathbf{x}}$处进行评估。由于误差状态平均值在此阶段为零（还没有观测到它），因此有${\hat{\mathbf{x}}}_t=\mathbf{x}$，并且可以使用名义误差$\text{x}$作为评估点，从而得出以下结果：
$$\mathbf{H}\equiv {\frac{\partial h}{\partial \delta \mathbf{x}}|}_{\mathbf{x}}$$

滤波器校正计算的雅可比矩阵

上述的雅可比矩阵可以通过多种方式计算。最具说明性的方法是利用链式法则，
$$\text{H}\triangleq {\frac{\partial h}{\partial \delta \text{x}}|}_{\text{x}}={\frac{\partial h}{\partial {\text{x}}_t}|}_{\text{x}}{\frac{\partial {\text{x}}_t}{\partial \delta \text{x}}|}_{\text{x}}={\text{H}}_{\text{x}}{\text{X}}_{\delta \text{x}}$$
其中，$\text{H}\triangleq {\frac{\partial h}{\partial \delta \text{x}}|}_{\text{x}}$是$h()$相对于其自身参数的标准雅各布系数（即在常规EKF中使用的雅各布系数）。链式规则的第一部分取决于所使用的特定传感器的测量功能.

  第二部分，${\text{X}}_{\delta \text{x}}\triangleq {\frac{\partial {\text{x}}_t}{\partial \delta \text{x}}|}_{\text{x}}$，是真实状态相对于误差状态的雅可比矩阵。这部分可以在这里导出，因为它只取决于状态的ESKF组成。有导数，
$${\mathbf{X}}_{\delta \mathbf{x}}=\left[\begin{array}{ccccccc}\frac{\partial (\mathbf{p}+\delta \mathbf{p})}{\partial \delta \mathbf{p}}& & & & & & \\ & \frac{\partial (\mathbf{v}+\delta \mathbf{v})}{\partial \delta \mathbf{v}}& & & & 0& \\ & & \frac{\partial (\mathbf{q}\otimes \delta \mathbf{q})}{\partial \delta \mathbf{\theta }}& & & & \\ & & & & \frac{\partial \left({\mathbf{a}}_b+\delta {\mathbf{a}}_b\right)}{\partial \delta {\mathbf{a}}_b}& & \\ & 0& & & & \frac{\partial \left({\mathbf{\omega }}_b+\delta {\mathbf{\omega }}_b\right)}{\partial \delta {\mathbf{\omega }}_b}& \\ & & & & & & \frac{\partial (\mathbf{g}+\delta \mathbf{g})}{\partial \delta \mathbf{g}}\end{array}\right]$$
这导致了除了$4\times 3$四元数项${\text{Q}}_{\delta \mathbf{\theta }}=\partial (\text{q}\otimes \delta \text{q})/\partial \delta \mathbf{\theta }$之外的所有恒等式$3\times 3$块（例如，$\frac{\partial (\text{p}+\delta \text{p})}{\partial \delta \text{p}}={\text{I}}_3$）。因此，有这样的形式，
$${\text{X}}_{\delta \text{x}}\triangleq {\frac{\partial {\text{x}}_t}{\partial \delta \text{x}}|}_{\text{x}}=\left[\begin{array}{ccc}{\text{I}}_6& 0& 0\\ 0& {\text{Q}}_{\delta \theta }& 0\\ 0& 0& {\text{I}}_9\end{array}\right]$$
四元数项${\text{Q}}_{\delta \mathbf{\theta }}$可推导为:
$${\text{Q}}_{\delta \mathbf{\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]$$

将观测误差注入名义状态

在ESKF更新之后，使用适当的组合（求和或四元数乘积）将观测误差状态与名义状态进行更新。
$$\text{x}\leftarrow \text{x}\oplus \widehat{\delta \text{x}}$$
也就是，
p ← p + δ p ^ v ← v + δ v ^ q ← q ⊗ q { δ θ ^ } a b ← a b + δ a b ^ ω b ← ω b + δ ω ^ b g ← g + δ g ^ \begin{array}{l} \mathbf{p} \leftarrow \mathbf{p}+\hat{\delta \mathbf{p}} \\ \mathbf{v} \leftarrow \mathbf{v}+\hat{\delta \mathbf{v}} \\ \mathbf{q} \leftarrow \mathbf{q} \otimes \mathbf{q}\{\hat{\delta \boldsymbol{\theta}}\} \\ \mathbf{a}_{b} \leftarrow \mathbf{a}_{b}+\hat{\delta {\mathbf{a}}_{b}} \\ \boldsymbol{\omega}_{b} \leftarrow \boldsymbol{\omega}_{b}+\delta \hat{\boldsymbol{\omega}}_{b} \\ \mathbf{g} \leftarrow \mathbf{g}+\hat{\delta \mathbf{g}} \end{array} p←p+δp^​v←v+δv^q←q⊗q{δθ^}ab​←ab​+δab​^​ωb​←ωb​+δω^b​g←g+δg^​​

重置ESKF

在将误差注入到名义状态后，误差状态均值$\widehat{\delta \text{x}}$将被重置。这在方向部分尤其重要，因为新的方向误差将相对于新名义状态的方向框架局部表示。为了使ESKF更新完整，误差的协方差需要根据这个修改进行更新。

  称误差重置函数为$g()$。写法如下：
$$\delta \mathbf{x}\leftarrow g(\delta \mathbf{x})=\delta \mathbf{x}\ominus \widehat{\delta \mathbf{x}}$$
其中，$\ominus$代表$\oplus$的组合的逆。因此，ESKF的误差重置操作为：
$$\begin{array}{l}\widehat{\delta \text{x}}\leftarrow 0\\ \text{P}\leftarrow \text{G}{\text{PG}}^{\top }\end{array}$$
其中G是雅可比矩阵，定义如下:
$$\text{G}\triangleq \frac{\partial g}{\partial \delta \text{x}}{|}_{\widehat{\delta \text{x}}}$$
  类似于上述更新雅可比矩阵所发生的情况，这个雅可比矩阵在除了方向误差之外的所有对角块上都是相同的。在此给出完整的表达式，$\partial \delta {\mathbf{\theta }}^{+}/\partial \delta \mathbf{\theta }=\mathbf{I}-{\left[\frac{1}{2}\widehat{\delta \mathbf{\theta }}\}\right]}_{\times }$，
$$\text{G}=\left[\begin{array}{ccc}{\text{I}}_6& 0& 0\\ 0& \text{I}-{\left[\begin{array}{c}\frac{1}{2}\widehat{\delta \mathbf{\theta }}\}\end{array}\right]}_{\times }& 0\\ 0& 0& {\text{I}}_9\end{array}\right]$$
  在大多数情况下，误差项ˇδθ可以忽略，只产生一个雅可比矩阵$\text{G}={\text{I}}_{18}$，从而导致微小的误差复位。

使用全局误差的ESKF

  本节探讨在全局参考系下定义角度误差与迄今为止所使用的局部定义之间的差异。在全局参考系下定义角度误差δθ意味着在左侧进行组合，即：
q t = δ q ⊗ q = q { δ θ } ⊗ q \mathbf{q}_{t}=\delta\mathbf{q}\otimes\mathbf{q}=\mathbf{q}\{\delta\mathbf{\boldsymbol{\theta}}\}\otimes\mathbf{q} qt​=δq⊗q=q{δθ}⊗q
  这里，无论角度误差是全局定义还是局部定义，仍然保留角速率向量$\omega$的局部定义，即在连续时间中，$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \omega$，因此在离散时间中， q ← q ⊗ q { ω Δ t } \textbf{q}\leftarrow\textbf{q}\otimes\textbf{q}\{\omega\Delta t\} q←q⊗q{ωΔt}。这是为了方便起见，因为陀螺仪提供的角速率测量值是以体坐标系为基准的，也就是局部坐标系。

连续时间中的系统运动学

真实状态和名义状态运动学

真实运动学和名义运动学不包含误差，它们的方程不变。

误差状态运动学

误差状态运动学方程
$$\begin{array}{rl}& \dot{\delta \text{p}}=\delta \text{v}\\ & \dot{\delta \text{v}}=-{\left[\text{R}({\text{a}}_m-b{\text{a}}_b)\right]}_{\times }\delta \mathbf{\theta }-\text{R}\delta {\text{a}}_b+\delta \text{g}-{\text{Ra}}_n\\ & \dot{\delta \mathbf{\theta }}=-\text{R}\delta {\omega }_b-\text{R}{\omega }_n\\ & \delta {\dot{\text{a}}}_b={\text{a}}_w\\ & \delta {\dot{\omega }}_b={\omega }_w\\ & \dot{\delta \text{g}}=0\end{array}$$

离散时间中的系统运动学

名义状态

名义状态方程不涉及误差，因此与角度误差局部定义情况下的方程相同。

误差状态

$$\begin{array}{l}\delta \text{p}\leftarrow \delta \text{p}+\delta \text{v}\Delta t\\ \delta \text{v}\leftarrow \delta \text{v}+(-{\left[\text{R}({\text{a}}_n-{\text{a}}_b)\right]}_{\times }\delta \mathbf{\theta }-\text{R}\delta {\text{a}}_b+\delta \text{g})\Delta t+{\text{v}}_{\text{i}}\\ \delta \mathbf{\theta }\leftarrow \delta \mathbf{\theta }-\text{R}\delta {\omega }_b\Delta t+{\mathbf{\theta }}_{\text{i}}\\ \delta {\text{a}}_b\leftarrow \delta {\text{a}}_b+{\text{a}}_{\text{i}}\\ \delta {\omega }_b\leftarrow \delta {\omega }_b+{\omega }_{\text{i}}\\ \delta \text{g}\leftarrow \delta \text{g}\end{array}$$

误差状态雅可比矩阵和扰动矩阵

通过简单地推导上述方程，可以得到转移矩阵：
$${\mathbf{F}}_{\mathbf{x}}=\left[\begin{array}{cccccc}\mathbf{I}& \mathbf{I}\Delta t& 0& 0& 0& 0\\ 0& \mathbf{I}& -[\mathbf{R}({\mathbf{a}}_m-{\mathbf{a}}_b){]}_{\times }\Delta t& -\mathbf{R}\Delta t& 0& \mathbf{I}\Delta t\\ 0& 0& \mathbf{I}& 0& -\mathbf{R}\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]$$
扰动雅可比矩阵和扰动矩阵保持不变。
$${\text{F}}_{\text{i}}=\left[\begin{array}{cccc}0& 0& 0& 0\\ \text{I}& 0& 0& 0\\ 0& \text{I}& 0& 0\\ 0& 0& \text{I}& 0\\ 0& 0& 0& \text{I}\\ 0& 0& 0& 0\end{array}\right],{\text{Q}}_{\text{i}}=\left[\begin{array}{cccc}{\text{V}}_{\text{i}}& 0& 0& 0\\ 0& {\mathbf{\Theta }}_{\text{i}}& 0& 0\\ 0& 0& {\text{A}}_{\text{i}}& 0\\ 0& 0& 0& {\mathbf{\Omega }}_{\text{i}}\end{array}\right]$$

与互补传感器数据融合

当考虑全局角度误差时，涉及ESKF机制的融合方程仅略有变化。将通过ESKF校正中的误差状态观测、误差注入到名义状态以及重置步骤来修订这些变化。

误差状态观测

与局部误差定义相比，唯一的区别在于将方向与角度误差相关联的观测函数的雅可比矩阵块。
$${\mathbf{Q}}_{\delta \theta }\triangleq \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]$$

将观测误差注入名义状态

p ← p + δ p v ← v + δ v q ← q { δ θ ^ } ⊗ q a b ← a b + δ a b ω b ← ω b + δ ω b g ← g + δ g \begin{array}{l}\textbf{p}\leftarrow\textbf{p}+\delta\textbf{p}\\ \textbf{v}\leftarrow\textbf{v}+\delta\textbf{v}\\ \textbf{q}\leftarrow\textbf{q}\{\hat{\delta \boldsymbol{\theta}}\}\otimes\textbf{q}\\ \textbf{a}_b\leftarrow\textbf{a}_b+\delta\textbf{a}_b\\ \omega_b\leftarrow\omega_b+\delta\omega_b\\ \textbf{g}\leftarrow\textbf{g}+\delta\textbf{g}\end{array} p←p+δpv←v+δvq←q{δθ^}⊗qab​←ab​+δab​ωb​←ωb​+δωb​g←g+δg​

重置ESKF

根据以下公式，ESKF误差均值被重置，协方差被更新：
$$\begin{array}{l}\widehat{\delta \text{x}}\leftarrow 0\\ \text{P}\leftarrow \text{G}{\text{PG}}^{\top }\end{array}$$
雅可比矩阵为：
$$\text{G}=\left[\begin{array}{ccc}{\text{I}}_6& 0& 0\\ 0& \text{I}+{\left[\begin{array}{c}\frac{1}{2}\widehat{\delta \mathbf{\theta }}\}\end{array}\right]}_{\times }& 0\\ 0& 0& {\text{I}}_9\end{array}\right]$$