开源代码

使用ESKF实现了IMU和GPS的融合定位，开源代码提供了ROS2的实现：https://github.com/Yaepiii/ESKF_IMU_GPS_Fusion

下面简单梳理一下推导过程

1. IMU基础

1.1 IMU测量模型

IMU可以测量加速度和角速度，定义测量值为$a_m,w_m$，IMU的噪声通常会受到偏置$b$和测量噪声$n$的影响，因此IMU测量值可以表示为：

$$\begin{array}{rl}{a}_m& {=}^I{R}_W(a-g)+b_a+n_a\\ w_m& =w+b_g+n_g\end{array}$$
其中，$a,w$为世界坐标系下真实值，偏置项符合随机游走模型，测量噪声符合零均值高斯分布。

1.2 IMU航迹推算

如果两次IMU测量的时间间隔为$\Delta t$，那么就可以使用积分的知识推导出下一个时刻的状态：

$$\begin{array}{rl}R(t+\Delta t)& =R(t)Exp((w_m-b_g)\Delta t)\\ p(t+\Delta t)& =p(t)+v\Delta t+\frac{1}{2}(R(t)(a_m-b_a))\Delta t^2+\frac{1}{2}g\Delta t^2\\ v(t+\Delta t)& =v(t)+R(t)(a_m-b_a)\Delta t+g\Delta t\end{array}$$

这里相当于使用了欧拉法的做法，认为$\Delta t$这段时间内的角速度和加速度就是起始时刻的值，在实际代码中，中值积分比较常用，例如：代码中的这里

2. GPS基础

GPS其实是美国的全球定位系统的简称，因为它出现的比较早，所以后面很多时候都把GPS等同于GNSS(全球卫星导航系统)了，其实GNSS才是一个更广的概念，GNSS包括很多国的卫星导航系统，比如：美国的全球定位系统(GPS)、中国的北斗卫星导航系统(Beidou)、俄罗斯的格罗纳斯系统(GLONASS)、欧盟的伽利略系统(GALILEO)。

2.1 测量精度影响因素

GPS是通过结果卫星信号解算进行定位的系统，最少需要接收4颗卫星的信号才能实现位置解算，通常分为单点定位和差分定位(RTK)技术，单点定位的精度比较低，消费级的GPS大概在米级精度，z轴的测量就会更高一些；而RTK是通过和额外的基站通信可以获得更高的精度，能够达到厘米级甚至毫米级的精度。

当然，即使使用RTK技术，在实际使用过程中仍然存在很多现实因素会影响定位精度，下面总结几条：

• 天气：比如今天天气不好，乌云密布，可能会导致搜星不好，搜星数量较少
• 遮挡：这种情况在城市运行时较为常见，高楼有时候会遮挡卫星信号，还会造成多径效应
• 安装：有时候接收器安装倾斜也会导致精度下降(很多厂商会提供倾斜测量的功能)
• 静置：很多情况下，让接收器原地静置几分钟精度会更高，因为很多接收器常常具有自校准功能

2.2 常见的坐标系

GPS受到的原始测量值大部分是经纬高(经度、纬度、海拔，LLA)，这通常被称为地理坐标系，这是全局的绝对测量值，而IMU得到的是相对测量，如果想让二者进行融合，需要将GPS测量值转换为局部坐标系，例如东北天(ENU)坐标系。

网上有很多讲解怎么进行转换的，这里就不展开了，实际上，在代码中可以调用GeographicLib这个第三方库来方便的实现这个过程。

3. 基于ESKF的GINS系统

IMU提供了短期高频的局部定位，GPS实现了低频的绝对定位，所以很自然地想到可以使用卡尔曼滤波将二者进行融合。

3.1 为什么用ESKF

经典数据[1]中给出了经典的总结，大致如下：

