在網上找的卡爾曼濾波在自己理解的基礎上更改了一下代碼！實測有用
單片機源程序如下:
- /* Includes ------------------------------------------------------------------*/
- #include "stm32f10x.h"
- #include "kalman.h"
- #include "matrix.h"
- /*==============================================================================
- 1.預估計
- X(k|k-1) = F(k,k-1)*X(k-1|k-1) //控制量為0
- 2.計算預估計協方差矩陣
- 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.計算更新后估計協防差矩陣
- P(k|k) =（I-Kg(k)*H）*P(k|k-1)
- 6. 更新最優值
- F(k,k-1): 狀態轉移矩陣
- X(k|k-1): 根據k-1時刻的最優值估計k時刻的值
- X(k-1|k-1): k-1時刻的最優值
- P(k|k-1): X(k|k-1)對應的covariance
- P(k-1|k-1): X(k-1|k-1)對應的covariance
- Q(k): 系統過程的covariance
- R(k): 測量過程的協方差
- H(k): 觀測矩陣轉移矩陣
- Z(k): k時刻的測量值
- 基本思路: 首先根據上一次(如果是第一次則根據預賦值計算)的數據計算出本次的估計值,
- 同理,根據上一次的數據計算出本次估計值的協方差; 接著,由本次估計值的協
- 方差計算出卡爾曼增益; 最后,根據估測值和測量值計算當前最優值及其協方差
- ==============================================================================*/
- //================================================//
- //== 最優值方差結構體 ==//
- //================================================//
- typedef struct _tCovariance
- {
- float PNowOpt[LENGTH];
- float PPreOpt[LENGTH];
- }tCovariance;
- static tOptimal tOpt;
- static tCovariance tCov;
- //float Z[LENGTH] = {4000}; // 測量值(每次測量的數據需要存入該數組)
- static float I[LENGTH] = {1}; // 單位矩陣
- static float X[LENGTH] = {9.8}; // 當前狀態的預測值
- static float P[LENGTH] = {0}; // 當前狀態的預測值的協方差
- static float K[LENGTH] = {0}; // 卡爾曼增益
- static float Temp3[LENGTH] = {0}; // 輔助變量
- //============================================================================//
- //== 卡爾曼濾波需要配置的變量 ==//
- //============================================================================//
- static float F[LENGTH] = {1}; // 狀態轉移矩陣 即：堅信當前的狀態與上一次狀態的關系，如果堅信是一樣的，則狀態轉移矩陣就為單位矩陣
- static float Q[LENGTH] = {0.0001f};//0.0001f // 系統過程的協方差 協方差的定義：真實值與期望值之差的平方的期望值
- static float R[LENGTH] = {2}; // 測量過程的協方差 協方差的定義：真實值與期望值之差的平方的期望值
- //如果你需要濾波結果更依賴于觀測量，那就調小R，增大Q；反之，調大R，調小Q，這樣估計值就取決于系統。
- //如果R大Q小，就是說，狀態估計值比測量值要可靠，這時，所得出的結果就是更接近估計值；
- //如果R小Q大，這時，計算出來的結果就會更接近測量值。
- static float H[LENGTH] = {1}; // 觀測矩陣轉移矩陣 測量值與狀態預測值之間的單位換算關系，即把預測值單位換算成測量值單位
- 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]; //零值初始化
- }
- }
- //============================================================================//
- //== 卡爾曼濾波 ==//
- //============================================================================//
- //==入口參數: 當前時刻的測量值 ==//
- //==出口參數: 當前時刻的最優值 ==//
- //==返回值: 當前時刻的最優值 ==//
- //============================================================================//
- float KalMan_Update(float *Z)
- {
- u8 i;
- MatrixMul(F, tOpt.XPreOpt, X, ORDER, ORDER, ORDER); // 基于系統的上一狀態而預測現在狀態; 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); // 預測數據的協方差矩陣; 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); // 根據估測值和測量值計算當前最優值; 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); // 計算更新后估計協防差矩陣; 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];
- }
所有資料51hei提供下載:
kalman.7z
(3.71 KB, 下載次數: 252)
2019-8-7 00:41 上傳
點擊文件名下載附件
卡爾曼濾波分享
|
[復制鏈接]
