kalman fliter代码

最新推荐文章于 2024-10-06 14:10:45 发布

拿铁不拿铁

最新推荐文章于 2024-10-06 14:10:45 发布

阅读量337

点赞数 10

文章标签：算法

本文链接：https://blog.csdn.net/weixin_74923758/article/details/141756955

版权

一、定义结构体

typedef struct Kalman_Filter{
	float x_last;
	float P_now;
	float P_last;
	float K;
	float R_cov;
	float Q_cov;
}KF_Struct;

x_last是上一次的最优估计值

P_now是现在时刻的预测值

P_last是上一时刻的预测值

K是增益

R_cov是观测噪声的协方差矩阵，此值一般取值于测量器件的固有噪声

Q_cov是过程激励噪声的协方差，它是状态转移矩阵与实际过程之间的误差。在一阶的滤波中可认为是一个可调的参数。

二、接下来初始化KF_Struct

void KF_Struct_Init(KF_Struct* KFS)
{
	KFS->x_last	=0;
	KFS->P_now	=0;
	KFS->P_last	=0.02;
	KFS->K		=0;
	KFS->Q_cov	=0.005;//过程激励噪声协方差,参数可调
	KFS->R_cov	=0.5;//测量噪声协方差，与仪器测量的性质有关，参数可调
	KFS->out	=0;
}

使用时创建一个KF_Struct的结构体变量KFS1，将结构体变量的地址传入init函数即可使用

usage example：
KF_Struct KFS1;
KF_Struct_Init(&KFS1);

卡尔曼滤波函数核心代码

目的就是为了得到当前状态下的最优估计值

/*
* @brief    卡尔曼滤波器
* @param    KFS:卡尔曼滤波器结构体指针
* @param    z:测量仪器的输入量
* @return   当前时刻的最优估计值
*/
float KMFilter(KF_Struct* KFS,float z)
{
	KFS->P_now = KFS->P_last + KFS->Q_cov;
    KFS->K = KFS->P_now / (KFS->P_now + KFS->R_cov );
    KFS->x_out = KFS->x_out + KFS->K * (z - KFS->x_out);
    P_last = (1f - K)* P_now;
    
    return KFS->x_out;
}

现在时刻的预测值 = 上一时刻预测值 + 过程协方差噪声

增益 = 现在时刻的预测值 / （现在时刻的预测值+观测噪声的协方差矩阵）

当前状态下的最优估计值第一次为0，会不断更新

当前状态下的最优估计值 = 当前状态下的最优估计值 + 增益 * （传感器输入 - 当前状态下的最优估计值）

最后更新，上一次的预测值 = （1 - 增益）* 当前状态下的预测值

最后输出当前状态下的最优估计值。