- /********************************************************************
- *@函数名：KalmanFilter(float newdata)
- *@功 能：卡尔曼滤波
- *@形 参：newdata 新数据
- *@返回值：滤波后数据
- *@备 注：要定义过程噪声和测量噪声
- ********************************************************************/
-
- /*
- Q: 过程噪声，Q增大，动态响应变快，收敛稳定性变坏
- R: 测量噪声，R增大，动态响应变慢，收敛稳定性变好
- 其中p的初值可以随便取，但是不能为0（为0的话卡尔曼滤波器就认为已经是最优滤波器了）
- q,r的值需要我们试出来，讲白了就是(买的破温度计有多破，以及你的超人力有多强)
- r参数调整滤波后的曲线与实测曲线的相近程度，r越小越接近。
- q参数调滤波后的曲线平滑程度，q越小越平滑。
- */
- //过程噪音
- #define P_Q 0.1
- //测量噪声
- #define M_R 0.01
- float KalmanFilter(float newdata)
- {
- static float lastdata=0;
- //其中p的初值可以随便取，但是不能为0（为0的话卡尔曼滤波器就认为已经是最优滤波器了）
- static float p=0.01;
- float q = P_Q, r = M_R, kGain = 0;
-
- p = p+q;
- kGain = p/(p+r); //卡尔曼滤波系数
- newdata = lastdata + (kGain*(newdata-lastdata));
- p = (1-kGain)*p;
- lastdata = newdata;
- return newdata;
- }
-
-
-
| 
楼主
