《Kalman滤波原理及程序(手册)》

《Kalman滤波原理及程序》

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

本手册的探讨内容主要有Kalman滤波，扩展Kalman滤波，无迹Kalman

滤波等，包括理论介绍和MATLAB源程序两部分。本手册所介绍的线性滤波器,

主要是Kalman滤波和a滤波，交互多模型Kalman滤波，这些算法的应用领

域主要有温度测量、自由落体，GPS导航、石油地震勘探、视频图像中的目标检

测和跟踪。

EKF和UKF主要在非线性领域有着重要的应用，目标跟踪是最主要的非线

性领域应用之一，除了讲解目标跟踪外,还介绍了通用非线性系统的EKF和UKF

滤波处理问题，信任读者可以通过学习本文通用的非线性系统，能快速驾驭EKF

和UKF滤波算法。

本文所涉及到的每一个应用实例，都包含原理介绍和程序代码（含具体的中

文注释）。

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

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

维，即状态X（Q=[x（Qx（k）y（k）双女）「包含水平方向的位置和速度和纵向

的位置和速度。则目标跟踪的系统方程可以用式（3.1）和（3.2）表示，

X(Z+l)=O)X(Z)+「u(Q(2-4-9)

Z*)=HX(k)+v(k)(2410)

——TLT

-1T0O-〃

0.5010X

O1OOT000X

其中，①二，r=，H=,X=

001T00.5T201

0000T00

X

z=，/U为零均值的过程噪声和观测噪声。T为采样周期。为了便于理解，

_y_

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

"1T0o-MD0.5T20

0100X(k-T)T°,",X1(*)

+

y(k)001Ty(k-i)00.5T2，

0001jd)0T

~x(k)

%(%)-1000]以外

