对于加速计与陀螺仪融合，介绍两种滤波算法：他们的核心思想不太相同，这两种方法现在也已经有了应用，现在你可以发现互补滤波融合在无人机上，EKF我做过在平衡车上面。mathony主要思想是用四元数来更新，最后转化为欧拉角。EKF主要是根据角度与角速度建立陀螺仪的模型，通过加速度算出角度来更新陀螺仪的累积误差。
1.首先介绍一下互补滤波算法：
先定义Kp，Ki，以及halfT 。
Kp，Ki，控制加速度计修正陀螺仪积分姿态的速度
halfT ，姿态解算时间的一半。此处解算姿态速度为500HZ，因此halfT 为0.001
#define Kp 2.0f
#define Ki 0.002f
#define halfT 0.001f
初始化四元数
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
定义姿态解算误差的积分
float exInt = 0, eyInt = 0, ezInt = 0;
以下为姿态解算函数。
参数gx，gy，gz分别对应三个轴的角速度，单位是弧度/秒;
参数ax，ay，az分别对应三个轴的加速度原始数据
由于加速度的噪声较大，此处应采用滤波后的数据
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
将加速度的原始数据，归一化，得到单位加速度
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
把四元数换算成“方向余弦矩阵”中的第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以这里的vx、vy、vz，其实就是当前的机体坐标参照系上，换算出来的重力单位向量。(用表示机体姿态的四元数进行换算)
vx = 2*(q1*q3 – q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 – q1*q1 – q2*q2 + q3*q3;
这里说明一点，加速度计由于噪声比较大，而且在飞行过程中，受机体振动影响比陀螺仪明显，短时间内的可靠性不高。陀螺仪噪声小，但是由于积分是离散的，长时间的积分会出现漂移的情况，因此需要将用加速度计求得的姿态来矫正陀螺仪积分姿态的漂移。
在机体坐标参照系上，加速度计测出来的重力向量是ax、ay、az;陀螺积分后的姿态来推算出的重力向量是vx、vy、vz;它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。
向量间的误差，可以用向量积(也叫外积、叉乘)来表示，ex、ey、ez就是两个重力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。简单的讲就是说本来陀螺仪转换的出的重力向量应该和加速度计重力向量相等的，但是现在可能由于加速度计积分误差导致不相等，所以需要校正。
叉乘是数学基础，百度百科里有详细解释。
ex = (ay*vz – az*vy);
ey = (az*vx – ax*vz);
ez = (ax*vy – ay*vx);
将叉乘误差进行积分
exInt = exInt + ex*Ki*halfT;
eyInt = eyInt + ey*Ki*halfT;
ezInt = ezInt + ez*Ki*halfT;
用叉乘误差来做PI修正陀螺零偏，通过调节Kp，Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
一阶龙格—库塔法解四元数微分方程。
q0 = q0 + (-q1*gx – q2*gy – q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz – q3*gy)*halfT;
q2 = q2 + (q0*gy – q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy – q2*gx)*halfT;
下面给出解释四元数更新：
四元数归一化：
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
最后根据四元数方向余弦阵和欧拉角的转换关系，把四元数转换成欧拉角。所以有：
ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
ANGLE.Y= asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
ANGLE.X= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}
2.EKF滤波算法
1.这里，因为用在平衡车上，所以只需要一个倾角模型就可以。根据上一时刻角度值得到了当前角度的预测值 Angle(k+1)：
Angle(k)=Angle(k-1)+(Gyro - Q_bias) * dt （1）
其中等号左边Angle(k)为此时的角度，等号右边Angle(k-1)为上一时刻的角度，Gyro 为陀螺仪测的角速度的值，dt是两次滤波之间的时间间隔,float dt=0.005; 这是程序中的定同时Q_bias也是一个变化的量,即使漂移误差。
通过上面（1）式即可得到下面矩阵形式：
2.
3
.
4.
代码如下：
void Kalman_Filter(float Gyro,float Accel)
{
Angle+=(Gyro - Q_bias) * dt;
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; /
Pdot[1]= - PP[1][1];
Pdot[2]= - PP[1][1];/
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt;
PP[0][1] += Pdot[1] * dt;
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle;
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0;
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle += K_0 * Angle_err;
Q_bias += K_1 * Angle_err;
Gyro_x = Gyro - Q_bias;
}
参考文章地址：
1./home.php?do=blog&id=517775&mod=space&uid=779081
手把手教用ROS做机器人控制（二）--加速度计与陀螺仪两种融合算法：mahony 互补滤波器算法与EKF算法
如果觉得《手把手教用ROS做机器人控制（二）--加速度计与陀螺仪两种融合算法：mahony 互补滤波》对你有帮助，请点赞、收藏，并留下你的观点哦！
