姿态解算(二)，姿态更新

相關源碼請參考開源飛控StarryPilot:https://github.com/JcZou/StarryPilot

根据1阶Runge-Kutta可以得到四元数的更新方程为：

q˙=0.5q⋅Ωbnb q ˙ = 0.5 q ⋅ Ω n b b

其中 Ωbnb=[0,ωbx,ωby,ωbz] Ω n b b = [ 0 , ω x b , ω y b , ω z b ] ,表示在b系测的的b系相对于n系的角速度。
利用四元数乘法的矩阵形式，可以得到：

q˙=⎡⎣⎢⎢⎢w˙x˙y˙z˙⎤⎦⎥⎥⎥=0.5⎡⎣⎢⎢⎢wxyz−xwz−y−y−zwx−zy−xw⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢⎢0ωbxωbyωbz⎤⎦⎥⎥⎥⎥ q ˙ = [ w ˙ x ˙ y ˙ z ˙ ] = 0.5 [ w − x − y − z x w − z y y z w − x z − y x w ] [ 0 ω x b ω y b ω z b ]

即：

w˙=−0.5(xωbx+yωby+zωbz)x˙=0.5(wωbx−zωby+yωbz)y˙=0.5(zωbx+wωby−xωbz)z˙=−0.5(yωbx−xωby−wωbz)(49) (49) w ˙ = − 0.5 ( x ω x b + y ω y b + z ω z b ) x ˙ = 0.5 ( w ω x b − z ω y b + y ω z b ) y ˙ = 0.5 ( z ω x b + w ω y b − x ω z b ) z ˙ = − 0.5 ( y ω x b − x ω y b − w ω z b )

所以为了更新姿态，就要先计算出 ωb=[ωbx,ωby,ωbz] ω b = [ ω x b , ω y b , ω z b ] . 通过对陀螺仪的输出值，可以直接得到角速度，但由于陀螺仪有误差，随着时间的累积，误差也会积累，所以不能单纯用陀螺仪的的输出作为系统的角速度参数，需要使用其他的传感器来进行补偿，如加速度计和磁力计。
这里使用互补滤波算法，对加速度计和磁力计的数据进行融合。
设加速度计数据 ab=[ax,ay,az] a b = [ a x , a y , a z ] ，磁力计数据 mb=[bx,by,bz] m b = [ b x , b y , b z ] ，重力常量 An=[0,0,1] A n = [ 0 , 0 , 1 ] ，磁力常量 Mn=[1,0,0] M n = [ 1 , 0 , 0 ] ，为了构建误差，首先将 ab a b 与 mb m b 转换到n系，即:

an=Cnbabmn=Cnbmb(50) (50) a n = C b n a b m n = C b n m b

通过分别对 an与An a n 与 A n 以及 mn与An m n 与 A n 做叉积，构建误差：

errn=an×An+(mn×Mn)⎡⎣⎢000000001⎤⎦⎥ e r r n = a n × A n + ( m n × M n ) [ 0 0 0 0 0 0 0 0 1 ]

由于加速度测量值和磁力计测量值所构成的坐标系一般不正交，所以这里将 mn与Mn m n 与 M n 叉积的结果投影到z轴上，表示只通过磁场数据来纠正绕z轴方向的旋转。
然后将误差从n系变化到b系：

errb=Cbnerrn e r r b = C n b e r r n

所以最后的角速度可以表示为：

ωb=Ω+P⋅errb ω b = Ω + P ⋅ e r r b

其中 Ω Ω 为陀螺仪的输出值，P为一很小的标量