卡尔曼滤波测试

参考几篇文章：

C语言实现卡尔曼滤波（转）
卡尔曼滤波的简单实现(Matlab & OC)
卡尔曼滤波简单理解及C语言代码

步骤：

1. X(k|k-1)=X(k-1|k-1);
   X(k-1|k-1)表示上个时刻的值，X(k|k-1)表示当前时刻的猜测值，猜测当前值和上一时刻的值相同；
2. P(k|k-1)=P(k-1|k-1)+Q;
   P(k|k-1)表示当前预测的协方差，P(k-1|k-1)表示上一时刻的协方差，Q表示过程噪声的协方差，运算时并没有用到过程噪声，而是使用其协方差进行运算；
3. K(k)=P(k|k-1)/(P(k|k-1)+R);
   R表示测量噪声的协方差，K(k)是用两个协方差得到的权值，称为卡尔曼增益;
4. X(k|k)=X(k|k-1)+K(k) * (Z(k)-X(k|k-1));
   Z(k)表示当前时刻的测量值，X(k|k)为当前修正的值，等于 当前预测值 + 卡尔曼增益 * (当前测量值 - 上一次滤波后的值)
5. P(k|k)=（1-K(k)）*P(k|k-1);
   最后更新修正值的协方差;

Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
R:测量噪声，R增大，动态响应变慢，收敛稳定性变好

调参主要是调QR, P可以随便设；

示例波形

1. Q = 0.0001 R = 0.005
   

2. Q = 0.1 R = 0.005 (Q增大响应变快)
   

3. Q = 0.0001 R = 0.5 (R增大收敛稳定性变好)
   

static float prevData=0; 
static float p=1000, q=0.0001, r=0.005, kGain=0;

float kalmanFilter_A(u32 inData) 
{
    p = p+q; 
    kGain = p/(p+r);

    inData = prevData+(kGain*(inData-prevData)); 
    p = (1-kGain)*p;

    prevData = inData;

    return inData; 
}