EKF 算法详解

对于高斯白噪声的非线性系统：

\[ x_{k + 1} = f(x_k) + w_k\\ z_k = h(x_k) + v_k \]

其中\(x_k\)为状态向量，\(z_k\)为量测向量，\(f(.), h(.)\)分别为系统非线性状态函数和量测函数，\(w_k,v_k\)分别为零均值，协方差为\(Q_k,R_k\)的高斯白噪声。

假设已知k时刻的状态估计值\(\hat {x_{k|k}}\)和协方差\(P_{k|k}\),将非线性函数\(f(x_k)\)在\(\hat{x_{k|k}}\)处一阶泰勒展开，得：

\[ f(x_k) = f(\hat{x_{k|k}}) + \frac{\partial f}{\partial x_k} |_{x_k = \hat{x_{k|k}}} (x_k - \hat{x_{k|k}}) + o(x_k - \hat{x_{k|k}}) \]

其中\(o(x_k - \hat{x_{k|k}})\)为高阶项，可以忽略不计，定义\(\frac{\partial f}{\partial x_k} |_{x_k = \hat{x_{k|k}}} = F_k\),则，状态方程简化为：

\[ f(x_k) = f(\hat{x_{k|k}}) + F_x(x_k - \hat{x_{k|k}}) + w_k \]

进行一次状态预测，得到：

\[ \hat{x}_{k+1 \mid k}=\mathbf{E}\left[f\left(\hat{x}_{k \mid k}\right)+F_{k}\left(x_{k}-\hat{x}_{k \mid k}\right)+w_{k}\right]=f\left(\hat{x}_{k \mid k}\right) \]

\[ \begin{aligned} P_{k+1 \mid k} &=\mathbf{E}\left[\left(x_{k+1}-\hat{x}_{k+1 \mid k}\right)\left(x_{k+1}-\hat{x}_{k+1 \mid k}\right)^{\mathrm{T}}\right] \\ &=\mathbf{E}\left\{\left[F_{k}\left(x_{k}-\hat{x}_{k \mid k}\right)+w_{k}\right]\left[F_{k}\left(x_{k}-\hat{x}_{k \mid k}\right)+w_{k}\right]^{\mathrm{T}}\right\} \\ &=F_{k} P_{k \mid k} F_{k}^{\mathrm{T}}+\mathrm{Q}_{\mathrm{k}} \end{aligned} \]

将非线性函数\(h(.)\)在状态预测\(\hat{x_{k+1|k}}\)处一阶泰勒展开，得：

\[ \boldsymbol{h}\left(\boldsymbol{x}_{k+1}\right)=\boldsymbol{h}\left(\hat{\boldsymbol{x}}_{k+1 \mid k}\right)+\left.\frac{\partial \boldsymbol{h}}{\partial x_{k+1}}\right|_{x_{k+1}=\hat{x}_{k+1| k}}\left(x_{k+1}-\hat{\boldsymbol{x}}_{k+1 \mid k}\right)+\boldsymbol{o}\left(\boldsymbol{x}_{k+1}-\hat{\boldsymbol{x}}_{k+1 \mid k}\right) \]

忽略高阶项，简化为：

\[ z_{k+1}=\boldsymbol{h}\left(\hat{x}_{k+1 \mid k}\right)+\boldsymbol{H}_{k+1}\left(x_{k+1}-\hat{\boldsymbol{x}}_{k+1 \mid k}\right)+\boldsymbol{v}_{k+1} \]

进行一次量测更新：均值：

\[ \hat{z}_{k+1}=\mathrm{E}\left[\boldsymbol{h}\left(\hat{x}_{k+1 \mid k}\right)+H_{k+1}\left(x_{k+1}-\hat{x}_{k+1 \mid k}\right)+v_{k+1}\right]=h\left(\hat{x}_{k+1 \mid k}\right) \]

量测预测误差协方差矩阵：

