Kalman濾波原理及程序(手冊(cè))解析

1、Kalman 濾波原理及仿真手冊(cè)KF/EKF/UKFKF/EKF/UKF 原理+ +應(yīng)用實(shí)例+MATLAB+MATLAB 程序本手冊(cè)的研究?jī)?nèi)容主要有Kalman濾波，擴(kuò)展Kalman濾波，無跡Kalman濾波等，包括理論介紹和MATLAB源程序兩部分。本手冊(cè)所介紹的線性濾波器，主要是Kalman濾波和a-B濾波，交互多模型Kalman濾波，這些算法的應(yīng)用領(lǐng) 域主要有溫度測(cè)量、自由落體，GPS導(dǎo)航、石油地震勘探、視頻圖像中的目標(biāo)檢 測(cè)和跟蹤。EKF和UKF主要在非線性領(lǐng)域有著重要的應(yīng)用，目標(biāo)跟蹤是最主要的非線 性領(lǐng)域應(yīng)用之一，除了講解目標(biāo)跟蹤外，還介紹了通用非線性系統(tǒng)的EKF和UKF濾波處理問題

2、，相信讀者可以通過學(xué)習(xí)本文通用的非線性系統(tǒng)， 能快速掌握EKF和UKF濾波算法。本文所涉及到的每一個(gè)應(yīng)用實(shí)例，都包含原理介紹和程序代碼(含詳細(xì)的中 文注釋)。一、四維目標(biāo)跟蹤 Kalman 線性濾波例子在不考慮機(jī)動(dòng)目標(biāo)自身的動(dòng)力因素，將勻速直線運(yùn)動(dòng)的船舶系統(tǒng)推廣到四維，即狀態(tài)X(k) - x(k) x(k) y(k) y(k)T包含水平方向的位置和速度和縱向X(k 1) =X(k) - u(k)Z(k) =HX (k) v(k)將狀態(tài)方程和觀測(cè)方程具體化:的位置和速度。則目標(biāo)跟蹤的系統(tǒng)方程可以用式3.1) 和(3.2)表示,(2-4-9)(2410)1T10000100【0T1S.5T20_1

3、0_TT000 x，H =，X00.5T201y0T _衛(wèi)0_Zu，u,v為零均值的過程噪聲和觀測(cè)噪聲。T為采樣周期ILyT為了便于理解,o0.5T假定船舶在二維水平面上運(yùn)動(dòng)，初始位置為(-100m,200m),水平運(yùn)動(dòng)速度為2m/s,垂直方向的運(yùn)動(dòng)速度為20 m/s,GPS接收機(jī)的掃描周期為T=1s，觀測(cè)噪聲的均值為0，方差為100。過程噪聲越小，目標(biāo)越接近勻速直線運(yùn)動(dòng)，反之,則為曲線運(yùn)動(dòng)。仿真得到以下結(jié)果:仿真程序% % Kalman 濾波在目標(biāo)跟蹤中的應(yīng)用實(shí)例% fun ctio nKalma n clc;clear;T=1;%雷達(dá)掃描周期，N=80/T; %總的采樣次數(shù)X=zeros(

4、4,N); % 目標(biāo)真實(shí)位置、速度X(:,1)=-100,2,200,20;%目標(biāo)初始位置、速度Z=zeros(2,N); % 傳感器對(duì)位置的觀測(cè)Z(:,1)=X(1,1),X(3,1);% 觀測(cè)初始化delta_w=1e-2;%如果增大這個(gè)參數(shù)，目標(biāo)真實(shí)軌跡就是曲線了Q=delta_w*diag(0.5,1,0.5,1) ; %過程噪聲均值R=100*eye (2); %觀測(cè)噪聲均值F=1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1;% 狀態(tài)轉(zhuǎn)移矩陣H=1,0,0,0;0,0,1,0;% 觀測(cè)矩陣x(k)0 10 0 x(k _1)+T0y(k)001 Ty(k-1)200.

