卡尔曼滤波算法

通过修改Q值和R值，来更改卡尔曼滤波是幅度，对于RSSI的滤波处理，建议采用Q = 7 ，R = 300

typedef struct 
{
    float startValue;    //k-1时刻的滤波值，即是k-1时刻的值
    float kalmanGain;    //Kalamn增益
    float A;        //x(n)=A*x(n-1)+u(n),u(n)~N(0,Q)
    float paraH;        //z(n)=H*x(n)+w(n),w(n)~N(0,R)
    float Q;        //预测过程噪声偏差的方差
    float R;        //测量噪声偏差，(系统搭建好以后，通过测量统计实验获得) 100.0
    float P;        //估计误差协方差; 下一时刻的协方差，初始化随意
} KalmanFilterPara;

void KalmanFilterParaUpdate(KalmanFilterPara *pKF, double Q, double R)
{
    pKF->Q = Q;            //预测过程噪声偏差的方差
    pKF->R = R;            //测量噪声偏差，(系统搭建好以后，通过测量统计实验获得) 100.0
}
void KalmanFilterParaInit(KalmanFilterPara *pKF, double Q, double R)//7,300  5,500 ，9,100
{
    pKF->startValue = 0xFF;//50;  //k-1时刻的滤波值，即是k-1时刻的值
    pKF->kalmanGain = 0;   //Kalamn增益
    pKF->A = 1;            //x(n)=A*x(n-1)+u(n),u(n)~N(0,Q)
    pKF->paraH = 1;        //z(n)=H*x(n)+w(n),w(n)~N(0,R)
    pKF->P = 10;           //估计误差协方差; 下一时刻的协方差，初始化随意
    pKF->Q = Q;            //预测过程噪声偏差的方差
    pKF->R = R;            //测量噪声偏差，(系统搭建好以后，通过测量统计实验获得) 100.0
}
uint8_t KalmanFilter(KalmanFilterPara *pKF,uint8_t newValue)
{
    float predictValue;
    //预测下一时刻的值 第一次计算时使用当前值
    if(0xFF == pKF->startValue)
    {
        pKF->startValue = newValue;
    }
    predictValue = pKF->A* pKF->startValue;
    //求预测下一时刻的协方差
    pKF->P = pKF->A*pKF->A*pKF->P + pKF->Q;  //计算先验均方差 p(n|n-1)=A^2*p(n-1|n-1)+q
    //计算kalman增益
    pKF->kalmanGain = pKF->P*pKF->paraH / (pKF->P*pKF->paraH*pKF->paraH + pKF->R);  //Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
    //修正结果，即计算滤波值
    pKF->startValue = predictValue + (newValue - predictValue)*pKF->kalmanGain;
    //利用残余的信息改善对x(t)的估计，给出后验估计，这个值也就是输出  X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))
    //更新后验估计
    pKF->P = (1 - pKF->kalmanGain*pKF->paraH)*pKF->P;//计算后验均方差  P[n|n]=(1-K[n]*H)*P[n|n-1]

    return  (uint8_t)pKF->startValue;
}