Kalman濾波原理及仿真手冊

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

本手冊H勺研究內(nèi)容重要有Kalman濾波，擴(kuò)展Kalman濾波，無跡Kalman濾

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

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

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

和跟蹤。

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

域應(yīng)用之一，除了講解目的跟蹤外，還簡介了通用非線性系統(tǒng)的EKF和UKF濾

波處理問題，相信讀者可以通過學(xué)習(xí)本文通用日勺非線性系統(tǒng)，能迅速掌握EKF

和UKF濾波算法。

本文所波及到F］每一種應(yīng)用實例，都包括原理簡介和程序代碼(含詳細(xì)口勺中文注

釋)。

一、四維目的跟蹤Kalman線性濾波例子

在不考慮機(jī)動目的自身的動力原因，將勻速直線運動E的船舶系統(tǒng)推廣到四維,

即狀態(tài)包括水平方向的位置和速度和縱向的位置和速度。則目的跟蹤的系統(tǒng)

方程可以用式(3.1)和(3.2)表達(dá)，

X伏+1)=①X(Z)+ru(k)(2-4-9)

Z(Q=HX(k)+v(k)(2-4-10)

其中，，，，，，u,v為零均值的過程噪聲和觀測噪聲。T為采樣周期。為了

便于理解，將狀態(tài)方程和觀測方程詳細(xì)化：

楸)一'lTo-0.5T2

x(k)010()T

_1.

十

001Tyd)0

yW0001J(D_0

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

2m/s,垂直方向的運動速度為2()m/s,GPS接受機(jī)日勺掃描周期為T=ls,觀測噪聲

的I均值為0,方差為100o過程噪聲越小，目日勺越靠近勻速直線運動，反之，則為

曲線運動。仿真得到如下成果：

圖3?1跟蹤軌跡圖圖3.2跟蹤誤差圖

仿真程序

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

%Kalman濾波在忖的跟蹤中U勺應(yīng)用實例

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

functionKalman

clc;clcar;

T=l;%雷達(dá)掃描周期，

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

X=zcros(4,N);%目H勺真實位置、速度

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

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

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

dclta_w=lc-2;%假如增大這個參數(shù)，目的真實軌跡就是曲線了

Q=delta_w*diag([0.5,l,0.5,1]);%過程噪聲均值

R=100*eye(2);%觀測噪聲均值

F=[1,T,0,0;0,1,0,0:0,0,1,T;0,0,0,1];%狀態(tài)轉(zhuǎn)移矩陣

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

二、視頻圖像目的跟蹤Kalman濾波算法實例

如下圖所示，對于自由下落的皮球，要在視頻中檢測目的J,這里重要檢測目日勺中

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

圖2-6-1下落的球圖2-6-2檢測下落日勺球圖2-6-3跟蹤下落的球

那么對該質(zhì)點跟蹤,它口勺狀態(tài)為，狀態(tài)方程如下

1(dt001ro-

01dt00

X(k+1)=X(Q+w(k)

00100

0001g

觀測方程為

1000

z(k)=X(k)+v(Q

0100

在這個過程中，前提是目的檢測，一定要找到重心，與雷達(dá)目的跟蹤中觀測目

的位置是一回事。

圖像目的檢測跟蹤程序

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

%目的I檢測函數(shù)，這個函數(shù)重要完畢將目日勺從背景中提取出來

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

functiondetect

clear,cic;%清除所有內(nèi)存變量、圖形窗口

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

Imzero=zeros(240,320,3)；

tori=1:5

%將圖像文獻(xiàn)i.jpg的圖像像素數(shù)據(jù)讀入矩陣Im

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

Imzero=Im{i}+Imzero;

end

Imback=Imzcro/5;

|MR,MC,Dini]=size(lmback);

%遍歷所有圖片

fori=1:60

%讀取所有幀

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

Y方向的位置偏差

X方向的位置偏差

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

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

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

一類是多項式方程，一種是非多項式方程。為了便于闡明非線性卡爾曼濾波一一

擴(kuò)展Kalman濾波H勺原理，我們選用如下系統(tǒng),

系統(tǒng)狀態(tài)為，它僅包括一維變量，即，系統(tǒng)狀態(tài)方程為

X(k)=0.5X(k-1)++8cos(1.2A:)+田行(3-2-1)

觀測方程為

丫(幻=^^+儀幻(3-2-2)

其中，式(3-1-1)是包括分式，平方，三角函數(shù)在內(nèi)歐I嚴(yán)重非線性日勺方程，為

過程噪聲，其均值為0,方差為Q,觀測方程中，觀測信號與狀態(tài)的關(guān)系也是

非線性日勺，也是均值為0,方差為R的高斯白噪聲。因此有關(guān)(3-1-1)和(3-2-2)是

一種狀態(tài)和觀測都為非線性日勺一維系統(tǒng)。以此為通用的非線性方程的代表，接下

來講述怎樣用擴(kuò)展Kalman濾波來處理噪聲問題。

第一步：初始化初始狀態(tài)，，協(xié)防差矩陣。

第二步：狀態(tài)預(yù)測

X(k\k-\)=0.5X(攵-1)+［：含：―+8cos(1.2k)(3-2-3)

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

y(jt|(t-l)=(3-2-4)

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

P(k)=3-K(k)H(k))P(k\k-l)(3-2-10)

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

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

濾波成果，如圖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(k)/(1+X(k)A2)+8cos(1.2k)+w(k)

%觀測方程:Z(k)=X(k)A2.20+v(k)

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

%%%

functionEKF_for_Onc_Div_UnLine_Systcni

%初始化

T=50;%總時間

Q=10;

R=l;

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

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

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

v=sqrt(R)*randn(I.T);

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

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

和加速度可用矢量可以表達(dá)為：

■?=卜=)ry(k)rz(k)vx(k)vy(k)vz(k)ax(k)ay(k)

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

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

工(4+1)=人*(外,似公)

一般狀況下，上述方程為線性日勺，即能表達(dá)為如下方式，

x(k+1)=(fa(k)+「“伏)+⑷伏)

其中

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

M&)=[o00000①、(k)0V伏)牡(幻F

并且

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

目前考慮一種帶有觀測器的飛行中的導(dǎo)彈，可以假設(shè)為質(zhì)點M,對移動的目

日勺進(jìn)行觀測，如下圖所示，導(dǎo)彈與目日勺的相對位置仍然可用x-y-z表達(dá)，

那么，導(dǎo)彈對目的純方位角觀測，重要是俯仰角和水平方向偏向角，實際測量中

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

z(k)=〃[x(k)]+u(￡i

式中，

,'?)arctan^

&(%)]=arctan

GXk)+rXk)h(k)

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

