STM32單片機(jī)卡爾曼濾波程序分享

ID:582568 · 發(fā)表于 2019-8-6 17:42

在網(wǎng)上找的卡爾曼濾波在自己理解的基礎(chǔ)上更改了一下代碼！實測有用

單片機(jī)源程序如下:

/* Includes ------------------------------------------------------------------*/
#include "stm32f10x.h"
#include "kalman.h"
#include "matrix.h"
/*==============================================================================
1.預(yù)估計
X(k|k-1) = F(k,k-1)*X(k-1|k-1) //控制量為0
2.計算預(yù)估計協(xié)方差矩陣
P(k|k-1) = F(k,k-1)*P(k-1|k-1)*F(k,k-1)'+Q(k)
Q(k) = U(k)×U(k)'
3.計算卡爾曼增益矩陣
Kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R(k))
R(k) = N(k)×N(k)'
4.更新估計
X(k|k) = X(k|k-1)+Kg(k)*(Z(k)-H*X(k|k-1))
5.計算更新后估計協(xié)防差矩陣
P(k|k) =（I-Kg(k)*H）*P(k|k-1)
6. 更新最優(yōu)值
F(k,k-1): 狀態(tài)轉(zhuǎn)移矩陣
X(k|k-1): 根據(jù)k-1時刻的最優(yōu)值估計k時刻的值
X(k-1|k-1): k-1時刻的最優(yōu)值
P(k|k-1): X(k|k-1)對應(yīng)的covariance
P(k-1|k-1): X(k-1|k-1)對應(yīng)的covariance
Q(k): 系統(tǒng)過程的covariance
R(k): 測量過程的協(xié)方差
H(k): 觀測矩陣轉(zhuǎn)移矩陣
Z(k): k時刻的測量值
基本思路: 首先根據(jù)上一次(如果是第一次則根據(jù)預(yù)賦值計算)的數(shù)據(jù)計算出本次的估計值,
同理,根據(jù)上一次的數(shù)據(jù)計算出本次估計值的協(xié)方差; 接著,由本次估計值的協(xié)
方差計算出卡爾曼增益; 最后,根據(jù)估測值和測量值計算當(dāng)前最優(yōu)值及其協(xié)方差
==============================================================================*/
//================================================//
//== 最優(yōu)值方差結(jié)構(gòu)體 ==//
//================================================//
typedef struct _tCovariance
{
float PNowOpt[LENGTH];
float PPreOpt[LENGTH];
}tCovariance;
static tOptimal tOpt;
static tCovariance tCov;
//float Z[LENGTH] = {4000}; // 測量值(每次測量的數(shù)據(jù)需要存入該數(shù)組)
static float I[LENGTH] = {1}; // 單位矩陣
static float X[LENGTH] = {9.8}; // 當(dāng)前狀態(tài)的預(yù)測值
static float P[LENGTH] = {0}; // 當(dāng)前狀態(tài)的預(yù)測值的協(xié)方差
static float K[LENGTH] = {0}; // 卡爾曼增益
static float Temp3[LENGTH] = {0}; // 輔助變量
//============================================================================//
//== 卡爾曼濾波需要配置的變量 ==//
//============================================================================//
static float F[LENGTH] = {1}; // 狀態(tài)轉(zhuǎn)移矩陣即：堅信當(dāng)前的狀態(tài)與上一次狀態(tài)的關(guān)系，如果堅信是一樣的，則狀態(tài)轉(zhuǎn)移矩陣就為單位矩陣
static float Q[LENGTH] = {0.0001f};//0.0001f // 系統(tǒng)過程的協(xié)方差協(xié)方差的定義：真實值與期望值之差的平方的期望值
static float R[LENGTH] = {2}; // 測量過程的協(xié)方差協(xié)方差的定義：真實值與期望值之差的平方的期望值
//如果你需要濾波結(jié)果更依賴于觀測量，那就調(diào)小R，增大Q；反之，調(diào)大R，調(diào)小Q，這樣估計值就取決于系統(tǒng)。
//如果R大Q小，就是說，狀態(tài)估計值比測量值要可靠，這時，所得出的結(jié)果就是更接近估計值；
//如果R小Q大，這時，計算出來的結(jié)果就會更接近測量值。
static float H[LENGTH] = {1}; // 觀測矩陣轉(zhuǎn)移矩陣測量值與狀態(tài)預(yù)測值之間的單位換算關(guān)系，即把預(yù)測值單位換算成測量值單位
static float Temp1[LENGTH] = {1}; // 輔助變量, 同時保存tOpt.XPreOpt[]的初始化值
static float Temp2[LENGTH] = {10000}; // 輔助變量, 同時保存tCov.PPreOpt[]的初始化值
void KalMan_PramInit(void)
{
unsigned char i;
for (i=0; i<LENGTH; i++)
{
tOpt.XPreOpt[i] = Temp1[i]; //零值初始化
}
for (i=0; i<LENGTH; i++)
{
tCov.PPreOpt[i] = Temp2[i]; //零值初始化
}
}
//============================================================================//
//== 卡爾曼濾波 ==//
//============================================================================//
//==入口參數(shù): 當(dāng)前時刻的測量值 ==//
//==出口參數(shù): 當(dāng)前時刻的最優(yōu)值 ==//
//==返回值: 當(dāng)前時刻的最優(yōu)值 ==//
//============================================================================//
float KalMan_Update(float *Z)
{
u8 i;
MatrixMul(F, tOpt.XPreOpt, X, ORDER, ORDER, ORDER); // 基于系統(tǒng)的上一狀態(tài)而預(yù)測現(xiàn)在狀態(tài); X(k|k-1) = F(k,k-1)*X(k-1|k-1)
MatrixCal(F, tCov.PPreOpt, Temp1, ORDER);
MatrixAdd(Temp1, Q, P, ORDER, ORDER); // 預(yù)測數(shù)據(jù)的協(xié)方差矩陣; P(k|k-1) = F(k,k-1)*P(k-1|k-1)*F(k,k-1)'+Q
MatrixCal(H, P, Temp1, ORDER);
MatrixAdd(Temp1, R, Temp1, ORDER, ORDER);
Gauss_Jordan(Temp1, ORDER);
MatrixTrans(H, Temp2, ORDER, ORDER);
MatrixMul(P, Temp2, Temp3, ORDER, ORDER, ORDER);
MatrixMul(Temp1, Temp3, K, ORDER, ORDER, ORDER); // 計算卡爾曼增益; Kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
MatrixMul(H, X, Temp1, ORDER, ORDER, ORDER);
MatrixMinus(Z, Temp1, Temp1, ORDER, ORDER);
MatrixMul(K, Temp1, Temp2, ORDER, ORDER, ORDER);
MatrixAdd(X, Temp2, tOpt.XNowOpt, ORDER, ORDER); // 根據(jù)估測值和測量值計算當(dāng)前最優(yōu)值; X(k|k) = X(k|k-1)+Kg(k)*(Z(k)-H*X(k|k-1))
MatrixMul(K, H, Temp1, ORDER, ORDER, ORDER);
MatrixMinus(I, Temp1, Temp1, ORDER, ORDER);
MatrixMul(Temp1, P, tCov.PNowOpt, ORDER, ORDER, ORDER); // 計算更新后估計協(xié)防差矩陣; P(k|k) =（I-Kg(k)*H）*P(k|k-1)
for (i=0; i<LENGTH; i++)
{
tOpt.XPreOpt[i] = tOpt.XNowOpt[i];
tCov.PPreOpt[i] = tCov.PNowOpt[i];
}
return tOpt.XNowOpt[0];
}

復(fù)制代碼

所有資料51hei提供下載:

kalman.7z (3.71 KB, 下載次數(shù): 252)

ID:261849 · 發(fā)表于 2020-3-23 15:24

謝謝分享,比較容易理解.

ID:401936 · 發(fā)表于 2020-4-2 20:14

首先感謝樓主分享！然后小白想問下卡爾曼濾波返回值不應(yīng)該是下一時刻的最優(yōu)值嗎

ID:920454 · 發(fā)表于 2021-5-13 09:52

tOptimal 這個結(jié)構(gòu)體的定義在哪兒？

ID:319585 · 發(fā)表于 2021-5-13 17:38

這個不錯，感覺論壇代碼的質(zhì)量越來越高了。

ID:829464 · 發(fā)表于 2021-7-21 16:07

6啊樓主

帳號		自動登錄	找回密碼
密碼			立即注冊