手把手教你学-卡尔曼滤波（附代码）

很多人都知道卡尔曼滤波的神奇之处，很多人也看过别人的公式和代码，你有可能会有疑问，这些公式究竟是如何写成代码的。下面我来为你们介绍以下我的学习过程：

卡尔曼滤波器的概念及特点

卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准。卡尔曼滤波得名自主要贡献者之一的鲁道夫·卡尔曼。

摘自维基百科

卡尔曼滤波器结合了上一状态的结果和当前状态的测量值预估出真正状态的测量值。

很多人可能会想：我直接那测量值作为真实值不可以吗？可以，但测量仪器总有误差，人们总想通过一些状态取获取真正的状态，这样就可以提高系统的稳定性。

我觉得卡尔曼真正的优点在于：只需要知道上一状态的值和当前状态的值就可以预估出结果，而不需要很多历史值。所以该滤波器的实时性非常好。

其他的也不说了，因为本作者知识储备有限，没有办法综合考量所有的情况。我们大概只需要知道卡尔曼滤波的大概样式以及作用，其实在作用这块我也不是很清楚，没有办法结合实际应用来和大家展开讲讲。

接下来我们需要了解以下基本动态系统模型：

基本动态系统模型

了解就行，不用深究因为有两个系数会在卡尔曼滤波中用到

卡尔曼滤波建立在线性代数和隐马尔可夫模型（hidden Markov model）上。其基本动态系统可以用一个马尔可夫链表示，该马尔可夫链建立在一个被高斯噪声（即正态分布的噪声）干扰的线性算子上的。系统的状态可以用一个元素为实数的向量表示。随着离散时间的每一个增加，这个线性算子就会作用在当前状态上，产生一个新的状态，并也会带入一些噪声，同时系统的一些已知的控制器的控制信息也会被加入。同时，另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。

为了从一系列有噪声的观察数据中用卡尔曼滤波器估计出被观察过程的内部状态，必须把这个过程在卡尔曼滤波的框架下建立模型。

摘自维基百科

卡尔曼滤波模型假设k时刻的真实状态是从（k − 1）时刻的状态演化而来，符合下式：

这里的变量解释在下面公式中有介绍，这里我们了解一下

：是过程噪声，并假定其符合均值为零，协方差矩阵为  的多元正态分布。

时刻k，对真实状态的一个测量满足下式：

这里的变量解释在下面公式中有介绍，这里我们了解一下

：是观测噪声，其均值为零，协方差矩阵为  ,且服从正态分布。

 和在下面公式中会使用到

接下来我们来看卡尔滤波的具体公式：

卡尔曼滤波器的公式

卡尔曼滤波器的操作包括两个阶段：预测与更新。

在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。

在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

预测

滤波器使用上一状态的估计，做出对当前状态的估计。

以上两个公式就是卡尔曼滤波在预测时的公式。

下面来介绍每个变量的意义：

：k-1时刻的最优估计值。

：k-1时刻在k时刻的最优估计值。

：k-1时刻估计值的协方差矩阵。

：k-1时刻在k时刻估计值的协方差矩阵。

：作用在上的状态变换模型。

：作用在控制器向量上的输入控制模型。

：控制向量。

：过程噪声的协方差矩阵。

以上变量均可是矩阵或标量。

更新

在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

首先，我们需要计算出一下三个量：

                    ：测量残差

        ：测量残差协方差

               ：最优卡尔曼增益

然后，更新x与P

以上两个公式就是卡尔曼滤波在更新时的公式。

：k时刻的最优估计值。

：k时刻估计值的协方差矩阵。

：观测噪声的协方差矩阵。

：观测模型，将真实空间状态映射成观测空间

：k时刻，真实状态的一个测量值

参数选定

了解完了公式之后，肯定会想，如果要预测到工程实际中，这些参数该使用何值？

下面为大家介绍一下参数的取值：

预测部分参数选定： 

 ：输入量。

：输入量的估计值。中间过程计算值。

：状态转移矩阵。

（1） 在标量卡尔曼滤波中，比如测量值是温度、湿度，一般认为下一个时刻该温度或者湿度维持不变，这种情况下状态转移矩阵通常就是标量1。

（2） 在导航和目标跟踪中卡尔曼滤波常被用来做位置估计，比如匀速直线运动（CV）和匀加速直线运动（CA）。

：表示可选的控制输入的增益，在大多数实际情况下并没有控制增益。

：表示k-1时刻的控制增益，一般没有这个变量，可以设为0。

：上一次滤波估计值协方差矩阵结果

：滤波估计值协方差矩阵中间值

：表示过程激励噪声的协方差，它是状态转移矩阵与实际过程之间的误差。在一阶的滤波中可认为是一个可调的参数。

更新部分参数选定

