《Kalman濾波原理及程序(手冊)》

《Kalman濾波原理及程序》

KF/EKF/UKF原理+應(yīng)用實(shí)例十MATLAB程序

本手冊的探討內(nèi)容主要有Kalman濾波，擴(kuò)展Kalman濾波，無跡Kalman

濾波等，包括理論介紹和MATLAB源程序兩部分。本手冊所介紹的線性濾波器,

主要是Kalman濾波和a濾波，交互多模型Kalman濾波，這些算法的應(yīng)用領(lǐng)

域主要有溫度測量、自由落體，GPS導(dǎo)航、石油地震勘探、視頻圖像中的目標(biāo)檢

測和跟蹤。

EKF和UKF主要在非線性領(lǐng)域有著重要的應(yīng)用，目標(biāo)跟蹤是最主要的非線

性領(lǐng)域應(yīng)用之一，除了講解目標(biāo)跟蹤外,還介紹了通用非線性系統(tǒng)的EKF和UKF

濾波處理問題，信任讀者可以通過學(xué)習(xí)本文通用的非線性系統(tǒng)，能快速駕馭EKF

和UKF濾波算法。

本文所涉及到的每一個應(yīng)用實(shí)例，都包含原理介紹和程序代碼（含具體的中

文注釋）。

一、四維目標(biāo)跟蹤Kalman線性濾波例子

在不考慮機(jī)動目標(biāo)自身的動力因素，將勻速直線運(yùn)動的船舶系統(tǒng)推廣到四

維，即狀態(tài)X（Q=[x（Qx（k）y（k）雙女）「包含水平方向的位置和速度和縱向

的位置和速度。則目標(biāo)跟蹤的系統(tǒng)方程可以用式（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_

將狀態(tài)方程和觀測方程具體化:

"1T0o-MD0.5T20

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

+

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

0001jd)0T

~x(k)

%(%)-1000]以外

7—+%(Q

」(幻一001ojy(k)

沁)

假定船舶在二維水平面上運(yùn)動，初始位置為(-100m,200m),水平運(yùn)動速度

為2m/s,垂直方向的運(yùn)動速度為20m/s,GPS接收機(jī)的掃描周期為T=ls,觀測

噪聲的均值為0,方差為100c過程噪聲越小，目標(biāo)越接近勻速直線運(yùn)動，反之,

則為曲線運(yùn)動。仿真得到以下結(jié)果：

圖3?2跟蹤誤差圖

仿真程序

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

%Kalman濾波在目標(biāo)跟蹤中的應(yīng)用實(shí)例

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

functionKalman

clc,clear

T=i；%若達(dá)掃描周期，

N=80/T;%總的采樣次數(shù)

X=zeros(4,N);%目標(biāo)真實(shí)位置、速度

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

Z=zeros(2,N);%傳感器對位置的觀測

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

dclta_w=le-2;%假如增大這個參數(shù)，目標(biāo)真實(shí)軌跡就是曲線了

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];%狀態(tài)轉(zhuǎn)移矩陣

H=[l,0,0,0;0,0,1,0];%觀測矩陣

二、視頻圖像目標(biāo)跟蹤Kalman濾波算法實(shí)例

如下圖所示，對于自由下落的皮球，要在視頻中檢測目標(biāo)，這里主要檢測目

標(biāo)中心，即紅心皮球的重心，在模型建立時可以將該重心抽象成為一個質(zhì)點(diǎn)，坐

標(biāo)為(冗,y)o

圖2-6-1卜落的球圖2-6-2檢測卜落的球圖2-6-3跟蹤卜落的球

那么對該質(zhì)點(diǎn)跟蹤,它的狀態(tài)為X(Z)=[xyiy]t狀態(tài)方程如下

1dt

,01

X(k+\)=

00

00

觀測方程為

Z(k)=1000,,

X(k)+v(k)

0100

在這個過程中，前提是目標(biāo)檢測，肯定要找到重心(x,y),與雷達(dá)目標(biāo)跟蹤中

觀測目標(biāo)位置是一回事。

圖像目標(biāo)檢測跟蹤程序

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

%目標(biāo)檢測函數(shù)，這個函數(shù)主要完成將目標(biāo)從背景中提取出來

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

functiondetect

clear,clc;%清除全部內(nèi)存變量、圖形窗口

%計(jì)算背景圖片數(shù)目

Imzero=zeros(240,320,3)；

fbri=1:5

%將圖像文件i.jpg的圖像像素?cái)?shù)據(jù)讀入矩陣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

%讀取全部幀

運(yùn)行程序得到的x,y方向的位置跟蹤偏差分析

