關于卡爾曼濾波的C程序
單片機源程序如下:
- #include "Kalman_Filter.h"
- /******************************
- 函數名稱：Kalman_Filter
- 輸入參數：x（需要優化的數據）、v（x的變化率）
- 返回參數：x_youhua（優化后的x數據）
- 其中Q_x為x的置信度，Q_v為v的置信度；
- ********************************/
- float Kalman_Filter(float x,float v)
- {
- static float v_sum; //v數據暫存
- static float x_youhua = 0.0;
- static float Q_bias = 0.0;
- static float angle_err = 0.0;
- static float Q_x = 0.1; //x數據置信度
- static float Q_v = 0.1; //v數據置信度
- static float R_x = 0.5; //測量噪聲的協方差 既測量偏差
- static float dt = 0.005; //dt為濾波器采樣時間(秒)
- static char C_0 = 1;
- static float PCt_0=0, PCt_1=0, E=0;
- static float K_0=0, K_1=0, t_0=0, t_1=0;
- static float Pdot[4] ={0,0,0,0};
- static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
- x_youhua += (v - Q_bias) * dt; //先驗估計
- angle_err = x - x_youhua; //zk-先驗估計
- Pdot[0]=Q_x - PP[0][1] - PP[1][0]; // Pk-先驗估計誤差協方差的微分
- Pdot[1]=- PP[1][1];
- Pdot[2]=- PP[1][1];
- Pdot[3]=Q_v;
- PP[0][0] += Pdot[0] * dt; // Pk-先驗估計誤差協方差微分的積分
- PP[0][1] += Pdot[1] * dt; // =先驗估計誤差協方差
- PP[1][0] += Pdot[2] * dt;
- PP[1][1] += Pdot[3] * dt;
- PCt_0 = C_0 * PP[0][0];
- PCt_1 = C_0 * PP[1][0];
-
- E = R_x + C_0 * PCt_0;
-
- K_0 = PCt_0 / E;
- K_1 = PCt_1 / E;
-
- t_0 = PCt_0;
- t_1 = C_0 * PP[0][1];
- PP[0][0] -= K_0 * t_0; //后驗估計誤差協方差
- PP[0][1] -= K_0 * t_1;
- PP[1][0] -= K_1 * t_0;
- PP[1][1] -= K_1 * t_1;
-
- x_youhua += K_0 * angle_err; //最優x
- Q_bias += K_1 * angle_err;
- v_sum = v - Q_bias; //最優v
- return (x_youhua); //最終x
- }
所有資料51hei提供下載:
卡爾曼濾波模塊.zip
(1.14 KB, 下載次數: 51)
2019-8-2 21:56 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
