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

Kalman 滤波原理及仿真手册

KF/EKF/UKF 原理+应用实例+MATLAB 程序

本手册的研究内容主要有Kalman 滤波，扩展Kalman 滤波，无迹Kalman 滤波等，包括理论介绍和MATLAB 源程序两部分。本手册所介绍的线性滤波器，主要是Kalman 滤波和α-β滤波，交互多模型Kalman 滤波，这些算法的应用领域主要有温度测量、自由落体，GPS 导航、石油地震勘探、视频图像中的目标检测和跟踪。

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

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

一、四维目标跟踪Kalman 线性滤波例子

在不考虑机动目标自身的动力因素，将匀速直线运动的船舶系统推广到四

维，即状态[]T k y

k y k x

k 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）

其中，⎥

⎥

⎥⎥⎦⎤⎢⎢⎢

⎢⎣⎡=Φ1000

1000010

001

T T

，⎥⎥⎥⎥

⎥⎦⎤⎢⎢⎢⎢⎢

⎣⎡=ΓT T T

T 05.00005.022

，T H ⎥⎥

⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=00100001

，T

y

y x x X ⎥

⎥⎥⎥⎦⎤

⎢⎢⎢⎢⎣⎡= ，⎥⎦⎤

⎢⎣⎡=y x Z ，u ,v 为零均值的过程噪声和观测噪声。T 为采样周期。为了便于理解，

将状态方程和观测方程具体化：

)(05.00

005.0)1()1()1()1(1000

1000010001)()()()(1

222

k w T T

T

T k y k y k x k x T T

k 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 跟踪下落的球 那么对该质点跟踪，它的状态为[]y

x

y x

k X =)(，状态方程如下 )(000)(10000100010001)1(k w g k X dt dt k X ⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎣⎡+⎥⎥⎥⎥⎦⎤⎢⎢⎢

⎢⎣⎡=+ 观测方程为

)()(00100001)(k v k X k Z +⎥⎦

⎤⎢⎣⎡= 在这个过程中，前提是目标检测，一定要找到重心),(y x ，与雷达目标跟踪中观测目标位置是一回事。 图像目标检测跟踪程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 目标检测函数，这个函数主要完成将目标从背景中提取出来

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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