卡尔曼滤波测试参考几篇文章：步骤：

90 阅读 0 评论 60 点赞

我是靠谱客的博主鲜艳柚子，最近开发中收集的这篇文章主要介绍卡尔曼滤波测试参考几篇文章：步骤：，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

参考几篇文章：

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; 
}