\[ \begin{aligned} \boldsymbol{P}_{z z, k+1 \mid k} &=\mathbf{E}\left[\left(z_{k+1}-\hat{\boldsymbol{z}}_{k+1}\right)\left(z_{k+1}-\hat{\boldsymbol{z}}_{k+1}\right)^{\mathrm{T}}\right] \\ &=\mathbf{E}\left\{\left[\boldsymbol{H}_{k+1}\left(\boldsymbol{x}_{k+1}-\hat{\boldsymbol{x}}_{k+1 \mid k}\right)+\boldsymbol{v}_{k+1}\right]\left[\boldsymbol{H}_{k+1}\left(\boldsymbol{x}_{k+1}-\hat{\boldsymbol{x}}_{k+1 \mid k}\right)+\boldsymbol{v}_{k+1}\right]^{\mathrm{T}}\right\} \\ &=\boldsymbol{H}_{k+1} \boldsymbol{P}_{k+1 \mid k} \boldsymbol{H}_{k+1}^{\mathrm{T}}+\boldsymbol{R}_{k+1} \end{aligned} \]

状态与量测间协方差矩阵：

\[ \begin{aligned} P_{x z, k+1 \mid k} &=\mathbf{E}\left[\left(\boldsymbol{x}_{k+1}-\hat{\boldsymbol{x}}_{k+1}\right)\left(z_{k+1}-\hat{z}_{k+1}\right)^{\mathrm{T}}\right] \\ &=\mathbf{E}\left\{\left(\boldsymbol{x}_{k+1}-\hat{\boldsymbol{x}}_{k+1}\right)\left[\boldsymbol{H}_{k+1}\left(\boldsymbol{x}_{k+1}-\hat{\boldsymbol{x}}_{k+1 \mid k}\right)+\boldsymbol{v}_{k+1}\right]^{\mathrm{T}}\right\} \\ &=\boldsymbol{P}_{k+1 \mid k} \boldsymbol{H}_{k+1}^{\mathrm{T}} \end{aligned} \]

状态增益矩阵：

\[ K_{k+1}=P_{x z, k+1 \mid k}\left(P_{z z, k+1 \mid k}\right)^{-1}=P_{k+1 \mid k} H_{k+1}^{\mathrm{T}}\left(H_{k+1} P_{k+1 \mid k} H_{k+1}^{\mathrm{T}}+R_{k+1}\right)^{-1} \]

k+1时刻状态估计值：

\[ \hat{\boldsymbol{x}}_{k+1 \mid \mathrm{k}+1}=\hat{\boldsymbol{x}}_{k+1 \mid \mathrm{k}}+K_{k+1}\left(\mathrm{z}_{\mathrm{k}+1}-\hat{\mathbf{z}}_{\mathrm{k}+1 \mid \mathrm{k}}\right) \]

状态估计协方差矩阵：

\[ \begin{aligned} P_{k+1 \mid k+1} &=\mathrm{E}\left[\left(x_{k+1}-\hat{x}_{k+1 \mid \mathrm{k}+1}\right)\left(x_{k+1}-\hat{x}_{k+1 \mid \mathrm{k}+1}\right)^{\mathrm{T}}\right] \\ &=\mathrm{E}\left\{\left[x_{k+1}-\hat{x}_{k+1 \mid \mathrm{k}}-K_{k+1}\left(\mathbf{z}_{\mathrm{k}+1}-\hat{\mathrm{z}}_{\mathrm{k}+1 |\mathrm{k}}\right)\right]\left[x_{k+1}-\hat{x}_{k+1 \mid \mathrm{k}}-K_{k+1}\left(\mathbf{z}_{\mathrm{k}+1}-\hat{\mathbf{z}}_{\mathrm{k}+1 \mid \mathrm{k}}\right)\right]^{\mathrm{T}}\right\} \\ &=\left(I-K_{k+1} H_{k+1}\right) P_{k+1 \mid \mathrm{k}}\left(I-K_{k+1} H_{k+1}\right)^{T}+K_{k+1} R_{k+1} K_{k+1}^{T} \\ &=\left(I-K_{k+1} H_{k+1}\right) P_{k+1 |\mathrm{k}} \end{aligned} \]

