IMU姿态融合（MPU9250从校订到滤波步骤）

ref:http://www.javashuo.com/article/p-gbuptkpl-np.html

1、动机

MPU9250包括三轴加速度计、三轴陀螺仪和三轴磁力计。由于加速度/磁力计具备高频噪声（须要低通滤波），将加速度/磁力计的信号当作是音频信号，它们的信号会有不少“毛刺“，也就是说它们的瞬时值不够精确，解算出来的姿态会震荡，但长期来看姿态方向是对的。而陀螺仪具备低频噪声（须要高通滤波），即每一个时刻的获得的角速度是比较精确的，使用积分就能获得旋转角度（姿态），可是积分会累积偏差，所以积分到后面姿态就不对了，也就是漂移现象。 加速度/磁力计和陀螺仪在频域上的特性互补，能够融合这三种传感器的数据，提升精度和系统的动态特性。
下图表示的是把传感器翻转90°一段时间后又翻转回来时，加速度/磁力计、陀螺仪以及融合传感信号，所求得的姿态信息：

然而对于原始的MPU9250来讲，在进行姿态融合前须要对传感器进行校订等“预处理”。

2、步骤

如上图所示，要对MPU9250进行姿态融合/滤波，大致须要三个步骤：一、校订，二、坐标转换，三、滤波。其中前两步须要离线执行。下面详解这三个步骤。

1. 传感器校订

按正常的状况来讲，加速度传感器在静止状态（即只受重力做用），重力在各个姿态下，在三维空间中，向量顶点会落在一个球面上。但在IMＵ中，x,y,z轴的度量单位很大可能不相同，假设各轴之间相互直，各姿态重力点会落在一个椭球面上，椭球的中心也不必定在原点，这其实就是加速度的偏移量。

在磁力计上，因为测量磁场强度，在环境不变的状况下，传感器每一个姿态感觉磁场强度是相同的，因此不须要静止状态，磁力计测量的x,y,z轴值，在没有误差且传感器内部x,y,z轴相互垂直的状况下，在三维空间中组成一个圆球面。可是磁力计存在Hard Iron Distortion和Soft Iron Distortion，使得x,y,z轴度量单位不相同，各轴也并不是相互垂直，椭球球心也并不是[0,0,0]坐标。

总的来讲加速度/磁力计校订，须要求出椭球的球心（偏移量）、椭球轴长（各轴单位长度；其实磁力计不须要求，后面会解释。），即椭球拟合。

加速度计校订

• 在离线数据中寻找静止状态下的加速度传感器值，假设静止时的不一样姿态共有n个，则获得数据 accData∈Rn×3；
• accData∈Rn×3做为样本数据，使用最小二乘法便可求出椭球方程的各个参数，接着根据我师兄博客的方法能够求出椭球的球心（偏移量）和轴长。

磁力计校订

• 提取离线数据的磁力计传感器的值（不须要静止状态），获得样本数据 magData∈Rm×3, 假设有m组数据；
• magData∈Rm×3做为样本数据，使用最小二乘法便可求出椭球方程的各个参数，进一步求出球心（偏移量）和轴长（轴长可忽略）。

陀螺仪校订

• 陀螺仪不存在椭球的问题，所以能够直接从离线数据中找到静止状态下的数据，对数据求均值便可获得偏移量。
   

2. 磁力计坐标系转换到加速度坐标系

磁力计的基坐标系和加速度的基坐标系（咱们以加速度的基坐标系做为世界坐标系）并不必定是同样的，好比加速度的基坐标系x轴指向东，磁力计的指向北。所以，须要将磁力计坐标系转换到加速度计坐标系，才能经过加速度/磁力计解算出正确的姿态。

3. 姿态融合

在使用各类滤波方法（如高低通滤波，EKF，Mahony）作姿态融合时，能够基于欧拉角、方向余弦和四元数等方法。欧拉角在求解姿态时存在奇点（万向节锁），不能用于全姿态解算；方向余弦可用于全姿态解算但计算量大，不能知足实时性要求；四元数，计算量小，无奇点，知足实时解算。

因此这里使用的是基于四元数姿态融合，滤波方法包括高低通滤波、EKF、Mahony滤波。下面简单介绍这三种算法的大体思想：

高低通滤波
两个方程说明问题

• Quaternion(t)=(1−α)Quaternion(t)^+α⋅accMagQuaternion(t)
• Quaternion(t)^=Quaternion(t−1)gyroSensorQ(t)

Quaternion是姿态融合后的四元数，gyroSensorQ 是陀螺仪测量到的角速度所表明的瞬时四元数， Quaternion(t)^ 是使用陀螺仪推算 t 时刻的估计姿态，accMagQuaternion 是加速度/磁力计解算出的四元数。使用 α 加权将陀螺仪的估算和加速度/磁力计的解算融合在一块儿，达到滤波效果。这里 α=0.1，通常比较小，由于加速度/磁力计有高频噪声，须要低通滤波，不然会抖动。

EKF
广义卡尔曼滤波，主要有两步：估算、校订。（公式和推导不详述）。

• 估算：和高低通滤波同样，使用陀螺仪估算出 t时刻四元数；

• 校订：计算出估算姿态下的重力加速度和磁通量向量，再用加速度/磁力计测量到的重力加速度和磁通量向量按照EKF校订方程进行校订。

Mahony滤波
主要是使用了PI控制器思想（未深刻，理解有误差欢迎指出）。一样也分两步：估算、校订。

• 估算：和前面同样，使用陀螺仪估算出 t时刻四元数；

• 校订：计算出估算姿态下的重力加速度和磁通量向量，再用加速度/磁力计测量到的重力加速度和磁通量向量叉乘估算姿态的计算结果，获得的是估算姿态与测量姿态的偏差 e。e 能够对陀螺仪测量到的角速度进行补偿，使用的是PI控制进行补偿。其中P用于控制加速度计和陀螺仪之间的交叉频率，I用于校订陀螺仪偏差。使用补偿后的角速度从新估算出 t 时刻四元数，此时已达到了滤波效果。

3、实验结果

加速度计椭球拟合

上图更像一个球，但球心明显偏离 [0,0,0]。

磁力计椭球拟合

一样球心明显偏离 [0,0,0]。

四元数姿态估计

下图中第一到第四行分别是四元数4个值在使用各类方法估计姿态时的变化状况，第五行是角速度的变化状况。

能够看到，除了Gyro估计的姿态，其余三种滤波算法估算的姿态基本同样。

 

Matlab实验：

前面两个图是加速度计和磁力计的椭球拟合；后一个图是各姿态融合算法对比，能够看到陀螺仪估算的姿态随时间累积偏差变大，加速度/磁力计估算的姿态有明显的抖动（高频噪声），其余三种姿态融合算法效果差异不大。

计算时间 Mahony < 高低通 < EKF

手机端姿态融合

计算所用的时间 : Mahony < 高低通 < EKF