误差状态卡尔曼滤波器推导

Error State Kalman Filter

前置知识：卡尔曼滤波器详细推导

1、误差状态卡尔曼滤波器

误差状态卡尔曼滤波器（ESKF)全称误差状态扩展卡尔曼滤波器（ESEKF），其将IMU视为运动模型，将其他传感器（如RTK，轮速计等）视为观测模型，并将状态变量的真值分为名义状态变量和误差状态变量，在滤波过程中，仅对误差状态变量进行预测和更新。

设ESKF的真值状态变量为${\mathbf{x}}_t=[{\mathbf{p}}_t,{\mathbf{v}}_t,{\mathbf{R}}_t,{\mathbf{b}}_{at},{\mathbf{b}}_{gt},{\mathbf{g}}_t]$（为简便起见，将旋转矩阵也写入向量，下同），下标$t$表示true，名义状态变量为$\mathbf{x}=[\mathbf{p},\mathbf{v},\mathbf{R},{\mathbf{b}}_a,{\mathbf{b}}_g,\mathbf{g}]$，误差状态变量为$\delta \mathbf{x}=[\delta \mathbf{p},\delta \mathbf{v},\delta \mathbf{\theta },\delta {\mathbf{b}}_a,\delta {\mathbf{b}}_g,\delta \mathbf{g}]$，三者各自内部元素依次表示平移，速度，旋转矩阵（误差状态变量采用误差旋转向量），加速度计零偏，陀螺仪零偏，重力。记加速度计读数为$\tilde{\mathbf{a}}$，IMU陀螺仪读数为$\tilde{\mathbf{\omega }}$，加速度计测量噪声为${\mathbf{\eta }}_a$，其零偏的导数为${\mathbf{\eta }}_{ba}$，陀螺仪测量噪声为${\mathbf{\eta }}_b$，其计零偏的导数为${\mathbf{\eta }}_{bg}$，上述测量噪声和零偏的导数均为白噪声高斯过程。

