【KW41Z】FXOS8700CQ (3D加速度计+3D磁力计) 融合算法

dvd1478

【KW41Z】FXOS8700CQ (3D加速度计+3D磁力计) 融合算法 [复制链接]

【KW41Z】FXOS8700CQ  (3D加速度计+3D磁力计)
https://bbs.eeworld.com.cn/forum ... 1318&fromuid=194541
(出处: 电子工程世界-论坛)

有一定的认识，这里主要是讲解融合算法
欧拉角是存在万向锁的问题，所以引入了Roll Pitch Yaw ，其相关的定义如下：




再看芯片

刚好是一个方向的，所以不用取反。

对应的推导结果公式如下：
Roll :

Pitch :
.
Yaw:



相关代码

1. /* Read sensor data */
   
2.             Sensor_ReadData(&g_Ax_Raw, &g_Ay_Raw, &g_Az_Raw, &g_Mx_Raw, &g_My_Raw, &g_Mz_Raw);
   
3. 
   
4.             /* Average accel value */
   
5.             for (i = 1; i < MAX_ACCEL_AVG_COUNT; i++)
   
6.             {   //缓冲区
   
7.                 g_Ax_buff[i] = g_Ax_buff[i - 1];
   
8.                 g_Ay_buff[i] = g_Ay_buff[i - 1];
   
9.                 g_Az_buff[i] = g_Az_buff[i - 1];
   
10.             }
    
11. 
    
12.             g_Ax_buff[0] = g_Ax_Raw; //最新的数据
    
13.             g_Ay_buff[0] = g_Ay_Raw;
    
14.             g_Az_buff[0] = g_Az_Raw;
    
15. 
    
16.             for (i = 0; i < MAX_ACCEL_AVG_COUNT; i++) //求平均值
    
17.             {
    
18.                 g_Ax += (double)g_Ax_buff[i];
    
19.                 g_Ay += (double)g_Ay_buff[i];
    
20.                 g_Az += (double)g_Az_buff[i];
    
21.             }
    
22. 
    
23.             g_Ax /= MAX_ACCEL_AVG_COUNT;
    
24.             g_Ay /= MAX_ACCEL_AVG_COUNT;
    
25.             g_Az /= MAX_ACCEL_AVG_COUNT;
    
26. 
    
27.             if(g_FirstRun)
    
28.             {
    
29.                 g_Mx_LP = g_Mx_Raw;
    
30.                 g_My_LP = g_My_Raw;
    
31.                 g_Mz_LP = g_Mz_Raw;
    
32.             }
    
33. 
    
34.             g_Mx_LP += ((double)g_Mx_Raw - g_Mx_LP) * 0.01;  //低通滤波
    
35.             g_My_LP += ((double)g_My_Raw - g_My_LP) * 0.01;
    
36.             g_Mz_LP += ((double)g_Mz_Raw - g_Mz_LP) * 0.01;
    
37. 
    
38.             /* Calculate magnetometer values 减去偏差*/
    
39.             g_Mx = g_Mx_LP - g_Mx_Offset;
    
40.             g_My = g_My_LP - g_My_Offset;
    
41.             g_Mz = g_Mz_LP - g_Mz_Offset;
    
42. 
    
43.             /* Calculate roll angle g_Roll (-180deg, 180deg) and sin, cos */
    
44.             g_Roll = atan2(g_Ay, g_Az) * RadToDeg;
    
45.             sinAngle = sin(g_Roll * DegToRad);
    
46.             cosAngle = cos(g_Roll * DegToRad);
    
47. 
    
48.             /* De-rotate by roll angle g_Roll */
    
49.             By = g_My * cosAngle - g_Mz * sinAngle;
    
50.             g_Mz = g_Mz * cosAngle + g_My * sinAngle;
    
51.             g_Az = g_Ay * sinAngle + g_Az * cosAngle;
    
52. 
    
53.             /* Calculate pitch angle g_Pitch (-90deg, 90deg) and sin, cos*/
    
54.             g_Pitch = atan2(-g_Ax , g_Az) * RadToDeg;
    
55.             sinAngle = sin(g_Pitch * DegToRad);
    
56.             cosAngle = cos(g_Pitch * DegToRad);
    
57. 
    
58.             /* De-rotate by pitch angle g_Pitch */
    
59.             Bx = g_Mx * cosAngle + g_Mz * sinAngle;
    
60. 
    
61.             /* Calculate yaw = ecompass angle psi (-180deg, 180deg) */
    
62.             g_Yaw = atan2(-By, Bx) * RadToDeg;
    
63.             if(g_FirstRun)
    
64.             {
    
65.                 g_Yaw_LP = g_Yaw;
    
66.                 g_FirstRun = false;
    
67.             }
    
68. 
    
69.             g_Yaw_LP += (g_Yaw - g_Yaw_LP) * 0.01;//低通滤波

复制代码

guanyanqin

楼主，你这个不用进行椭球拟合校准吗？这样的数据测出来准确？

me浩

楼主，你好。g_Mx_Offset，RadToDeg，DegToRad这几个值怎么取呢？

me浩

楼主，你好，能发一些融合算法的研究资料给我吗？875640457@qq.com。谢谢！