卡尔曼滤波器只对正数起作用，请大家指导

陶新成

#define KALMAN_Q        0.000001
#define KALMAN_R        1.0000
#define KALMAN_P0        1.0000


float KalmanFilter(float ResrcData,float ProcessNiose_Q,float MeasureNoise_R,float InitialPrediction)
{
        float R = MeasureNoise_R;
        float Q = ProcessNiose_Q;

        static        float x_last;

        float x_mid = x_last;
        float x_now;

        static        float p_last;

        float p_mid ;
        float p_now;
        float kg;       
   
        x_mid=x_last;
        p_mid=p_last+Q;
        kg=p_mid/(p_mid+R);
        x_now=x_mid+kg*(ResrcData-x_mid);
               
        p_now=(1-kg)*p_mid;       

        p_last = p_now;
        x_last = x_now;
   
    return x_now;
}
这段程序是我在网上找到的，用VC验证通过调节参数，滤波效果很好，但是只对正数起作用，对负数不能进行有效滤波，哪位能给分析一下，好做更改