7—+%(Q

」(幻一001ojy(k)

沁)

假定船舶在二维水平面上运动，初始位置为(-100m,200m),水平运动速度

为2m/s,垂直方向的运动速度为20m/s,GPS接收机的扫描周期为T=ls,观测

噪声的均值为0,方差为100c过程噪声越小，目标越接近匀速直线运动，反之,

则为曲线运动。仿真得到以下结果：

图3・2跟踪误差图

仿真程序

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

%Kalman滤波在目标跟踪中的应用实例

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

functionKalman

clc,clear

T=i；%若达扫描周期，

N=80/T;%总的采样次数

X=zeros(4,N);%目标真实位置、速度

X(:J)=[-100,2,200,20];%目标初始位置、速度

Z=zeros(2,N);%传感器对位置的观测

Z(:,1)=[X(U),X(3,1)];%观测初始化

dclta_w=le-2;%假如增大这个参数，目标真实轨迹就是曲线了

Q=delta_w*diag([0.5,1,0,5,1]);%过程噪声均值

R=I00*eye(2);%观测噪声均值

F=[1,T,0,0;0,1A0;0,0,1,T;0,0,0,1];%状态转移矩阵

H=[l,0,0,0;0,0,1,0];%观测矩阵

二、视频图像目标跟踪Kalman滤波算法实例

如下图所示，对于自由下落的皮球，要在视频中检测目标，这里主要检测目

标中心，即红心皮球的重心，在模型建立时可以将该重心抽象成为一个质点，坐

标为(冗,y)o

图2-6-1卜落的球图2-6-2检测卜落的球图2-6-3跟踪卜落的球

那么对该质点跟踪,它的状态为X(Z)=[xyiy]t状态方程如下

1dt

,01

X(k+\)=

00

00

观测方程为

Z(k)=1000,,

X(k)+v(k)

0100

在这个过程中，前提是目标检测，肯定要找到重心(x,y),与雷达目标跟踪中

观测目标位置是一回事。

图像目标检测跟踪程序

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

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

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

functiondetect

clear,clc;%清除全部内存变量、图形窗口

%计算背景图片数目

Imzero=zeros(240,320,3)；

fbri=1:5

%将图像文件i.jpg的图像像素数据读入矩阵Im

Im{i}=doubIe(imread([,DATA/,,int2str(i),'.jpg,]));

Imzero=Im{i}+Imzero;

end

Imback=Imzero/5;

[MR,MC,Dim]=size(Imback);

%遍历全部图片

fori=1:60

%读取全部帧

运行程序得到的x,y方向的位置跟踪偏差分析

三、通用非线性系统的EKF实现例子：

所谓的非线性方程，就是因变量与自变量的关系不是线性的，这类方程许多,

例如平方关系，对数关系，指数关系，三角函数关系等等。这些方程可分为两类,

一类是多项式方程，一种是非多项式方程。为了便于说明非线性卡尔曼滤波一一

扩展Kalman滤波的原理，我们选用以下系统，

系统状态为X(k),它仅包含一维变量，即X(左)=■%)],系统状态方程为

X(k)=0.5X(火-1)+:含+8cos(l.2幻+w(k)(3-2-1)

观测方程为

山、X2(k)小

V伏)=——+v(k)(3-2-2)

20

其中，式(3-1J)是包含分式，平方，三角函数在内的严峻非线性的方程，演幻为

过程噪声，其均值为0,方差为Q,观测方程中，观测信号F伏)与状态X(6的

关系也是非线性的，仪灯也是均值为0,方差为R的高斯白噪声。因此关于(3-1-1)

和(3・2-2)是一个状态和观测都为非线性的一维系统°以此为通用的非线性方程的

代表，接下来讲解并描述如何用扩展Kalman滤波来处理噪声问题。

第一步：初始化初始状态X(0),7(0),协防差矩阵不。

其次步：状态预料

X(k\k-\)=0.5X(A-1)++8cos(1.2k)(3-2-3)

第三步：观测预料

丫⑶IX玛a

(3-2-4)

第九步：协方差更新

P(k)=(/“一K(k)H(k))P(k[%-1)(3-2-10)

以上九步为扩展卡尔曼滤波的一个计算周期，如此循环下去就是各个时刻

EKF对非线性系统的处理过程。其他参数设置请查看源程序，仿真以上系统得

到状态滤波结果，如图3-2-1所示，滤波后的状态与真值之间的偏差如图图3-2-2

所示。

图3・2・1EKF滤波处理后的状态与真值对比图3・2・2偏差分析

EKF一维非线性系统仿真程序

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

%%%

%函数功能：一维非线性系统扩展Kalman淀波问题

%状态函数：X(k+1)=0.5X(k)+2.5X(ki/(1+X(k)A2)+8cos(1.2k)+w(k)

%观测方程：Z(k)=X(k)A2/20+v(k：

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

%%%

functionEKF_for_One_Div_UnLine_Sysiem

%初始化

T=50;%总时间

Q=10;

R=l;

%产生过程噪声

w=sqrt(Q)*randn(1,T);

%产生观测噪声

v=sqrt(R)*randn(1,T);

四、EKF在纯方位寻的导弹制导中的应用例子：

考虑一个在三维平面x-y-z内运动的质点M,其在某一时刻k的位置、速度

和加速度可用矢量可以表示为：

x(Z)=L(左)0(左)rz(k)匕(k)vy(k)匕(幻%(k)ay(k)见(后)『

质点M可以在三维空间内做任何运动，同时假设三个x-y-z方向上运动具有加性

系统噪声w伏),则在笛卡尔坐标系下该质点的运动状态方程为：

M无+1)=—(忒k),—

通常状况下，上述方程为线性的，即能表示为以下方式,

x(k+1)=勿(Z)+ru(k)4-w(左)

其中

+2Ar-l)/

△%32

A--(Ar/2)/3

。=。3A,r=-

A

03。3e-^I..。3

。为测量周期，也叫扫描周期，采样时间间隔等。动态噪声w幻为

vv{k}=[oOOOOOcox{k)%(k)牡⑹了

而且

T

可以外]=%=09xI/E\w(k)w(k)\=Q]=?°；：

一。3x6。，3_

忒6是高斯型白色随机向量序列。

现在考虑一个带有观测器的飞行中的导弹，可以假设为质点M,对移动的

目标进行观测，如下图所示，导弹与目标的相对位置依旧可用x-y-z表示，

那么，导弹对目标纯方位角观测，主要是俯仰角和水平方向偏向角，实际测

量中雷达具有加性测量噪声v(幻，则在笛卡尔坐标系下，观测方程为

z(k)=〃卜(%)]+u(Z)

式中,

T

ry(k)

心(幻]=arctanarctanZ2kW

；；

yjr(k)+r(k)rzW

为测量噪声，他也是高斯型白色随机向量序列，而且

T

E[v(A:)]=r,=02xI,E\y(k)v(/:)]=&

对于Ri，其定义为

R^k)=D~\k)xD-T(k)

其中，x=0.1/2

D®=J片/)+4"k)+d(幻°