ESKF 算法详解

设ESKF真值状态为：\(\mathbf{x}_t = [\mathbf{p}_t, \mathbf{v}_t, \mathbf{R}_t, \mathbf{b}_{at}, \mathbf{b}_{gt}, \mathbf{g}_t]^\mathrm{T}\), IMU观测值为\(\tilde{\boldsymbol{\omega}}, \tilde{\mathbf{a}}\)。可以写出状态变量与观测之间的关系：

\[ \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 \ \left( \tilde{\boldsymbol{\omega}} - \mathbf{b}_{gt} - \boldsymbol{\eta}_g \right)^\wedge \\ \dot{\mathbf{b}}_{gt} & = \boldsymbol{\eta}_{bg} \\ \dot{\mathbf{b}}_{at} & = \boldsymbol{\eta}_{ba} \\ \dot{\mathbf{g}} &= \mathbf{0} \end{aligned} \]

定义误差状态为：

\[ \delta \mathbf{x} = \begin{bmatrix} \delta \mathbf{p} \\ \delta \mathbf{v} \\ \delta \boldsymbol{\theta} \\ \delta \mathbf{b}_a \\ \delta \mathbf{b}_g \end{bmatrix} \in \mathbb{R}^{15 \times 1} \]

那么error状态和nomal状态可以表示为：

\[ \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{或} \ \mathbf{q}_t = \mathbf{q} \delta \mathbf{q} \quad \text{或} \mathbf{R}_t = \mathbf{R} \mathrm{Exp}(\delta \boldsymbol{\theta}) \\ \mathbf{b}_{gt} &= \mathbf{b}_g + \delta \mathbf{b}_g \\ \mathbf{b}_{at} &= \mathbf{b}_a + \delta \mathbf{b}_a \\ \mathbf{g}_t &= \mathbf{g} + \delta \mathbf{g} \end{aligned} \]

关于误差的导数，可以得到：

\[ \begin{aligned} \delta \dot{\mathbf{p}} &= \delta \mathbf{v} \\ \delta \dot{\mathbf{v}} &= - \mathbf{R}(\tilde{\mathbf{a}} - \mathbf{b}_a)^\wedge \delta \boldsymbol{\theta} - \mathbf{R} \delta \mathbf{b}_a - \boldsymbol{\eta}_a + \delta \mathbf{g} \\ \delta \dot{\boldsymbol{\theta}} &\approx -(\tilde{\boldsymbol{\omega}} - \mathbf{b}_g)^\wedge \delta \boldsymbol{\theta} - \delta \mathbf{b}_g - \boldsymbol{\eta}_g \\ \delta \dot{\mathbf{b}_g} &= \boldsymbol{\eta}_g \\ \delta \dot{\mathbf{b}_a} &= \boldsymbol{\eta}_a \\ \delta \mathbf{g} &= \mathbf{0} \end{aligned} \]

误差状态位置项推导

\[ \dot{\delta p_t} = \dot{p_t} - \dot{p} = v_t -v = \delta v \]

误差状态加速度bias项推导

\[ \dot{\delta b_a} = \dot{b_{at}} - \dot{b_a} = \boldsymbol{\eta}_a \]

误差状态角速度bias项推导

\[ \dot{\delta b_g} = \dot{b_{gt}} - \dot{b_g} = \boldsymbol{\eta}_g \]

误差状态的速度项推导

对两侧求时间导数，就可以得到\(\delta \dot{\mathbf{v}}\)的表达式，等式左侧：

