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

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

/ * 5 F8 W0'P）X * R / \ 0 d“M，[
* AHRS
* Copyright 2010 SOH Madgwick
*
*本程序是免费软件：您可以重新发布它和/或：n）L3 \“} 0 j1 A3 V3 C1 j＆O4 e
*根据GNU Lesser Public License的条款修改它
*由自由软件基金会发布，或者是第三版的
*许可证，或（根据您的选择）任何更新的版本。
* “K * D8 G + j- u，_”v）p
*这个程序分发时希望它有用，但是
*没有任何担保; 甚至没有％U0的默示担保：o6 v6 n％A＆F
*适销性或针对特定用途的适用性。见GNU
*更低的公共许可证了解更多详情。
*
*您应该收到GNU Lesser Public License的副本
*随着这个程序。如果不是，请参阅/ e2 e2 P0 U，g3 U. l
* <http://www.gnu.org/licenses/>。
* / （？5 p E“} $ a
// AHRS.c ，Y7 X w％j！b7 N; ] / R8 Y1 V
// SOH麦格维克
// 2010年8月25日
//
//'DCM过滤器'的四元数实现[Mayhony等人]。结合了磁场失真
//补偿算法从我的过滤器[Madgwick]中消除了对参考（F）s，a，p＃u，b + I
//磁通方向（bx bz）需要预定义，并限制磁偏转对偏航的影响
//仅限轴。4 H * O3 k0 e％{
//
//用户必须定义'halfT'为（采样周期/ 2），并且过滤器获得'Kp'和'Ki'。
//
//全局变量'q0'，'q1'，'q2'，'q3'是四元数元素，代表估计的6 r3 U + R1 r- D，A; J 1 X4 e
//定位。请参阅我的报告，了解在此应用程序中使用四元数的概述。
// 4`5 r + l / E1 l3 k）D
//每个采样周期用户必须调用'AHRSupdate（）'并解析校准后的陀螺仪（'gx'，'gy'，'gz'），9 W！k，`1 i / O'a-B
//'accelerometer'（'ax'，'ay'，'ay'）和磁力计（'mx'，'my'，'mz'）的数据。陀螺仪的单位是
//弧度/秒，加速度计和磁力计单位与矢量归一化无关。＃Z1 a：X1 W * _7？
// （] - c，g6 T“n1 e'\＃j
：a0 v F7 J％U“h）R＆q
#include“stm32f10x.h”
#include“AHRS.h” $ g＆{“q- v！H＆s：g
#include“Positioning.h”
#include <math.h> 9 f0 C“r6 g $ @
#include <stdio.h> $ r3 S5`6 w6 d％\ 6 A- P1 R
6 [）h1 v; j＆Y- H＆{3 c8 X
/ *私人定义---------------------------------------------- -------------- * /
#define Kp 2.0f //比例增益控制加速度计/磁力计'm'的收敛速度B＆f：Q！C！我知道了
#define Ki 0.005f //积分增益控制陀螺仪偏差的收敛速度6 Z！ü！`3 Y3 l。| + j'C
#define halfT 0.0025f //采样周期的一半：0.005s / 2 = 0.0025s
#define ACCEL_1G 1000 //重力加速度为：1000 mg 7 Z $ T + P6 h5 w
/ *私有变量---------------------------------------------- ----------- * /
静态浮点数q0 = 1，q1 = 0，q2 = 0，q3 = 0; //表示估计方向的四元数元素
static float exInt = 0，eyInt = 0，ezInt = 0; //比例积分误差* A％a0 L0 a2 P＆q * I.`：A
/ *公共变量---------------------------------------------- ------------ * / ％v4 u6 g $〜！V * t。E * g
EulerAngle_Type EulerAngle; //单位：弧度
u8 InitEulerAngle_Finished = 0;
float magnetoresistor_m高斯_X = 0，磁电阻_m高斯_Y = 0，磁电阻_m高斯_Z = 0; //单位：毫高斯
float Accelerate_mg_X，Accelerate_mg_Y，Accelerate_mg_Z; //单位：mg 5 k / _'a＃n。A，t1 N
float AngularRate_dps_X，AngularRate_dps_Y，AngularRate_dps_Z; //单位：dps：每秒度数 4 L：k，j / d！q）O
5 O5 T- D / q6 O- d0 b
int16_t Magnetoresistor_X，Magnetoresistor_Y，Magnetoresistor_Z; 2 e）c7 z8 o} 1 z！R3 u。H
uint16_t Accelerate_X = 0，Accelerate_Y = 0，Accelerate_Z = 0;
uint16_t AngularRate_X = 0，AngularRate_Y = 0，AngularRate_Z = 0; + s）P！q / w“d5 j'w
1 q + M / s：S“K（〜0？％H + b
u8 Quaternion_Calibration_ok = 0;
/ *私有宏---------------------------------------------- --------------- * /
/ * Private typedef ---------------------------------------------- ------------- * / 6 w（n“p3 L H I; F
/ *私有函数原型--------------------------------------------- - * /
/ ******************* ******************************
*函数名称：AHRSupdate / {A / b＃x + O's
*说明：无
*输入：无
*输出：无
*返回：无
************************************************** ************ /
void AHRSupdate（float gx，float gy，float gz，float ax，float ay，float az，float mx，float my，float mz）{
浮标
float hx，hy，hz，bx，bz; ＃q＆U'q-] * Y7 R
float vx，vy，vz，wx，wy，wz; 0 b S8 w。p0 T3 F）G. n8 k“c
float ex，ey，ez;
）x（16 N-q＆|“B（r
//辅助变量减少重复操作次数
float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2; 6 z）B $}：Y'〜+ n- [9 N
float q0q3 = q0 * q3; ：I“I5 h＆`）c6 O2 d
float q1q1 = q1 * q1; ！k5 S6 g P5 I＃U5 s- o。C
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3; 5 W $ j * J：z！C
float q3q3 = q3 * q3; 7 e“b $ T（F！f＆u6 j3 x- g
//标准化测量
norm = sqrt（ax * ax + ay * ay + az * az）;
ax = ax / norm; $ o：E％b0 U5 V6 |
ay = ay / norm;
az = az / norm;
norm = sqrt（mx * mx + my * my + mz * mz）;
mx = mx / norm; 9 o＆z * L4 g / n：t + k
我=我的/标准;
mz = mz / norm; A：m7 o！R3 @ 5 B
* z @＆| 5`$ n6`$ V，J8 h
//计算通量的参考方向
hx = 2 * mx *（0.5-q2q2-q3q3）+ 2 * my *（q1q2-q0q3）+ 2 * mz *（q1q3 + q0q2）;
hy = 2 * mx *（q1q2 + q0q3）+ 2 * my *（0.5-q1q1-q3q3）+ 2 * mz *（q2q3 -q0q1）;
hz = 2 * mx *（q1q3-q0q2）+ 2 * my *（q2q3 + q0q1）+ 2 * mz *（0.5-q1q1-q2q2）; “V：{”d * p3 X＃T
bx = sqrt（（hx * hx）+（hy * hy））;
bz = hz; T3 O，J0 H'\ $ Z5 k
＃D + m＃w1 p％〜7 N
//估计重力和通量方向（v和w）
vx = 2 *（q1q3-q0q2）;
vy = 2 *（q0q1 + q2q3）; - T3 E0 j / S'R- U％B6 V
vz = q0q0-q1q1-q2q2 + q3q3;
wx = 2 * bx *（0.5-q2q2-q3q3）+ 2 * bz *（q1q3-q0q2）; ％K！[7 V. g8 i（U“y
wy = 2 * bx *（q1q2-q0q3）+ 2 * bz *（q0q1 + q2q3）;
wz = 2 * bx *（q0q2 + q1q3）+ 2 * bz *（0.5-q1q1-q2q2）;
'w2 \ 3`'g8 n $ l＆^ 3 L
//误差是场的参考方向与传感器测量的方向之间的叉积的总和
ex =（ay * vz - az * vy）+（my * wz - mz * wy）;
ey =（az * vx-ax * vz）+（mz * wx-mx * wz）;
ez =（ax * vy - ay * vx）+（mx * wy - my * wx）; 5 |＆J＃G9 K6] 8 S
4 z1 d9 X8 S！T / a“g！M- b％t
//积分误差缩放积分增益
exInt = exInt + ex * Ki;
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki; 2〜“c”C0 _＆@
：〜7 ^ - _4 | 2`（A＆g）S＆C B
//调整陀螺仪测量
gx = gx + Kp * ex + exInt; $ N6 q + s $ s6 z1] 4 f + p
gy = gy + Kp * ey + eyInt; 9 I：A）B！P％I N“你
gz = gz + Kp * ez + ezInt;
//整合四元数和正常化。？：E. U＆ - Q＆H＃A8 m
q0 = q0 +（ - q1 * gx-q2 * gy-q3 * gz）* halfT;
q1 = q1 +（q0 * gx + q2 * gz-q3 * gy）* halfT;
q2 = q2 +（q0 * gy-q1 * gz + q3 * gx）* halfT; 3 @！Z：I2'％s3 H / B5 P，e8 @
q3 = q3 +（q0 * gz + q1 * gy-q2 * gx）* halfT; 3 A（@'+ G％C8 d“e $ V
//标准化四元数
norm = sqrt（q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3）;
q0 = q0 / norm; t3 _2 M + Z0 H. M＃o4 Q
q1 = q1 / norm; 0 Z V0 X + Z7] * K5 V
q2 = q2 / norm;
q3 = q3 / norm; + C！1 + T9 w0 F] 2 n％z2 k
} 5 z“H，t，l1 X; X. y
浮动钳位（浮动值，浮动最小值，浮动最大值）0 T9 j％Z. R M. J8 p＆m＃G; X
{
如果（值>最大）
{ “c（O3 S2 W9 d3 W％]
返回最大值; 6 s2 s）n（h，T，V，C1 V
} else if（Value <Min）：w8 S）Q2 E * \ 9 y，D2 h＆i
{ ，V9 f'H“}（M't3 s”d1 _
返回Min; 5 O5 {1 B0 M $ W + Z3 Z + Y
}其他
{
返回值; 4 T3 F; T2？$ p / _！S * {3]“m：L5 P
} 0 @。\ 6〜7 f！\）n4 z z
}