中位值滤波法与卡尔曼滤波法

bcwx

#define M  10
/***中位值滤波法***/
unsigned int MedianFiltering(u16 (ADC)(void))
{
    unsigned int value_buf[M];
    unsigned int count, i, j, temp;
    for( count = 0; count < M; count++ )
    {
        value_buf[count] = ADC();
    }
    for( j = 0; j < M - 1; j++ )
    {
        for( i = 0; i < M - j - 1; i++ )
        {
            if( value_buf > value_buf[i + 1] )
            {
                temp = value_buf;
                value_buf = value_buf[i + 1];
                value_buf[i + 1] = temp;
            }
        }
    }
    return value_buf[( M - 1 ) / 2];
}


/*
卡尔曼
    R值固定，Q值越大，代表越信任测量值，Q值无穷大，代表只用测量值。
             Q值越小，代表越信任模型预测值，Q值为0，代表只用模型预测值。
Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏

R:测量噪声，R增大，动态响应变慢，收敛稳定性变好
*/
//参数一
float  KalmanFiltering( float inData)
{
    static float prevData = 0;                                 //上一个数据
    static float p = 10, q = 0.009, r = 0.009, kGain = 0;      // q 控制误差 r 控制响应速度
    p = p + q;
    kGain = p / ( p + r );                                      //计算卡尔曼增益
    inData = prevData + ( kGain * ( inData - prevData ) );      //计算本次滤波估计值
    p = ( 1 - kGain ) * p;                                      //更新测量方差
    prevData = inData;
    return inData;                                             //返回估计值
}





蓝色线条：直接的AD值
绿色线条：中位值滤波
红色线条：中位值滤波+卡尔曼滤波