Kalman滤波原理及仿真手册
KF/EKF/UKF原理+应用实例+MATLAB程序
本手册的研究内容主要有Kalman滤波，扩展Kalman滤波，无迹Kalman滤波等，包括理论介绍和MATLAB源程序两部分。本手册所介绍的线性滤波器，主要是Kalman滤波和α-β滤波，交互多模型Kalman滤波，这些算法的应用领域主要有温度测量、自由落体，GPS导航、石油地震勘探、视频图像中的目标检测和跟踪。
EKF和UKF主要在非线性领域有着重要的应用，目标跟踪是最主要的非线性领域应用之一，除了讲解目标跟踪外，还介绍了通用非线性系统的EKF和UKF滤波处理问题，相信读者可以通过学习本文通用的非线性系统，能快速掌握EKF和UKF滤波算法。
本文所涉及到的每一个应用实例，都包含原理介绍和程序代码（含详细的中文注释）。
一、四维目标跟踪Kalman线性滤波例子
在不考虑机动目标自身的动力因素，将匀速直线运动的船舶系统推广到四
?(k)维，即状态X(k)??x(k)xy(k)?(k)?T包含水平方向的位置和速度和纵向y的位置和速度。则目标跟踪的系统方程可以用式（3.1）和（3.2）表示，
X(k?1)??X(k)??u(k) （2-4-9） Z(k)?HX(k)?v(k) （2-4-10）
?1?0其中，????0??0T1000010?0.5T20??0??，???T?0T???1??0?0??1??00?，H??2??00.5T??T???00??x??x??0??，X???，?y?1??????0??yTT?x?Z???，u,v为零均值的过程噪声和观测噪声。T为采样周期。为了便于理解，
?y?将状态方程和观测方程具体化：
?x(k)??1?x?(k)??0?????y(k)??0????(k)??0?yT00??x(k?1)??0.5T2?x?(k?1)??100??????T01T??y(k?1)??0?????(k?1)??001??y?00??0?w(k) 2?2?10.5T?T???x(k)???(k)??x(k)??1000??x??v2?1(k) Z???????y(k)??0010??y(k)????y(k)??假定船舶在二维水平面上运动，初始位置为（-100m,200m），水平运动速度为2m/s，垂直方向的运动速度为20 m/s，GPS接收机的扫描周期为T=1s，观测噪声的均值为0，方差为100。过程噪声越小，目标越接近匀速直线运动，反之，则为曲线运动。仿真得到以下结果：
1600真实轨迹观测轨迹滤波轨迹 25滤波前误差滤波后误差 1400201200151000800106005400200 -120-100-80-60-40-20020400 0102030405060
图3-1 跟踪轨迹图 图3-2 跟踪误差图
仿真程序
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Kalman滤波在目标跟踪中的应用实例
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function Kalman clc;clear;
T=1;%雷达扫描周期, N=80/T; %总的采样次数
X=zeros(4,N); % 目标真实位置、速度
X(:,1)=[-100,2,200,20];% 目标初始位置、速度 Z=zeros(2,N); % 传感器对位置的观测 Z(:,1)=[X(1,1),X(3,1)]; % 观测初始化
delta_w=1e-2; %如果增大这个参数，目标真实轨迹就是曲线了 Q=delta_w*diag([0.5,1,0.5,1]) ; % 过程噪声均值 R=100*eye(2); %观测噪声均值
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1]; % 状态转移矩阵 H=[1,0,0,0;0,0,1,0]; % 观测矩阵
……
……
二、视频图像目标跟踪Kalman滤波算法实例
如下图所示，对于自由下落的皮球，要在视频中检测目标，这里主要检测目标中心，即红心皮球的重心，在模型建立时可以将该重心抽象成为一个质点，坐标为(x,y)。
图2-6-1 下落的球 图2-6-2 检测下落的球 图2-6-3 跟踪下落的球 那么对该质点跟踪，它的状态为X(k)??x?yx??，状态方程如下 y?1dt00??0??01dt0??0??X(k)???w(k) X(k?1)???0010??0?????0001???g?观测方程为
?1000?Z(k)???X(k)?v(k) 0100??在这个过程中，前提是目标检测，一定要找到重心(x,y)，与雷达目标跟踪中观测目标位置是一回事。 图像目标检测跟踪程序
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 目标检测函数，这个函数主要完成将目标从背景中提取出来
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function detect
clear,clc; %清除所有内存变量、图形窗口 % 计算背景图片数目 Imzero = zeros(240,320,3); for i = 1:5
% 将图像文件 i.jpg 的图像像素数据读入矩阵Im Im{i} = double(imread(['DATA/',int2str(i),'.jpg'])); Imzero = Im{i}+Imzero; end
Imback = Imzero/5;
[MR,MC,Dim] = size(Imback); % 遍历所有图片 for i = 1 : 60
% 读取所有帧
…… ……
运行程序得到的x,y方向的位置跟踪偏差分析
2001501005000102030405060
Y方向的位置偏差
1501005000102030405060
X方向的位置偏差
三、通用非线性系统的EKF实现例子：
所谓的非线性方程，就是因变量与自变量的关系不是线性的，这类方程很多，例如平方关系，对数关系，指数关系，三角函数关系等等。这些方程可分为两类，一类是多项式方程，一种是非多项式方程。为了便于说明非线性卡尔曼滤波——扩展Kalman滤波的原理，我们选用以下系统，
系统状态为X(k)，它仅包含一维变量，即X(k)??x(k)?，系统状态方程为
X(k)?0.5X(k?1)?2.5X(k?1)?8cos(1.2k)?w(k) (3-2-1)
1?X2(k?1)观测方程为
X2(k)Y(k)??v(k) (3-2-2)
20其中，式(3-1-1)是包含分式，平方，三角函数在内的严重非线性的方程，w(k)为过程噪声，其均值为0，方差为Q，观测方程中，观测信号Y(k)与状态X(k)的
v(k)也是均值为0，关系也是非线性的，方差为R的高斯白噪声。因此关于(3-1-1)
和(3-2-2)是一个状态和观测都为非线性的一维系统。以此为通用的非线性方程的代表，接下来讲述如何用扩展Kalman滤波来处理噪声问题。
第一步：初始化初始状态X(0)，Y(0)，协防差矩阵P0。 第二步：状态预测
X(k|k?1)?0.5X(k?1)?2.5X(k?1)?8cos(1.2k) (3-2-3)
1?X2(k?1)
第三步：观测预测
X2(k|k?1)Y(k|k?1)? (3-2-4)
20…… ……
第九步：协方差更新
P(k)?(In?K(k)H(k))P(k|k?1) (3-2-10)
以上九步为扩展卡尔曼滤波的一个计算周期，如此循环下去就是各个时刻EKF对非线性系统的处理过程。其他参数设置请查看源程序，仿真以上系统得到状态滤波结果，如图3-2-1所示，滤波后的状态与真值之间的偏差如图图3-2-2