BlueMicrosystem2使用MahonyAHRS融合算法解出来的数据不对

littleshrimp

BlueMicrosystem2使用MahonyAHRS融合算法解出来的数据不对 [复制链接]

 
因为ST官方提供的MotionFX没用明白
在网上找了一个传感器融合的代码，输入9轴数据输出四元数用MahonyAHRS替换原来的MotionFX_manager_run

将四元数按照原来的方式输出到手机，用BLUEMS查看骰子是一直转动的
不知道问题出在哪，官方是直接把寄存器值输入到MotionFX库
单位是LSB
我在调用 MahonyAHRSupdate函数时把对应的寄存器做了转换
加速度的单位是g
磁场的单位是gauss
陀螺仪的单位是dps
难道问题出在这？

1. static void ComputeQuaternions(void)
   
2. {
   
3.   static SensorAxes_t quat_axes[SEND_N_QUATERNIONS];
   
4.   static int32_t calibIndex =0;
   
5.   static int32_t CounterFX  =0;
   
6.   static int32_t CounterEC  =0;
   
7.   SensorAxes_t ACC_Value;
   
8.   SensorAxesRaw_t ACC_Value_Raw;
   
9.   SensorAxes_t GYR_Value;
   
10.   SensorAxes_t MAG_Value;
    
11. 
    
12.   float ax,ay,az,gx,gy,gz,mx,my,mz;
    
13.   /* Increment the Counter */
    
14.   if(W2ST_CHECK_CONNECTION(W2ST_CONNECT_EC)) {
    
15.     CounterEC++;
    
16.   } else {
    
17.     CounterFX++;
    
18.   }
    
19. 
    
20.   /* Read the Acc RAW values */
    
21.   BSP_ACCELERO_Get_AxesRaw(TargetBoardFeatures.HandleAccSensor,&ACC_Value_Raw);
    
22. 
    
23.   /* Read the Magneto values */
    
24.   BSP_MAGNETO_Get_Axes(TargetBoardFeatures.HandleMagSensor,&MAG_Value);
    
25. 
    
26.   /* Read the Gyro values */
    
27.   BSP_GYRO_Get_Axes(TargetBoardFeatures.HandleGyroSensor,&GYR_Value);
    
28. 
    
29. //  MotionFX_manager_run(ACC_Value_Raw,GYR_Value,MAG_Value);
    
30.       
    
31. /*
    
32. LSM303AGR
    
33. Linear acceleration sensitivity mg/LSB
    
34. FS = ±2 g and in high-resolution mode -7% 0.98 +7%
    
35. FS = ±4 g and in high-resolution mode -7% 1.95 +7%
    
36. FS = ±8 g and in high-resolution mode -7% 3.9 +7%
    
37. FS = ±16 g and in high-resolution mode -7% 11.72 +7%
    
38. FS = ±2 g and in normal mode -7% 3.9 +7%
    
39. FS = ±4 g and in normal mode -7% 7.82 +7%
    
40. FS = ±8 g and in normal mode -7% 15.63 +7%
    
41. FS = ±16 g and in normal mode -7% 46.9 +7%
    
42. FS = ±2 g and in low-power mode -7% 15.63 +7%
    
43. FS = ±4 g and in low-power mode -7% 31.26 +7%
    
44. FS = ±8 g and in low-power mode -7% 62.52 +7%
    
45. FS = ±16 g and in low-power mode -7% 187.58 +7%
    
46. Magnetic sensitivity(2) mgauss/LSB
    
47. -7% 1.5 +7%
    
48.   
    
49. LSM6DSM
    
50. Linear acceleration sensitivity(2) mg/LSB
    
51. FS = ±2 0.061
    
52. FS = ±4 0.122
    
53. FS = ±8 0.244
    
54. FS = ±16 0.488
    
55.   
    
56. Angular rate sensitivity(2) mdps/LSB
    
57. FS = ±125 4.375
    
58. FS = ±245 8.75
    
59. FS = ±500 17.50
    
60. FS = ±1000 35
    
61. FS = ±2000 70
    
62. */
    
63.   //+-2g = 0.061mg/LSB
    
64.   ax = ACC_Value_Raw.AXIS_X * 0.061 / 1000.00;
    
65.   ay = ACC_Value_Raw.AXIS_Y * 0.061 / 1000.00;
    
66.   az = ACC_Value_Raw.AXIS_Z * 0.061 / 1000.00;
    
67.   //1.5 mgauss/LSB
    
68.   mx = MAG_Value.AXIS_X * 1.5 / 1000.00;
    
69.   my = MAG_Value.AXIS_Y * 1.5 / 1000.00;
    
70.   mz = MAG_Value.AXIS_Z * 1.5 / 1000.00;
    
71.   //+-2000 = 70mdps/LSB
    
72.   gx = GYR_Value.AXIS_X * 70.00 / 1000.00;
    
73.   gy = GYR_Value.AXIS_Y * 70.00 / 1000.00;
    
74.   gz = GYR_Value.AXIS_Z * 70.00 / 1000.00;
    
75.   
    
76.   MahonyAHRSupdate(gx,gy,gz,ax,ay,az,mx,my,mz);
    
77.   int32_t QuaternionNumber = (CounterFX>SEND_N_QUATERNIONS) ? (SEND_N_QUATERNIONS-1) : (CounterFX-1);
    
78. 
    
79.   /* Scaling quaternions data by a factor of 10000
    
80.     (Scale factor to handle float during data transfer BT) */
    
81. 
    
82.   /* Save the quaternions values */
    
83.   if(q3 < 0){
    
84.     quat_axes[QuaternionNumber].AXIS_X = (int32_t)(q0 * (-10000));
    
85.     quat_axes[QuaternionNumber].AXIS_Y = (int32_t)(q1 * (-10000));
    
86.     quat_axes[QuaternionNumber].AXIS_Z = (int32_t)(q2 * (-10000));
    
87.   } else {
    
88.     quat_axes[QuaternionNumber].AXIS_X = (int32_t)(q0 * 10000);
    
89.     quat_axes[QuaternionNumber].AXIS_Y = (int32_t)(q1 * 10000);
    
90.     quat_axes[QuaternionNumber].AXIS_Z = (int32_t)(q2 * 10000);
    
91.   }
    
92.    
    
93.   /* Every QUAT_UPDATE_MUL_10MS*10 mSeconds Send Quaternions informations via bluetooth */
    
94.   if(CounterFX==QUAT_UPDATE_MUL_10MS){
    
95.     Quat_Update(quat_axes);
    
96.     CounterFX=0;
    
97.   }
    
98. }