\[ \begin{aligned} \dot{\mathbf{v}}_t &= \mathbf{R}_t(\tilde{\mathbf{a}} - \mathbf{b}_{at} - \boldsymbol{\eta}_a) + \mathbf{g}_t \\ &= \mathbf{R} \mathrm{Exp}(\delta \boldsymbol{\theta}) (\tilde{\mathbf{a}} - \mathbf{b}_a - \delta \mathbf{b}_a - \boldsymbol{\eta}_a ) + \mathbf{g} + \delta \mathbf{g} \\ &\approx \mathbf{R} (\mathbf{I} + \delta \boldsymbol{\theta}^\wedge ) (\tilde{\mathbf{a}} - \mathbf{b}_a - \delta \mathbf{b}_a - \boldsymbol{\eta}_a) + \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} \boldsymbol{\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} \]

从第三行推向第四行时，需要忽略\(\delta \boldsymbol{\theta}^\wedge\)与\(\delta \mathbf{b}_a, \boldsymbol{\eta}_a\)相乘的二阶小量。从第四行推第五行则用到了叉乘符号交换顺序之后需加负号的性质。等式右侧为：

\[ \dot{\mathbf{v}} + \delta \dot{\mathbf{v}} = \mathbf{R}(\tilde{\mathbf{a}} - \mathbf{b}_a) + \mathbf{g} + \delta \dot{\mathbf{v}} \]

两式相等，得到：

\[ \delta \dot{\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} \]

这样就得到了\(\delta \mathbf{v}\)的运动学，由于\(\boldsymbol{\eta}_a\)是零均值白噪声，它乘上任意旋转矩阵之后仍然是一个零均值白噪声，\(\mathbf{R}^\mathrm{T} \mathbf{R} = \mathbf{I}\),其协方差矩阵也不变，可以简化得到：

\[ \delta \dot{\mathbf{v}} = - \mathbf{R}(\tilde{\mathbf{a}} - \mathbf{b}_a)^\wedge \delta \boldsymbol{\theta} - \mathbf{R} \delta \mathbf{b}_a - \boldsymbol{\eta}_a + \delta \mathbf{g} \]

误差状态的旋转项推导

对旋转项两边对时间求导，得：

\[ \begin{aligned} \dot{\mathbf{R}}_t &= \dot{\mathbf{R}} \mathrm{Exp} (\delta \boldsymbol{\theta}) + \mathbf{R} \dot{\mathrm{Exp}(\delta \boldsymbol{\theta})} \\ &= \mathbf{R}_t \left( \tilde{\boldsymbol{\omega}} - \mathbf{b}_{gt} - \boldsymbol{\eta}_g \right) ^\wedge \end{aligned} \]

其中：

\[ \dot{\mathrm{Exp}(\delta \boldsymbol{\theta})} = \mathrm{Exp}(\delta \boldsymbol{\theta}) \delta \dot{\boldsymbol{\theta}}^\wedge. \]

可得到：

\[ \begin{aligned} \dot{\mathbf{R}} \mathrm{Exp} (\delta \boldsymbol{\theta}) + \mathbf{R} \dot{\mathrm{Exp}(\delta \boldsymbol{\theta})} &= \mathbf{R} (\tilde{\boldsymbol{\omega}}-\mathbf{b}_g)^\wedge \mathrm{Exp}(\delta \boldsymbol{\theta}) + \mathbf{R} \mathrm{Exp}(\delta \boldsymbol{\theta} ) \delta \dot{\boldsymbol{\theta}}^\wedge \\ \end{aligned} \]

由于：

\[ \begin{aligned} \mathbf{R}_t \left( \tilde{\boldsymbol{\omega}} - \mathbf{b}_{gt} - \boldsymbol{\eta}_g \right)^\wedge &= \mathbf{R} \mathrm{Exp} (\delta \boldsymbol{\theta}) \left( \tilde{\boldsymbol{\omega}} - \mathbf{b}_{gt} - \boldsymbol{\eta}_g \right)^\wedge \\ \end{aligned} \]

将\(\delta \dot{\boldsymbol{\theta}}\)移到一侧，约掉两侧左边的\(\mathbf{R}\),得到：

其中\(\mathrm{Exp}(\delta \boldsymbol{\theta})\)是一个\(\mathrm{SO}(3)\)矩阵，利用伴随性质：

