下面是我对匿名源码修改整理后得到的代码，去掉了冗余便于讲解，此外给出了一些优化建议。
一、函数调用
二、Drv_Icm20602_Read()
读取16位ADC采样值
三、imu_origDataPrepare()
因为传感器安装角度和载体坐标系可能不一致，因此需要一个基础的坐标转换，最终得到x前、y右、z下的载体坐标系。
因为加速度计对高频抖动敏感，因此需要一个低通滤波器。陀螺仪加一个低通滤波无伤大雅。
当传感器不在载体中心时，载体旋转时传感器会有一个向心加速度，该加速度会被传感器测量出来，因此要去掉。
将16位ADC转换为物理单位值，对于该例程为±16g的加速度，单位是cm/s^2，就是说重力加速度测出来的值是980；角速度为±2000dps（degree per second）。
注：
之后使用时建议根据场景降低量程，提高精度
四、imu_postureSolution()
姿态解算的基本思路就是更新四元数，并且利用四元数到旋转矩阵的公式更新旋转矩阵。
下面是具体的实现：
初始时要求使模长为1，然后通过迭代得到正确的四元数。
这个是旋转矩阵的定义，最终求解欧拉角是利用该旋转矩阵。
读取载体坐标系下的加速度，并单位化。
利用四元数求旋转矩阵，同时也获得了世界坐标系下重力加速度(0,0,1)T在载体坐标系下的各轴分量。
求PI积分值。这里使用测量的角速度加上误差。由于误差是叉积，无量纲，因此可以理解为与角速度同一量纲，然后乘上时间就是更新的角度量。
说明：
1.初始四元数随意给定，因此刚开始会有一个较长的迭代时间将四元数收敛到正确的值。解决办法是刚开始（或判断无人机停止飞行时），将PI参数调大，使之快速收敛；
2.为了得到更好的效果，应当在初始开机后对加速度计、角速度计求零偏，即使用较长的时间求测量值的平均值，对之后的数据减去这个零偏。这个的好处包括：增大PI响应速度，在没有磁力计的情况下减小yaw漂移。
注：向量积无法区分0-90度和90-180度：
因此使用向量积作为误差，姿态角roll和pitch取值范围为±90度。
yaw之所以是±180度，是因为还增加了点乘判断向量方向：
五、imu_getEuler()
通过旋转矩阵求解欧拉角：
atan2相关知识汇总_weixin_42142612的博客-CSDN博客
/weixin_42142612/article/details/80972768
欧拉角和旋转矩阵的相互转换 - summer91 - 博客园
/larry-xia/p/11926121.html
如果觉得《匿名mahony互补滤波代码详解》对你有帮助，请点赞、收藏，并留下你的观点哦！
