小李飞刀 发表于 2016-1-20 09:51
单一量的话 没法对它进行概率的最优化求解

以前见过一个说是卡尔曼。不知道是不是。 套用后有效果。但是数据滞后严重。
代码如下：
[mw_shl_code=c,true]#include "KF.h"


   
double KF_Q = 0.01;                // Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
double KF_R = 0.1;                // R:测量噪声，R增大，动态响应变慢，收敛稳定性变好     



u16 KalmanFilter(u16 v)
{
        static double x_last;
        static double p_last;
        double x_mid;
        double x_now;
        double p_mid;
        double p_now;//协方差
        double kg;   //卡尔曼     
       
       
        x_mid = x_last;                                                 //x_last=x(k-1|k-1), x_mid=x(k|k-1)
        p_mid = p_last + KF_Q;                                 //p_mid=p(k|k-1),p_last=p(k-1|k-1),KF_Q=噪声
        kg = p_mid / (p_mid+KF_R);                 //kg为kalman filter，R为噪声
        x_now = x_mid + kg * (v-x_mid);        //估计出的最优值

        p_now = (1-kg) * p_mid;                                //最优值对应的covariance        

        p_last = p_now; //更新covariance值//协方差
        x_last = x_now; //更新系统状态值              

        return        x_now;
}

//end[/mw_shl_code]