上一篇講了四元數、歐拉角和方向餘弦的知識，不熟悉的請到這篇部落格檢視點選打開連結
介紹兩種算法前，先定義兩個坐标系。導航坐标系(參考坐标系)n，選取東北天右手直角坐标系作為導航坐标系n、載體坐标系(機體坐标系)b，選取右手直角坐标系定義：四軸向右為X正方向，向前為Y軸正方向，向上為Z軸正方向。
互補濾波算法：通過加速度計的輸出穩定性來修正陀螺儀的積分漂移誤差。(既通過修正陀螺儀的測量角速度來實作)一句話就是利用加速度計來修正陀螺儀。
取導航坐标系中的标準重力加速度
，定義為
，那麼将
轉換至載體坐标系b中的表達式為
定義載體坐标系b系中加速度計的輸出為
，由于重力加速度為标準重力加速度，是以這裡要對加速度計輸出歸一化後才能繼續運算，是以歸一化後的載體坐标系加速度計輸出為
。
對
和
做向量積運算，即可得到對陀螺儀的補償校正誤差
其中
為向量積運算，坐标運算公式為
，
，
采用PI控制器來消除誤差
其中
即為陀螺儀的校正補償項，
采用離散累加來計算。
将
補償到陀螺儀後，再由四元數微分方程計算出最後的姿态。下面給出互補濾波的算法原理框圖
梯度下降法：梯度下降法的思想與互補濾波相同，都是用加速度計的穩定性來補償陀螺儀的漂移。
不同點：梯度下降法是由加速度計計算出一組姿态四元數 四旋翼姿态解算常用的兩種算法-互補濾波和梯度下降算法 ，再和陀螺儀計算出來的姿态四元數 四旋翼姿态解算常用的兩種算法-互補濾波和梯度下降算法 進行融合。互補濾波算法是利用中立即速度轉換到b系後與歸一化的b系加加速度計輸出做向量積，求得陀螺儀的校正誤差來校正陀螺儀。
因為在求解 四旋翼姿态解算常用的兩種算法-互補濾波和梯度下降算法 中用了梯度下降的原理，是以稱為梯度下降算法。
首選構造目标函數
:
對目标函數求偏導得到對應的雅可比矩陣
:
再由雅可比矩陣求出對應目标函數的梯度
四旋翼姿态解算常用的兩種算法-互補濾波和梯度下降算法
根據梯度下降法的定義有：
式中，
為梯度下降的步長。然後對将陀螺儀計算出來的姿态
和梯度下降法求出來的姿态
進行融合，
其中
為權重，并且滿足
。上述公式取得最優姿态解的條件為
的收斂速度和
的發散速度相等，是以可得，
式中，
為系統的采樣周期，
為陀螺儀的測量誤差，可以查詢PDF得到。因為四旋翼在高速運作下，
比較大，是以上式近似為，
由于
比較大，則對由梯度下降計算公式
來說，上一次的姿态可以忽略，直接由梯度負方向疊代到目标姿态，即可以重新定義為
由陀螺儀計算的姿态四元數公式為，
将
、
和
公式代入
得到，
将上述公式簡單定義為如下所示，
其中
是以可以得到梯度下降算法的姿态融合公式，
下面給出梯度下降的算法原理框圖
這兩種方法介紹完畢，都是自己的了解，歡迎大家交流指正。
由于梯度下降的代碼，師兄已經在部落格中貼出，這裡我就不再給出，大家可以到這個部落格中檢視點選打開連結
互補濾波的代碼給出如下，
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;
float delta;
gx = gx * DEG2RAD;
gy = gy * DEG2RAD;
gz = gz * DEG2RAD;
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
{
arm_sqrt_f32(ax * ax + ay * ay + az * az, &recipNorm);
ax /= recipNorm;
ay /= recipNorm;
az /= recipNorm;
halfvx = att.q[1] * att.q[3] - att.q[0] * att.q[2];
halfvy = att.q[0] * att.q[1] + att.q[2] * att.q[3];
halfvz = att.q[0] * att.q[0] - 0.5f + att.q[3] * att.q[3];
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
if(g_twoKi > 0.0f)
{
g_integralFBx += g_twoKi * halfex * CNTLCYCLE;
g_integralFBy += g_twoKi * halfey * CNTLCYCLE;
g_integralFBz += g_twoKi * halfez * CNTLCYCLE;
gx += g_integralFBx;
gy += g_integralFBy;
gz += g_integralFBz;
}
else
{
g_integralFBx = 0.0f;
g_integralFBy = 0.0f;
g_integralFBz = 0.0f;
}
gx += g_twoKp * halfex;
gy += g_twoKp * halfey;
gz += g_twoKp * halfez;
}
/*
*
* . 1
* q = - * q x Omega
* 2
* .
* [q0] [0 -wx -wy -wz] [q0]
* .
* [q1] [wx 0 wz -wy] [q1]
* . = *
* [q2] [wy -wz 0 wx] [q2]
* .
* [q3] [wz wy -wx 0 ] [q3]
*/
gx *= (0.5f * CNTLCYCLE);
gy *= (0.5f * CNTLCYCLE);
gz *= (0.5f * CNTLCYCLE);
qa = att.q[0];
qb = att.q[1];
qc = att.q[2];
delta = (CNTLCYCLE * gx) * (CNTLCYCLE * gx) + (CNTLCYCLE * gy) * (CNTLCYCLE * gy) + (CNTLCYCLE * gz) * (CNTLCYCLE * gz);
att.q[0] = (1.0f - delta / 8.0f) * att.q[0] + (-qb * gx - qc * gy - att.q[3] * gz);
att.q[1] = (1.0f - delta / 8.0f) * att.q[1] + ( qa * gx + qc * gz - att.q[3] * gy);
att.q[2] = (1.0f - delta / 8.0f) * att.q[2] + ( qa * gy - qb * gz + att.q[3] * gx);
att.q[3] = (1.0f - delta / 8.0f) * att.q[3] + ( qa * gz + qb * gy - qc * gx);
arm_sqrt_f32(att.q[0] * att.q[0] + att.q[1] * att.q[1] + att.q[2] * att.q[2] + att.q[3] * att.q[3], &recipNorm);
att.q[0] /= recipNorm;
att.q[1] /= recipNorm;
att.q[2] /= recipNorm;
att.q[3] /= recipNorm;
att.angle[PITCH] = atan2(2.0f * att.q[2] * att.q[3] + 2.0f * att.q[0] * att.q[1], -2.0f * att.q[1] * att.q[1] - 2.0f * att.q[2]* att.q[2] + 1.0f) * RAD2DEG; // Pitch
att.angle[ROLL] = asin(-2.0f * att.q[1] * att.q[3] + 2.0f * att.q[0]* att.q[2]) * RAD2DEG; // Roll
att.angle[YAW] = atan2(2.0f * att.q[1] * att.q[2] + 2.0f * att.q[0] * att.q[3], -2.0f * att.q[3] * att.q[3] - 2.0f * att.q[2] * att.q[2] + 1.0f) * RAD2DEG; // Yaw