给你个卡尔曼滤波的代码看看
- #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);
- //更新最优值偏差---------⑤
- }
|