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

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

2、，相信讀者可以通過學習本文通用的非線性系統(tǒng)， 能快速掌握EKF和UKF濾波算法。本文所涉及到的每一個應(yīng)用實例，都包含原理介紹和程序代碼(含詳細的中 文注釋)。一、四維目標跟蹤 Kalman 線性濾波例子在不考慮機動目標自身的動力因素，將勻速直線運動的船舶系統(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)方程和觀測方程具體化:的位置和速度。則目標跟蹤的系統(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為零均值的過程噪聲和觀測噪聲。T為采樣周期ILyT為了便于理解,o0.5T假定船舶在二維水平面上運動，初始位置為(-100m,200m),水平運動速度為2m/s,垂直方向的運動速度為20 m/s,GPS接收機的掃描周期為T=1s，觀測噪聲的均值為0，方差為100。過程噪聲越小，目標越接近勻速直線運動，反之,則為曲線運動。仿真得到以下結(jié)果:仿真程序% % Kalman 濾波在目標跟蹤中的應(yīng)用實例% fun ctio nKalma n clc;clear;T=1;%雷達掃描周期，N=80/T; %總的采樣次數(shù)X=zeros(

4、4,N); % 目標真實位置、速度X(:,1)=-100,2,200,20;%目標初始位置、速度Z=zeros(2,N); % 傳感器對位置的觀測Z(:,1)=X(1,1),X(3,1);% 觀測初始化delta_w=1e-2;%如果增大這個參數(shù)，目標真實軌跡就是曲線了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;% 狀態(tài)轉(zhuǎn)移矩陣H=1,0,0,0;0,0,1,0;% 觀測矩陣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、視頻圖像目標跟蹤 Kalman 濾波算法實例如下圖所示，對于自由下落的皮球，要在視頻中檢測目標，這里主要檢測目標中心，即紅心皮球的重心，在模型建立時可以將該重心抽象成為一個質(zhì)點， 坐那么對該質(zhì)點跟蹤，它的狀態(tài)為X（k）二X y x y 1,狀態(tài)方程如下觀測方程為在這個過程中，前提是目標

6、檢測，一定要找到重心（x, y），與雷達目標跟蹤中觀測目標位置是一回事。圖像目標檢測跟蹤程序%目標檢測函數(shù)，這個函數(shù)主要完成將目標從背景中提取出來% functio n detectclear,clc;%清除所有內(nèi)存變量、圖形窗口%計算背景圖片數(shù)目Imzero = zeros(240,320,3);for i = 1:5%將圖像文件 i.jpg 的圖像像素數(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標為（x, y）。圖2-6-1下落的球圖2-6-2檢測下落的球圖2-6-3跟蹤下落的球0100X(k) +1Imback = lmzero/5; MR,MC,Dim = size(lmback);%遍歷所有圖片for i = 1 : 60%讀取所有幀運行程序得到的x,y方向的位置跟蹤偏差分析三、通用非線性系統(tǒng)的 EKF 實現(xiàn)例子：所謂的非線性方程， 就是因變量與自變量的關(guān)系不是線性的， 這類方程很多, 例如平方關(guān)系，對數(shù)關(guān)系，指數(shù)關(guān)系，三角函數(shù)關(guān)系等等。這些方程可分為兩類, 一類是多項式方程，一種是非多項式方程。為了便于說明非線性卡爾曼濾波一一 擴展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)觀測方程為2Y(k)v(k)其中，式(3-1-1)是包含分式，平方，三角函數(shù)在內(nèi)的嚴重非線性的方程，w(k)為過程噪聲，其均值為0,方差為Q，觀測方程中，觀測信號Y(k)與狀態(tài)X(k)的 關(guān)系也是非線性的，v(k)也是均值為0,方差為R的高斯白噪聲。 因此關(guān)于(3-1-1)和(3-2-2)是一個狀態(tài)和觀測都為非線性的一維系統(tǒng)。以此為通用的非線性方程的代表，接下來講述如何用擴展Kalman濾波來處理噪聲問題。第一步：初始化初始狀態(tài)X(0)，Y(0)，協(xié)防差矩陣Po

9、o第二步：狀態(tài)預(yù)測X(k|k -1) -0.5X(k -12.5X2(k -1)8cos(1.2k)(3-2-3)1+X2(k-1)第三步：觀測預(yù)測第九步：協(xié)方差更新P(k) =(ln-K(k)H(k)P(k|k-1)(3-2-10)以上九步為擴展卡爾曼濾波的一個計算周期，如此循環(huán)下去就是各個時刻EKF對非線性系統(tǒng)的處理過程。其他參數(shù)設(shè)置請查看源程序，仿真以上系統(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)與真值對比圖3-

10、2-2偏差分析EKFEKF 維非線性系統(tǒng)仿真程序% %函數(shù)功能：一維非線性系統(tǒng)擴展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)% %function EKF_for_One_Div_UnLine_System%初始化T=50;%總時間Q=10;R=1;%產(chǎn)生過程噪聲w=sqrt(Q)*randn(1,T);%產(chǎn)生觀測噪聲v=sqrt(R)*randn(1,T);-150_ K 氓 I 旦.EKF估計值1050-5-105101520253035404

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

12、(k)為w(k) = 00000 0 x(k)y(k) .z(k)T而且w(k)是咼斯型白色隨機向量序列。現(xiàn)在考慮一個帶有觀測器的飛行中的導彈，可以假設(shè)為質(zhì)點M，對移動的目標進行觀測，如下圖所示，導彈與目標的相對位置依然可用x-y-z表示，那么，導彈對目標純方位角觀測，主要是俯仰角和水平方向偏向角， 實際測 量中雷達具有加性測量噪聲v(k)，則在笛卡爾坐標系下，觀測方程為z(k)二h x(k)丨v(k)式中,v(k)為測量噪聲，他也是高斯型白色隨機向量序列，而且其中(申+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)% %程序說明：目標跟蹤程序，實現(xiàn)運動彈頭對運動物體的三維跟蹤，主函數(shù)% 狀態(tài)方程：x(t)=Ax(t-1)+Bu(t-1)+w(t )%參考資料：尋的導彈新型導引第 5.5 和 5.6 節(jié)中仿真參數(shù)設(shè)置% %function main對于 R R，其定義為顯然在

14、笛卡爾坐標系下該模型運動觀測方程為非線性的。仿真結(jié)果為:650速度誤差尋的制導 matlabmatlab 仿真程序error of position600550450400350300250500%delta_t=0.01; %測量周期，采樣周期longa=1;%機動時間常數(shù)的倒數(shù)，即機動頻率T=3.7/delta_t;%時間長度 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ū)動矩陣 gama五、UKF 在六維 CA 目標跟蹤模型中的應(yīng)用例子：一、仿真問題描述考慮一個在二維平面x-y內(nèi)運動的質(zhì)點M,其在某一時刻k的位置、速度和 加速度可用矢量x(k)二Xk, yk,xk,yk,xk,y訂丁表示。假設(shè)M在水平方向(x)作近似 勻加速直線運動，垂直方向(y)上亦作近似勻加速直線運動。兩方向上運動具

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

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

18、F 在目標跟蹤中的應(yīng)用% 參數(shù)說明：狀態(tài) 6 維，x 方向的位置、速度、加速度；y 方向的位置、速度、加速度;%觀測信息為距離和角度；%fu nctio n ukf_for_track_6_div_system%n=6;%狀態(tài)位數(shù)t=0.5;%采樣時間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;%量測噪聲協(xié)方差陣%狀態(tài)方程跟蹤位置偏差05101520253035404550Kalman濾波原理及仿真手冊K

19、F/EKFfUKF 原理+應(yīng)用實例+ MATLAB 仿點利卡目錄第一章 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 濾波原理推導 82.1.3 Kalman 濾波過程描述 102.2 Kalman 濾波在溫度測量中的應(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 導航定位系統(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 濾波在視頻圖像目標跟蹤中的應(yīng)用31782.6.1 視頻圖像處理的基本方法及程序 312.6.2 Kalman 濾波對自由下落的皮球跟蹤應(yīng)用 332.6.3 目標檢測程序 332.6.4 Kalman 濾波視頻跟蹤程序 35第三章 擴展卡爾曼濾波 EKF 373.1 擴展 Kalman 濾波原理 373.2 擴展卡爾曼在一維非線性系統(tǒng)中的應(yīng)用 383.2.1 狀態(tài)方程和觀測方程都為非線性的通用系統(tǒng) 383.2.2 EKF 一維非線性系統(tǒng)仿真程序 403.3 擴展卡爾曼在二維非線性系統(tǒng)中的應(yīng)用 413.3.1 狀態(tài)方程和觀測方程都為非線性的通用二維系統(tǒng)3.3.2 EKF 二維非線性系統(tǒng)仿真程序 433.4 擴展卡爾曼在二維目標跟蹤中的應(yīng)