這塊區域主要是很多重點概念的理解吧：姿態解算、四元數、歐拉角、PID算法、互補濾波、卡爾曼濾波。這些不論哪一個都需要好好理解，才能將整個四軸飛行器飛控部分大致流程理解下來
-
互補濾波、卡爾曼濾波算法：
參考博客：https://blog.csdn.net/u013608300/article/details/52459515
在四軸飛行器用到了MPU6050六軸傳感器模塊，裏面包含加速度傳感器、陀螺儀，通過它們都可以得到三軸的角度。本來僅通過一個陀螺儀就可以得到姿態角，但是有一定誤差，就需要這兩個算法進行調整誤差。通過加速度計的數據對陀螺儀的數據進行校正。這裏我採用的是互補濾波算法。
互補濾波算法就是將兩者數據進行一個叉乘，將會得到一個誤差向量，再對這個誤差向量通過一定方法補償到陀螺儀數據中。這裏就參考這個博客。 -
四元數：
參考博客：https://www.cnblogs.com/hjlweilong/p/6018746.html
四元數：類似於高中學習的複數，它是一種超複數，二元複數由一個實數a加上一個複數bi，表示爲a+bi，i^2=-1，而四元數則是，a+bi+cj+dk，i^2=j^2=k^2=-1。
一個重點是四元數可以表示一個旋轉，通過一個點與四元數的四元數乘積可以得到一個新的四元數，這個轉化後的四元數的後三項即爲旋轉後的x、y、z軸座標。
二元複數可以表示爲一個平面，也就是二維空間，a和b表示二維空間上的座標，可以表示爲一個點進行二維旋轉。
而四元數表示的則是四維空間，a、b、c、d表示四維空間上的座標，可以表示爲一個點進行四維旋轉。
看上去好像是很難的樣子，因爲表示的是在四維空間上的運算，實際上的確是有一些複雜。
但是迴歸到主題，四元數這個工具是用於我們四軸飛行器求姿態角的，應對的是三維空間，可以將四元數表示爲一個在三維空間上的四維旋轉，也就是將這個四維空間限制在設立的一個三維空間內將這個3維空間進行旋轉，就像是在三維直角座標系中選取一個平面進行旋轉一樣。
舉個例子理解了一下，從平面旋轉來看，比如是你現在所在的平面是xoz，要旋轉到yoz平面上就需要引入三維空間，xoz平面圍繞z軸旋轉到yoz平面。
在四軸飛行器的姿態解算中也是需要完成一個旋轉的過程，需要將一個空間進行旋轉，這個空間就是四軸飛行器當前的參考空間，就是以四軸飛行器爲參考，它的前方上方右方分別對應x軸y軸z軸，我們姿態調整的目的就是需要讓四軸飛行器轉到與地球的參考空間相同，那就需要進行一個四維空間上對一個三維空間的旋轉。
如果你想算一個點(wx,wy,wz)在這個旋轉下新的座標,需要進行如下操作，
1.定義純四元數
qw = (0,wx,wy,wz) = 0 + wxi + wyj +wz*k
2.進行四元數運算
qw’ = q*qw*q^-1
3.產生的一定是純四元數，也就是說它的第一項爲0，有如下形式：
qw’ = (0,wx’,wy’,wz’) = 0 + wx’*i+wy’*j + wz’*k
5.中的後三項(wx’,wy’,wz’)就是：
W’ = (wx’,wy’,wz’)
可以看到這個四元數就是q，座標點是w，q與w以一定方式相乘後得到了一個四元數後三項就是轉換後的座標。
接下來就需要在程序中實現對這個四元數的求解，參考博客：
https://blog.csdn.net/qq_15063463/article/details/82455265
程序中需要求出q，知道了q就相當於知道了這個物體是如何旋轉的，q是一個單位旋轉向量，也就是說它的模是1，也就是式子q0^2+q1^2+q2^2+q3^2=1，不會改變目標座標的模。所以其中每次求得q後需要進行一個歸一化的操作，就是要保證這個向量的模爲一。
norm=1.0f/sqrt(ax*ax+ay*ay+az*az);
ax*=norm;
ay*=norm;
az*=norm;
//對加速度數據進行歸一化處理
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
//利用四元數參數來估算重力分量
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
//利用叉積來計算估算和實際重力的偏差
exInt = exInt + halfex * Ki;
eyInt = eyInt + halfey * Ki;
ezInt = ezInt + halfez * Ki;
//對重力差進行積分運算將積分後的結果累加到陀螺儀數據中，修正陀螺儀數據
gx += Kp*halfex + exInt;
gy += Kp*halfey + eyInt;
gz += Kp*halfez + ezInt;
//對重力差進行比例運算，將比例運算後的結果累加到陀螺儀數據中，用於修正陀螺儀數據
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;
//四元數方程的求解，使用了一階畢卡算法
norm = 1.0f/sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 *= norm;
q1 *= norm;
q2 *= norm;
q3 *= norm;
//將四元數歸一化
AngleX=asin(2*(q0*q2-q1*q3 ))* 57.2957795f; // 俯仰 pitch
AngleY=asin(2*(q0*q1+q2*q3 ))* 57.2957795f; // 橫滾 roll
這樣，就完成了一次四元數旋轉運算。
最後就得到了四元數，經過一個換算公式即可得到歐拉角，俯仰角pitch和橫滾角roll，而航向角yaw缺少地磁傳感器的數據小四軸可以不採用，所以這裏就不需要求。
AngleX=asin(2*(q0*q2-q1*q3 ))* 57.2957795f; // 俯仰 pitch
AngleY=asin(2*(q0*q1+q2*q3 ))* 57.2957795f; // 橫滾 roll
-
歐拉角
歐拉角是一種常用的描述方位的方法，是由歐拉提出的。基本思想就是將兩個座標系的變換分解爲繞三個不同的座標軸的三次連續轉動組成的序列。歐拉角的旋轉規定爲連續兩次旋轉，必須繞着不同的轉動軸旋轉，所以一共有12種旋轉順規。比如我們選用Z-Y-X的旋轉順規來描述b系與n系的關係。Z-Y-X順規就是指：繞Z軸旋轉偏航角(YAW)，繞Y軸旋轉橫滾角(ROLL)，繞X軸旋轉俯仰角(PITCH)。
所以通過互補濾波融合數據四元數解算飛行姿態，得到三軸歐拉角，再對這個過程引入PID算法，最後得到PID算法調整後的歐拉角，再與之前瞭解到的四軸飛行器飛行姿態轉換與PWM波控制電機的知識，在油門控制的基礎上，歐拉角乘以一定比例得到一個對電機轉速進行微調的參數speed，再把這個值加到對應的電機上。
參考歐拉角的概念，分別對三個軸進行角度的轉換就得到了目標的空間旋轉，對x軸旋轉是橫滾運動，對2、4電機速度進行調整；對y軸旋轉就是俯仰運動，改變1、3電機轉速；對z軸旋轉就是偏航運動，對1、3電機速度增加，對2、4電機速度減小。
接下來還需要理解PID算法，可以將整個飛控的大概輪廓理解了，不得不說這個四軸飛行器涵蓋的知識點是真的挺多的，要真正理解是要花上一定功夫的。