真值状态的运动学方程为
$$\begin{array}{rl}{\dot{\mathbf{p}}}_t& ={\mathbf{v}}_t\\ {\dot{\mathbf{v}}}_t& ={\mathbf{R}}_t(\tilde{\mathbf{a}}-{\mathbf{b}}_{at}-{\mathbf{\eta }}_a)+{\mathbf{g}}_t\\ {\dot{\mathbf{R}}}_t& ={\mathbf{R}}_t(\tilde{\mathbf{\omega }}-{\mathbf{b}}_{gt}-{\mathbf{\eta }}_g{)}^{\wedge }\\ {\dot{\mathbf{b}}}_{at}& ={\mathbf{\eta }}_{{\mathbf{b}}_a}\\ {\dot{\mathbf{b}}}_{gt}& ={\mathbf{\eta }}_{{\mathbf{b}}_g}\\ {\dot{\mathbf{g}}}_t& =0\end{array}\text{\hspace{1em}(1a)(1b)(1c)(1d)(1e)(1f)}$$
在名义变量的运动学方程与真值状态运动学方程相似，只是在该方程中不必考虑噪声，因为噪声会在误差状态方程中考虑，该方程为
$$\begin{array}{rl}\dot{\mathbf{p}}& =\mathbf{v}\\ \dot{\mathbf{v}}& =\mathbf{R}(\tilde{\mathbf{a}}-{\mathbf{b}}_a)+\mathbf{g}\\ \dot{\mathbf{R}}& =\mathbf{R}(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g{)}^{\wedge }\\ {\dot{\mathbf{b}}}_a& =0\\ {\dot{\mathbf{b}}}_g& =0\\ \dot{\mathbf{g}}& =0\end{array}\text{\hspace{1em}(2a)(2b)(2c)(2d)(2e)(2f)}$$
真值状态变量，名义状态变量，误差状态变量三者之间满足关系${\mathbf{x}}_t=\mathbf{x}\oplus \delta \mathbf{x}$，其中$\oplus$为广义加法，该关系也是误差状态的定义，该式可展开写为
$$\begin{array}{rl}{\mathbf{p}}_t& =\mathbf{p}+\delta \mathbf{p}\\ {\mathbf{v}}_t& =\mathbf{v}+\delta \mathbf{v}\\ {\mathbf{R}}_t& =\mathbf{R}\mathrm{Exp}(\delta \mathbf{\theta })\\ {\mathbf{b}}_{at}& ={\mathbf{b}}_a+\delta {\mathbf{b}}_a\\ {\mathbf{b}}_{gt}& ={\mathbf{b}}_g+\delta {\mathbf{b}}_g\\ {\mathbf{g}}_t& =\mathbf{g}+\delta \mathbf{g}\end{array}\text{\hspace{1em}(3a)(3b)(3c)(3d)(3e)(3f)}$$
由$(1),(2),(3)$通过变形和约简可以得到误差状态变量的运动学方程，为方便后续推导，此处将约等于号写成等于号，下同
$$\begin{array}{rl}\dot{\delta \mathbf{p}}& =\delta \mathbf{v}\\ \dot{\delta \mathbf{v}}& =-\mathbf{R}(\tilde{\mathbf{a}}-{\mathbf{b}}_a{)}^{\wedge }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{b}}_a-{\mathbf{\eta }}_a+\delta \mathbf{g}\\ \dot{\delta \mathbf{\theta }}& =-(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g{)}^{\wedge }\delta \mathbf{\theta }-\delta {\mathbf{b}}_g-{\mathbf{\eta }}_g\\ {\dot{\delta \mathbf{b}}}_a& ={\mathbf{\eta }}_{{\mathbf{b}}_a}\\ {\dot{\delta \mathbf{b}}}_g& ={\mathbf{\eta }}_{{\mathbf{b}}_g}\\ \dot{\delta \mathbf{g}}& =0\end{array}\text{\hspace{1em}(4a)(4b)(4c)(4d)(4e)(4f)}$$
因为实际是在离散时间进行采样，故需要将名义状态变量和误差状态变量对应的运动学方程离散化。在$[t,t+\Delta t]$这段非常短的时间段内，可以假定加速度和角速度不变，恒为$t$时刻的取值，在该假定下，名义状态变量运动学方程$(2)$可离散化为（省略等号右侧所有的$(t)$以化简公式）
$$\begin{array}{rl}\mathbf{p}(t+\Delta t)& =\mathbf{p}+\mathbf{v}\Delta t+\frac{1}{2}(\mathbf{R}(\tilde{\mathbf{a}}-{\mathbf{b}}_a))\Delta t^2+\frac{1}{2}\mathbf{g}\Delta t^2\\ \mathbf{v}(t+\Delta t)& =\mathbf{v}+\mathbf{R}(\tilde{\mathbf{a}}-{\mathbf{b}}_a)\Delta t+\mathbf{g}\Delta t\\ \mathbf{R}(t+\Delta t)& =\mathbf{R}\mathrm{Exp}((\tilde{\mathbf{\omega }}-{\mathbf{b}}_g)\Delta t)\\ {\mathbf{b}}_a(t+\Delta t)& ={\mathbf{b}}_a\\ {\mathbf{b}}_g(t+\Delta t)& ={\mathbf{b}}_g\\ \mathbf{g}(t+\Delta t)& =\mathbf{g}\end{array}\text{\hspace{1em}(5a)(5b)(5c)(5d)(5e)(5f)}$$
误差状态变量运动学方程$(4)$可离散化为（同样省略等号右侧所有的$(t)$以化简公式）
$$\begin{array}{rl}\delta \mathbf{p}(t+\Delta t)& =\delta \mathbf{p}+\delta \mathbf{v}\Delta t\\ \delta \mathbf{v}(t+\Delta t)& =\delta \mathbf{v}+(-\mathbf{R}(\tilde{\mathbf{a}}-{\mathbf{b}}_a{)}^{\wedge }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{b}}_a+\delta \mathbf{g})\Delta t-{\mathbf{w}}_v\\ \delta \mathbf{\theta }(t+\Delta t)& =\mathrm{Exp}(-(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g)\Delta t)\delta \mathbf{\theta }-\delta {\mathbf{b}}_g\Delta t-{\mathbf{w}}_{\theta }\\ \delta {\mathbf{b}}_a(t+\Delta t)& =\delta {\mathbf{b}}_a+{\mathbf{w}}_a\\ \delta {\mathbf{b}}_g(t+\Delta t)& =\delta {\mathbf{b}}_g+{\mathbf{w}}_g\\ \delta \mathbf{g}(t+\Delta t)& =\delta \mathbf{g}\end{array}\text{\hspace{1em}(6a)(6b)(6c)(6d)(6e)(6f)}$$
${\mathbf{w}}_{\mathbf{v}},{\mathbf{w}}_{\mathbf{\theta }},{\mathbf{w}}_{{\mathbf{b}}_a},{\mathbf{w}}_{{\mathbf{b}}_g}$分别为白噪声高斯随机过程${\mathbf{\eta }}_a,{\mathbf{\eta }}_g,{\mathbf{\eta }}_{{\mathbf{b}}_a},{\mathbf{\eta }}_{{\mathbf{b}}_g}$的积分，即表示维纳过程，则上述维纳过程的标准差为
$$\begin{array}{llc}& \sigma ({\mathbf{w}}_{\mathbf{v}})=\Delta t{\sigma }_k({\mathbf{\eta }}_a)=\sqrt{\Delta t}\sigma ({\mathbf{\eta }}_a)& \sigma ({\mathbf{w}}_{\mathbf{\theta }})=\Delta t{\sigma }_k({\mathbf{\eta }}_g)=\sqrt{\Delta t}\sigma ({\mathbf{\eta }}_g)\\ & \sigma ({\mathbf{w}}_{{\mathbf{b}}_a})={\sigma }_k({\mathbf{b}}_a)=\sqrt{\Delta t}\sigma ({\mathbf{b}}_a)& \sigma ({\mathbf{w}}_{{\mathbf{b}}_g})={\sigma }_k({\mathbf{b}}_g)=\sqrt{\Delta t}\sigma ({\mathbf{b}}_g)\end{array}$$
记$t_k=t$，$t_{k+1}=t+\Delta t$，将$\mathbf{u}=[\tilde{\mathbf{a}},\tilde{\mathbf{\omega }}{]}^T$视为输入，$(6)$可以整体地写成
$$\delta {\mathbf{x}}_{k+1}=\mathbf{f}(\delta {\mathbf{x}}_k,{\mathbf{x}}_k,{\mathbf{u}}_{k+1})+{\mathbf{w}}_{k+1},{\mathbf{w}}_{k+1}\sim \mathcal{N}(0,{\mathbf{Q}}_{k+1})\text{\hspace{1em}(7)}$$
$\mathbf{w}$为随机游走噪声，其分布与时间步无关，下标只起标记时间步的作用，按照前面的定义，其协方差矩阵为
$${\mathbf{Q}}_{k+1}=\mathrm{diag}({\mathbf{O}}_{3\times 3},\mathbf{\Sigma }({\mathbf{w}}_v),\mathbf{\Sigma }({\mathbf{w}}_{\theta }),\mathbf{\Sigma }({\mathbf{w}}_{{\mathbf{b}}_a}),\mathbf{\Sigma }({\mathbf{w}}_{{\mathbf{b}}_g}),{\mathbf{O}}_{3\times 3})$$
将$(7)$中的预测方程在$(0,{\hat{\mathbf{x}}}_k,{\mathbf{u}}_{k+1})$处展开，得到
$$\delta {\mathbf{x}}_{k+1}\approx \mathbf{f}(0,{\hat{\mathbf{x}}}_k,{\mathbf{u}}_{k+1})+{\frac{\partial \mathbf{f}(\delta {\mathbf{x}}_k,{\mathbf{x}}_k,{\mathbf{u}}_{k+1})}{\partial \delta {\mathbf{x}}_k}|}_{0,{\hat{\mathbf{x}}}_k,{\mathbf{u}}_{k+1}}\delta {\mathbf{x}}_k+{\mathbf{w}}_{k+1}$$
上式第一项，代入$(6)$可知其值为$0$，记第二项中偏导数矩阵为${\mathbf{F}}_{k+1}$，由$(21)$可以得到
$${\mathbf{F}}_{k+1}=\left[\begin{array}{cccccc}\mathbf{I}& \mathbf{I}\Delta t& \mathbf{O}& \mathbf{O}& \mathbf{O}& \mathbf{O}\\ \mathbf{O}& \mathbf{I}& -\mathbf{R}(\tilde{\mathbf{a}}-{\mathbf{b}}_a{)}^{\wedge }\Delta t& \mathbf{O}& -\mathbf{R}\Delta t& \mathbf{I}\Delta t\\ \mathbf{O}& \mathbf{O}& \mathrm{Exp}(-(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g)\Delta t)& -\mathbf{I}\Delta t& \mathbf{O}& \mathbf{O}\\ \mathbf{O}& \mathbf{O}& \mathbf{O}& \mathbf{I}& \mathbf{O}& \mathbf{O}\\ \mathbf{O}& \mathbf{O}& \mathbf{O}& \mathbf{O}& \mathbf{I}& \mathbf{O}\\ \mathbf{O}& \mathbf{O}& \mathbf{O}& \mathbf{O}& \mathbf{O}& \mathbf{I}\end{array}\right]$$
将偏导数矩阵表达式代入展开式可以得到
$$\delta {\mathbf{x}}_{k+1}\approx {\mathbf{F}}_{k+1}\delta {\mathbf{x}}_k+{\mathbf{w}}_{k+1}\text{\hspace{1em}(8)}$$
在此基础上，执行ESKF的预测过程。预测过程包括对名义状态的预测和对误差状态的预测，对名义状态的预测即按$(5)$式进行IMU积分即得到${\check{\mathbf{x}}}_{k+1}$。注意，初始时刻$k=0$，误差状态后验均值为${\widehat{\delta \mathbf{x}}}_0=0$，后验协方差矩阵为${\hat{\mathbf{P}}}_0=\mathbf{I}$，即$\delta {\mathbf{x}}_0\sim \mathcal{N}({\widehat{\delta \mathbf{x}}}_0,{\hat{\mathbf{P}}}_0)$，并未经过修正，故预测方程应写为
$$\begin{array}{rl}{\check{\delta \mathbf{x}}}_{k+1}& ={\mathbf{F}}_{k+1}{\widehat{\delta \mathbf{x}}}_k\\ {\check{\mathbf{P}}}_{k+1}& ={\mathbf{F}}_{k+1}{\hat{\mathbf{P}}}_k{\mathbf{F}}_{k+1}^T+{\mathbf{Q}}_{k+1}\end{array}\text{\hspace{1em}(9a)(9b)}$$
对于后续时间步$k=1,2,\cdots$，因为原误差状态已经过修正，故下一时间步误差状态的预测方程如下
$$\begin{array}{rl}{\check{\delta \mathbf{x}}}_{k+1}& ={\mathbf{F}}_{k+1}{\widehat{\delta \mathbf{x}}}_k^{+}\\ {\check{\mathbf{P}}}_{k+1}& ={\mathbf{F}}_{k+1}{\hat{\mathbf{P}}}_k^{+}{\mathbf{F}}_{k+1}^T+{\mathbf{Q}}_{k+1}\end{array}\text{\hspace{1em}(10a)(10b)}$$
事实上，因为重置后的均值${\widehat{\delta \mathbf{x}}}_{k+1}^{+}=0$，因此${\check{\delta \mathbf{x}}}_{k+1}=0$

