gps/imu融合（卡尔曼滤波）学习笔记

航位推算

因为imu更新频率很快（100Hz)，假设车辆为匀速匀角速度模型，分别为v和w，模型如下：

GPS坐标变换

GPS原始输出定位是基于WGS-84坐标的经纬度信息。GPS坐标系需要投影为平面地图才可以做航位推算。常用的投影方法有高斯克吕格投影，UTM投影，墨卡托投影等。墨卡托投影转换方式如下：

其中 x、 y 是地面坐标系两轴坐标， lon 是经度， lat 是纬度， EARTH_RAD 是地球半径，取 EARTH_RAD 为 6378137 米。

GPS/IMU耦合方式

分为松耦合和紧耦合方式以及深耦合方式和超紧耦合方式，后两种对硬件有更高要求。松耦合方式虽然效果相比其他几种方式较差，但是可以满足我们的使用需求且实现方便，易于工程化，同时不干扰其他模块工作，容错性高。

扩展卡尔曼滤波（EKF）

卡尔曼滤波算法利用目标的动态信息，在离散时间系统中，每次时间增加都会在系统当前状态中加入过程噪声，而对于当前的系统观测结果中也会加入观测噪声。卡尔曼滤波利用观测方程和状态方程对系统的当前状态及变化进行描述。采用逐点递推的方法。当gps失效时也可以基于预测值给予结果，即仅依靠imu推算得到定位结果。在此基础上，扩展卡尔曼滤波将非线性系统在工作点附近使用泰勒级数展开线性近似。我们使用EKF。系统中卡尔曼滤波器选取状态变量为车辆位姿Xi = (xi yi ψ i)，

常规的卡尔曼滤波在系统动态误差下很难得到最优估计。

使用四元数表示姿态的原因：用四元数的表示方式表示方法唯一， 同时具有存储空间小、 计算效率高的优点。