复制代码

1. //=====================================================================================================
   
2. // MahonyAHRS.c
   
3. //=====================================================================================================
   
4. //
   
5. // Madgwick's implementation of Mayhony's AHRS algorithm.
   
6. // See: [url]http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms[/url]
   
7. //
   
8. // Date                        Author                        Notes
   
9. // 29/09/2011        SOH Madgwick    Initial release
   
10. // 02/10/2011        SOH Madgwick        Optimised for reduced CPU load
    
11. //
    
12. //=====================================================================================================
    
13. 
    
14. //---------------------------------------------------------------------------------------------------
    
15. // Header files
    
16. 
    
17. #include "MahonyAHRS.h"
    
18. #include <math.h>
    
19. 
    
20. //---------------------------------------------------------------------------------------------------
    
21. // Definitions
    
22. 
    
23. #define sampleFreq        1000.0f                        // sample frequency in Hz
    
24. #define twoKpDef        (2.0f * 0.5f)        // 2 * proportional gain
    
25. #define twoKiDef        (2.0f * 0.0f)        // 2 * integral gain
    
26. 
    
27. //---------------------------------------------------------------------------------------------------
    
28. // Variable definitions
    
29. 
    
30. volatile float twoKp = twoKpDef;                                                                                        // 2 * proportional gain (Kp)
    
31. volatile float twoKi = twoKiDef;                                                                                        // 2 * integral gain (Ki)
    
32. volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;                                        // quaternion of sensor frame relative to auxiliary frame
    
33. volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;        // integral error terms scaled by Ki
    
34. 
    
35. //---------------------------------------------------------------------------------------------------
    
36. // Function declarations
    
37. 
    
38. float invSqrt(float x);
    
39. 
    
40. //====================================================================================================
    
41. // Functions
    
42. 
    
43. //---------------------------------------------------------------------------------------------------
    
44. // AHRS algorithm update
    
45. 
    
46. void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
    
47.         float recipNorm;
    
48.     float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;  
    
49.         float hx, hy, bx, bz;
    
50.         float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
    
51.         float halfex, halfey, halfez;
    
52.         float qa, qb, qc;
    
53. 
    
54.         // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
    
55.         if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
    
56.                 MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
    
57.                 return;
    
58.         }
    
59. 
    
60.         // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    
61.         if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
    
62. 
    
63.                 // Normalise accelerometer measurement
    
64.                 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    
65.                 ax *= recipNorm;
    
66.                 ay *= recipNorm;
    
67.                 az *= recipNorm;     
    
68. 
    