\[ \boldsymbol{\phi}^\wedge \mathbf{R} = \mathbf{R} (\mathbf{R}^\mathrm{T} \boldsymbol{\phi})^\wedge \]

得到：

\[ \begin{aligned} \mathrm{Exp} (\delta \boldsymbol{\theta}) \delta \dot{\boldsymbol{\theta}}^\wedge &= \mathrm{Exp} (\delta \boldsymbol{\theta}) \left( \tilde{\boldsymbol{\omega}} - \mathbf{b}_{gt} - \boldsymbol{\eta}_g \right)^\wedge - \mathrm{Exp} (\delta \boldsymbol{\theta}) \left( \mathrm{Exp} (-\delta \boldsymbol{\theta}) (\tilde{\boldsymbol{\omega}}-\mathbf{b}_g) \right)^\wedge \\ &= \mathrm{Exp} (\delta \boldsymbol{\theta}) \left[ (\tilde{\boldsymbol{\omega}} - \mathbf{b}_{gt} - \boldsymbol{\eta}_g)^\wedge - (\mathrm{Exp} (-\delta \boldsymbol{\theta}) (\tilde{\boldsymbol{\omega}}-\mathbf{b}_g) )^\wedge \right] \\ &\approx \mathrm{Exp} (\delta \boldsymbol{\theta}) \left[ (\tilde{\boldsymbol{\omega}} - \mathbf{b}_{gt} - \boldsymbol{\eta}_g)^\wedge - \left((\mathbf{I} - \delta \boldsymbol{\theta}^\wedge)(\tilde{\boldsymbol{\omega}}-\mathbf{b}_g )\right)^\wedge \right] \\ &= \mathrm{Exp} (\delta \boldsymbol{\theta}) \left[ \mathbf{b}_g - \mathbf{b}_{gt} -\boldsymbol{\eta}_g + \delta \boldsymbol{\theta}^\wedge \tilde{\boldsymbol{\omega}} - \delta \boldsymbol{\theta}^\wedge \mathbf{b}_{g} \right]^\wedge \\ &= \mathrm{Exp} (\delta \boldsymbol{\theta}) \left[ (-\tilde{\boldsymbol{\omega}}+\mathbf{b}_g)^\wedge \delta \boldsymbol{\theta} - \delta \mathbf{b}_g - \boldsymbol{\eta}_g \right]^\wedge \end{aligned} \]

约掉等式左侧的系数,得：

\[ \delta \dot{\boldsymbol{\theta}} \approx -(\tilde{\boldsymbol{\omega}} - \mathbf{b}_g)^\wedge \delta \boldsymbol{\theta} - \delta \mathbf{b}_g - \boldsymbol{\eta}_g \]

对于离散时间状态方程，nomal状态运动学方程可以写为：

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

对应的误差状态方程可写为：

\[ \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}(\tilde{\mathbf{a}} - \mathbf{b}_a)^\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) &= \mathrm{Exp}\left( -(\tilde{\boldsymbol{\omega}} - \mathbf{b}_g) \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} \]

噪声项并不参与递推，需要把它们单独归入噪声部分中。连续时间的噪声项可以视为随机过程的能量谱密度，而离散时间下的噪声变量就是我们日常看到的随机变量了。这些噪声随机变量的标准差可以列写如下：

\[ \begin{aligned} \sigma(\boldsymbol{\eta}_v) = \Delta t \sigma_a, \quad \sigma(\boldsymbol{\eta}_{\theta}) = \Delta t \sigma_{g}, \quad \sigma(\boldsymbol{\eta}_g) = \sqrt{\Delta t} \sigma_{bg}, \quad \sigma(\boldsymbol{\eta}_a) = \sqrt{\Delta t} \sigma_{ba} \end{aligned} \]

ESKF预测过程

误差状态\(\delta \mathbf{x}\)离散时间运动方程已经给出，整体记为：

