网站编辑工具软件,网站做外链的具体步骤,推广网页,营销软文范文目录代码kalman.ckalman.h滤波效果很久以前抄的#xff0c;忘了是从哪弄的了 我把它改成了这种结构体指针传参的形式#xff0c;方便在比赛中应用。应用举例见MSP432 PID 电机调角度、调速。 它曾助力笔者获2020年电赛省一等奖。
代码
Q:过程噪声协方差 Q参数调滤波后的曲线…
目录代码kalman.ckalman.h滤波效果很久以前抄的忘了是从哪弄的了 我把它改成了这种结构体指针传参的形式方便在比赛中应用。应用举例见MSP432 PID 电机调角度、调速。 它曾助力笔者获2020年电赛省一等奖。
代码
Q:过程噪声协方差 Q参数调滤波后的曲线平滑程度Q越小越平滑; R:观测噪声协方差 R参数调整滤波后的曲线与实测曲线的相近程度R越小越接近(收敛越快)
kalman.c
/*
卡尔曼滤波器
整理By 乙酸氧铍
*/
#include kalman.hdouble KalmanFilter(Kalman_Typedef *klm, double input)
{//预测协方差方程k时刻系统估算协方差 k-1时刻的系统协方差 过程噪声协方差klm-Now_P klm-LastP klm-Q;//卡尔曼增益方程卡尔曼增益 k时刻系统估算协方差 / k时刻系统估算协方差 观测噪声协方差klm-Kg klm-Now_P / (klm-Now_P klm-R);//更新最优值方程k时刻状态变量的最优值 状态变量的预测值 卡尔曼增益 * 测量值 - 状态变量的预测值klm-out klm-out klm-Kg * (input -klm-out);//因为这一次的预测值就是上一次的输出值//更新协方差方程: 本次的系统协方差赋给 klm-LastP 为下一次运算准备。klm-LastP (1-klm-Kg) * klm-Now_P;return (klm-out);
}void Kalman_Init(Kalman_Typedef *klm, const double klm_Q, const double klm_R)//温度klm_Q0.01 klm_R0.25
{klm-LastP0.02; //上次估算协方差klm-Now_P0; //当前估算协方差klm-out0; //卡尔曼滤波器输出klm-Kg0; //卡尔曼增益klm-Qklm_Q; //Q:过程噪声协方差 Q参数调滤波后的曲线平滑程度Q越小越平滑;klm-Rklm_R; //R:观测噪声协方差 R参数调整滤波后的曲线与实测曲线的相近程度R越小越接近(收敛越快)
}
kalman.h
#ifndef __KALMAN_H__
#define __KALMAN_H__typedef struct
{/*不用动*/double LastP;//上次估算协方差double Now_P;//当前估算协方差double out;//卡尔曼滤波器输出double Kg;//卡尔曼增益double Q;double R;
}Kalman_Typedef;void Kalman_Init(Kalman_Typedef *klm, const double klm_Q, const double klm_R);
double KalmanFilter(Kalman_Typedef *klm, double input);#endif
滤波效果