三、通用非線性系統(tǒng)的EKF實(shí)現(xiàn)例子：

所謂的非線性方程，就是因變量與自變量的關(guān)系不是線性的，這類方程許多,

例如平方關(guān)系，對數(shù)關(guān)系，指數(shù)關(guān)系，三角函數(shù)關(guān)系等等。這些方程可分為兩類,

一類是多項(xiàng)式方程，一種是非多項(xiàng)式方程。為了便于說明非線性卡爾曼濾波一一

擴(kuò)展Kalman濾波的原理，我們選用以下系統(tǒng)，

系統(tǒng)狀態(tài)為X(k),它僅包含一維變量，即X(左)=■%)],系統(tǒng)狀態(tài)方程為

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

觀測方程為

山、X2(k)小

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

20

其中，式(3-1J)是包含分式，平方，三角函數(shù)在內(nèi)的嚴(yán)峻非線性的方程，演幻為

過程噪聲，其均值為0,方差為Q,觀測方程中，觀測信號F伏)與狀態(tài)X(6的

關(guān)系也是非線性的，儀燈也是均值為0,方差為R的高斯白噪聲。因此關(guān)于(3-1-1)

和(3?2-2)是一個狀態(tài)和觀測都為非線性的一維系統(tǒng)°以此為通用的非線性方程的

代表，接下來講解并描述如何用擴(kuò)展Kalman濾波來處理噪聲問題。

第一步：初始化初始狀態(tài)X(0),7(0),協(xié)防差矩陣不。

其次步：狀態(tài)預(yù)料

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

第三步：觀測預(yù)料

丫⑶IX瑪a

(3-2-4)

第九步：協(xié)方差更新

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

以上九步為擴(kuò)展卡爾曼濾波的一個計(jì)算周期，如此循環(huán)下去就是各個時刻

EKF對非線性系統(tǒng)的處理過程。其他參數(shù)設(shè)置請查看源程序，仿真以上系統(tǒng)得

到狀態(tài)濾波結(jié)果，如圖3-2-1所示，濾波后的狀態(tài)與真值之間的偏差如圖圖3-2-2

所示。

圖3?2?1EKF濾波處理后的狀態(tài)與真值對比圖3?2?2偏差分析

EKF一維非線性系統(tǒng)仿真程序

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

%%%

%函數(shù)功能：一維非線性系統(tǒng)擴(kuò)展Kalman淀波問題

%狀態(tài)函數(shù)：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;

%產(chǎn)生過程噪聲

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

%產(chǎn)生觀測噪聲

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

四、EKF在純方位尋的導(dǎo)彈制導(dǎo)中的應(yīng)用例子：

考慮一個在三維平面x-y-z內(nèi)運(yùn)動的質(zhì)點(diǎn)M,其在某一時刻k的位置、速度

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

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

質(zhì)點(diǎn)M可以在三維空間內(nèi)做任何運(yùn)動，同時假設(shè)三個x-y-z方向上運(yùn)動具有加性

系統(tǒng)噪聲w伏),則在笛卡爾坐標(biāo)系下該質(zhì)點(diǎn)的運(yùn)動狀態(tài)方程為：

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

。為測量周期，也叫掃描周期，采樣時間間隔等。動態(tài)噪聲w幻為

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

而且

T

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

一。3x6。，3_

忒6是高斯型白色隨機(jī)向量序列。

現(xiàn)在考慮一個帶有觀測器的飛行中的導(dǎo)彈，可以假設(shè)為質(zhì)點(diǎn)M,對移動的

目標(biāo)進(jìn)行觀測，如下圖所示，導(dǎo)彈與目標(biāo)的相對位置依舊可用x-y-z表示，

那么，導(dǎo)彈對目標(biāo)純方位角觀測，主要是俯仰角和水平方向偏向角，實(shí)際測