69.                 // Normalise magnetometer measurement
    
70.                 recipNorm = invSqrt(mx * mx + my * my + mz * mz);
    
71.                 mx *= recipNorm;
    
72.                 my *= recipNorm;
    
73.                 mz *= recipNorm;   
    
74. 
    
75.         // Auxiliary variables to avoid repeated arithmetic
    
76.         q0q0 = q0 * q0;
    
77.         q0q1 = q0 * q1;
    
78.         q0q2 = q0 * q2;
    
79.         q0q3 = q0 * q3;
    
80.         q1q1 = q1 * q1;
    
81.         q1q2 = q1 * q2;
    
82.         q1q3 = q1 * q3;
    
83.         q2q2 = q2 * q2;
    
84.         q2q3 = q2 * q3;
    
85.         q3q3 = q3 * q3;   
    
86. 
    
87.         // Reference direction of Earth's magnetic field
    
88.         hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
    
89.         hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
    
90.         bx = sqrt(hx * hx + hy * hy);
    
91.         bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
    
92. 
    
93.                 // Estimated direction of gravity and magnetic field
    
94.                 halfvx = q1q3 - q0q2;
    
95.                 halfvy = q0q1 + q2q3;
    
96.                 halfvz = q0q0 - 0.5f + q3q3;
    
97.         halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
    
98.         halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
    
99.         halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);  
    
100.        
     
101.                 // Error is sum of cross product between estimated direction and measured direction of field vectors
     
102.                 halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
     
103.                 halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
     
104.                 halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
     
105. 
     
106.                 // Compute and apply integral feedback if enabled
     
107.                 if(twoKi > 0.0f) {
     
108.                         integralFBx += twoKi * halfex * (1.0f / sampleFreq);        // integral error scaled by Ki
     
109.                         integralFBy += twoKi * halfey * (1.0f / sampleFreq);
     
110.                         integralFBz += twoKi * halfez * (1.0f / sampleFreq);
     
111.                         gx += integralFBx;        // apply integral feedback
     
112.                         gy += integralFBy;
     
113.                         gz += integralFBz;
     
114.                 }
     
115.                 else {
     
116.                         integralFBx = 0.0f;        // prevent integral windup
     
117.                         integralFBy = 0.0f;
     
118.                         integralFBz = 0.0f;
     
119.                 }
     
120. 
     
121.                 // Apply proportional feedback
     
122.                 gx += twoKp * halfex;
     
123.                 gy += twoKp * halfey;
     
124.                 gz += twoKp * halfez;
     
125.         }
     
126.        
     
127.         // Integrate rate of change of quaternion
     
128.         gx *= (0.5f * (1.0f / sampleFreq));                // pre-multiply common factors
     
129.         gy *= (0.5f * (1.0f / sampleFreq));
     
130.         gz *= (0.5f * (1.0f / sampleFreq));
     
131.         qa = q0;
     
132.         qb = q1;
     
133.         qc = q2;
     
134.         q0 += (-qb * gx - qc * gy - q3 * gz);
     
135.         q1 += (qa * gx + qc * gz - q3 * gy);
     
136.         q2 += (qa * gy - qb * gz + q3 * gx);
     
137.         q3 += (qa * gz + qb * gy - qc * gx);
     
138.        
     
139.         // Normalise quaternion
     
140.         recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
     
141.         q0 *= recipNorm;
     
142.         q1 *= recipNorm;
     
143.         q2 *= recipNorm;
     
144.         q3 *= recipNorm;
     
145. }
     
146. 
     
147. //---------------------------------------------------------------------------------------------------
     
148. // IMU algorithm update
     
149. 
     
150. void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
     
151.         float recipNorm;
     
152.         float halfvx, halfvy, halfvz;
     
153.         float halfex, halfey, halfez;
     
154.         float qa, qb, qc;
     
155. 
     
156.         // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
     
157.         if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
     
158. 
     
159.                 // Normalise accelerometer measurement
     
160.                 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
     
161.                 ax *= recipNorm;
     
162.                 ay *= recipNorm;
     
163.                 az *= recipNorm;        
     
164. 
     
165.                 // Estimated direction of gravity and vector perpendicular to magnetic flux
     
166.                 halfvx = q1 * q3 - q0 * q2;
     
167.                 halfvy = q0 * q1 + q2 * q3;
     
168.                 halfvz = q0 * q0 - 0.5f + q3 * q3;
     
169.        
     
170.                 // Error is sum of cross product between estimated and measured direction of gravity
     
171.                 halfex = (ay * halfvz - az * halfvy);
     