前文阐述了ESKF的预测过程，现考虑更新过程。假设一个抽象的传感器能对状态变量产生观测（其输出为向量），观测方程为
$${\mathbf{z}}_{k+1}=\mathbf{h}({{\mathbf{x}}_t}_{k+1})+{\mathbf{ϵ}}_{k+1}$$
其中${\mathbf{ϵ}}_{k+1}\sim \mathcal{N}(0,{\mathbf{V}}_{k+1})$。可以认为观测方程实际具有如下形式
$${\mathbf{z}}_{k+1}=\mathbf{h}({\check{\mathbf{x}}}_{k+1}\oplus \delta {\mathbf{x}}_{k+1})+{\mathbf{ϵ}}_{k+1}\text{\hspace{1em}(11)}$$
因为在ESKF中，需要更新的是误差状态变量，故将$\mathbf{h}({\mathbf{x}}_{k+1}\oplus \delta {\mathbf{x}}_{k+1})$视为$\delta {\mathbf{x}}_{k+1}$的函数，并将其在${\check{\delta \mathbf{x}}}_{k+1}=0$处展开，得到
$$\begin{array}{rl}{\mathbf{z}}_{k+1}& \approx \mathbf{h}({\check{\mathbf{x}}}_{k+1}\oplus {\check{\delta \mathbf{x}}}_{k+1})+{\frac{\partial \mathbf{h}({\check{\mathbf{x}}}_{k+1}\oplus {\delta \mathbf{x}}_{k+1})}{\partial {\delta \mathbf{x}}_{k+1}}|}_{{\check{\delta \mathbf{x}}}_{k+1}}({\delta \mathbf{x}}_{k+1}-{\check{\delta \mathbf{x}}}_{k+1})+{\mathbf{ϵ}}_{k+1}\\ & =\mathbf{h}({\check{\mathbf{x}}}_{k+1})+{\mathbf{H}}_{k+1}\delta {\mathbf{x}}_{k+1}+{\mathbf{ϵ}}_{k+1}\end{array}\text{\hspace{1em}(12)}$$
其中${\mathbf{H}}_{k+1}$是观测方程相对于误差状态的雅可比矩阵，其具体计算方式为
$$\begin{array}{rl}{\mathbf{H}}_{k+1}& =\underset{\delta {\mathbf{x}}_{k+1}\to 0}{\lim }\frac{\mathbf{h}({\check{\mathbf{x}}}_{k+1}\oplus ({\check{\delta \mathbf{x}}}_{k+1}+{\delta \mathbf{x}}_{k+1}))-\mathbf{h}({\check{\mathbf{x}}}_{k+1}\oplus {\check{\delta \mathbf{x}}}_{k+1})}{\delta {\mathbf{x}}_{k+1}}\end{array}$$
下面求出卡尔曼增益，进而更新误差状态的协方差矩阵和均值
$$\begin{array}{rl}{\mathbf{K}}_{k+1}& ={\check{\mathbf{P}}}_{k+1}{\mathbf{H}}_{k+1}^T({\mathbf{V}}_{k+1}+{\mathbf{H}}_{k+1}{\check{\mathbf{P}}}_{k+1}{\mathbf{H}}_{k+1}^T{)}^{-1}\\ {\widehat{\delta \mathbf{x}}}_{k+1}& ={\check{\delta \mathbf{x}}}_{k+1}+{\mathbf{K}}_{k+1}({\mathbf{z}}_{k+1}-\mathbf{h}({\check{\mathbf{x}}}_{k+1}\oplus {\check{\delta \mathbf{x}}}_{k+1}))\\ {\hat{\mathbf{P}}}_{k+1}& =(\mathbf{I}-{\mathbf{K}}_{k+1}{\mathbf{H}}_{k+1}){\check{\mathbf{P}}}_{k+1}\end{array}\text{\hspace{1em}(13a)(13b)(13c)}$$
实际上，上式得到了误差状态的后验均值和后验协方差矩阵，即满足$\delta {\mathbf{x}}_{k+1}\sim \mathcal{N}({\widehat{\delta \mathbf{x}}}_{k+1},{\hat{\mathbf{P}}}_{k+1})$

