卡尔曼滤波器原理介绍

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

        卡尔曼滤波器的算法是二步骤的程序。在估计步骤中，卡尔曼滤波器会产生有关目前状态的估计，其中也包括不确定性。只要观察到下一个测量（其中一定含有某种程度的误差，包括随机噪声）。会通过加权平均来更新估计值，而确定性越高的量加权平均的比重也越高。算法是迭代的，可以在实时控制系统中执行，只需要目前的输入量，以往的计算值以及其不确定性矩阵，不需要其他以往的资讯。

1.卡尔曼滤波器的原理与理解

1.1状态预测

 

 1.2协方差矩阵

 

 

 

       现在我们得到上面的两个公式，运用这两个公式能够对现在状态进行预测。但是预测结果不可能完全正确，肯定有误差。而恰好，我们可能会有多个传感器(如测距雷达)来测量系统当前的状态，哪个传感器具体测量的是哪个状态变量并不重要，也许一个是测量位置，一个是测量速度，每个传感器间接地告诉了我们一些状态信息，因此我们可以用测量来修正(更新)预测。

1.3状态更新

 

 

 1.4示例仿真

 实验结果：

 

由上图看到，值经过几次迭代，速度就基本上在 2 附近摆动，摆动的原因是我们加入了噪声

matlab代码如下：

%卡尔曼滤波(小车[速度,位置]例子)观测数据多
clc,clear,close all
%% 传感器观测
%--------------------------------------------------------------------------
%Z = H * X + v
%X为t-1时刻实际状态
%Z为t-1时刻实际观测数据
%H为测量系统的参数,即观察矩阵
%v为观测噪声,其协方差矩阵为R
%--------------------------------------------------------------------------
Z=(1:2:200); %理想观测值 (汽车的位置,也就是我们要修改的量),设定变化是1:2:200，则速度就是2
noise=randn(1,100); %在理想观测值上叠加方差为1的高斯噪声
Z=Z+noise;%模拟的实际观测值
H=[1,0];%传感器提供的观测矩阵
R=1;%传感器的观测噪声协方差矩阵
%% 初始状态(均值)和状态协方差
%基于高斯分布建立状态变量需要均值和协方差(即X和P)
X=[0;0]; %初始状态 X=[位置;速度]
P=[1 0;0 1]; %状态协方差矩阵
%% 状态转移矩阵(表示如何由上一状态推测当前状态),它同时作用与X和P来预测下一时刻的X和P
F=[1 1;0 1]; %在速度例题中为[1 delta(t);0 1]
%% 外部干扰用状态转移协方差矩阵Q表示
Q=[0.0001,0;0 , 0.0001];
%% 卡尔曼滤波
figure;
hold on;
for i = 1:length(Z) %迭代次数
%% 预测
X_ = F*X;%基于上一状态预测当前状态  X_为t时刻状态预测(这里没有控制)
P_ = F*P*F'+Q;%更新协方差  Q系统过程的协方差
%% 计算卡尔曼增益
K = P_*H'/(H*P_*H'+R);
%% 更新
X = X_+K*(Z(i)-H*X_);% 得到当前状态的最优化估算值  增益乘以残差
P = (eye(2)-K*H)*P_;%更新K状态的协方差
%% 绘图
set(0,'defaultfigurecolor','w')
scatter(X(1),X(2));
xlabel('位置'),ylabel('速度')
grid on
%在代码中，我们设定x的变化是1:2:200，则速度就是2
%由上图看到，值经过几次迭代，速度就基本上在 2 附近摆动，摆动的原因是我们加入了噪声。
end

    如果文章有用的话，麻烦扫描下方二维码给作者公众号点点关注，后续会优先在公众号逐步更新卡尔曼滤波器在智能驾驶领域（比如车道线处理，目标融合）是如何使用。