172.                 halfey = (az * halfvx - ax * halfvz);
     
173.                 halfez = (ax * halfvy - ay * halfvx);
     
174. 
     
175.                 // Compute and apply integral feedback if enabled
     
176.                 if(twoKi > 0.0f) {
     
177.                         integralFBx += twoKi * halfex * (1.0f / sampleFreq);        // integral error scaled by Ki
     
178.                         integralFBy += twoKi * halfey * (1.0f / sampleFreq);
     
179.                         integralFBz += twoKi * halfez * (1.0f / sampleFreq);
     
180.                         gx += integralFBx;        // apply integral feedback
     
181.                         gy += integralFBy;
     
182.                         gz += integralFBz;
     
183.                 }
     
184.                 else {
     
185.                         integralFBx = 0.0f;        // prevent integral windup
     
186.                         integralFBy = 0.0f;
     
187.                         integralFBz = 0.0f;
     
188.                 }
     
189. 
     
190.                 // Apply proportional feedback
     
191.                 gx += twoKp * halfex;
     
192.                 gy += twoKp * halfey;
     
193.                 gz += twoKp * halfez;
     
194.         }
     
195.        
     
196.         // Integrate rate of change of quaternion
     
197.         gx *= (0.5f * (1.0f / sampleFreq));                // pre-multiply common factors
     
198.         gy *= (0.5f * (1.0f / sampleFreq));
     
199.         gz *= (0.5f * (1.0f / sampleFreq));
     
200.         qa = q0;
     
201.         qb = q1;
     
202.         qc = q2;
     
203.         q0 += (-qb * gx - qc * gy - q3 * gz);
     
204.         q1 += (qa * gx + qc * gz - q3 * gy);
     
205.         q2 += (qa * gy - qb * gz + q3 * gx);
     
206.         q3 += (qa * gz + qb * gy - qc * gx);
     
207.        
     
208.         // Normalise quaternion
     
209.         recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
     
210.         q0 *= recipNorm;
     
211.         q1 *= recipNorm;
     
212.         q2 *= recipNorm;
     
213.         q3 *= recipNorm;
     
214. }
     
215. 
     
216. //---------------------------------------------------------------------------------------------------
     
217. // Fast inverse square-root
     
218. // See: [url]http://en.wikipedia.org/wiki/Fast_inverse_square_root[/url]
     
219. 
     
220. float invSqrt(float x) {
     
221.         float halfx = 0.5f * x;
     
222.         float y = x;
     
223.         long i = *(long*)&y;
     
224.         i = 0x5f3759df - (i>>1);
     
225.         y = *(float*)&i;
     
226.         y = y * (1.5f - (halfx * y * y));
     
227.         return y;
     
228. }
     
229. 
     
230. //====================================================================================================
     
231. // END OF CODE
     
232. //====================================================================================================
     

复制代码

littleshrimp

效果如图，感觉应该是陀螺仪输出的数据不对

yujie2510

陀螺仪，磁力计没有做校准？MotionFX库里调用的Acc应该是用的寄存器原始数据，Gyro应该是mdps单位，而Mag应该是mGauss的单位。

littleshrimp

yujie2510 发表于 2017-6-21 20:57
陀螺仪，磁力计没有做校准？MotionFX库里调用的Acc应该是用的寄存器原始数据，Gyro应该是mdps单位，而Mag应 ...

陀螺仪有一个轴静止时输出数据比较大
可能是这个原因
例程里掉用MahonyAHRSupdate函数的频率是100次每秒
MahonyAHRS算法里的sampleFreq改成100
陀螺仪使用DPS数据单位会出现轻微转动传感器骰子就高速转动的情况
适当的修改这些数值能变正常
但是找不出这几个数字之间的关系

还有一个问题是加速度和地磁在算法里不起作用
修改了KP和KI参数也没见到加速度补偿陀螺仪误差的效果

yujie2510

在 osxMotionFX 的库里会用磁力计来补偿陀螺仪，如果你用的算法如果没有用到磁力计的数据的话，就需要保证所有的输入都是校准过后的数据。

littleshrimp

yujie2510 发表于 2017-6-25 20:00
在 osxMotionFX 的库里会用磁力计来补偿陀螺仪，如果你用的算法如果没有用到磁力计的数据的话，就需要保证 ...

允许输入9轴数据，应该全通过加速度和磁力计补偿
可能是设置的问题，在实际测试时加速度和磁力计几乎看不到作用

hezhengjia

嗯哼。是不对。不光是参数设置的问题。还有好多临界点的处理都没有。所以网上开源的的算法只有一个指导方向。实际应用还有许多需要处理的地方