Kalman滤波原理及程序(手册).doc

Kalman滤波原理及仿真手册 KF/EKF/UKF原理+应用实例+MATLAB程序 本手册的研究内容主要有Kalman滤波，扩展Kalman滤波，无迹Kalman滤波等，包括理论介绍和MATLAB源程序两部分。本手册所介绍的线性滤波器，主要是Kalman滤波和α-β滤波，交互多模型Kalman滤波，这些算法的应用领域主要有温度测量、自由落体，GPS导航、石油地震勘探、视频图像中的目标检测和跟踪。 EKF和UKF主要在非线性领域有着重要的应用，目标跟踪是最主要的非线性领域应用之一，除了讲解目标跟踪外，还介绍了通用非线性系统的EKF和UKF滤波处理问题，相信读者可以通过学习本文通用的非线性系统，能快速掌握EKF和UKF滤波算法。 本文所涉及到的每一个应用实例，都包含原理介绍和程序代码（含详细的中文注释）。 一、四维目标跟踪Kalman线性滤波例子 在不考虑机动目标自身的动力因素，将匀速直线运动的船舶系统推广到四维，即状态包含水平方向的位置和速度和纵向的位置和速度。则目标跟踪的系统方程可以用式（3.1）和（3.2）表示， （2-4-9） （2-4-10） 其中，，，，，，u,v为零均值的过程噪声和观测噪声。T为采样周期。为了便于理解，将状态方程和观测方程具体化： 假定船舶在二维水平面上运动，初始位置为（-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滤波算法实例 如下图所示，对于自由下落的皮球，要在视频中检测目标，这里主要检测目标中心，即红心皮球的重心，在模型建立时可以将该重心抽象成为一个质点，坐标为。 图2-6-1 下落的球 图2-6-2 检测下落的球 图2-6-3 跟踪下落的球 那么对该质点跟踪，它的状态为，状态方程如下 观测方程为 在这个过程中，前提是目标检测，一定要找到重心，与雷达目标跟踪中观测目标位置是一回事。 图像目标检测跟踪程序 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 目标检测函数，这个函数主要完成将目标从背景中提取出来 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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方向的位置跟踪偏差分析 Y方向的位置偏差 X方向的位置偏差 三、通用非线性系统的EKF实现例子： 所谓的非线性方程，就是因变量与自变量的关系不是线性的，这类方程