STM32用的卡尔曼滤波程序分享

经测试，在STM32上面可以用的卡尔曼滤波程序，效果的话，有一定的滤波效果，大家拿去参考吧; P6 M.}

1.  / * 5 F8 W0'P）X * R / \ 0 d“M，[
   
2.     * AHRS
   
3.     * Copyright 2010 SOH Madgwick
   
4.     *
   
5.     *本程序是免费软件：您可以重新发布它和/或：n）L3 \“} 0 j1 A3 V3 C1 j＆O4 e
   
6.     *根据GNU Lesser Public License的条款修改它
   
7.     *由自由软件基金会发布，或者是第三版的
   
8.     *许可证，或（根据您的选择）任何更新的版本。
   
9.     * “K * D8 G + j- u，_”v）p
   
10.     *这个程序分发时希望它有用，但是
    
11.     *没有任何担保; 甚至没有％U0的默示担保：o6 v6 n％A＆F
    
12.     *适销性或针对特定用途的适用性。见GNU
    
13.     *更低的公共许可证了解更多详情。
    
14.     *
    
15.     *您应该收到GNU Lesser Public License的副本
    
16.     *随着这个程序。如果不是，请参阅/ e2 e2 P0 U，g3 U. l
    
17.     * 。
    
18.     * / （？5 p E“} $ a
    
19.     // AHRS.c ，Y7 X w％j！b7 N; ] / R8 Y1 V
    
20.     // SOH麦格维克
    
21.     // 2010年8月25日
    
22.     //
    
23.     //'DCM过滤器'的四元数实现[Mayhony等人]。结合了磁场失真
    
24.     //补偿算法从我的过滤器[Madgwick]中消除了对参考（F）s，a，p＃u，b + I
    
25.     //磁通方向（bx bz）需要预定义，并限制磁偏转对偏航的影响
    
26.     //仅限轴。4 H * O3 k0 e％{
    
27.     //
    
28.     //用户必须定义​​'halfT'为（采样周期/ 2），并且过滤器获得'Kp'和'Ki'。
    
29.     //
    
30.     //全局变量'q0'，'q1'，'q2'，'q3'是四元数元素，代表估计的6 r3 U + R1 r- D，A; J 1 X4 e
    
31.     //定位。请参阅我的报告，了解在此应用程序中使用四元数的概述。
    
32.     // 4`5 r + l / E1 l3 k）D
    
33.     //每个采样周期用户必须调用'AHRSupdate（）'并解析校准后的陀螺仪（'gx'，'gy'，'gz'），9 W！k，`1 i / O'a-B
    
34.     //'accelerometer'（'ax'，'ay'，'ay'）和磁力计（'mx'，'my'，'mz'）的数据。陀螺仪的单位是
    
35.     //弧度/秒，加速度计和磁力计单位与矢量归一化无关。＃Z1 a：X1 W * _7？
    
36.     // （] - c，g6 T“n1 e'\＃j
    
37. 
    
38.                                                                                      ：a0 v F7 J％U“h）R＆q
    
39.     #include“stm32f10x.h”                 
    
40.     #include“AHRS.h” $ g＆{“q- v！H＆s：g
    
41.     #include“Positioning.h”       
    
42.     #include                            9 f0 C“r6 g $ @
    
43.     #include  $ r3 S5`6 w6 d％\ 6 A- P1 R
    
44. 
    
45. 
    
