姿態解算
姿態傳感器讀出加速度和角速度，而對一個系統的自動控制往往需要更加上層和貼近應用的的一個屬性：角度。所以需要通過加速度和角速度進行數據融合轉化得到姿態角度。
以MPU6050為例，姿態解算有硬解算即DMP解算和軟解算，配置傳感器內部的運動處理器搭配DMP軟件驅動庫可進行DMP解算，根據IMU的特性在MCU等開發平台上進行一些移植就可以輸出三個姿態角，軟件解算則只關注將傳感器原始數據進行算法融合，這里我們只討論軟件姿態解算。
互補濾波
軟件解算的核心原理是互補濾波，實際上單獨通過角速度積分可以得出角度，單獨通過加速度根據反切也能得到角度，但是這兩種方式都有着自己的缺陷：通過角速度積分得到的角度因為積分會積累誤差，所以准確度會隨着積分時間的延長逐漸減少，而角加速度瞬間穩定性較差，通過角加速度得出的角度穩定時效果還可以，但是當物體運動時效果便不理想。互補濾波就是針對這種情況將兩者結合起來，揚長補短。首先通過角速度的積分得出角度值，然后再用加速度對此值進行不斷矯正，這樣便可達到動態和長時間的情況所得角度值都比較准確的效果。
一階互補濾波的簡單C語言實現姿態解算
基本步驟：
1.根據反切公式由加速度求出角度
2.角速度積分求出角度，加積分限幅
3.求出兩者誤差對角速度積分進行補償
4.調節積分參數和角度融合參數
注：
1.求得是哪一軸角度需要注意，取決於積分用的那一軸的角速度
2.僅適用於平衡車等簡單場景，不適用於無人機飛控，因存在萬向節死鎖問題
#define PI_MATH 3.14159265358979f
#define GYRO_K 0.005
#define ANGLE_K 0.02
float angle_get( float gyro_y,float gyro_z,float accel_x,float accel_z)
{
float acc_angle_y,angle_err,angle_out;
static float gyro_integration;
acc_angle_y = atan2(accel_x,accel_z) * 180 / PI_MATH;
gyro_integration += gyro_y * GYRO_K;
if(acc_angle_y >= gyro_integration + 10) acc_angle_y = gyro_integration + 10 ;
if(acc_angle_y <= gyro_integration - 10) acc_angle_y = gyro_integration - 10 ;
angle_err = acc_angle_y - gyro_integration;
gyro_integration += angle_err * ANGLE_K;
angle_out = gyro_integration;
return angle_out;
}
四元數姿態解算
IMU（慣性導航單元）包含三軸的加速度和三軸的角速度，通過每個軸的數據融合可實現姿態解算，姿態實質上是機體坐標系相對於地理坐標系的狀態，表達姿態以地理坐標系為基准表達兩者的轉化關系。機體的姿態可以用向量在地理坐標系各軸夾角余弦（投影）來表示。三個向量在三個軸上的方向余弦可組成一個三階矩陣。
姿態有三種表達方式：歐拉角、余弦旋轉矩陣和四元數，歐拉角雖然最符合人們的思維習慣，但是存在着萬向節死鎖問題（旋轉中存在兩軸重合的情況下就無法按規則正確到達目標位置），而且計算較慢，所以使用計算快、內存小的四元數進行姿態解算的過程，解算完成再生成可用的歐拉角。
這里使用了四元數形式的PI控制思想的Mahony互補濾波算法，使用傳感器讀出的角速度求出四元數，提取四元數中的重力分量，使用機體坐標系上的重力分量和機體坐標系的向量進行差積運算得出誤差，
對此重力（也就是加速度誤差）進行積分和比例運算（PI）然后補償到陀螺儀上修正角速度積分漂移誤差。q為四元數，b為機體坐標系，R為地理坐標系，則地理坐標系到機體坐標系的轉換為：
根據余弦矩陣形式的歐拉角旋轉表達和四元數所表示的旋轉關系之間的對等。可以用四元數求出歐拉角。四元數轉換為歐拉角，即三維空間角度：
Mahony互補濾波算法C實現
筆者閱讀過多種無人機飛控源碼，Mahony互補濾波算法是姿態解算時最常用的，源碼實現也大同小異。最后總結主要步驟為：
1、歸一化重力加速度
2、使用各軸夾角余弦矩陣求出估計重力分量
3、估計重力方向與測量重力方向的叉積之和求出誤差
4、積分誤差反饋調節及比例反饋調節（互補濾波）
5、一階龍格庫塔求解四元數微分方程
6、歸一化四元數
7、四元數轉歐拉角
float my_sqrt(float number)
{
long i;
float x, y;
const float f = 1.5F;
x = number * 0.5F;
y = number;
i = * ( long * ) &y;
i = 0x5f3759df - ( i >> 1 );
y = * ( float * ) &i;
y = y * ( f - ( x * y * y ) );
y = y * ( f - ( x * y * y ) );
return number * y;
}
#define Kp (80.0f)
#define Ki (50.0f)
void mahony_imu_update(float *gyro , float *acc*,float *angle*)
{
float gx,gy, gz, ax, ay, az;
gx = gyro[0];
gy = gyro[1];
gz = gyro[2];
ax = acc[0];
ay = acc[1];
az = acc[2];
static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
static float exInt = 0, eyInt = 0, ezInt = 0;
float norm;
float vx, vy, vz;// wx, wy, wz;
float ex, ey, ez;
float halfT =0.001f ;
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
{
// Normalise accelerometer measurement
norm = my_sqrt(ax * ax + ay * ay + az * az);
norm = 1/norm;
ax *= norm;
ay *= norm;
az *= norm;
// Estimated direction of gravity and vector perpendicular to magnetic flux
vx = (q1 * q3 - q0 * q2)*2;
vy = (q0 * q1 + q2 * q3)*2;
vz = q0 * q0 - q1*q1-q2*q2 + q3 * q3;
// Error is sum of cross product between estimated and measured direction of gravity
ex = (ay * vz - az * vy);
ey = (az * vx - ax * vz);
ez = (ax * vy - ay * vx);
// Compute and apply integral feedback if enabled
exInt = exInt + ex * Ki;
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
// Apply proportional feedback
gx += Kp*ex + exInt;
gy += Kp*ey + eyInt;
gz += Kp*ez + ezInt;
}
// Integrate rate of change of quaternion
q0 += (-q1 * gx - q2 * gy -q3 * gz)*halfT;
q1 += (q0 * gx + q2 * gz - q3 * gy)*halfT;
q2 += (q0 * gy - q1 * gz + q3 * gx)*halfT;
q3 += (q0 * gz + q1 * gy - q2 * gx)*halfT;
// Normalise quaternion
norm = my_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
norm = 1/norm;
q0 *= norm;
q1 *= norm;
q2 *= norm;
q3 *= norm;
angle[PITCH]=asin(-2*q1*q3 + 2*q0*q2) * 57.3;
angle[ROLL] = atan2(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1) * 57.3;
angle[YAW] = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1) * 57.3;
}