對于，其定義為

Ri(k)=D"(k)xDT(k)

其中，_________________

八小r)/；』)+/伏)+廠：(幻o

D(k)=*、------------------

0伏)+〃2(幻+片伙)

顯然在笛卡爾坐標(biāo)系下，該模型運動觀測方程為非線性的。仿真成果為:

軌跡跟蹤圖位置誤差

速度誤差加速度誤差

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

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

%%%

%程序闡明：目時跟蹤程序，實現(xiàn)運動彈頭對運動物體的三維跟蹤，主函數(shù)

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

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

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

%%%

functionmain

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

%%%

delta_t=0.0l;%測量周期,采樣周期

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

T=3.7/delta_(;%時間長度3.7秒鐘，一共采樣T=370次

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

zeros(3).eye(3),(I-exp(-l*bnga*delta_t))/longa*eye(3);

zeros(3),zeros(3),exp(-1*longa*delta_t)*eye(3)];%狀態(tài)轉(zhuǎn)移矩陣fai

G=(-1*0.5*delta_tA2*eye(3);-1*delta_t*eye(3);zeros(3)];%控制量卵動矩陣gama

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

一、仿真問題描述

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

加速度可用矢量表達(dá)。假設(shè)M在水平方向（x）作近似勻加速直線運動，垂直

方向（y）上亦作近似勻加速直線運動。兩方向上運動具有加性系統(tǒng)噪聲.則在

笛々爾坐標(biāo)系下該質(zhì)點的運動狀態(tài)方程為

x（k+1）=-+w（Z）=垃x（Z）+w（女）

其中

尸八

1010-0

2

r

01070—

2

—0010r0

00010r

000010

000001

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

加性測量噪聲，則在傳感器極坐標(biāo)系下；觀測方程為

Jxj+yj+1、伏)

7+以外

z(Z)=h*(x(k))+v(6=

%.+%（%）tan"總+%伏)

%

顯然在笛卡爾坐標(biāo)系下，該模型運動觀測方程為非線性日勺。我們根據(jù)雷達(dá)測量值

使用UKF算法對目的進(jìn)行跟蹤，并與EKF算法成果進(jìn)行比較。

三、試驗仿真與成果分析

假設(shè)設(shè)系統(tǒng)噪聲團(tuán)具有協(xié)方差陣回，團(tuán)具有協(xié)方差陣團(tuán)兩者不有關(guān)。觀測次數(shù)N=50,采樣

時間為t=0.5。初始狀態(tài)團(tuán)。則生成的運動軌跡如圖1所示。

5350

5300

5250

5200

5150

5100

5050

5000

1000110012001300140015001600170018001900

凱跡跟蹤圖

4.3.2仿真程序

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

%%%

%功能闡明：UKF在目的I跟蹤中的J應(yīng)用

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

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

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

%%%functionukLfor_track_6_div_system

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

%%%

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

t=0.5;%采樣時間

Q=[l00000;

010000;

000.01000;

0000.0100;

00000.00010;

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

R=[10()0;

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

%狀態(tài)方程

Kalman濾波原理及仿真手冊

KF/EKF/UKF原理+應(yīng)用實例?MATLAB仿真程序

目錄

第一章Kalman漉波概述I

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.1matkib仿真程序13

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

2.3.1原理簡介14

2.3.2Mallab仿真程序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石油地震勘探白噪聲反卷積濾波仿真程序28

2.6Kalman濾波在視頻圖像目的跟蹤中的應(yīng)用31

2.6.1視頻圖像處理口勺基本措施及程序31

2.6.2Kalman濾波對自由二落的J皮球跟蹤應(yīng)用33

2.6.3目的檢測程序33

2.6.4Kalman濾波視頻跟蹤程序35

第三章擴(kuò)展卡爾曼濾波EKF37

3.1擴(kuò)展Kalman濾波原理37

3.2擴(kuò)展卡爾曼在一維非