46. 6 [）h1 v; j＆Y- H＆{3 c8 X
    
47.     / *私人定义---------------------------------------------- -------------- * /
    
48.     #define Kp 2.0f //比例增益控制加速度计/磁力计'm'的收敛速度B＆f：Q！C！我知道了
    
49.     #define Ki 0.005f //积分增益控制陀螺仪偏差的收敛速度6 Z！ü！`3 Y3 l。| + j'C
    
50.     #define halfT 0.0025f //采样周期的一半：0.005s / 2 = 0.0025s
    
51. 
    
52.     #define ACCEL_1G 1000 //重力加速度为：1000 mg 7 Z $ T + P6 h5 w
    
53. 
    
54.     / *私有变量---------------------------------------------- ----------- * /
    
55.     静态浮点数q0 = 1，q1 = 0，q2 = 0，q3 = 0; //表示估计方向的四元数元素
    
56.     static float exInt = 0，eyInt = 0，ezInt = 0; //比例积分误差* A％a0 L0 a2 P＆q * I.`：A
    
57. 
    
58.     / *公共变量---------------------------------------------- ------------ * / ％v4 u6 g $〜！V * t。E * g
    
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                                                                5 k / _'a＃n。A，t1 N
    
64.     float AngularRate_dps_X，AngularRate_dps_Y，AngularRate_dps_Z; //单位：dps：每秒度数       4 L：k，j / d！q）O
    
65. 5 O5 T- D / q6 O- d0 b
    
66.     int16_t Magnetoresistor_X，Magnetoresistor_Y，Magnetoresistor_Z;                                                                                                                                                                                                       2 e）c7 z8 o} 1 z！R3 u。H
    
67.     uint16_t Accelerate_X = 0，Accelerate_Y = 0，Accelerate_Z = 0;                                                                                                                                                                                               
    
68.     uint16_t AngularRate_X = 0，AngularRate_Y = 0，AngularRate_Z = 0; + s）P！q / w“d5 j'w
    
69. 1 q + M / s：S“K（〜0？％H + b
    
70.     u8 Quaternion_Calibration_ok = 0;
    
71. 
    
72.     / *私有宏---------------------------------------------- --------------- * /
    
73.     / * Private typedef ---------------------------------------------- ------------- * / 6 w（n“p3 L H I; F
    
74.     / *私有函数原型--------------------------------------------- - * /
    
75. 
    
76.     / ******************* ******************************
    
77.     *函数名称：AHRSupdate / {A / b＃x + O's
    
78.     *说明：无
    
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; ＃q＆U'q-] * Y7 R
    
86.             float vx，vy，vz，wx，wy，wz; 0 b S8 w。p0 T3 F）G. n8 k“c
    
87.             float ex，ey，ez;
    
88. ）x（16 N-q＆|“B（r
    
89.             //辅助变量减少重复操作次数
    
90.             float q0q0 = q0 * q0;
    
91.             float q0q1 = q0 * q1;
    
92.             float q0q2 = q0 * q2; 6 z）B $}：Y'〜+ n- [9 N
    
93.             float q0q3 = q0 * q3; ：I“I5 h＆`）c6 O2 d
    
94.             float q1q1 = q1 * q1; ！k5 S6 g P5 I＃U5 s- o。C
    
95.             float q1q2 = q1 * q2;
    
96.             float q1q3 = q1 * q3;
    
97.             float q2q2 = q2 * q2;   
    
98.             float q2q3 = q2 * q3; 5 W $ j * J：z！C
    
99.             float q3q3 = q3 * q3;         7 e“b $ T（F！f＆u6 j3 x- g
    
100.            
     
101.             //标准化测量
     
102.             norm = sqrt（ax * ax + ay * ay + az * az）;      
     
103.             ax = ax / norm; $ o：E％b0 U5 V6 |
     
104.             ay = ay / norm;
     
105.             az = az / norm;
     
106.             norm = sqrt（mx * mx + my * my + mz * mz）;         
     
107.             mx = mx / norm; 9 o＆z * L4 g / n：t + k
     
108.             我=我的/标准;
     
109.             mz = mz / norm;           A：m7 o！R3 @ 5 B
     
110.            * z @＆| 5`$ n6`$ V，J8 h
     
111.             //计算通量的参考方向
     
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）;         “V：{”d * p3 X＃T
     
115.             bx = sqrt（（hx * hx）+（hy * hy））;
     
116.             bz = hz;          T3 O，J0 H'\ $ Z5 k
     
117.            ＃D + m＃w1 p％〜7 N
     
118.             //估计重力和通量方向（v和w）
     
119.             vx = 2 *（q1q3-q0q2）;
     
120.             vy = 2 *（q0q1 + q2q3）; - T3 E0 j / S'R- U％B6 V
     
121.             vz = q0q0-q1q1-q2q2 + q3q3;
     
122.             wx = 2 * bx *（0.5-q2q2-q3q3）+ 2 * bz *（q1q3-q0q2）; ％K！[7 V. g8 i（U“y
     
123.             wy = 2 * bx *（q1q2-q0q3）+ 2 * bz *（q0q1 + q2q3）;
     
124.             wz = 2 * bx *（q0q2 + q1q3）+ 2 * bz *（0.5-q1q1-q2q2）;  
     
125.            'w2 \ 3`'g8 n $ l＆^ 3 L
     
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）; 5 |＆J＃G9 K6] 8 S
     
130.            4 z1 d9 X8 S！T / a“g！M- b％t
     
131.             //积分误差缩放积分增益
     
132.             exInt = exInt + ex * Ki;
     
133.             eyInt = eyInt + ey * Ki;
     
134.             ezInt = ezInt + ez * Ki; 2〜“c”C0 _＆@
     
135.            ：〜7 ^ - _4 | 2`（A＆g）S＆C B
     
136.             //调整陀螺仪测量
     
137.             gx = gx + Kp * ex + exInt; $ N6 q + s $ s6 z1] 4 f + p
     
138.             gy = gy + Kp * ey + eyInt; 9 I：A）B！P％I N“你
     
139.             gz = gz + Kp * ez + ezInt;
     
140.            
     
141.             //整合四元数和正常化。？：E. U＆ - Q＆H＃A8 m
     
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; 3 @！Z：I2'％s3 H / B5 P，e8 @
     
145.             q3 = q3 +（q0 * gz + q1 * gy-q2 * gx）* halfT;  3 A（@'+ G％C8 d“e $ V
     
146.            
     
147.             //标准化四元数
     
148.             norm = sqrt（q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3）;
     
149.             q0 = q0 / norm;   t3 _2 M + Z0 H. M＃o4 Q
     
150.             q1 = q1 / norm; 0 Z V0 X + Z7] * K5 V
     
151.             q2 = q2 / norm;
     
152.             q3 = q3 / norm; + C！1 + T9 w0 F] 2 n％z2 k
     
153.     } 5 z“H，t，l1 X; X. y
     
154. 
     
155.     浮动钳位（浮动值，浮动最小值，浮动最大值）0 T9 j％Z. R M. J8 p＆m＃G; X
     
156.     {
     
157.             如果（值>最大）
     
158.             { “c（O3 S2 W9 d3 W％]
     
159.                     返回最大值; 6 s2 s）n（h，T，V，C1 V
     
160.             } else if（Value ：w8 S）Q2 E * \ 9 y，D2 h＆i
     
161.             { ，V9 f'H“}（M't3 s”d1 _
     
162.                     返回Min; 5 O5 {1 B0 M $ W + Z3 Z + Y
     
163.             }其他
     
164.             {
     
165.                     返回值; 4 T3 F; T2？$ p / _！S * {3]“m：L5 P
     
166.             } 0 @。\ 6〜7 f！\）n4 z z
     
167.     }