卡尔曼滤波测试
程序员文章站
2022-04-16 20:31:12
...
参考几篇文章:
C语言实现卡尔曼滤波(转)
卡尔曼滤波的简单实现(Matlab & OC)
卡尔曼滤波简单理解及C语言代码
步骤:
- X(k|k-1)=X(k-1|k-1);
X(k-1|k-1)表示上个时刻的值,X(k|k-1)表示当前时刻的猜测值,猜测当前值和上一时刻的值相同; - P(k|k-1)=P(k-1|k-1)+Q;
P(k|k-1)表示当前预测的协方差,P(k-1|k-1)表示上一时刻的协方差,Q表示过程噪声的协方差,运算时并没有用到过程噪声,而是使用其协方差进行运算; - K(k)=P(k|k-1)/(P(k|k-1)+R);
R表示测量噪声的协方差,K(k)是用两个协方差得到的权值,称为卡尔曼增益; - X(k|k)=X(k|k-1)+K(k) * (Z(k)-X(k|k-1));
Z(k)表示当前时刻的测量值,X(k|k)为当前修正的值,等于 当前预测值 + 卡尔曼增益 * (当前测量值 - 上一次滤波后的值) - P(k|k)=(1-K(k))*P(k|k-1);
最后更新修正值的协方差;
Q:过程噪声,Q增大,动态响应变快,收敛稳定性变坏
R:测量噪声,R增大,动态响应变慢,收敛稳定性变好
调参主要是调QR, P可以随便设;
示例波形
Q = 0.0001 R = 0.005
Q = 0.1 R = 0.005 (Q增大响应变快)
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;
}
上一篇: 卡尔曼滤波算法