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.                                                                                      :a0 v F7 J%U“h)R&q
  38.     #include“stm32f10x.h”                 
  39.     #include“AHRS.h” $ g&{“q- v!H&s:g
  40.     #include“Positioning.h”       
  41.     #include                            9 f0 C“r6 g $ @
  42.     #include  $ r3 S5`6 w6 d%\ 6 A- P1 R


  43. 6 [)h1 v; j&Y- H&{3 c8 X
  44.     / *私人定义---------------------------------------------- -------------- * /
  45.     #define Kp 2.0f //比例增益控制加速度计/磁力计'm'的收敛速度B&f:Q!C!我知道了
  46.     #define Ki 0.005f //积分增益控制陀螺仪偏差的收敛速度6 Z!ü!`3 Y3 l。| + j'C
  47.     #define halfT 0.0025f //采样周期的一半:0.005s / 2 = 0.0025s

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

  49.     / *私有变量---------------------------------------------- ----------- * /
  50.     静态浮点数q0 = 1,q1 = 0,q2 = 0,q3 = 0; //表示估计方向的四元数元素
  51.     static float exInt = 0,eyInt = 0,ezInt = 0; //比例积分误差* A%a0 L0 a2 P&q * I.`:A

  52.     / *公共变量---------------------------------------------- ------------ * / %v4 u6 g $〜!V * t。E * g
  53.     EulerAngle_Type EulerAngle; //单位:弧度
  54.     u8 InitEulerAngle_Finished = 0;

  55.     float magnetoresistor_m高斯_X = 0,磁电阻_m高斯_Y = 0,磁电阻_m高斯_Z = 0; //单位:毫高斯                                                                                                                                                                                                       
  56.     float Accelerate_mg_X,Accelerate_mg_Y,Accelerate_mg_Z; //单位:mg                                                                5 k / _'a#n。A,t1 N
  57.     float AngularRate_dps_X,AngularRate_dps_Y,AngularRate_dps_Z; //单位:dps:每秒度数       4 L:k,j / d!q)O
  58. 5 O5 T- D / q6 O- d0 b
  59.     int16_t Magnetoresistor_X,Magnetoresistor_Y,Magnetoresistor_Z;                                                                                                                                                                                                       2 e)c7 z8 o} 1 z!R3 u。H
  60.     uint16_t Accelerate_X = 0,Accelerate_Y = 0,Accelerate_Z = 0;                                                                                                                                                                                               
  61.     uint16_t AngularRate_X = 0,AngularRate_Y = 0,AngularRate_Z = 0; + s)P!q / w“d5 j'w
  62. 1 q + M / s:S“K(〜0?%H + b
  63.     u8 Quaternion_Calibration_ok = 0;

  64.     / *私有宏---------------------------------------------- --------------- * /
  65.     / * Private typedef ---------------------------------------------- ------------- * / 6 w(n“p3 L H I; F
  66.     / *私有函数原型--------------------------------------------- - * /

  67.     / ******************* ******************************
  68.     *函数名称:AHRSupdate / {A / b#x + O's
  69.     *说明:无
  70.     *输入:无
  71.     *输出:无
  72.     *返回:无
  73.     ************************************************** ************ /
  74.     void AHRSupdate(float gx,float gy,float gz,float ax,float ay,float az,float mx,float my,float mz){
  75.             浮标
  76.             float hx,hy,hz,bx,bz; #q&U'q-] * Y7 R
  77.             float vx,vy,vz,wx,wy,wz; 0 b S8 w。p0 T3 F)G. n8 k“c
  78.             float ex,ey,ez;
  79. )x(16 N-q&|“B(r
  80.             //辅助变量减少重复操作次数
  81.             float q0q0 = q0 * q0;
  82.             float q0q1 = q0 * q1;
  83.             float q0q2 = q0 * q2; 6 z)B $}:Y'〜+ n- [9 N
  84.             float q0q3 = q0 * q3; :I“I5 h&`)c6 O2 d
  85.             float q1q1 = q1 * q1; k5 S6 g P5 I#U5 s- o。C
  86.             float q1q2 = q1 * q2;
  87.             float q1q3 = q1 * q3;
  88.             float q2q2 = q2 * q2;   
  89.             float q2q3 = q2 * q3; 5 W $ j * J:z!C
  90.             float q3q3 = q3 * q3;         7 e“b $ T(F!f&u6 j3 x- g
  91.            
  92.             //标准化测量
  93.             norm = sqrt(ax * ax + ay * ay + az * az);      
  94.             ax = ax / norm; $ o:E%b0 U5 V6 |
  95.             ay = ay / norm;
  96.             az = az / norm;
  97.             norm = sqrt(mx * mx + my * my + mz * mz);         
  98.             mx = mx / norm; 9 o&z * L4 g / n:t + k
  99.             我=我的/标准;
  100.             mz = mz / norm;           A:m7 o!R3 @ 5 B
  101.            * z @&| 5`$ n6`$ V,J8 h
  102.             //计算通量的参考方向
  103.             hx = 2 * mx *(0.5-q2q2-q3q3)+ 2 * my *(q1q2-q0q3)+ 2 * mz *(q1q3 + q0q2);
  104.             hy = 2 * mx *(q1q2 + q0q3)+ 2 * my *(0.5-q1q1-q3q3)+ 2 * mz *(q2q3 -q0q1);
  105.             hz = 2 * mx *(q1q3-q0q2)+ 2 * my *(q2q3 + q0q1)+ 2 * mz *(0.5-q1q1-q2q2);         “V:{”d * p3 X#T
  106.             bx = sqrt((hx * hx)+(hy * hy));
  107.             bz = hz;          T3 O,J0 H'\ $ Z5 k
  108.            #D + m#w1 p%〜7 N
  109.             //估计重力和通量方向(v和w)
  110.             vx = 2 *(q1q3-q0q2);
  111.             vy = 2 *(q0q1 + q2q3); - T3 E0 j / S'R- U%B6 V
  112.             vz = q0q0-q1q1-q2q2 + q3q3;
  113.             wx = 2 * bx *(0.5-q2q2-q3q3)+ 2 * bz *(q1q3-q0q2); %K![7 V. g8 i(U“y
  114.             wy = 2 * bx *(q1q2-q0q3)+ 2 * bz *(q0q1 + q2q3);
  115.             wz = 2 * bx *(q0q2 + q1q3)+ 2 * bz *(0.5-q1q1-q2q2);  
  116.            'w2 \ 3`'g8 n $ l&^ 3 L
  117.             //误差是场的参考方向与传感器测量的方向之间的叉积的总和
  118.             ex =(ay * vz - az * vy)+(my * wz - mz * wy);
  119.             ey =(az * vx-ax * vz)+(mz * wx-mx * wz);
  120.             ez =(ax * vy - ay * vx)+(mx * wy - my * wx); 5 |&J#G9 K6] 8 S
  121.            4 z1 d9 X8 S!T / a“g!M- b%t
  122.             //积分误差缩放积分增益
  123.             exInt = exInt + ex * Ki;
  124.             eyInt = eyInt + ey * Ki;
  125.             ezInt = ezInt + ez * Ki; 2〜“c”C0 _&@
  126.            :〜7 ^ - _4 | 2`(A&g)S&C B
  127.             //调整陀螺仪测量
  128.             gx = gx + Kp * ex + exInt; $ N6 q + s $ s6 z1] 4 f + p
  129.             gy = gy + Kp * ey + eyInt; 9 I:A)B!P%I N“你
  130.             gz = gz + Kp * ez + ezInt;
  131.            
  132.             //整合四元数和正常化?:E. U& - Q&H#A8 m
  133.             q0 = q0 +( - q1 * gx-q2 * gy-q3 * gz)* halfT;
  134.             q1 = q1 +(q0 * gx + q2 * gz-q3 * gy)* halfT;
  135.             q2 = q2 +(q0 * gy-q1 * gz + q3 * gx)* halfT; 3 @!Z:I2'%s3 H / B5 P,e8 @
  136.             q3 = q3 +(q0 * gz + q1 * gy-q2 * gx)* halfT;  3 A(@'+ G%C8 d“e $ V
  137.            
  138.             //标准化四元数
  139.             norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  140.             q0 = q0 / norm;   t3 _2 M + Z0 H. M#o4 Q
  141.             q1 = q1 / norm; 0 Z V0 X + Z7] * K5 V
  142.             q2 = q2 / norm;
  143.             q3 = q3 / norm; + C!1 + T9 w0 F] 2 n%z2 k
  144.     } 5 z“H,t,l1 X; X. y

  145.     浮动钳位(浮动值,浮动最小值,浮动最大值)0 T9 j%Z. R M. J8 p&m#G; X
  146.     {
  147.             如果(值>最大)
  148.             { “c(O3 S2 W9 d3 W%]
  149.                     返回最大值; 6 s2 s)n(h,T,V,C1 V
  150.             } else if(Value :w8 S)Q2 E * \ 9 y,D2 h&i
  151.             { ,V9 f'H“}(M't3 s”d1 _
  152.                     返回Min; 5 O5 {1 B0 M $ W + Z3 Z + Y
  153.             }其他
  154.             {
  155.                     返回值; 4 T3 F; T2?$ p / _!S * {3]“m:L5 P
  156.             } 0 @。\ 6〜7 f!\)n4 z z
  157.     }

你可能感兴趣的:(STM)