事实上，用于预测的观测数据和用于更新的观测数据并不是一一对应的，用于预测的观测数据往往获取频率更高。当用于预测的观测数据到来时，应先根据$(5)$进行IMU积分得到名义状态，并根据$(9),(10)$对误差状态变量进行预测，直到观测数据到来时，再根据$(13)$更新误差状态变量。

得到误差状态的分布后，将其均值按$(3)$的方式合并入名义状态，得到真值状态的具体数值
$${\hat{\mathbf{x}}}_{k+1}={\check{\mathbf{x}}}_{k+1}\oplus {\widehat{\delta \mathbf{x}}}_{k+1}\text{\hspace{1em}(14)}$$
因为在每次将误差状态合并入名义状态后，误差的实际分布会发生改变，该改变可理解为通过重置函数$\mathbf{g}$对误差状态进行重置。下面考虑重置对于误差状态的影响，设重置后误差状态为$\delta {\mathbf{x}}_{k+1}^{+}$，即
$$\delta {\mathbf{x}}_{k+1}^{+}=\mathbf{g}(\delta {\mathbf{x}}_{k+1})\text{\hspace{1em}(15)}$$
首先要确定重置函数的形式。考虑到应保证重置前后真值服从的分布不变
$$({\check{\mathbf{x}}}_{k+1}\oplus {\widehat{\delta \mathbf{x}}}_{k+1})\oplus \delta {\mathbf{x}}^{+}={\check{\mathbf{x}}}_{k+1}\oplus \delta {\mathbf{x}}_{k+1}\text{\hspace{1em}(16)}$$
该式等号两侧均表示分布，将该式展开可知
$$\begin{array}{rl}\delta {\mathbf{p}}_{k+1}^{+}& =\delta {\mathbf{p}}_{k+1}-{\widehat{\delta \mathbf{p}}}_{k+1}\\ \delta {\mathbf{v}}_{k+1}^{+}& =\delta {\mathbf{v}}_{k+1}-{\widehat{\delta \mathbf{v}}}_{k+1}\\ \mathrm{Exp}(\delta {\mathbf{\theta }}_{k+1}^{+})& =\mathrm{Exp}(-{\widehat{\delta \mathbf{\theta }}}_{k+1})\mathrm{Exp}(\delta {\mathbf{\theta }}_{k+1})\\ \delta {{\mathbf{b}}_a}_{k+1}^{+}& ={\delta {\mathbf{b}}_a}_{k+1}-{{\widehat{\delta \mathbf{b}}}_a}_{k+1}\\ \delta {{\mathbf{b}}_g}_{k+1}^{+}& ={\delta {\mathbf{b}}_g}_{k+1}-{{\widehat{\delta \mathbf{b}}}_g}_{k+1}\\ \delta {\mathbf{g}}_{k+1}^{+}& =\delta {\mathbf{g}}_{k+1}-{\widehat{\delta \mathbf{g}}}_{k+1}\end{array}\text{\hspace{1em}(17a)(17b)(17c)(17d)(17e)(17f)}$$
令等号右侧到左侧的映射为函数$\mathbf{g}$即可。易知重置后均值为${\widehat{\delta \mathbf{x}}}_{k+1}^{+}=0$，协方差矩阵为
$${\hat{\mathbf{P}}}_{k+1}^{+}={\mathbf{J}}_{k+1}{\hat{\mathbf{P}}}_{k+1}{\mathbf{J}}_{k+1}^T$$
其中
$${\mathbf{J}}_{k+1}={\frac{\partial \mathbf{g}(\delta {\mathbf{x}}_{k+1})}{\partial \delta {\mathbf{x}}_{k+1}}|}_{{\widehat{\delta \mathbf{x}}}_{k+1}}$$
由$(17)$的结构知，${\mathbf{J}}_{k+1}=\mathrm{diag}({\mathbf{I}}_3,{\mathbf{I}}_3,{\mathbf{J}}_{\mathbf{\theta }},{\mathbf{I}}_3,{\mathbf{I}}_3,{\mathbf{I}}_3)$，其中${\mathbf{J}}_{\mathbf{\theta }}={\frac{\partial \delta {\mathbf{\theta }}_{k+1}^{+}}{\partial \delta {\mathbf{\theta }}_{k+1}}|}_{{\widehat{\delta \mathbf{\theta }}}_{k+1}}=\mathbf{I}-\frac{1}{2}{\widehat{\delta \mathbf{\theta }}}_{k+1}^{\wedge }$

