DSP

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

2019-07-13 19:13发布

站内文章 / DSP
17965 0 1045
经测试，在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

  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


  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

  52.     #define ACCEL_1G 1000 //重力加速度为：1000 mg 7 Z $ T + P6 h5 w

  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

  58.     / *公共变量---------------------------------------------- ------------ * / ％v4 u6 g $〜！V * t。E * g
  59.     EulerAngle_Type EulerAngle; //单位：弧度
  60.     u8 InitEulerAngle_Finished = 0;

  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;

  72.     / *私有宏---------------------------------------------- --------------- * /
  73.     / * Private typedef ---------------------------------------------- ------------- * / 6 w（n“p3 L H I; F
  74.     / *私有函数原型--------------------------------------------- - * /

  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

  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.     }

热门文章