卡尔曼滤波器算法浅析及matlab实战
卡尔曼滤波器是1种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。而且由于观测包括系统的噪声和干扰的影响，所以最优估计也可看作是滤波进程。
卡尔曼滤波器的核心内容就是5条公式，计算简单快速，合适用于少许数据的预测和估计。
下面我们用1个例子来讲明1下卡尔曼算法的利用。
假定我们想在有1辆小车，在 t 时刻其速度为 Vt ，位置坐标为 Pt，ut 表示 t 时刻的加速度，那末我们可以用Xt表示 t 时刻的状态，以下：
则我们可以得到，由t⑴ 时刻到 t 时刻，位置和速度的转换以下：
用向量表示上述转换进程，以下：
以下图：
那末我们可以得到以下的状态转移公式：
(1)其中矩阵 F 为状态转移矩阵，表示如何从上1状态来推测当前时刻的状态，B 为控制矩阵，表示控制量u如何作用于当前矩阵，上面的公式 x 有顶帽子，表示只是估计值，其实不是最优的。
有了状态转移公式就能够用来推测当前的状态，但是所有的推测都是包括噪声的，噪声越大，不肯定越大，协方差矩阵用来表示这次推测带来的不肯定性
协方差矩阵
假定我们有1个1维的数据，这个数据每次丈量都不同，我们假定服从高斯散布，那末我们可以用均值和方差来表示该数据集，我们将该1维数据集投影到坐标轴上，以下图：
可以看到，服从高斯散布的1维数据大部份散布在均值附近。
现在我们来看看服从高斯散布的2维数据投影到坐标轴的情况，以下图：
2维数据比1维数据略微复杂1点，投影后有3种情况，分别是：
左图：两个维的数据互不相干；
中图：两个维的数据正相干，也就是 y 随着 x 的增大而增大（假定两个维分别为 x 和 y）
右图：两个维的数据负相干，也就是 y 随着 x 的增大而减小。
那怎样来表示两个维的数据的相干性呢？答案就是协方差矩阵。
状态协方差矩阵传递
在公式（1）当中，我们已得到了状态的转移公式，但是由上面可知，2维数据的协方差矩阵对描写数据的特点是很重要的，那末我们应当如何更新或说传递我们的2维数据的协方差矩阵呢？假设我们用 P 来表示状态协方差，即
那末加入状态转换矩阵 F ，得到
(2)也即：
因此我们便得到了协方差的转换公式。
现在我们得到了两个公式，应用这两个公式能够对现在状态进行预测。依照我们的正常思路来理解，预测结果不1定会对嘛，肯定有误差。而且在我们大多数回归算法或是拟合算法中，1般思路都是先预测，然后看看这个预测结果跟实际结果的误差有多大，再根据这个误差来调剂预测函数的参数，不断迭代调剂参数直到预测误差小于1定的阈值。
卡尔曼算法的迭代思想也类似，不过这里根据误差调剂的是状态 X 。
在这里，我们的实际数据就是 Z， 以下图：
其中，矩阵 H 为丈量系统的参数，即视察矩阵，v 为观测噪声， 其协方差矩阵为R
那末我们的状态更新公式以下：
其中K 为卡尔曼系数， Z-Hx 则为残差，也就是我们说的，预测值与实际值的误差。
K的作用：
1.K 权衡预测协方差P和视察协方差矩阵R那个更加重要，相信预测，残差的权重小，相信视察，残差权重大，由 K 的表达是可以退出这个结论
2,将残差的表现情势从视察域转换到状态域（残差与1个标量，通过K 转换为向量），由 状态 X 的更新公式可得到该结论。
至此，我们已得到了 t 状态下的最优估计值 Xt。但为了能让我们的迭代算法延续下去，我们还必须更新状态协方差的值。
状态协方差的更新
以上就是卡尔曼滤波算法的思想，只有简单的 5 条公式，总结以下：
Matlab 实现
function kalmanFiltering
%%
clc
close all
%%
%
% Description : kalmanFiltering
% Author : Liulongpo
% Time：2015⑷⑵9 16:42:34
%
%%
Z=(1:2:200); %观测值 汽车的位置 也就是我们要修改的量
noise=randn(1,100); %方差为1的高斯噪声
Z=Z+noise;
X=[0 ; 0 ]; %初始状态
P=[1 0;0 1]; %状态协方差矩阵
F=[1 1;0 1]; %状态转移矩阵
Q=[0.0001,0;0 , 0.0001]; %状态转移协方差矩阵
H=[1,0]; %观测矩阵
R=1; %观测噪声方差
figure;
hold on;
for i = 1:100
%基于上1状态预测当前状态
X_ = F*X;
% 更新协方差 Q系统进程的协方差 这两个公式是对系统的预测
P_ = F*P*F'+Q;
% 计算卡尔曼增益
K = P_*H'/(H*P_*H'+R);
% 得到当前状态的最优化估算值 增益乘以残差
X = X_+K*(Z(i)-H*X_);
%更新K状态的协方差
P = (eye(2)-K*H)*P_;
scatter(X(1), X(2),4); %画点，横轴表示位置，纵轴表示速度
end
end
效果以下
其中 x 轴为位置，y轴为速度。
在代码中，我们设定x的变化是 1:2:200，则速度就是2，可以由上图看到，值经过几次迭代，速度就基本上在 2 附近摆动，摆动的缘由是我们加入了噪声。
接下来来看1个实际例子。
我们的数据为 data = [149.360 , 150.06, 151.44, 152.81,154.19 ,157.72];
这是应用光流法从视频中获得角点的实际x轴坐标，总共有6个数据，也就是代表了1个点的连续6帧的x轴坐标。接下来这个例子，我们将实现用5帧的数据进行训练，然后预测出第6帧的x轴坐标。
在上1个matlab例子中，我们的训练数据比较多，因此我们的初始状态设置为[0,0]，也就是位置为0，速度为0，在训练数据比较多的情况下，初始化数据为0并没有关系，由于我们在上面的效果图中可以看到，算法的经太短暂的迭代就可以够发挥作用。
但在这里，我们的训练数据只有5帧，所以为了加快训练，我们将位置状态初始化为第1帧的位置，速度初始化为第2帧与第1帧之差。
代码以下：
KF.m
function [predData,dataX] = KF(dataZ)
%%
%
% Description : kalmanFiltering
% Author : Liulongpo
% Time：2015⑷⑵9 16:42:34
%
%%
Z = dataZ';
len = length(Z);
%Z=(1:2:200); %观测值 汽车的位置 也就是我们要修改的量
noise=randn(1,len); %方差为1的高斯噪声
dataX = zeros(2,len);
Z=Z+noise;
X=[Z(1) ; Z(2)-Z(1) ]; %初始状态 分别为 位置 和速度
P=[1 0;0 1]; %状态协方差矩阵
F=[1 1;0 1]; %状态转移矩阵
Q=[0.0001,0;0 , 0.0001]; %状态转移协方差矩阵
H=[1,0]; %观测矩阵
R=1; %观测噪声方差
%figure;
%hold on;
for i = 1:len
%基于上1状态预测当前状态
% 2x1 2x1
X_ = F*X;
% 更新协方差 Q系统进程的协方差 这两个公式是对系统的预测
% 2x1 2x1 1x2 2x2
P_ = F*P*F'+Q;
% 计算卡尔曼增益
K = P_*H'/(H*P_*H'+R);
% 得到当前状态的最优化估算值 增益乘以残差
X = X_+K*(Z(i)-H*X_);
%更新K状态的协方差
P = (eye(2)-K*H)*P_;
dataX(:,i) = [X(1);X(2)];
%scatter(X(1), X(2),4); %画点，横轴表示位置，纵轴表示速度
end
predData = F*X;
endtestKF.m
function testKF
%%
clc
close all
%%
%data = load('D:a.txt');
%data = [149.360 , 150.06, 151.44, 152.81,154.19,157.72,157.47,159.33,153.66];
data = [149.360 , 150.06, 151.44, 152.81,154.19 ,157.72];
[predData , DataX] = KF(data');
error = DataX(1,:) - data;
i = 1:length(data);
figure
subplot 311
scatter(i,data,3),title('原始数据')
subplot 312
scatter(i,DataX(1,:),3),title('预测数据')
subplot 313
scatter(i,error,3),title('预测误差')
predData(1)
%{
scatter(i,error,3);
figure
scatter(i,data,3)
figure
scatter(i,predData(1,:),3)
%}
end效果以下：
预测结果为： 155.7493 ，跟实际结果 157.72 唯一1.9 的误差，可以看到，卡尔曼滤波器算法对少许数据的预测效果还是挺不错的。固然，预测位置的同时，我们也得到了预测速度。
参考文献： 视频: 卡尔曼滤波的原理和在MATLAB中的实现