欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

卡尔曼滤波测试

程序员文章站 2022-04-16 20:31:12
...

参考几篇文章:

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; 
}
相关标签: 卡尔曼 滤波