• ESKF使用误差项做为状态量，这是一个小量，其二阶变量可以忽略，大多数情况下可以对雅克比矩阵进行简化，一些部分可以使用单位阵替代
• ESKF总是在原点附近，离奇异点较远，数值更稳定一些，这也会降低线性化近似误差
• ESKF使用三维变量来表达旋转增量，避免了传统KF中用四元数或旋转矩阵来表达的奇异性问题

3.2 公式推导

3.2.1 变量定义

经典的KF可能会将状态变量设置为：

$$x=[\bar{p},\bar{v},\bar{R},{\bar{b}}_a,{\bar{b}}_g,\bar{g}]$$
运动方程为：

$$\begin{array}{rl}\dot{\bar{p}}& =\bar{v}\\ \dot{\bar{v}}& =\bar{R}(a_m-{\bar{b}}_a-n_a)+\bar{g}\\ \dot{\bar{R}}& =\bar{R}(w_m-{\bar{b}}_g-n_g{)}^{\wedge }\\ \dot{{\bar{b}}_g}& =n_g\\ \dot{{\bar{b}}_a}& =n_a\\ \dot{\bar{g}}& =0\end{array}$$

而在ESKF中通常会称上述状态变量为名义状态变量，而将真正的"传播"过程中的状态变量成为误差状态变量：

$$\delta x=[\delta p,\delta v,\delta R,\delta b_a,\delta b_g,\delta g]$$

将名义状态变量和误差状态变量相加，就可以得到真值状态

$$\begin{array}{rl}p& =\bar{p}+\delta p\\ v& =\bar{v}+\delta v\\ R& =\bar{R}\delta R\\ b_a& ={\bar{b}}_a+\delta b_a\\ b_g& ={\bar{b}}_g+\delta b_g\\ g& =\bar{g}+\delta g\end{array}$$

这个过程相当于一个将状态传播和噪声分离的过程，在积分的过程中使用名义状态变量，认为是无噪声的，然后使用EKF的过程更新误差状态变量，最后进行"合并"，这样其实是使得整个过程更简洁了。

整体流程引用高翔博士书中[2]的总结：

既然是使用误差状态变量$\delta x$进行EKF的过程，那么接下来需要推导一下关于$\delta x$的运动方程。

3.2.2 运动方程

在真值状态定义公式中，两边对时间求导可以得到：

$$\begin{array}{rl}\delta \dot{p}& =\delta v\\ \delta {\dot{b}}_a& =n_a\\ \delta {\dot{b}}_g& =n_g\\ \delta \dot{g}& =0\end{array}$$

多说一句推导过程，比如对于$\delta \dot{p}$，左边求导是$v$，右边求导是$\bar{v}+\delta \dot{p}$,，移个项就能得到结论了。

对于速度项和旋转项与$\delta R$相关，因此需要更复杂的推导。

对于旋转误差项

求导后：
$$\dot{R}=\dot{\bar{R}}exp(\delta \theta )+\bar{R}\dot{exp(\delta \theta )}=\bar{R}(w_m-b_g{)}^{\wedge }exp(\delta \theta )+\bar{R}exp(\delta \theta ){\delta \dot{\theta }}^{\wedge }$$

注意到，对真值$R$求导还等于：
$$\dot{R}=R(w_m-b_g-n_g{)}^{\wedge }=\bar{R}\delta R(w_m-b_g-n_g{)}^{\wedge }$$

联立这两个式子，可以消掉$\bar{R}$，再整理一下可以得到：

$$exp(\delta \theta ){\delta \dot{\theta }}^{\wedge }=\delta R(w_m-b_g-n_g{)}^{\wedge }-(w_m-b_g{)}^{\wedge }exp(\delta \theta )$$

