调试 Mahony 滤波算法的思考 17

1. 说在前面的

之前调试基于QMI8658 6轴姿态解算的时候，我对Mahony滤波的认识还比较浅薄。初次的学习和代码的移植让我对四元数、欧拉角、旋转余弦矩阵有了一定的接触。然而，直到我将地磁传感器加入到我的硬件，进行9轴姿态解算时，我深深地感受到对四元数与姿态阵之间关系的理解是非常重要的。在此之前，我检索了网络文章，并记录了《MEMS_惯性传感器09 - Mahony姿态解算算法详解》、《MEMS_惯性传感器14 - Mahony滤波算法的代码分析》两篇文章。可能对于大佬、资深工程师来说，这些文章的严谨性不够，可能会误导别人。但我必须承认，我仍然没有扎实的数学计算研究背后更深层次的理论。即使如此，我依然希望通过自己的努力在惯性导航算法上学习得更深入一些。同时，我也想多记录一些学到的知识，多记录一些自己的理解和思考。希望这些记录能帮助到一些初学者。如果我的文章有错误的引导，我非常希望大佬们能够指正，因为这对我来说是最大的收获，我非常乐意倾听和学习。

我愿意自我突围，向着理想前进！

这次学习检索时发现了一个博文内容硬核、有理有据的博主¹路痴导航员。 她的博文 《AHRS互补滤波（Mahony）算法及开源代码》对我帮助很大。

2.Mahony滤波算法的核心思想

Mahony滤波算法的核心思想是通过将角速度和角度的测量值与历史估计值进行加权平均，从而得到更加准确的姿态估计结果。同时，滤波算法还利用加速度计的测量值来纠