量中雷達(dá)具有加性測量噪聲v(幻，則在笛卡爾坐標(biāo)系下，觀測方程為

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

式中,

T

ry(k)

心(幻]=arctanarctanZ2kW

；；

yjr(k)+r(k)rzW

為測量噪聲，他也是高斯型白色隨機(jī)向量序列，而且

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(幻°

明顯在笛卡爾坐標(biāo)系下，該模型運(yùn)動觀測方程為非線性的。仿真結(jié)果為:

軌跡跟蹤圖位置誤差

e(ror0(\etodtyeffortdcwteralion

速度誤差加速度誤差

尋的制導(dǎo)matlab仿真程序

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

%%%

%程序說明：目標(biāo)跟蹤程序，實(shí)現(xiàn)運(yùn)動彈頭對運(yùn)動物體的三維跟蹤，主函數(shù)

%狀態(tài)方程:x(t)=Ax(t-1)+Bu(l-1)+w(t)

%參考資料：《尋的導(dǎo)彈新型導(dǎo)引》第5.5和5.6節(jié)中仿真參數(shù)設(shè)置

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

%%%

functionmain

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

%%%

delta_t=0.01;%測量周期，采樣周期

longa=l;%機(jī)動時間常數(shù)的倒數(shù)，即機(jī)動頻率

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⑶];%狀態(tài)轉(zhuǎn)移矩陣fai

G=[-1*0.5*delta_tA2*eye(3);-1*delta_t*eye(3);zeros(3)l；%限制量驅(qū)動矩陣gama

五、UKF在六維CA目標(biāo)跟蹤模型中的應(yīng)用例子：

一、仿真問題描述

考慮一個在二維平面x-y內(nèi)運(yùn)動的質(zhì)點(diǎn)M,其在某一時刻k的位置、速度和

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

勻加速直線運(yùn)動，垂直方向（y）上亦作近似勻加速直線運(yùn)動。兩方向上運(yùn)動具

有加性系統(tǒng)噪聲w（Q,則在笛卡爾坐標(biāo)系下該質(zhì)點(diǎn)的運(yùn)動狀態(tài)方程為

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

其中

-

c廠c

10r0—0

2

010r0—

2

E=

0010r0

00010r

000010

000001

假設(shè)一坐標(biāo)位置為（0,0）的雷達(dá)對M進(jìn)行測距4和測角外，實(shí)際測量中雷

達(dá)具有加性測量噪聲取幻，則在傳感器極坐標(biāo)系下，觀測方程為

收+短+匕伏）

rk+v(k)

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

[外+%(幻」

/J

明顯在笛卡爾坐標(biāo)系下，該模型運(yùn)動觀測方程為非線性的。我們依據(jù)雷達(dá)測

量值運(yùn)用UKF算法對目標(biāo)進(jìn)行跟蹤，并與EKF算法結(jié)果進(jìn)行比較。

三、試驗(yàn)仿真與結(jié)果分析

100000

010000

000.12000

假設(shè)設(shè)系統(tǒng)噪聲w(Q具有協(xié)方差陣Q，二,V伏)具有

0000.1200

00000.CU20

000000.012

520

協(xié)方差陣R?=,,二者不相關(guān)。觀測次數(shù)N=50,采樣時間為t=0.5。初始

00.012

r

狀態(tài)X(O)=[1000,5000,10,50,2,-4]o則生成的運(yùn)動軌跡如圖i所示。

軌跡跟蹤圖

4.3.2仿真程序

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

%%%

%功能說明：UKF在目標(biāo)跟蹤中的應(yīng)用

%參數(shù)說明：狀態(tài)6維，x方向的位置、速度、加速度；y方向的位置、速度、加速度;

%觀測信息為距離和角度；

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

%%%functionukflfbr_track_6_div_system

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

%%%

n=6;%狀態(tài)位數(shù)

t=0.5;%采樣時間

Q=ll00000;

010000;

000.01000;

0000.0100;

00000.00010;

000000.0001];%過程噪聲協(xié)方差陣

R=[1000;

00.001人2];%量測噪聲協(xié)方差陣

%狀態(tài)方程

Kalman濾波原理及仿真手冊

KF/EKFAJKFBW?應(yīng)用實(shí)例?MATLAB仇向程序

目錄

第一章Kalman濾波概述1

1.1kalman濾波的背景1

1.2kalman濾波的發(fā)展過程3

1.3kalman濾波應(yīng)用領(lǐng)域5

其次章線性卡爾曼濾波6

2.1Kalman濾波原理6

2.1.1射影定理6

2.1.2Kalman濾波原理推導(dǎo)8

2.1.3Kalman濾波過程描述10

2.2Kalman濾波在溫度測量中的應(yīng)用12

2.2.1原理介紹12

2.2.1mallab仿真程序13

2.3Kalman濾波在自由落體中的應(yīng)用14

2.3.1原理介紹14

2.3.2Matlab仿真程序19

2.3.3C語言程序20

2.4Kalman濾波在船舶GPS導(dǎo)航定位系統(tǒng)中的應(yīng)用22

2.4.1原理介紹22

2.4.2mallab仿真程序25

2.5Kalman濾波在石油地震勘探中的應(yīng)用26

2.5.1石油地震勘探白噪聲反卷積濾波原理26

2.5.2石油地震勘探白噪聲反卷積濾波仿真程序