【滤波】卡尔曼滤波器.C代码

#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);    //更新最优值偏差---------⑤}