利用伴随性质(${\varphi }^{\wedge }R=R(R^T\varphi {)}^{\wedge }$)，可得：
$$\begin{array}{rl}exp(\delta \theta ){\delta \dot{\theta }}^{\wedge }& =\delta R(w_m-b_g-n_g{)}^{\wedge }-\delta R(\delta R^T(w_m-b_g){)}^{\wedge }\\ & =\delta R((w_m-b_g-n_g{)}^{\wedge }-(\delta R^T(w_m-b_g){)}^{\wedge })\\ & \approx \delta R((w_m-b_g-n_g{)}^{\wedge }-((I-\delta {\theta }^{\wedge })(w_m-b_g){)}^{\wedge })\\ & =\delta R(w_m-b_g-n_g-(\delta {\theta }^{\wedge }{w}_m-\delta {\theta }^{\wedge }{b}_g){)}^{\wedge }\\ & =\delta R((-w_m+b_g{)}^{\wedge }\delta \theta -\delta b_g-n_g{)}^{\wedge }\end{array}$$

其中，$\approx$这一步，是对$\delta R^T$应用了$exp(\cdot )$泰勒展开，最后，约掉两侧的$exp(\delta \theta )$和${}^{\wedge }$可得：

$$\delta \dot{\theta }=(-w_m+b_g{)}^{\wedge }\delta \theta -\delta b_g-n_g$$

对于速度误差项

对两侧求导，根据公式(4)，左边为：

$$\begin{array}{rl}\dot{v}& =R(a_m-b_a-n_a)+g\\ & =\bar{R}\delta R(a_m-{\bar{b}}_a-\delta b_a-n_a)+\bar{g}+\delta g\\ & \approx \bar{R}(I+\delta {\theta }^{\wedge })(a_m-{\bar{b}}_a-\delta b_a-n_a)+\bar{g}+\delta g\\ & \approx \bar{R}{a}_m-\bar{R}{\bar{b}}_a-\bar{R}\delta b_a-\bar{R}{n}_a-\bar{R}{a_m)}^{\wedge }\delta \theta -\bar{R}{{\bar{b}}_a}^{\wedge }\delta \theta +\bar{g}+\delta g\end{array}$$

推导过程中需要忽略$\delta {\theta }^{\wedge }$与$\delta b_a,n_a$相乘的二阶小量。

右边为：
$$\begin{array}{r}\dot{\bar{v}}+\delta \dot{v}=\bar{R}(a_m-{\bar{b}}_a)+\bar{g}+\delta \dot{v}\end{array}$$
联立两式可得：
$$\begin{array}{rl}\delta \dot{v}& =-\bar{R}(a_m-{\bar{b}}_a{)}^{\wedge }\delta \theta -\bar{R}\delta b_a-R{n}_a+\delta g\\ & =-\bar{R}(a_m-{\bar{b}}_a{)}^{\wedge }\delta \theta -\bar{R}\delta b_a-n_a+\delta g\end{array}$$
至此，我们推导了所有状态量的运动方程形式，也不难写出它们的离散形式：

$$\begin{array}{rl}\delta p(t+\Delta t)& =\delta p+\delta v\Delta t\\ \delta v(t+\Delta t)& =\delta v+(-\bar{R}(a_m-{\bar{b}}_a{)}^{\wedge }\delta \theta -\bar{R}\delta b_a+\delta g)\Delta t-n_v\\ \delta \dot{\theta }(t+\Delta t)& =(-w_m+b_g{)}^{\wedge }\Delta t\delta \theta -\delta b_g\Delta t-n_{\theta }\\ \delta b_a(t+\Delta t)& =\delta b_a+n_a\\ \delta b_g(t+\Delta t)& =\delta b{b}_g+n_g\\ \delta g(t+\Delta t)& =\delta g\end{array}$$

整体简写为：
$$\begin{array}{r}\delta x_{k+1}=f(\delta x_k)+w\end{array}$$
w是协方差为Q的0均值高斯噪声，那么接下来我们就可以按照EKF的方式进行线性化：
$$\begin{array}{r}\delta x(t+\delta t)=f(\delta x(t))+F\delta x+w\end{array}$$
这里的F就是由我们上面推导的运动方程推导出来的，把他们的系数拿出来就可以得到F矩阵的形式：

