Kalman滤波原理及程序(手册)解析

Kalman 滤波原理及仿真手册KF/EKF/UKF 原理+应用实例+MATLAB 程序本手册的研究内容主要有Kalman 滤波，扩展Kalman 滤波，无迹Kalman 滤波等，包括理论介绍和MATLAB 源程序两部分。

本手册所介绍的线性滤波器，主要是Kalman 滤波和α-β滤波，交互多模型Kalman 滤波，这些算法的应用领域主要有温度测量、自由落体，GPS 导航、石油地震勘探、视频图像中的目标检测和跟踪。

EKF 和UKF 主要在非线性领域有着重要的应用，目标跟踪是最主要的非线性领域应用之一，除了讲解目标跟踪外，还介绍了通用非线性系统的EKF 和UKF 滤波处理问题，相信读者可以通过学习本文通用的非线性系统，能快速掌握EKF 和UKF 滤波算法。

本文所涉及到的每一个应用实例，都包含原理介绍和程序代码（含详细的中文注释）。

一、四维目标跟踪Kalman 线性滤波例子在不考虑机动目标自身的动力因素，将匀速直线运动的船舶系统推广到四维，即状态[]T k yk y k xk x k X )()()()()( =包含水平方向的位置和速度和纵向的位置和速度。

则目标跟踪的系统方程可以用式（3.1）和（3.2）表示，)()()1(k u k X k X Γ+Φ=+ （2-4-9） )()()(k v k HX k Z += （2-4-10）其中，⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=Φ10001000010001T T，⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡=ΓT T TT 05.00005.022，T H ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=00100001，Tyy x x X ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡= ，⎥⎦⎤⎢⎣⎡=y x Z ，u ,v 为零均值的过程噪声和观测噪声。

T 为采样周期。

为了便于理解，将状态方程和观测方程具体化：)(05.00005.0)1()1()1()1(10001000010001)()()()(1222k w T TTT k y k y k x k x T Tk y k y k x k x ⨯⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡+⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡----⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡ )()()()()(01000001)()(12k v k y k y k x k x k y k x Z ⨯+⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡⎥⎦⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡= 假定船舶在二维水平面上运动，初始位置为（-100m,200m ），水平运动速度为2m/s ，垂直方向的运动速度为20 m/s ，GPS 接收机的扫描周期为T=1s ，观测噪声的均值为0，方差为100。

过程噪声越小，目标越接近匀速直线运动，反之，则为曲线运动。

仿真得到以下结果：图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 滤波算法实例如下图所示，对于自由下落的皮球，要在视频中检测目标，这里主要检测目标中心，即红心皮球的重心，在模型建立时可以将该重心抽象成为一个质点，坐标为),(y x 。

图2-6-1 下落的球 图2-6-2 检测下落的球 图2-6-3 跟踪下落的球 那么对该质点跟踪，它的状态为[]yxy xk X =)(，状态方程如下 )(000)(10000100010001)1(k w g k X dt dt k X ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡+⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=+ 观测方程为)()(00100001)(k v k X k Z +⎥⎦⎤⎢⎣⎡= 在这个过程中，前提是目标检测，一定要找到重心),(y x ，与雷达目标跟踪中观测目标位置是一回事。

图像目标检测跟踪程序%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 目标检测函数，这个函数主要完成将目标从背景中提取出来%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function detectclear,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; endImback = Imzero/5;[MR,MC,Dim] = size(Imback); % 遍历所有图片 for i = 1 : 60% 读取所有帧…… ……运行程序得到的x,y 方向的位置跟踪偏差分析Y 方向的位置偏差50100150X 方向的位置偏差三、通用非线性系统的EKF 实现例子：所谓的非线性方程，就是因变量与自变量的关系不是线性的，这类方程很多，例如平方关系，对数关系，指数关系，三角函数关系等等。

这些方程可分为两类，一类是多项式方程，一种是非多项式方程。

为了便于说明非线性卡尔曼滤波——扩展Kalman 滤波的原理，我们选用以下系统，系统状态为)(k X ，它仅包含一维变量，即[])()(k x k X =，系统状态方程为)()2.1cos(8)1(1)1(5.2)1(5.0)(2k w k k X k X k X k X ++-+-+-= (3-2-1)观测方程为)(20)()(2k v k X k Y += (3-2-2)其中，式(3-1-1)是包含分式，平方，三角函数在内的严重非线性的方程，)(k w 为过程噪声，其均值为0，方差为Q ，观测方程中，观测信号)(k Y 与状态)(k X 的关系也是非线性的，)(k v 也是均值为0，方差为R 的高斯白噪声。

因此关于(3-1-1)和(3-2-2)是一个状态和观测都为非线性的一维系统。

以此为通用的非线性方程的代表，接下来讲述如何用扩展Kalman 滤波来处理噪声问题。

第一步：初始化初始状态)0(X ，)0(Y ，协防差矩阵0P 。

第二步：状态预测)2.1cos(8)1(1)1(5.2)1(5.0)1|(2k k X k X k X k k X +-+-+-=- (3-2-3)第三步：观测预测20)1|()1|(2-=-k k X k k Y (3-2-4)…… ……第九步：协方差更新)1|())()(()(--=k k P k H k K I k P n (3-2-10)以上九步为扩展卡尔曼滤波的一个计算周期，如此循环下去就是各个时刻EKF 对非线性系统的处理过程。

其他参数设置请查看源程序，仿真以上系统得到状态滤波结果，如图3-2-1所示，滤波后的状态与真值之间的偏差如图图3-2-2所示。

图3-2-1 EKF 滤波处理后的状态与真值对比 图3-2-2 偏差分析 EKF 一维非线性系统仿真程序%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 函数功能：一维非线性系统扩展Kalman 滤波问题% 状态函数：X(k+1)=0.5X(k)+2.5X(k)/(1+X(k)^2)+8cos(1.2k) +w(k) % 观测方程：Z （k ）=X(k)^2/20 +v(k)%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%function EKF_for_One_Div_UnLine_System % 初始化T=50; % 总时间 Q=10; R=1;% 产生过程噪声 w=sqrt(Q)*randn(1,T); % 产生观测噪声 v=sqrt(R)*randn(1,T);…… ……四、EKF 在纯方位寻的导弹制导中的应用例子：考虑一个在三维平面x-y-z 内运动的质点M ，其在某一时刻k 的位置、速度和加速度可用矢量可以表示为：[]Tz y x z y x z y x k a k a k a k v k v k v k r k r k r k x )()()()()()()()()()(= 质点M 可以在三维空间内做任何运动，同时假设三个x-y-z 方向上运动具有加性系统噪声()k w ，则在笛卡尔坐标系下该质点的运动状态方程为：))(),(()1(k w k x f k x k =+通常情况下，上述方程为线性的，即能表示为以下方式，)()()()1(k w k u k x k x +Γ+=+φ其中⎥⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎢⎣⎡--∆+∆=∆-∆-∆-333333323300)1(10)1(1I e I e I I t e tI I t ttλλλλλλφ，⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡∆-∆-=Γ33320)2/(tI I t t ∆为测量周期，也叫扫描周期，采样时间间隔等。

动态噪声)(k w 为[]Tz y x k k k k w )()()(000000)(ωωω=而且[]1910)(⨯==q k w E ，[]⎥⎦⎤⎢⎣⎡==⨯⨯32633661000)()(I Q k w k w E T σ )(k w 是高斯型白色随机向量序列。

现在考虑一个带有观测器的飞行中的导弹，可以假设为质点M ，对移动的目标进行观测，如下图所示，导弹与目标的相对位置依然可用x-y-z 表示，那么，导弹对目标纯方位角观测，主要是俯仰角和水平方向偏向角，实际测量中雷达具有加性测量噪声()k v ，则在笛卡尔坐标系下，观测方程为[])()()(k v k x h k z +=式中，[]Tz x z x y k r k r k r k r k r k x h ⎥⎥⎦⎤⎢⎢⎣⎡-+=)()(arctan)()()(arctan)(22 )(k v 为测量噪声，他也是高斯型白色随机向量序列，而且[]1210)(⨯==r k v E ，[]1)()(R k v k v E T =对于1R ，其定义为)()()(11k xD k D k R T --=其中，21.0I x =⎥⎥⎦⎤⎢⎢⎣⎡++++=)()()(00)()()()(222222k r k r k r k r k r k r k D z y x zy x显然在笛卡尔坐标系下，该模型运动观测方程为非线性的。