明显在笛卡尔坐标系下，该模型运动观测方程为非线性的。仿真结果为:

轨迹跟踪图位置误差

e(ror0(\etodtyeffortdcwteralion

速度误差加速度误差

寻的制导matlab仿真程序

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

%%%

%程序说明：目标跟踪程序，实现运动弹头对运动物体的三维跟踪，主函数

%状态方程:x(t)=Ax(t-1)+Bu(l-1)+w(t)

%参考资料：《寻的导弹新型导引》第5.5和5.6节中仿真参数设置

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

%%%

functionmain

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

%%%

delta_t=0.01;%测量周期，采样周期

longa=l;%机动时间常数的倒数，即机动频率

T=3.7/delta_t;%时间长度3.7秒钟，一共采样T=370次

F=[eye(3),delta_t*eye(3),(exp(-1*longa*dslta_t)+longa*delta_t-1)/longaA2*eye(3);

zeros(3),eye(3),(1-exp(-1*longa*deha_t))/longa*eye(3);

zeros(3),zeros(3),exp(-l*longa*delta_t)*eye⑶];%状态转移矩阵fai

G=[-1*0.5*delta_tA2*eye(3);-1*delta_t*eye(3);zeros(3)l；%限制量驱动矩阵gama

五、UKF在六维CA目标跟踪模型中的应用例子：

一、仿真问题描述

考虑一个在二维平面x-y内运动的质点M,其在某一时刻k的位置、速度和

加速度可用矢量X（A）=N，”,/，%，%,凡F表示。假设M在水平方向（x）作近似

匀加速直线运动，垂直方向（y）上亦作近似匀加速直线运动。两方向上运动具

有加性系统噪声w（Q,则在笛卡尔坐标系下该质点的运动状态方程为

x(k+1)=((x(&))+w(A)=Rx(A)+w(A)

其中

-

c厂c

10r0—0

2

010r0—

2

E=

0010r0

00010r

000010

000001

假设一坐标位置为（0,0）的雷达对M进行测距4和测角外，实际测量中雷

达具有加性测量噪声取幻，则在传感器极坐标系下，观测方程为

收+短+匕伏）

rk+v(k)

z(R)=M(x(Q)+▼(%)=

[外+%(幻」

/J

明显在笛卡尔坐标系下，该模型运动观测方程为非线性的。我们依据雷达测

量值运用UKF算法对目标进行跟踪，并与EKF算法结果进行比较。

三、试验仿真与结果分析

100000

010000

000.12000

假设设系统噪声w(Q具有协方差阵Q，二,V伏)具有

0000.1200

00000.CU20

000000.012

520

协方差阵R«=,,二者不相关。观测次数N=50,采样时间为t=0.5。初始

00.012

r

状态X(O)=[1000,5000,10,50,2,-4]o则生成的运动轨迹如图i所示。

轨迹跟踪图

4.3.2仿真程序

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

%%%

%功能说明：UKF在目标跟踪中的应用

%参数说明：状态6维，x方向的位置、速度、加速度；y方向的位置、速度、加速度;

%观测信息为距离和角度；

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

%%%functionukflfbr_track_6_div_system

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

%%%

n=6;%状态位数

t=0.5;%采样时间

Q=ll00000;

010000;

000.01000;

0000.0100;

00000.00010;

000000.0001];%过程噪声协方差阵

R=[1000;

00.001人2];%量测噪声协方差阵

%状态方程

Kalman滤波原理及仿真手册

KF/EKFAJKFBW♦应用实例♦MATLAB仇向程序

目录

第一章Kalman滤波概述1

1.1kalman滤波的背景1

1.2kalman滤波的发展过程3

1.3kalman滤波应用领域5

其次章线性卡尔曼滤波6

2.1Kalman滤波原理6

2.1.1射影定理6

2.1.2Kalman滤波原理推导8

2.1.3Kalman滤波过程描述10

2.2Kalman滤波在温度测量中的应用12

2.2.1原理介绍12

2.2.1mallab仿真程序13

2.3Kalman滤波在自由落体中的应用14

2.3.1原理介绍14

2.3.2Matlab仿真程序19

2.3.3C语言程序20

2.4Kalman滤波在船舶GPS导航定位系统中的应用22

2.4.1原理介绍22

2.4.2mallab仿真程序25

2.5Kalman滤波在石油地震勘探中的应用26

2.5.1石油地震勘探白噪声反卷积滤波原理26

2.5.2石油地震勘探白噪声反卷积滤波仿真程序