STM32⽤的卡尔曼滤波程序分享经测试，在STM32上⾯可以⽤的卡尔曼滤波程序，效果的话，有⼀定的滤波效果，⼤家拿去参考吧; P6 M.}
1. / *
2. * AHRS
3. * Copyright 2010 SOH Madgwick
4. *
5. *本程序是免费软件：您可以重新发布它和/或
6. *根据GNU Lesr Public Licen的条款修改它
7. *由⾃由软件基⾦会发布，或者是第三版的
8. *许可证，或（根据您的选择）任何更新的版本。
9. *
10. *这个程序分发时希望它有⽤，但是
11. *没有任何担保; 甚⾄没有的默⽰担保
12. *适销性或针对特定⽤途的适⽤性。见GNU
13. *更低的公共许可证了解更多详情。
14. *
root是什么意思15. *您应该收到GNU Lesr Public Licen的副本
16. *随着这个程序。如果不是，请参阅
17. * &u/licens/>。
18. * /
19. // AHRS.c
20. // SOH麦格维克
21. // 2010年8⽉25⽇
22. //
23. //'DCM过滤器'的四元数实现[Mayhony等⼈]。结合了磁场失真
24. //补偿算法从我的过滤器[Madgwick]中消除了对参考
25. //磁通⽅向（bx bz）需要预定义，并限制磁偏转对偏航的影响
26. //仅限轴。
27. //
28. //⽤户必须定义'halfT'为（采样周期/ 2），并且过滤器获得'Kp'和'Ki'。
29. //
30. //全局变量'q0'，'q1'，'q2'，'q3'是四元数元素，代表估计的
31. //定位。请参阅我的报告，了解在此应⽤程序中使⽤四元数的概述。
32. //
33. //每个采样周期⽤户必须调⽤'AHRSupdate（）'并解析校准后的陀螺仪（'gx'，'gy'，'gz'），
34. //'accelerometer'（'ax'，'ay'，'ay'）和磁⼒计（'mx'，'my'，'mz'）的数据。陀螺仪的单位是
35. //弧度/秒，加速度计和磁⼒计单位与⽮量归⼀化⽆关。
acquit
36. //
37.
38.
39. #include“stm32f10x.h”
40. #include“AHRS.h”
41. #include“Positioning.h”
42. #include <math.h>
43. #include <stdio.h>
44.
45.
46.
47. / *私⼈定义---------------------------------------------- -------------- * /
48. #define Kp 2.0f //⽐例增益控制加速度计/磁⼒计的收敛速度
49. #define Ki 0.005f //积分增益控制陀螺仪偏差的收敛速度
suppo50. #define halfT 0.0025f //采样周期的⼀半：0.005s / 2 = 0.0025s
51.doll
52. #define ACCEL_1G 1000 //重⼒加速度为：1000 mg
七夕节 英语
53.
54. / *私有变量---------------------------------------------- ----------- * /
55. 静态浮点数q0 = 1，q1 = 0，q2 = 0，q3 = 0; //表⽰估计⽅向的四元数元素
56. static float exInt = 0，eyInt = 0，ezInt = 0; //⽐例积分误差
57.
58. / *公共变量---------------------------------------------- ------------ * /
59. EulerAngle_Type EulerAngle; //单位：弧度
60. u8 InitEulerAngle_Finished = 0;
61.
62. float magnetoresistor_m⾼斯_X = 0，磁电阻_m⾼斯_Y = 0，磁电阻_m⾼斯_Z = 0; //单位：毫⾼斯
63. float Accelerate_mg_X，Accelerate_mg_Y，Accelerate_mg_Z; //单位：mg
64. float AngularRate_dps_X，AngularRate_dps_Y，AngularRate_dps_Z; //单位：dps：每秒度数
65.
66. int16_t Magnetoresistor_X，Magnetoresistor_Y，Magnetoresistor_Z;
67. uint16_t Accelerate_X = 0，Accelerate_Y = 0，Accelerate_Z = 0;
68. uint16_t AngularRate_X = 0，AngularRate_Y = 0，AngularRate_Z = 0;
69.
belong是什么意思70. u8 Quaternion_Calibration_ok = 0;
71.
72. / *私有宏---------------------------------------------- --------------- * /
73. / * Private typedef ---------------------------------------------- ------------- * /
74. / *私有函数原型--------------------------------------------- - * /
75.
76. / ******************* ******************************
77. *函数名称：AHRSupdate
78. *说明：⽆conquest
79. *输⼊：⽆
80. *输出：⽆
81. *返回：⽆
82. ************************************************** ************ /
83. void AHRSupdate（float gx，float gy，float gz，float ax，float ay，float az，float mx，float my，float mz）{
84. 浮标
85. float hx，hy，hz，bx，bz;
86. float vx，vy，vz，wx，wy，wz;
87. float ex，ey，ez;
88.
89. //辅助变量减少重复操作次数
90. float q0q0 = q0 * q0;
91. float q0q1 = q0 * q1;
92. float q0q2 = q0 * q2;
242693. float q0q3 = q0 * q3;
94. float q1q1 = q1 * q1;
95. float q1q2 = q1 * q2;
96. float q1q3 = q1 * q3;
97. float q2q2 = q2 * q2;
98. float q2q3 = q2 * q3;
99. float q3q3 = q3 * q3;
100.
101. //标准化测量
102. norm = sqrt（ax * ax + ay * ay + az * az）;
103. ax = ax / norm;
104. ay = ay / norm;
105. az = az / norm;
106. norm = sqrt（mx * mx + my * my + mz * mz）;
107. mx = mx / norm;
108. 我=我的/标准;
109. mz = mz / norm;
110.
111. //计算通量的参考⽅向五年级英语上册教案
nth
112. hx = 2 * mx *（0.5-q2q2-q3q3）+ 2 * my *（q1q2-q0q3）+ 2 * mz *（q1q3 + q0q2）;
113. hy = 2 * mx *（q1q2 + q0q3）+ 2 * my *（0.5-q1q1-q3q3）+ 2 * mz *（q2q3 -q0q1）;
114. hz = 2 * mx *（q1q3-q0q2）+ 2 * my *（q2q3 + q0q1）+ 2 * mz *（0.5-q1q1-q2q2）;
115. bx = sqrt（（hx * hx）+（hy * hy））;
116. bz = hz;
117.
118. //估计重⼒和通量⽅向（v和w）
119. vx = 2 *（q1q3-q0q2）;
120. vy = 2 *（q0q1 + q2q3）;
121. vz = q0q0-q1q1-q2q2 + q3q3;
122. wx = 2 * bx *（0.5-q2q2-q3q3）+ 2 * bz *（q1q3-q0q2）;
123. wy = 2 * bx *（q1q2-q0q3）+ 2 * bz *（q0q1 + q2q3）;
124. wz = 2 * bx *（q0q2 + q1q3）+ 2 * bz *（0.5-q1q1-q2q2）;
125.
126. //误差是场的参考⽅向与传感器测量的⽅向之间的叉积的总和
127. ex =（ay * vz - az * vy）+（my * wz - mz * wy）;
128. ey =（az * vx-ax * vz）+（mz * wx-mx * wz）;
129. ez =（ax * vy - ay * vx）+（mx * wy - my * wx）; 130.
131. //积分误差缩放积分增益
132. exInt = exInt + ex * Ki;
133. eyInt = eyInt + ey * Ki;
134. ezInt = ezInt + ez * Ki;
135.
136. //调整陀螺仪测量
137. gx = gx + Kp * ex + exInt;
138. gy = gy + Kp * ey + eyInt;
139. gz = gz + Kp * ez + ezInt;
140.
141. //整合四元数和正常化
142. q0 = q0 +（ - q1 * gx-q2 * gy-q3 * gz）* halfT; 143. q1 = q1 +（q0 * gx + q2 * gz-q3 * gy）* halfT; 144. q2 = q2 +（q0 * gy-q1 * gz + q3 * gx）* halfT; 145. q3 = q3 +（q0 * gz + q1 * gy-q2 * gx）* halfT; 146.
147. //标准化四元数
148. norm = sqrt（q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3）; 149. q0 = q0 / norm;
150. q1 = q1 / norm;
151. q2 = q2 / norm;
152. q3 = q3 / norm;
153. }
154.
155. 浮动钳位（浮动值，浮动最⼩值，浮动最⼤值）
156. {
157. 如果（值>最⼤）
158. {
159. 返回最⼤值;
160. } el if（Value <Min）
161. {
162. 返回Min;
163. }其他
164. {
165. 返回值;
166. }
167. }
