|
|
關(guān)于卡爾曼濾波的C程序
單片機(jī)源程序如下:
- #include "Kalman_Filter.h"
- /******************************
- 函數(shù)名稱：Kalman_Filter
- 輸入?yún)?shù)：x（需要優(yōu)化的數(shù)據(jù)）、v（x的變化率）
- 返回參數(shù)：x_youhua（優(yōu)化后的x數(shù)據(jù)）
- 其中Q_x為x的置信度，Q_v為v的置信度；
- ********************************/
- float Kalman_Filter(float x,float v)
- {
- static float v_sum; //v數(shù)據(jù)暫存
- 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數(shù)據(jù)置信度
- static float Q_v = 0.1; //v數(shù)據(jù)置信度
- static float R_x = 0.5; //測(cè)量噪聲的協(xié)方差 既測(cè)量偏差
- static float dt = 0.005; //dt為濾波器采樣時(shí)間(秒)
- 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; //先驗(yàn)估計(jì)
- angle_err = x - x_youhua; //zk-先驗(yàn)估計(jì)
- Pdot[0]=Q_x - PP[0][1] - PP[1][0]; // Pk-先驗(yàn)估計(jì)誤差協(xié)方差的微分
- Pdot[1]=- PP[1][1];
- Pdot[2]=- PP[1][1];
- Pdot[3]=Q_v;
- PP[0][0] += Pdot[0] * dt; // Pk-先驗(yàn)估計(jì)誤差協(xié)方差微分的積分
- PP[0][1] += Pdot[1] * dt; // =先驗(yàn)估計(jì)誤差協(xié)方差
- 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; //后驗(yàn)估計(jì)誤差協(xié)方差
- 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; //最優(yōu)x
- Q_bias += K_1 * angle_err;
- v_sum = v - Q_bias; //最優(yōu)v
- return (x_youhua); //最終x
- }
所有資料51hei提供下載:
卡爾曼濾波模塊.zip
(1.14 KB, 下載次數(shù): 51)
2019-8-2 21:56 上傳
點(diǎn)擊文件名下載附件
|
|
