关于卡尔曼滤波的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, 下载次数: 50)
|