故重置后的误差状态服从新的分布，即满足
$$\delta {\mathbf{x}}_{k+1}^{+}\sim \mathcal{N}({\widehat{\delta \mathbf{x}}}_{k+1}^{+},{\hat{\mathbf{P}}}_{k+1}^{+})\text{\hspace{1em}(18)}$$
并以此进行下一步的迭代。

从贝叶斯滤波的角度再理解ESKF，可以写出误差状态的贝叶斯滤波公式
$$p(\delta {\mathbf{x}}_{k+1}|\delta {\mathbf{x}}_0,{\mathbf{u}}_{1:k+1},{\mathbf{z}}_{1:k+1})=\eta \ast p({\mathbf{z}}_{k+1}|{\delta \mathbf{x}}_{k+1},\delta {\mathbf{x}}_0,{\mathbf{u}}_{1:k+1},{\mathbf{z}}_{1:k})\int p(\delta {\mathbf{x}}_{k+1}|\delta {\mathbf{x}}_k,\delta {\mathbf{x}}_0,{\mathbf{u}}_{1:k+1},{\mathbf{z}}_{1:k})p(\delta {\mathbf{x}}_k|\delta {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k})d\delta {\mathbf{x}}_k$$
上式中，积分式（先验）由$(9),(10),(18)$给出，即预测部分；并通过$(13)$进行校正和更新，算入似然，即观测部分，而名义状态变量相当于系统的时变参数。