\[ \delta \mathbf{x} = f(\delta \mathbf{x}) + \mathbf{w}, \mathbf{w} \sim \mathcal{N}(0, \mathbf{Q}) \]

其中\(\mathbf{w}\)为噪声，\(\mathbf{Q}\)为：

\[ \mathbf{Q} = \mathrm{diag}(\mathbf{0}_3, \mathrm{Cov}(\boldsymbol{\eta}_v), \mathrm{Cov}(\boldsymbol{\eta}_{\theta}), \mathrm{Cov}(\boldsymbol{\eta}_{g}), \mathrm{Cov}(\boldsymbol{\eta}_{a}), \mathbf{0}_3) \]

由于第一个和最后一个方程本身没有噪声,所以为零。

线性化，得：

\[ \delta \mathbf{x} = \mathbf{F_x} \delta \mathbf{x} + \mathbf{F_i}\mathbf{w} \]

其中\(\mathbf{F}\)为线性化后的雅可比举证，由于运动方程已经是线性化的，只需要拼接成矩阵即可。

\[ \mathbf{F_x} = \left.\frac{\partial f}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}, \mathbf{u}_{m}} = \begin{bmatrix} \mathbf{I} & \mathbf{I} \Delta t & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{I} & - \mathbf{R}(\tilde{\mathbf{a}} - \mathbf{b}_a)^\wedge \Delta t & -\mathbf{R} \Delta t & \mathbf{0} & \mathbf{I} \Delta t \\ \mathbf{0} & \mathbf{0} & \mathrm{Exp}\left( -(\tilde{\boldsymbol{\omega}} - \mathbf{b}_g) \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{bmatrix} \]

\[ \mathbf{F}_{\mathbf{i}}= \left.\frac{\partial f}{\partial \mathbf{i}}\right|_{\mathbf{x}, \mathbf{u}_{m}}= \left[\begin{array}{llll} 0 & 0 & 0 & 0 \\ \mathbf{I} & 0 & 0 & 0 \\ 0 & \mathbf{I} & 0 & 0 \\ 0 & 0 & \mathbf{I} & 0 \\ 0 & 0 & 0 & \mathbf{I} \\ 0 & 0 & 0 & 0 \end{array}\right] \]

\[ \mathbf{Q}_{\mathbf{i}}= \left[\begin{array}{cccc} \mathbf{V}_{\mathbf{i}} & 0 & 0 & 0 \\ 0 & \boldsymbol{\Theta}_{\mathbf{i}} & 0 & 0 \\ 0 & 0 & \mathbf{A}_{\mathbf{i}} & 0 \\ 0 & 0 & 0 & \Omega_{\mathbf{i}} \end{array}\right] \]

均值和协方差预测如下：

\[ \begin{aligned} \delta \mathbf{x}_{\mathrm{pred}} &= \mathbf{F} \delta \mathbf{x} \\ \mathbf{P}_{\mathrm{pred}} &= \mathbf{F} \mathbf{P} \mathbf{F}^\mathrm{T} + \mathbf{F_i}\mathbf{Q} \mathbf{F_i}^{\mathrm{T}} \end{aligned} \]

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

ESKF修正过程

假设一个抽象的传感器能够对状态变量产生观测，其观测方程为抽象的\(h\),可写为：

\[ \mathbf{z} = h(\mathbf{x}) + \mathbf{v}, \mathbf{v} \sim \mathcal{N}(0, \mathbf{V}) \]

其中\(\mathbf{z}\)为观测数据，\(\mathbf{v}\)为观测噪声，\(\mathbf{V}\)为该噪声的协方差矩阵。

在传统EKF中，我们可以直观对观测方程线性化，求出观测方程相对于状态变量的雅可比矩阵，进而更新卡尔曼滤波器。而在ESKF中，我们当前拥有名义状态\(\mathbf{x}\)的估计以及误差状态\(\mathbf{\delta x}\)的估计，且希望更新的是误差状态，因此要计算观测方程相比于误差状态的雅可比矩阵：

