关于卡尔曼滤波的C语言程序

ID:590486 · 发表于 2019-8-2 21:56

关于卡尔曼滤波的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)

ID:693716 · 发表于 2020-2-19 12:32

谢谢分享，正学习呢

帐号		自动登录	找回密码
密码			立即注册