实验：ESKF组合导航

本实验通过ESKF，将IMU，卫星导航和轮式编码器进行融合，实现传统的组合导航方案。下面首先研究RTK和轮式编码器这两种实际的传感器的观测方程，即将观测方程的抽象形式$(11)$具体化。

4.1、GNSS观测

得到GNSS观测数据后，可转换为对机体在世界坐标系下的真实位姿的观测${{\tilde{\mathbf{p}}}_t^G}_{k+1},{{\tilde{\mathbf{R}}}_t^G}_{k+1}$，观测方程为
$$\begin{array}{rl}{{\tilde{\mathbf{p}}}_t^G}_{k+1}& ={{\mathbf{p}}_t}_{k+1}+{{\mathbf{ϵ}}_p}_{k+1}={\check{\mathbf{p}}}_{k+1}+\delta {\mathbf{p}}_{k+1}+{{\mathbf{ϵ}}_{\mathbf{p}}}_{k+1}\\ {{\tilde{\mathbf{R}}}_t^G}_{k+1}& ={{\mathbf{R}}_t}_{k+1}+{{\mathbf{ϵ}}_R}_{k+1}={\check{\mathbf{R}}}_{k+1}\mathrm{Exp}(\delta {\mathbf{\theta }}_{k+1})+{{\mathbf{ϵ}}_{\mathbf{R}}}_{k+1}\end{array}\text{\hspace{1em}(19a)(19b)}$$
为更容易求误差状态的雅可比矩阵，将GNSS对真实旋转矩阵的观测转换成对误差旋转向量的观测，即将$(19b)$写成
$$\mathrm{Log}({\check{\mathbf{R}}}_{k+1}^T{{\tilde{\mathbf{R}}}_t^G}_{k+1})=\delta {\mathbf{\theta }}_{k+1}+{{\mathbf{ϵ}}_{\mathbf{\theta }}}_{k+1}\text{\hspace{1em}(20)}$$
$(19a)$和$(20)$共同组成了转换后的观测方程
$$\begin{array}{r}{\mathbf{z}}_{k+1}=\left[\begin{array}{c}{{\tilde{\mathbf{p}}}_t^G}_{k+1}\\ \mathrm{Log}({\check{\mathbf{R}}}_{k+1}^T{{\tilde{\mathbf{R}}}_t^G}_{k+1})\end{array}\right]=\left[\begin{array}{c}{\check{\mathbf{p}}}_{k+1}+\delta {\mathbf{p}}_{k+1}+{{\mathbf{ϵ}}_p}_{k+1}\\ \delta {\mathbf{\theta }}_{k+1}+{{\mathbf{ϵ}}_{\theta }}_{k+1}\end{array}\right]=\mathbf{h}({\check{\mathbf{x}}}_{k+1}\oplus \delta {\mathbf{x}}_{k+1})+{\mathbf{ϵ}}_{k+1}\end{array}\text{\hspace{1em}(21)}$$
由$(21)$知，误差状态的雅可比矩阵为
$$\begin{array}{r}{\mathbf{H}}_{k+1}=\left[\begin{array}{cccccc}\mathbf{I}& \mathbf{O}& \mathbf{O}& \mathbf{O}& \mathbf{O}& \mathbf{O}\\ \mathbf{O}& \mathbf{O}& \mathbf{I}& \mathbf{O}& \mathbf{O}& \mathbf{O}\end{array}\right]\end{array}$$
该矩阵中各分块矩阵均为$3\times 3$矩阵

