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
}