写在前面
本案例实验采用RflySim平台，该平台可以高效快速编写代码，使用simulink模型搭建，可以见代码直接生成对应的C代码，并一键将代码烧录Pixhawk中，是一种快速开发平台，RflySim获取更多可以参考网站
开发需要
matlab2018b及以上版本
simulink可能需要对应版本才可以打开，我用的2018b的matlab
pixhawk2.4.8硬件
PX4 1.7.3stable版本软件代码
mahony思想
mahony互补滤波是一种高效的姿态解算方法。
mahony核心思想： 机体水平时，加速度计无法测量绕 Z 轴的旋转量，即偏航角。磁力计也有同样问题，无法测得绕磁轴的旋转量。故需要加速度计和磁力计同时对陀螺仪进行校正。
mahony算法步骤
1. 加速度计与磁力计测量值进行归一化。
对应代码：
2. 磁力计的输出在机体坐标系下，将其转换到导航坐标系。
对应代码：
3. 假定理想情况下，让导航坐标系n中x轴指向正北方向，并设这个新的导航坐标系下磁力计数据为[bx,by,bz]’， Z轴方向分量（即重力方向分量）相等，水平方向分量相等，可以将磁力计在xoy平面的投影折算到 x轴则容易得到：
by = 0；对应代码：
4. 计算理论载体坐标系b下的三轴加速度大小。
对应代码：
5. 将磁力计在导航坐标系n中的理想输出[bx,by,bz] 再次旋转到载体坐标系b中，得到在b系中的理想输出。
对应代码：
6. 将理想输出[bx,by,bz]与原始输出[mx,my,mz]叉乘得到误差emag，将其作为补偿项送给陀螺仪进行矫正，这部分只是用来修正yaw的还要加上加速度计补偿量，加速计测量值与理论值进行叉乘得到误差eacc，两者相加得到总误差e。
对应代码：
7. 误差e使用PI修正
对应代码：
8. 将PI修正量补偿到陀螺仪中
对应代码：
9. 采用一阶龙格库塔法求解四元数，并作归一化处理
对应代码：
mahony仿真实验
1. 如果有硬件设备可以打开（e4.4\mahony仿真实验\实物采集加速度计、陀螺仪、磁力计数据）目录下的cf.slx文件。
上图是搭建的一个简单的读取Pixhawk的陀螺、加计、磁罗盘数据的simulink模型通过编译烧录两个步骤就可以实现将搭建的模型以一个topic形式加入到源代码中。相当于在源代码的基础上添加了一个进程，并不改变源代码。（具体PSP工具箱下载安装等操作参考这个网站）
编译模型后下载烧录代码，通过遥控器5通道控制是否开始采集数据，并通过指示灯显示当前系统是否正常运转。
写卡期间可以晃动飞控，结束操作后，先通过遥控器5通道到关闭写卡操作，再关闭遥控器，最后给飞控断电。取出SD卡，使用读卡器读出SD数据。
采集完成数据后会生成一个.bin文件，文件名字可以有数据采集模型的datalog模块更改。
2. 打开e4.4\mahony仿真实验\实物采集加速度计、陀螺仪、磁力计数据\plot_filter.m文件
将数二进制数据读取解包并存住在名为da.mat的文件中。
第二行读取文件的名字，这个名字是我在datalog模块中设置的名字，也可以自行更改。
3. 将第2步得到的da.mat文件复制到e4.4\mahony仿真实验，并在文件夹同目录下打开Attitude_estimator.m文件，将第二行loda 后面改成第2步生成发文件名字。
4. 运行
红色线是加速度计直接解算结果
蓝色线是陀螺仪直接积分解算结果
黑色线是互补滤波解算结果
Roll解算结果
Pitch解算结果
红色线是磁力计直接解算结果
蓝色线是陀螺仪直接积分解算结果
黑色线是互补滤波解算结果
Yaw解算结果
因为yaw我没有做角度限制变化，所以看到上图是±90变化，这个可以自行更改。
以上是有硬件设备的，可以自行采集数据，将数据打包进行实验。
###没有硬件设备的可以直接使用e4.4\mahony仿真实验\da.mat文件作为数据源。
mahony实物实验
1. 连接好飞控并打开e4.5\mahony实物实验\cf.slx文件
cf1模块是设计的mahony互补滤波器其最终输出的是已经解算好的四元数，同时vehicle_altitude模块是PX4自带的姿态解算角度输出模块，可以作为解算对比。
上图是CF1模块，输出是mahony解算的四元数。以上用到的所有资料、数据、详细代码下载
下载烧录代码后，通过遥控器5通道指定是否进行写卡操作，并通过指示灯判断系统是否正常运行。
2. 写卡期间可以晃动飞控，结束操作后，先通过遥控器5通道到关闭写卡操作，再关闭遥控器，最后给飞控断电。取出SD卡，使用读卡器读出SD数据（一个.bin文件）
3. 打开e4.5\mahony实物实验\plot_filter.m文件进行数据读取。
通过解算写入的四元数数据得到三轴姿态角。
实物解算的Roll
实物解算的Pitch
实物解算的Yaw
上图可以看出互补滤波相对于PX4内部自带的解算机制还是差一些，解算结果有毛刺。
上述用到的数据、代码下载