4.2、轮式编码器观测

得到Odom观测数据后，可转换为对机体在世界坐标系下的真实速度的观测${{\tilde{\mathbf{v}}}_t^O}_{k+1}$，观测方程为
$${{\tilde{\mathbf{v}}}_t^O}_{k+1}={{\mathbf{R}}_t}_{k+1}{{\mathbf{v}}_t}_{k+1}+{{\mathbf{ϵ}}_{{\mathbf{v}}^W}}_{k+1}=({\check{\mathbf{R}}}_{k+1}\mathrm{Exp}(\delta {\mathbf{\theta }}_{k+1}){)}^T({\check{\mathbf{v}}}_{k+1}+\delta {\mathbf{v}}_{k+1})+{{\mathbf{ϵ}}_{{\mathbf{v}}^W}}_{k+1}\text{\hspace{1em}(22)}$$
为更容易求误差状态的雅可比矩阵，忽略误差旋转向量，只保留当前预测出的旋转矩阵名义状态，并将对机体坐标系下速度的观测转换成对世界坐标系下速度的观测
$${\mathbf{z}}_{k+1}={\check{\mathbf{R}}}_{k+1}{{\tilde{\mathbf{v}}}_t^O}_{k+1}={\check{\mathbf{v}}}_{k+1}+\delta {\mathbf{v}}_{k+1}+{{\mathbf{ϵ}}_{\mathbf{v}}}_{k+1}=\mathbf{h}({\check{\mathbf{x}}}_{k+1}\oplus \delta {\mathbf{x}}_{k+1})+{{\mathbf{ϵ}}_{\mathbf{v}}}_{k+1}\text{\hspace{1em}(23)}$$
由$(23)$知，误差状态的雅可比矩阵为
$$\begin{array}{r}{\mathbf{H}}_{k+1}=\left[\begin{array}{cccccc}\mathbf{O}& \mathbf{I}& \mathbf{O}& \mathbf{O}& \mathbf{O}& \mathbf{O}\end{array}\right]\end{array}$$
该矩阵中各分块矩阵均为$3\times 3$矩阵

4.3 实验结果分析

ESKF通过将IMU与卫星导航进行组合，得到GINS（GNSS-INS）系统，该系统将IMU观测与GNSS观测进行融合，融合结果整体趋近于GNSS观测，但获得了相比于GNSS更高的频率（因为IMU频率更高）。通过下图可以看到，存在某些RTK信号不良的场合，ESKF会缺少观测，位移部分呈快速发散状态，当RTK恢复以后，位移部分会收敛到RTK轨迹上。

如果长时间缺少RTK观测数据，ESKF就变为纯靠IMU积分的递推模式，该模式下位移将很快发散，位移发散的主要原因是缺少速度观测，导致速度发散。 引入速度传感器，如轮式编码器，将速度测量融入ESKF，则可以限制速度的发散。通过下图可以看到，几处因缺少RTK观测而发散的地方得到了明显改善，可见速度观测对于组合导航是有积极作用的。

附录

§1、向量函数的一阶泰勒展开

若一个多元向量函数$\mathbf{f}=[f_1,\cdots ,f_M{]}^T:{\mathbb{R}}^N\to {\mathbb{R}}^M$在$\mathbf{x}$点处可微，则存在$\mathbf{x}$的邻域 U ( x , δ ) = { x + Δ x ∣ ∥ Δ x ∥ < δ } U(\mathbf{x},\delta)=\{\mathbf{x}+\Delta\mathbf{x}|\|\Delta\mathbf{x}\|<\delta\} U(x,δ)={x+Δx∣∥Δx∥<δ}，使得在该邻域中
$$\mathbf{f}(\mathbf{x}+\Delta \mathbf{x})=\mathbf{f}(\mathbf{x})+\mathbf{Jf}(\mathbf{x})\Delta \mathbf{x}+\mathrm{o}(\Vert \Delta \mathbf{x}\Vert )\text{\hspace{1em}(A1)}$$
其中$\mathbf{Jf}(\mathbf{x})=\nabla \mathbf{f}(\mathbf{x}{)}^T={\lim }_{\Delta \mathbf{x}\to 0}\frac{\mathbf{f}(\mathbf{x}+\Delta \mathbf{x})-\mathbf{f}(\mathbf{x})}{\Delta {\mathbf{x}}^T}\in {\mathbb{R}}^{M\times N}$称为雅可比矩阵。