$$\begin{array}{r}F=\left[\begin{array}{cccccc}I& I\Delta t& 0& 0& 0& 0\\ 0& I& -\bar{R}{\left(a_m-{\bar{b}}_a\right)}^{\wedge }\Delta t& -\bar{R}\Delta t& 0& I\Delta t\\ 0& 0& {\left(-w_m+b_g\right)}^{\wedge }\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{array}\right]\end{array}$$

至此，我们就可以愉快地套用EKF的预测过程了：

$$\begin{array}{rl}\delta \hat{x}& =F\delta x\\ \hat{P}& =FP{P}^T+Q\end{array}$$

3.2.3 观测方程

当收到GPS矫正信息后，可以执行EKF的更新过程，观测方程如下：

$$\begin{array}{r}z=h(\hat{x})+v\end{array}$$

v是观测噪声，符合协方差为V的0均值高斯分布。

同样会对观测方程执行一阶泰勒展开：
$$\begin{array}{r}z=H\widehat{\delta x}+v\end{array}$$

这里的矩阵H可以通过链式法则获得对$\delta x$的导数：

$$H=\frac{\partial h}{\partial x}\frac{\partial x}{\partial \delta x}$$

对于第一项其实就是GPS测量的位置$[x,y,z]$对名义状态量的导数，就是单位阵，对于第二项，根据之前真值、名义状态量、误差状态量之间的关系可以得到：

$$\frac{\partial x}{\partial \delta x}=diag(I,I,J_r^{-1}(\bar{R}),I,I,I)$$

其中旋转项求导的时候，相当于$\bar{R}$右乘了一个小量，可以用BCH近似公式得到。

3.2.3 善后工作

在完成上述过程后，只需要把误差状态归入名义状态，然后重置ESKF即可，归入就很简单：

$$\begin{array}{rl}{p}_{k+1}& ={\bar{p}}_k+\delta p\\ v_{k+1}& ={\bar{v}}_k+\delta v\\ R_{k+1}& ={\bar{R}}_k\delta R\\ {b_a}_{k+1}& ={{\bar{b}}_a}_k+\delta b_a\\ {b_g}_{k+1}& ={{\bar{b}}_g}_k+\delta b_g\\ g_{k+1}& ={\bar{g}}_k+\delta g\end{array}$$

对于重置部分，均值的重置就直接把状态量全置零即可，对于协方差的重置有点说法，它对于除了旋转部分以外的状态量是没有影响的，对旋转部分有微小的影响，具体可参考[2]，一般是有以下关系：

$$P_{reset}=J_{k}P{J}_k^T$$

不过，由于一般认为ESKF的误差状态量为小量，$\delta \theta$很小，一般代码中直接将$J_k$作为单位阵处理。

3.3 总结

总之，整个过程其实和EKF类似，有这么几个公式：

$$\begin{array}{rl}\delta \hat{x}& =F\delta x\\ \hat{P}& =F\bar{P}{F}^T+Q\\ K& =\hat{P}{H}^T(H\hat{P}{H}^T+V{)}^{-1}\\ \delta x& =K(z-h(\hat{x}))\\ \bar{x}& =\hat{x}+\delta x\\ \bar{P}& =(I-KH)\hat{P}\end{array}$$

4. 代码细节

代码中和文中描述有些不同之处，总结如下：

• 代码中为了省事，将重力项纳入状态变量中
• 代码目前没有实现IMU自动初始化过程，使用手动设置的一些参数，如偏置、噪声
• 注意代码统一使用的东北天坐标系
• 实际上，IMU和GPS的外参也需要考虑，代码给出了接口

参考文献

[1] Solà, J. (2017). Quaternion kinematics for the error-state Kalman filter. arXiv. https://arxiv.org/abs/1711.02508

[2]高翔, 等人. 自动驾驶与机器人中的SLAM技术[M]