给你个卡尔曼滤波的代码看看
#include <math.h>
#define Forecast_Noise 4//预测不确定度，根据实际调节，一般很小
#define Measure_Noise 4//可以通过传感器技术手册查找到测量噪声
void Kalman_v1(double Now_Measure_Value, double Last_Best_Value)
{
double Forecast_Value, Measure_Value, Best_Value;//预测值、测量值和最优值
static double Forecast_Error, Measure_Error, Best_Error = 0;//预测值、测量值和最优值偏差
static double Kalman_Gain;//卡尔曼滤波器增益
Forecast_Value = Last_Best_Value;
//更新预测值---------①
Measure_Value = Now_Measure_Value;//测量值
Forecast_Error = sqrt(Forecast_Noise ^ 2 + Best_Error ^ 2);
//更新预测值偏差---------②
Measure_Error = Measure_Noise;//测量偏差=测量噪声
Kalman_Gain = sqrt(Forecast_Error ^ 2 / (Forecast_Error ^ 2 + Measure_Error ^ 2));
//求滤波器增益---------③
Best_Value = Forecast_Value + Kalman_Gain * (Measure_Error - Forecast_Error);
//当前最优值=预测值+增益*（测量值-预测值）---------④
Best_Error = sqrt((1 - Kalman_Gain) * Forecast_Error ^ 2);
//更新最优值偏差---------⑤
}
| 
楼主
标题置顶
标题高亮