$(A4)$称为多元向量函数$\mathbf{f}$的一阶泰勒展开。

下面根据上述理论推导当$\mathbf{f}$的定义域为非欧式空间$\mathrm{Dom}(\mathbf{x})$时的一阶泰勒展开

记$\mathbf{h}(\Delta \mathbf{x})=\mathbf{f}(\mathbf{x}\oplus \Delta \mathbf{x})$，故$\mathbf{h}:{\mathbb{R}}^N\to {\mathbb{R}}^M$，$\oplus :\mathrm{Dom}(\mathbf{x})\times {\mathbb{R}}^N\to \mathrm{Dom}(\mathbf{x})$，若$\mathbf{h}$在$0$点处可微，则存在$0$的邻域 U ( 0 , δ ) = { Δ x ∣ ∥ Δ x ∥ < δ } U(\mathbf{0},\delta)=\{\Delta\mathbf{x}|\|\Delta\mathbf{x}\|<\delta\} U(0,δ)={Δx∣∥Δx∥<δ}，使得在该邻域中
$$\mathbf{h}(\Delta \mathbf{x})=\mathbf{h}(0)+\mathbf{Jh}(0)\Delta \mathbf{x}+\mathrm{o}(\Vert \Delta \mathbf{x}\Vert )\text{\hspace{1em}(A2)}$$
其中$\mathbf{Jh}(0)=\nabla \mathbf{f}(\mathbf{x}{)}^T={\lim }_{\Delta \mathbf{x}\to 0}\frac{\mathbf{h}(\Delta \mathbf{x})-\mathbf{h}(0)}{\Delta {\mathbf{x}}^T}={\lim }_{\Delta \mathbf{x}\to 0}\frac{\mathbf{f}(\mathbf{x}\oplus \Delta \mathbf{x})-\mathbf{f}(\mathbf{x})}{\Delta {\mathbf{x}}^T}\in {\mathbb{R}}^{M\times N}$，可以发现$\mathbf{Jh}(0)$和$(A4)$中$\mathbf{Jf}(\mathbf{x})$具有类似的结构，故记$\mathbf{Jf}(\mathbf{x})=\mathbf{Jh}(0)$，仍称$\mathbf{Jf}(\mathbf{x})$为对增量$\Delta \mathbf{x}$的雅可比矩阵。

综上，$(A5)$也可以写成
$$\mathbf{f}(\mathbf{x}\oplus \Delta \mathbf{x})=\mathbf{f}(\mathbf{x})+\mathbf{Jf}(\mathbf{x})\Delta \mathbf{x}+\mathrm{o}(\Vert \Delta \mathbf{x}\Vert )\text{\hspace{1em}(A3)}$$
$(A6)$称为$\mathrm{Dom}(\mathbf{x})$上的向量函数$\mathbf{f}$的一阶泰勒展开。

为简便起见，常将上面的雅可比矩阵用$\mathbf{J}(\cdot )$代替

§2、极限形式的多元向量函数导数

若一个多元向量函数$\mathbf{f}=[f_1,\cdots ,f_M{]}^T:{\mathbb{R}}^N\to {\mathbb{R}}^M$在$\mathbf{x}$点处可微，则
$$\underset{\Delta \mathbf{x}\to 0}{\lim }\frac{\mathbf{f}(\mathbf{x}+\Delta \mathbf{x})-\mathbf{f}(\mathbf{x})}{\Delta {\mathbf{x}}^T}=\frac{\partial \mathbf{f}}{\partial {\mathbf{x}}^T}={\left[\begin{array}{ccc}\frac{\partial f_1}{\partial x_1}& \cdots & \frac{\partial f_1}{\partial x_N}\\ ⋮& \ddots & ⋮\\ \frac{\partial f_M}{\partial x_1}& \cdots & \frac{\partial f_M}{\partial x_N}\end{array}\right]}_{M\times N}$$
在SLAM领域，通常省略上式中的转置符号，直接记为
$$\underset{\Delta \mathbf{x}\to 0}{\lim }\frac{\mathbf{f}(\mathbf{x}+\Delta \mathbf{x})-\mathbf{f}(\mathbf{x})}{\Delta \mathbf{x}}=\frac{\partial \mathbf{f}}{\partial \mathbf{x}}={\left[\begin{array}{ccc}\frac{\partial f_1}{\partial x_1}& \cdots & \frac{\partial f_1}{\partial x_N}\\ ⋮& \ddots & ⋮\\ \frac{\partial f_M}{\partial x_1}& \cdots & \frac{\partial f_M}{\partial x_N}\end{array}\right]}_{M\times N}\text{\hspace{1em}(A4)}$$
在$(A7)$定义下，$\frac{\partial \mathbf{Ax}}{\partial \mathbf{x}}=\mathbf{A}$