基于Manony滤波算法的姿态解算

姿态解算的目的是得到飞行器在导航坐标系下的欧拉角，同时还要求算法能及时的更新姿态角度数据。

一，坐标系

1，导航坐标系/地理坐标系：以东边，北边，天上为坐标系正轴建立坐标系，这样的坐标系有助于人类观察飞机。

2，机体坐标系：以飞机自身为坐标系。

飞行器上的姿态传感器读取的数据是在机体坐标系下的数值，必须转换成导航坐标系下的数值才对我们有用。方法是通过旋转机体坐标系，使其与导航坐标系重合。

二 ，旋转矩阵

旋转矩阵可用于表示一个旋转，利用旋转矩阵可表示机体坐标系下的向量旋转到导航坐标系下的角度。此角度就是欧拉角；旋转矩阵的推导如下：

其中V‘是旋转后的新向量[x1,y1,z1];

三，四元数旋转形式

四元数也可以用于表示一个旋转，且四元数的计算相比于旋转矩阵更加方便，两者可互补。

向量与四元数密切相关，先从向量的角度看旋转：

三维空间的旋转可看成绕轴u旋转θ角度：

将向量V和V’分解正交分解成平行于U和垂直于U的两个向量。

主要观察垂直于U的分向量Vt,从上往下看，可得：

可得Vt’=Vt×cosθ+(Vt×U)×sinθ

所以新向量V’=Vu+Vt’=Vu+(cosθ+U×sinθ)×Vt

令四元数q=(cosθ+U×sinθ)，所以V’=Vu+q*Vt

四元数知识：

四元数q=[cosΘ,sinθ×U]，q表示将某个向量V绕向量U旋转θ角度。
若v平行是纯四元数，而q=[a,bu]，u是单位向量，若v平行平行于u，则q×v平行=v平行×q（满足交换律）
若v平行是纯四元数，而q=[a,bu]，u是单位向量，若v垂直垂直于u，则q×v垂直=v垂直×q*

新的向量V’可由原向量V左乘p，并右乘p的共轭计算。

矩阵的左右乘法规则如下：

得到四元数的旋转矩阵形式：

四，计算欧拉角

四元数矩阵与角度旋转矩阵在数学上是旋转两种不同的表示方式，将两个矩阵的元素一一对应，即可求出角度α，β，γ：

两个3*3矩阵里的元素一一对应，可得欧拉角的计算公式：

五，一阶龙格库塔法更新四元数

以上步骤得到了四元数到欧拉角的计算公式。由于飞行器是动态的，欧拉角的数据需要实时更新，而陀螺仪传感器测量的是机体的角速度，需要使用角速度来更新欧拉角的大小。

方式上是使用一阶龙格库塔法解微分方程，一阶龙格库塔法的方程是：

y(n+1)=y(n)+▲t dy(n)/dt;

设四元数向量Q=[a,b,c,d]将Q代入y可得：

Q(n+1)=Q(n)+▲t dQ(n)/dt;

Q是我们通过上面的运算已经计算出来的向量，重点在对Q的时间导数：

然后根据一阶龙格库塔法，得到四元数的更新方程：

六，mahony滤波

理论上只需要陀螺仪读取角速度我们就能获取姿态角，但由于陀螺仪在积分的过程中会积累误差，所以需要用加速度计在水平面上对重力进行比对和补偿，用来修正陀螺仪的垂直误差。

方法是：读取加速度计的数据，并由四元数计算出重力分量，加速度计的数据与重力分量的叉积得到误差，对误差进行比例，积分运算，补偿到角速度中。

//归一化加速度计数据
recipNorm=invSqrt(ax*ax+ay*ay+az*az);
ax*=recipNorm;
ay*=recipNorm;
az*=recipNorm;

//由四元数计算重力分量
vx=2*(b*d-a*c);
vy=2*(a*b+c*d);
vz=2*(a*a+d*d-b*b-c*c);

//叉积求误差
ex=(ay*vz-az*vy);
ey=(az*vx-ax*vz);
ez=(ax*vy-ay*vx);

//对误差进行积分运算
inte_ex+=ki*ex*t;
inte_ey+=ki*ey*t;
inte_ez+=ki*ez*t;

//gx,gy,gz即角速度向量
gx+=inte_ex;
gy+=inte_ey;
gz+=inte_ez;

//对误差进行比例运算
gx+=kp*ex;
gy+=kp*ey;
gz+=kp*ez;

推荐资料

姿态解算算法笔记
一阶龙格库塔法
欧拉角 旋转矩阵
四元数与欧拉角