5、5T丿(kL10 0 0 1一y(k-1)_0T _W2X1(k)x(k)y(k)0 0 x(k)0 x(k) +0y(k)$(kLv21(k)250-:II.1 1 J 13|!hi :!l1ii1600圖 3-1跟蹤軌跡圖2015105圖 3-2 跟蹤誤差圖_濾波前誤差 濾波后誤差0102030405060、視頻圖像目標(biāo)跟蹤 Kalman 濾波算法實(shí)例如下圖所示，對(duì)于自由下落的皮球，要在視頻中檢測(cè)目標(biāo)，這里主要檢測(cè)目標(biāo)中心，即紅心皮球的重心，在模型建立時(shí)可以將該重心抽象成為一個(gè)質(zhì)點(diǎn)， 坐那么對(duì)該質(zhì)點(diǎn)跟蹤，它的狀態(tài)為X（k）二X y x y 1,狀態(tài)方程如下觀測(cè)方程為在這個(gè)過程中，前提是目標(biāo)

6、檢測(cè)，一定要找到重心（x, y），與雷達(dá)目標(biāo)跟蹤中觀測(cè)目標(biāo)位置是一回事。圖像目標(biāo)檢測(cè)跟蹤程序%目標(biāo)檢測(cè)函數(shù)，這個(gè)函數(shù)主要完成將目標(biāo)從背景中提取出來% functio n detectclear,clc;%清除所有內(nèi)存變量、圖形窗口%計(jì)算背景圖片數(shù)目Imzero = zeros(240,320,3);for i = 1:5%將圖像文件 i.jpg 的圖像像素?cái)?shù)據(jù)讀入矩陣Imlmi = double(imread(DATA/, in t2str(i),.jpg);Imzero = Imi+Imzero;endjdt001dtX(k+1)=門001P00Z(k) =1|00 0 0X(k) v(k)

7、1 0 0標(biāo)為（x, y）。圖2-6-1下落的球圖2-6-2檢測(cè)下落的球圖2-6-3跟蹤下落的球0100X(k) +1Imback = lmzero/5; MR,MC,Dim = size(lmback);%遍歷所有圖片for i = 1 : 60%讀取所有幀運(yùn)行程序得到的x,y方向的位置跟蹤偏差分析三、通用非線性系統(tǒng)的 EKF 實(shí)現(xiàn)例子：所謂的非線性方程， 就是因變量與自變量的關(guān)系不是線性的， 這類方程很多, 例如平方關(guān)系，對(duì)數(shù)關(guān)系，指數(shù)關(guān)系，三角函數(shù)關(guān)系等等。這些方程可分為兩類, 一類是多項(xiàng)式方程，一種是非多項(xiàng)式方程。為了便于說明非線性卡爾曼濾波一一 擴(kuò)展Kalman濾波的原理，我們選用以

8、下系統(tǒng)，系統(tǒng)狀態(tài)為x(k)，它僅包含一維變量，即X(k)二l-x(k) 1,系統(tǒng)狀態(tài)方程為X(k)5x(k)8cos(2k) w(k)觀測(cè)方程為2Y(k)v(k)其中，式(3-1-1)是包含分式，平方，三角函數(shù)在內(nèi)的嚴(yán)重非線性的方程，w(k)為過程噪聲，其均值為0,方差為Q，觀測(cè)方程中，觀測(cè)信號(hào)Y(k)與狀態(tài)X(k)的 關(guān)系也是非線性的，v(k)也是均值為0,方差為R的高斯白噪聲。 因此關(guān)于(3-1-1)和(3-2-2)是一個(gè)狀態(tài)和觀測(cè)都為非線性的一維系統(tǒng)。以此為通用的非線性方程的代表，接下來講述如何用擴(kuò)展Kalman濾波來處理噪聲問題。第一步：初始化初始狀態(tài)X(0)，Y(0)，協(xié)防差矩陣Po

9、o第二步：狀態(tài)預(yù)測(cè)X(k|k -1) -0.5X(k -12.5X2(k -1)8cos(1.2k)(3-2-3)1+X2(k-1)第三步：觀測(cè)預(yù)測(cè)第九步：協(xié)方差更新P(k) =(ln-K(k)H(k)P(k|k-1)(3-2-10)以上九步為擴(kuò)展卡爾曼濾波的一個(gè)計(jì)算周期，如此循環(huán)下去就是各個(gè)時(shí)刻EKF對(duì)非線性系統(tǒng)的處理過程。其他參數(shù)設(shè)置請(qǐng)查看源程序，仿真以上系統(tǒng)得到狀態(tài)濾波結(jié)果，如圖3-2-1所示，濾波后的狀態(tài)與真值之間的偏差如圖圖3-2-2(3-2-1)(3-2-2)Y(k |k -1)2X2(k |k -1)20(3-2-4)所示。15圖3-2-1 EKF濾波處理后的狀態(tài)與真值對(duì)比圖3-

10、2-2偏差分析EKFEKF 維非線性系統(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)% 觀測(cè)方程：Z( k)=X(k)A2/20 +v(k)% %function EKF_for_One_Div_UnLine_System%初始化T=50;%總時(shí)間Q=10;R=1;%產(chǎn)生過程噪聲w=sqrt(Q)*randn(1,T);%產(chǎn)生觀測(cè)噪聲v=sqrt(R)*randn(1,T);-150_ K 氓 I 旦.EKF估計(jì)值1050-5-105101520253035404

11、550四、EKF 在純方位尋的導(dǎo)彈制導(dǎo)中的應(yīng)用例子：考慮一個(gè)在三維平面x-y-z內(nèi)運(yùn)動(dòng)的質(zhì)點(diǎn)M，其在某一時(shí)刻k的位置、速度 和加速度可用矢量可以表示為：x(k)= rx(k) ry(k) rz(k) Vx(k) Vy(k) Vz(k) ax(k) ay(k) az(k)T質(zhì)點(diǎn)M可以在三維空間內(nèi)做任何運(yùn)動(dòng)， 同時(shí)假設(shè)三個(gè)x-y-z方向上運(yùn)動(dòng)具有加性 系統(tǒng)噪聲w(k)，則在笛卡爾坐標(biāo)系下該質(zhì)點(diǎn)的運(yùn)動(dòng)狀態(tài)方程為：x(k 1) = fk(x(k),w(k)通常情況下，上述方程為線性的，即能表示為以下方式,x(k 1) = x(k)u(k) w(k)t為測(cè)量周期，也叫掃描周期，采樣時(shí)間間隔等。動(dòng)態(tài)噪聲w

12、(k)為w(k) = 00000 0 x(k)y(k) .z(k)T而且w(k)是咼斯型白色隨機(jī)向量序列?，F(xiàn)在考慮一個(gè)帶有觀測(cè)器的飛行中的導(dǎo)彈，可以假設(shè)為質(zhì)點(diǎn)M，對(duì)移動(dòng)的目標(biāo)進(jìn)行觀測(cè)，如下圖所示，導(dǎo)彈與目標(biāo)的相對(duì)位置依然可用x-y-z表示，那么，導(dǎo)彈對(duì)目標(biāo)純方位角觀測(cè)，主要是俯仰角和水平方向偏向角， 實(shí)際測(cè) 量中雷達(dá)具有加性測(cè)量噪聲v(k)，則在笛卡爾坐標(biāo)系下，觀測(cè)方程為z(k)二h x(k)丨v(k)式中,v(k)為測(cè)量噪聲，他也是高斯型白色隨機(jī)向量序列，而且其中(申+XAt 1) 丄(1 eM)*(At2/2)l3：F = Atl3-3一Ew( k)I =qi = Og 1，Ew(k)wT

13、(k)gp0:h-x( k)=-arctanry(k)r：(k) rz2(k)arctan4rz(k)120306 3L7E V(k) I- *=。21,E V(k)vT(k) l- R1R(k)二D(k)xD(k)其中，x = 0.112Jx2(k)+r：(k)+rz2(k)0():22 20pr：(k) + r：(k)+r；(k)% %程序說明：目標(biāo)跟蹤程序，實(shí)現(xiàn)運(yùn)動(dòng)彈頭對(duì)運(yùn)動(dòng)物體的三維跟蹤，主函數(shù)% 狀態(tài)方程：x(t)=Ax(t-1)+Bu(t-1)+w(t )%參考資料：尋的導(dǎo)彈新型導(dǎo)引第 5.5 和 5.6 節(jié)中仿真參數(shù)設(shè)置% %function main對(duì)于 R R，其定義為顯然在

14、笛卡爾坐標(biāo)系下該模型運(yùn)動(dòng)觀測(cè)方程為非線性的。仿真結(jié)果為:650速度誤差尋的制導(dǎo) matlabmatlab 仿真程序error of position600550450400350300250500%delta_t=0.01; %測(cè)量周期，采樣周期longa=1;%機(jī)動(dòng)時(shí)間常數(shù)的倒數(shù)，即機(jī)動(dòng)頻率T=3.7/delta_t;%時(shí)間長(zhǎng)度 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),(1-exp(-1*longa*del

15、ta_t)/longa*eye(3);zeros(3),zeros(3),exp(-1*longa*delta_t)*eye(3); % 狀態(tài)轉(zhuǎn)移矩陣 faiG=-1*0.5*delta_tA2*eye(3);-1*delta_t*eye(3);zeros(3); %控制量驅(qū)動(dòng)矩陣 gama五、UKF 在六維 CA 目標(biāo)跟蹤模型中的應(yīng)用例子：一、仿真問題描述考慮一個(gè)在二維平面x-y內(nèi)運(yùn)動(dòng)的質(zhì)點(diǎn)M,其在某一時(shí)刻k的位置、速度和 加速度可用矢量x(k)二Xk, yk,xk,yk,xk,y訂丁表示。假設(shè)M在水平方向(x)作近似 勻加速直線運(yùn)動(dòng)，垂直方向(y)上亦作近似勻加速直線運(yùn)動(dòng)。兩方向上運(yùn)動(dòng)具

16、有加性系統(tǒng)噪聲w(k)，則在笛卡爾坐標(biāo)系下該質(zhì)點(diǎn)的運(yùn)動(dòng)狀態(tài)方程為x(k 1) = fk(x(k) w(k)二Fkx(k) w(k)其中假設(shè)一坐標(biāo)位置為(0,0)的雷達(dá)對(duì)M進(jìn)行測(cè)距 rk和測(cè)角k，實(shí)際測(cè)量中雷 達(dá)具有加性測(cè)量10t0010tFk =001000010000-000020t0 t1 00 1噪聲v(k)，則在傳感器極坐標(biāo)系下，觀測(cè)方程為；k+Vr(k)_1(Xk2+ yk2+Vr(k)z(k)=h hk(x(k)+v v(k+wKk)_tan*出 +v(p(k)-Xk一顯然在笛卡爾坐標(biāo)系下， 該模型運(yùn)動(dòng)觀測(cè)方程為非線性的。 我們根據(jù)雷達(dá)測(cè) 量值使用UKF算法對(duì)目標(biāo)進(jìn)行跟蹤，并與EK

17、F算法結(jié)果進(jìn)行比較。、實(shí)驗(yàn)仿真與結(jié)果分析軌跡跟蹤圖假設(shè)設(shè)系統(tǒng)噪聲w(k)具有協(xié)方差陣000.10000000.120000000000，V(k)具有0.0120200.01協(xié)方差陣Rk= 52二者不相關(guān)。觀測(cè)次數(shù)0 0.01 _N=50，采樣時(shí)間為 t=0.5。初始狀態(tài)x(0)二1000,5000,10,50,2, -4丁。則生成的運(yùn)動(dòng)軌跡如圖 1 所示。53505000530052505200515051005050實(shí)際值測(cè)量值 ukf估計(jì)值10001100120013001400150016001700180019001098765432104.3.24.3.2 仿真程序% 功能說明：UK

18、F 在目標(biāo)跟蹤中的應(yīng)用% 參數(shù)說明：狀態(tài) 6 維，x 方向的位置、速度、加速度；y 方向的位置、速度、加速度;%觀測(cè)信息為距離和角度；%fu nctio n ukf_for_track_6_div_system%n=6;%狀態(tài)位數(shù)t=0.5;%采樣時(shí)間Q=1 0 0 0 0 0;0 1 0 0 0 0;0 0 0.01 0 0 0;0 0 0 0.01 0 0;0 0 0 0 0.0001 0;0 0 0 0 0 0.0001;%過程噪聲協(xié)方差陣R = 100 0;0 0.0022;%量測(cè)噪聲協(xié)方差陣%狀態(tài)方程跟蹤位置偏差05101520253035404550Kalman濾波原理及仿真手冊(cè)K

19、F/EKFfUKF 原理+應(yīng)用實(shí)例+ MATLAB 仿點(diǎn)利卡目錄第一章 Kalman 濾波概述11.1 kalman 濾波的背景 11.2 kalman 濾波的發(fā)展過程 31.3 kalman 濾波應(yīng)用領(lǐng)域5第二章線性卡爾曼濾波62.1 Kalman 濾波原理 62.1.1 射影定理62.1.2 Kalman 濾波原理推導(dǎo) 82.1.3 Kalman 濾波過程描述 102.2 Kalman 濾波在溫度測(cè)量中的應(yīng)用122.2.1 原理介紹122.2.1 matlab 仿真程序 132.3 Kalman 濾波在自由落體中的應(yīng)用142.3.1 原理介紹142.3.2 Matlab 仿真程序 192.

20、3.3 C 語言程序202.4 Kalman 濾波在船舶 GPS 導(dǎo)航定位系統(tǒng)中的應(yīng)用222.4.1 原理介紹222.4.2 matlab 仿真程序 252.5 Kalman 濾波在石油地震勘探中的應(yīng)用262.5.1 石油地震勘探白噪聲反卷積濾波原理262.5.2 石油地震勘探白噪聲反卷積濾波仿真程序282.6 Kalman 濾波在視頻圖像目標(biāo)跟蹤中的應(yīng)用31782.6.1 視頻圖像處理的基本方法及程序 312.6.2 Kalman 濾波對(duì)自由下落的皮球跟蹤應(yīng)用 332.6.3 目標(biāo)檢測(cè)程序 332.6.4 Kalman 濾波視頻跟蹤程序 35第三章 擴(kuò)展卡爾曼濾波 EKF 373.1 擴(kuò)展 Kalman 濾波原理 373.2 擴(kuò)展卡爾曼在一維非線性系統(tǒng)中的應(yīng)用 383.2.1 狀態(tài)方程和觀測(cè)方程都為非線性的通用系統(tǒng) 383.2.2 EKF 一維非線性系統(tǒng)仿真程序 403.3 擴(kuò)展卡爾曼在二維非線性系統(tǒng)中的應(yīng)用 413.3.1 狀態(tài)方程和觀測(cè)方程都為非線性的通用二維系統(tǒng)3.3.2 EKF 二維非線性系統(tǒng)仿真程序 433.4 擴(kuò)展卡爾曼在二維目標(biāo)跟蹤中的應(yīng)