，，：更新过程中的中间量

 ：本次滤波的结果，也是下一次滤波的输入值

：估计值协方差矩阵的输出值，也是下次的输入值。

：测量值，做输入值使用。

：观测噪声的协方差矩阵，此值一般取绝于测量器件的固有噪声。

：将观测值的位数与估计值的位数相匹配。一般在矩阵中的元素只有1和0。在一维中H=1

：单位矩阵。

卡尔曼滤波器的代码

根据卡尔曼滤波矩阵式所有的参量都取标量

,,,,,这些值取定值。

简化原始方程并写成伪代码

P_now = P_last + Q_cov;
K = P_now / (P_now + R_cov);
x_last = x_last + K * (z - x_last);
P_last = (1 - K) * P_now;

上面四个等式就是一次卡尔曼滤波的过程，下面定义结构体，使过程更简洁。

typedef struct Kalman_Filter{
	float x_last;
	float P_now;
	float P_last;
	float K;
	float R_cov;
	float Q_cov;
}KF_Struct; 

接下来初始化KF_Struct

void KF_Struct_Init(KF_Struct* KFS)
{
	KFS->x_last	=0;
	KFS->P_now	=0;
	KFS->P_last	=0.02;
	KFS->K		=0;
	KFS->Q_cov	=0.005;//过程激励噪声协方差,参数可调
	KFS->R_cov	=0.5;//测量噪声协方差，与仪器测量的性质有关，参数可调
	KFS->out	=0;
}

使用时创建一个KF_Struct的结构体变量KFS1，将结构体变量的地址传入init函数即可使用

usage example：
KF_Struct KFS1;
KF_Struct_Init(&KFS1);

接下来，就是使用卡尔曼滤波函数了：

/*
* @brief    卡尔曼滤波器
* @param    KFS:卡尔曼滤波器结构体指针
* @param    z:测量仪器的输入量
* @return   当前时刻的最优估计值
*/
float KMFilter(KF_Struct* KFS,float z)
{
	KFS->P_now = KFS->P_last + KFS->Q_cov;
    KFS->K = KFS->P_now / (KFS->P_now + KFS->R_cov );
    KFS->x_out = KFS->x_out + KFS->K * (z - KFS->x_out);
    P_last = (1f - K)* P_now;
    
    return KFS->x_out;
}

测试 

写完了这些我们就需要试一试实际效果到底怎么样了

我是用matlab来进行代码测试，因为matlab可视化比较简单

N=500;
%kalman init
P_last = 0.02;
P_now = 0.0;
Q_cov = 0.013; %过程激励噪声协方差,参数可调
R_cov = 0.543; %测量噪声协方差，与仪器测量的性质有关，参数可调
x_last = 0.0;
K = 0.0;
out = 0.0;
%standard&bias
x = 0:pi/N:2*pi;
y = 10*sin(x);
%y=0;
bias = randn(1,2*N+1);
y_bias = y+bias;

%%%%%kalman
x_last = y_bias(1);
out(1)=0;
for i=2:2*N+1
    z=y_bias(i);

    P_now = P_last + Q_cov;
    K = P_now / (P_now + R_cov );
    x_last = x_last + K * (z - x_last);
    P_last = (1 - K)* P_now;
    out(i) = x_last;
end

%%%%一阶低通滤波
k=0.565;
ditong_filter(1)=0;
for j=2:2*N+1
    ditong_filter(j)=k*y_bias(j)+(1-k)*y_bias(j-1);
end

plot(x,y,x,y_bias,x,out,x,ditong_filter);

 实现的方式为在曲线上叠加一个随机噪声服从正态分布，然后通过滤波器进行滤波处理。

以下为实验图片

蓝色=初始曲线，黄色=叠加噪声后的曲线，绿色=卡尔曼滤波后的曲线，红色=低通滤波后的曲线

此图为全周期视图

 下图是上图局部放大后的样子

 下图为初始值为恒定值的滤波曲线，图例与上图相同

 总结

卡尔曼滤波器滤波效果还挺好的，但我对于其的了解仅限于此。

如果大家找到觉得很好的中文或外文资料可以评论一下，大家相互分享。

我觉得有问题的一点在于此种卡尔曼滤波器调Q_cov时，真实值为静态时滤波效果较好，真实值为动态时滤波Q_cov越小曲线越平滑但有一定之后性，这个问题有什么办法解决，我觉得应该时一阶算法的局限性，有大佬可以评论区解答一下吗？^_^

若作者有更多对卡尔曼滤波的理解，我也会及时分享。

觉得此文写的不错的兄弟，可以留下你的一个赞吗

感谢！

引用

卡尔曼滤波 - 维基百科，自由的百科全书 (wikipedia.org)

卡尔曼滤波的理解以及参数调整_张京林要加油的技术专栏-CSDN博客_卡尔曼滤波参数