\[ \mathbf{H} = \frac{\partial h}{\partial \delta \mathbf{x}}, \]

卡尔曼增益，均值和协方差更新如下：

\[ \begin{aligned} \mathbf{K} &= \mathbf{P}_{\mathrm{pred}} \mathbf{H}^\mathrm{T}(\mathbf{H} \mathbf{P}_{\mathrm{pred}} \mathbf{H}^\mathrm{T} + \mathbf{V})^{-1} \\ \delta \mathbf{x} &= \mathbf{K} (\mathbf{z} - h(\mathbf{x}_t)) \\ \mathbf{P} &= (\mathbf{I} - \mathbf{K} \mathbf{H}) \mathbf{P}_{\mathrm{pred}} \end{aligned} \]

其中\(\mathbf{K}\)为卡尔曼增益，\(\mathbf{P_{pred}}\)为预测的协方差矩阵，最后的\(\mathbf{P}\)为修正后的协方差矩阵. 其中\(\mathbf{H}\)的计算可以通过链式法则来生成：

\[ \mathbf{H} = \frac{\partial h}{\partial \mathbf{x}} \frac{\partial \mathbf{x}}{\partial \delta \mathbf{x}} \]

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

\[ \begin{aligned} \frac{\partial \mathbf{x}}{\partial \delta \mathbf{x}} &= \mathrm{diag}(\mathbf{I}_3, \mathbf{I}_3, \frac{\partial \mathrm{Log} (\mathbf{R}(\mathrm{Exp}(\delta \boldsymbol{\theta})))}{\partial \delta \boldsymbol{\theta}}, \mathbf{I}_3, \mathbf{I}_3, \mathbf{I}_3) \\ &=\left[\begin{array}{ccccc} \frac{\partial(\mathbf{p}+\delta \mathbf{p})}{\partial \delta \mathbf{p}} & 0 & 0 & 0 & 0 & 0\\ 0 & \frac{\partial(\mathbf{v}+\delta \mathbf{v})}{\partial \delta \mathbf{v}} & 0 & 0 & 0 & 0 \\ 0 & 0 & \frac{\partial( \mathbf{R}(\mathrm{Exp}(\delta \boldsymbol{\theta})) )}{\partial \delta \theta} & 0 & 0 & 0 \\ 0 & 0 & 0 & \frac{\partial\left(\mathbf{a}_{b}+\delta \mathbf{a}_{b}\right)}{\partial \delta \mathbf{a}_{b}} & 0 & 0 \\ 0 & 0 & 0 & 0 & \frac{\partial\left(\boldsymbol{\omega}_{b}+\delta \omega_{b}\right)}{\partial \delta \omega_{b}} & 0\\ 0& 0 & 0 & 0 & 0 & \frac{\partial(\mathbf{g}+\delta \mathbf{g})}{\partial \delta \mathbf{g}} \end{array}\right] \end{aligned} \]

对于旋转部分，\(\delta\mathbf{\theta}\)定义为\(\mathbf{R}\)的右乘，使用右乘BCH可得：

\[ \begin{aligned} \frac{\partial \mathrm{Log} (\mathbf{R}(\mathrm{Exp}(\delta \boldsymbol{\theta})))}{\partial \delta \boldsymbol{\theta}} = \mathbf{J}_r^{-1} (\mathbf{R}) \end{aligned} \]

在经过预测和更新过程之后，我们修正了误差状态的估计。接下来，只需把误差状态归入nomal状态，然后重置滤波器：

\[ \mathbf{x}_{k+1} = \mathbf{x}_k \oplus \delta \mathbf{x}_{k} \]

即：

\[ \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 \mathrm{Exp}(\delta \boldsymbol{\theta}_k) \\ \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} \]

Reference

[1] Quaternion kinematics for the error-state Kalman filter
[2] https://github.com/MapIV/eagleye
[3] https://ahrs.readthedocs.io/en/latest/filters/ekf.html
[4] https://zhuanlan.zhihu.com/p/441182819