姿态解算之显式互补滤波法（MahonyAHRS）（包含初始姿态角的计算）

姿态解算之显式互补滤波法（MahonyAHRS）

1 前言

本文以四旋翼为背景，说明一下姿态解算中经典的互补滤波算法，也称为Mahony算法。
文章说明：本文使用的机体系是“右-前-上”坐标系， 导航系是“东-北-天坐标系”
欧拉角的旋转顺序为： Z-X-Y,（即312的顺序） 绕Z轴转动是偏航角，绕x轴转动是俯仰角，绕y轴转动是横滚角。
由导航系到机体系的旋转矩阵为：

2 互补滤波的原理

两路信号，一个带有高频噪声，一个带有低频噪声，所以我把他们的噪声分别滤除，然后合并，就得到了没有噪声的原始信号。

算法框图:

3 算法分析

3.1 初始姿态获取

四旋翼放在某个位置，根据加速度计和磁力计的位置可以获取到它的初始姿态
其中由加速度计数据可以获取到横滚角 和俯仰角 ，由磁力计数据可以获取到航向角
所以可得俯仰角和横滚角：

此时我们已经知道了俯仰角和横滚角，那么由此我们便可以计算航向角了。

地磁场是一个矢量，这个矢量可以分解成一个水平分量和一个垂直分量，当四旋翼或者磁力计与水平面平行时，此时根据磁力计的两个水平分量便可计算出传感器x轴与磁北的夹角。

此时航向角:

如果我们认为磁北是0度的话，那么就可以得到当前的航向角
但是四旋翼或者磁力计往往都不是处于水平面上，那么我们就需要将磁力计的数据转换到水平面上。此时只需要旋转横滚和俯仰角，即令航向角为0
得到：

此时：

即只将横滚和俯仰角转到导航系，不动航向角，仅仅是将机体系上旋转至水平状态。
这样一来便可得到航向角 ：

注意：我们这里使用的加速度数据是四旋翼在静止或者相对静止的得到的数据，否则是不成立的。

3.2 互补原理以及算法实现

这里低通滤波器和高通滤波器我就简单叙述一下
低通滤波就是可以滤除高频噪声，例如加速度计中存在的就是高频噪声，高通滤波器可以滤除低频噪声，例如陀螺仪中就存在由于低频噪声。

算法实现：

matlab程序：

function Q = MahonyA_Update(Q, dt, Accelerometer, Gyroscope, Magnetometer)
%% Function Introduction
% 本函数是采用 Mahony论文中的互补滤波方法更新四元数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Variables:
%   The input is: 四元数，采样时间dt，加计数据，陀螺数据，磁力计数据 
%   The output as:更新后的四元数  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Author : 
% Version :  
% Date : 
% File : 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    Kp = 5;
	q = Q;        
    % 归一化加计和磁计数据
    Accelerometer = Accelerometer / norm(Accelerometer);    
    Magnetometer = Magnetometer / norm(Magnetometer);   
    C_b_n = quaternion_rotation(q); % 机体系----》导航系
    C_n_b = C_b_n';
    h = C_b_n * Magnetometer'; 
    b = [0; norm([h(1) h(2)]); h(3)]; 
    v =  C_n_b * [0;0;1];
    e = cross(Accelerometer, v) + cross(Magnetometer, w); 
    % Gyroscope为补偿后的陀螺数据, 这里只是简单的比例P控制，MahonyB中的是PI控制器，
    Gyroscope = Gyroscope + Kp * e;           
    q = qmul(q, [1 0.5*dt*Gyroscope]); % 这一句和下面注释的是等价的
    %qDot = 0.5 * qmul(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]);
    %q = q + qDot * dt; 
    Q = q / norm(q); % 更新后的四元数   
end

重点解释：为什么 cross(Accelerometer, v) 叉乘结果可以对陀螺仪进行补偿？
其中Accelerometer表示归一化后的加速度计三轴数据，v由方向余弦矩阵(导航系–>机体系)预测的三轴加速度数据，
所以： e = Accelerometer × v = |Accelerometer| · |v| · sinθ = sinθ ≈ θ 角度很小时成立
e近似等于当前时刻相对于上一时刻旋转的角度

本文也是借鉴了很多其他优秀的论文以及博客，由于能力有限，难免会有很多遗漏或者错误的地方。
该文章属于姿态解算中最最基础的姿态估计算法，后续还会推出其他的姿态估计算法。