融合磁力计的Mahony互补滤波算法

https://blog.csdn.net/qq_21842557/article/details/50993809

上面博客有关于磁力计的详细解释，不过由于本人资质愚钝，至今还不是完全理解,不过思想大致和加速度计差不多。
结合下面代码看效果更好

大致意思如下

1. 加速度计 已知地理坐标系的标准重力加速度 [0 0 1]，然后变换到机体坐标系，理论与实际叉乘求出误差
2. 磁力计 由于标准重力加速度在所有地方都是一样的，可以由其直接从地理坐标系变换到机体坐标系，但是在地理坐标系中，地磁大小却不是相同的，需要根据实际情况测出来，因此磁力计的来源则必然是实际测量，结合代码，首先进行机体坐标系变换到地理坐标系，相当于求出理论地磁（至于怎么求出的原谅我愚钝，看了几遍没看懂），然后变换到机体坐标系中，和重力加速度类似，利用理论和实际测量叉乘求出误差
3. 将误差累加，利用PI补偿至陀螺仪，求解四元数，变换成欧拉角。

// 加速度计、地磁计、陀螺仪数据融合，更新四元数
/*
   [gx,gy,gz]为陀螺仪的测量值
   [ax,at,az]为加速度的测量值
   [mx,my,mz]为地磁计的测量值 
*/
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) 
{  
            float norm;               
            float hx, hy, hz, bx, bz;