https://blog.csdn.net/qq_21842557/article/details/50993809
上面博客有关于磁力计的详细解释,不过由于本人资质愚钝,至今还不是完全理解,不过思想大致和加速度计差不多。
结合下面代码看效果更好
大致意思如下
- 加速度计 已知地理坐标系的标准重力加速度 [0 0 1],然后变换到机体坐标系,理论与实际叉乘求出误差
- 磁力计 由于标准重力加速度在所有地方都是一样的,可以由其直接从地理坐标系变换到机体坐标系,但是在地理坐标系中,地磁大小却不是相同的,需要根据实际情况测出来,因此磁力计的来源则必然是实际测量,结合代码,首先进行机体坐标系变换到地理坐标系,相当于求出理论地磁(至于怎么求出的原谅我愚钝,看了几遍没看懂),然后变换到机体坐标系中,和重力加速度类似,利用理论和实际测量叉乘求出误差
- 将误差累加,利用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;