基本的数据处理方法

在kalman滤波上再次对数据进行处理进一步使数据的杂点滤除，使用了概率算法。
static float HaFilter(float Raw)
{
#define MAXRAW 100
    static short InitFlag=1;
    static float RangeHits[MAXRAW]={0};
    static float RangeVal[MAXRAW]={0};
    int RawIndex=0;
    float MaxHitsVal=0;
    short i=0;

    if(Raw>100)
    {
        Raw=100;
    }
    else if(Raw<1)
    {
        Raw = 1;
    }
    if(InitFlag)
    {
        InitFlag=0;
        for(i=0;i<=MAXRAW-1;i++)
        {
            RangeVal[i] = i+1;
        }
    }

    for(i=0;i<=MAXRAW-1;i++)
    {
        RangeHits[i] = RangeHits[i]*0.99;
    }

    RawIndex = (int)(Raw+0.5)-1;
    RangeHits[RawIndex] = RangeHits[RawIndex]+1;
    RangeVal[RawIndex] = RangeVal[RawIndex]*0.9 + Raw*0.1;

    for(i=0;i<=MAXRAW-1;i++)
    {
        if(RangeHits[i]>MaxHitsVal)
        {
            MaxHitsVal = RangeHits[i];
            RawIndex = i;
        }
    }
    return RangeVal[RawIndex];
}

动态kalman滤波 P:协方差矩阵，R：测量误差协方差矩阵，K：增益矩阵，Q：过程协方差矩阵

static float Kalman(float Z)
{
#define MAXDISTANCE 150
    static short Initflag=1;
    static float OldX=0,X=0;
    static float P=6,K=0,R=6,Q=0.33;
    if(Initflag)
    {
        Initflag=0;
        P=R;
        OldX=Z;
        X=Z;
    }
    else
    {
        X=OldX+K*(Z-OldX);
        if(X<0)
        {
            X=0;
        }
        else if(X>MAXDISTANCE)
        {
            X=OldX;
        }
        OldX=X;
    }
    P=P+Q;
    K=P/(P+R);//增益计算
    P=P-P*K;//协方差更新方程
    return X;
}

简易的kalman滤波,主要用于简单跳过突然出现的点。

static uint32_t lfR_Filter1( uint32_t nNewVal, uint32_t GpMiss)
{

    static uint32_t cntMiss = 0;
    static uint32_t cntKeep = 0;
    static int32_t oldData = 0;
    int32_t tmpV;
    int32_t newData = nNewVal;

    if( oldData == 0 )
    {
        oldData = nNewVal;
    }
    else
    {
        tmpV = oldData/10;
        if( (newData > (-1)*tmpV+oldData) && (newData < tmpV+oldData) )
        {
            oldData = ( oldData*9 + newData )/10;
            cntKeep = cntKeep + 1;
            cntMiss = 0;
        }
        else
        {
            oldData = oldData;
            cntKeep = cntKeep - 1;
            cntMiss = cntMiss +1;
            if( cntMiss > GpMiss )
            {
                oldData = newData;
            }
        }
    }
    return oldData;
}