Kalman濾波原理及程序手冊

1、Kalman濾波原理及程序（手冊）KF/EKF/UKF原理+應(yīng)用實(shí)例+MATLAB程序本手冊的研究內(nèi)容主要有Kalman濾波，擴(kuò)展Kalman濾波，無跡Kalman濾波等，包括理論介紹和MATLAB源程序兩部分。本手冊所介紹的線性濾波器，主要是Kalman濾波和-濾波，交互多模型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)，能快速

2、掌握EKF和UKF濾波算法。本文所涉及到的每一個(gè)應(yīng)用實(shí)例，都包含原理介紹和程序代碼（含詳細(xì)的中文注釋）。一、四維目標(biāo)跟蹤Kalman線性濾波例子在不考慮機(jī)動目標(biāo)自身的動力因素，將勻速直線運(yùn)動的船舶系統(tǒng)推廣到四維，即狀態(tài)包含水平方向的位置和速度和縱向的位置和速度。則目標(biāo)跟蹤的系統(tǒng)方程可以用式（3.1）和（3.2）表示， （2-4-9） （2-4-10）其中，u,v為零均值的過程噪聲和觀測噪聲。T為采樣周期。為了便于理解，將狀態(tài)方程和觀測方程具體化：假定船舶在二維水平面上運(yùn)動，初始位置為（-100m,200m），水平運(yùn)動速度為2m/s，垂直方向的運(yùn)動速度為20 m/s，GPS接收機(jī)的掃描周期為T=

3、1s，觀測噪聲的均值為0，方差為100。過程噪聲越小，目標(biāo)越接近勻速直線運(yùn)動，反之，則為曲線運(yùn)動。仿真得到以下結(jié)果：圖3-1 跟蹤軌跡圖 圖3-2 跟蹤誤差圖仿真程序% Kalman濾波在目標(biāo)跟蹤中的應(yīng)用實(shí)例%function Kalmanclc;clear;T=1;%雷達(dá)掃描周期,N=80/T; %總的采樣次數(shù)X=zeros(4,N); % 目標(biāo)真實(shí)位置、速度X(:,1)=-100,2,200,20;% 目標(biāo)初始位置、速度Z=zeros(2,N); % 傳感器對位置的觀測Z(:,1)=X(1,1),X(3,1); % 觀測初始化delta_w=1e-2; %如果增大這個(gè)參數(shù)，目標(biāo)真實(shí)軌跡就是

4、曲線了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; % 觀測矩陣二、視頻圖像目標(biāo)跟蹤Kalman濾波算法實(shí)例如下圖所示，對于自由下落的皮球，要在視頻中檢測目標(biāo)，這里主要檢測目標(biāo)中心，即紅心皮球的重心，在模型建立時(shí)可以將該重心抽象成為一個(gè)質(zhì)點(diǎn)，坐標(biāo)為。 圖2-6-1 下落的球 圖2-6-2 檢測下落的球 圖2-6-3 跟蹤下落的球那么對該質(zhì)點(diǎn)跟蹤，它的狀態(tài)為，狀態(tài)方程如下觀測方程為在這個(gè)過程中，前提是目

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

6、 = size(Imback);% 遍歷所有圖片for i = 1 : 60% 讀取所有幀運(yùn)行程序得到的x,y方向的位置跟蹤偏差分析Y方向的位置偏差X方向的位置偏差三、通用非線性系統(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)為，它僅包含一維變量，即，系統(tǒng)狀態(tài)方程為 (3-2-1)觀測方程為 (3-2-2)其中，式(3-1-1)是包含分式，平方，三角函數(shù)在內(nèi)的嚴(yán)重

7、非線性的方程，為過程噪聲，其均值為0，方差為Q，觀測方程中，觀測信號與狀態(tài)的關(guān)系也是非線性的，也是均值為0，方差為R的高斯白噪聲。因此關(guān)于(3-1-1)和(3-2-2)是一個(gè)狀態(tài)和觀測都為非線性的一維系統(tǒng)。以此為通用的非線性方程的代表，接下來講述如何用擴(kuò)展Kalman濾波來處理噪聲問題。第一步：初始化初始狀態(tài)，協(xié)防差矩陣。第二步：狀態(tài)預(yù)測 (3-2-3)第三步：觀測預(yù)測 (3-2-4)第九步：協(xié)方差更新 (3-2-10)以上九步為擴(kuò)展卡爾曼濾波的一個(gè)計(jì)算周期，如此循環(huán)下去就是各個(gè)時(shí)刻EKF對非線性系統(tǒng)的處理過程。其他參數(shù)設(shè)置請查看源程序，仿真以上系統(tǒng)得到狀態(tài)濾波結(jié)果，如圖3-2-1所示，濾波后

8、的狀態(tài)與真值之間的偏差如圖圖3-2-2所示。 圖3-2-1 EKF濾波處理后的狀態(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)2)+8cos(1.2k) +w(k)% 觀測方程：Z（k）=X(k)2/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)生觀測噪聲v=sqrt(R)*randn(1,T

9、);四、EKF在純方位尋的導(dǎo)彈制導(dǎo)中的應(yīng)用例子：考慮一個(gè)在三維平面x-y-z內(nèi)運(yùn)動的質(zhì)點(diǎn)M，其在某一時(shí)刻k的位置、速度和加速度可用矢量可以表示為：質(zhì)點(diǎn)M可以在三維空間內(nèi)做任何運(yùn)動，同時(shí)假設(shè)三個(gè)x-y-z方向上運(yùn)動具有加性系統(tǒng)噪聲，則在笛卡爾坐標(biāo)系下該質(zhì)點(diǎn)的運(yùn)動狀態(tài)方程為：通常情況下，上述方程為線性的，即能表示為以下方式，其中，為測量周期，也叫掃描周期，采樣時(shí)間間隔等。動態(tài)噪聲為而且，是高斯型白色隨機(jī)向量序列?，F(xiàn)在考慮一個(gè)帶有觀測器的飛行中的導(dǎo)彈，可以假設(shè)為質(zhì)點(diǎn)M，對移動的目標(biāo)進(jìn)行觀測，如下圖所示，導(dǎo)彈與目標(biāo)的相對位置依然可用x-y-z表示，那么，導(dǎo)彈對目標(biāo)純方位角觀測，主要是俯仰角和水平方向偏

10、向角，實(shí)際測量中雷達(dá)具有加性測量噪聲，則在笛卡爾坐標(biāo)系下，觀測方程為式中，為測量噪聲，他也是高斯型白色隨機(jī)向量序列，而且，對于，其定義為其中，顯然在笛卡爾坐標(biāo)系下，該模型運(yùn)動觀測方程為非線性的。仿真結(jié)果為： 軌跡跟蹤圖 位置誤差 速度誤差 加速度誤差尋的制導(dǎo)matlab仿真程序% 程序說明：目標(biāo)跟蹤程序，實(shí)現(xiàn)運(yùn)動彈頭對運(yùn)動物體的三維跟蹤，主函數(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%delta_t=0.01; % 測量周期,采樣周期longa=1; % 機(jī)動時(shí)間常數(shù)的倒數(shù)，即機(jī)

11、動頻率T=3.7/delta_t;% 時(shí)間長度3.7秒鐘，一共采樣T=370次F=eye(3),delta_t*eye(3),(exp(-1*longa*delta_t)+longa*delta_t-1)/longa2*eye(3); zeros(3),eye(3),(1-exp(-1*longa*delta_t)/longa*eye(3); zeros(3),zeros(3),exp(-1*longa*delta_t)*eye(3); % 狀態(tài)轉(zhuǎn)移矩陣faiG=-1*0.5*delta_t2*eye(3);-1*delta_t*eye(3);zeros(3); % 控制量驅(qū)動矩陣gama五、

12、UKF在六維CA目標(biāo)跟蹤模型中的應(yīng)用例子：一、仿真問題描述考慮一個(gè)在二維平面x-y內(nèi)運(yùn)動的質(zhì)點(diǎn)M，其在某一時(shí)刻k的位置、速度和加速度可用矢量表示。假設(shè)M在水平方向（x）作近似勻加速直線運(yùn)動，垂直方向（y）上亦作近似勻加速直線運(yùn)動。兩方向上運(yùn)動具有加性系統(tǒng)噪聲，則在笛卡爾坐標(biāo)系下該質(zhì)點(diǎn)的運(yùn)動狀態(tài)方程為其中假設(shè)一坐標(biāo)位置為（0,0）的雷達(dá)對M進(jìn)行測距和測角，實(shí)際測量中雷達(dá)具有加性測量噪聲，則在傳感器極坐標(biāo)系下，觀測方程為顯然在笛卡爾坐標(biāo)系下，該模型運(yùn)動觀測方程為非線性的。我們根據(jù)雷達(dá)測量值使用UKF算法對目標(biāo)進(jìn)行跟蹤，并與EKF算法結(jié)果進(jìn)行比較。三、實(shí)驗(yàn)仿真與結(jié)果分析假設(shè)設(shè)系統(tǒng)噪聲具有協(xié)方差陣，具

13、有協(xié)方差陣，二者不相關(guān)。觀測次數(shù)N=50，采樣時(shí)間為t=0.5。初始狀態(tài)。則生成的運(yùn)動軌跡如圖1所示。軌跡跟蹤圖4.3.2 仿真程序% 功能說明： UKF在目標(biāo)跟蹤中的應(yīng)用% 參數(shù)說明： 狀態(tài)6維，x方向的位置、速度、加速度；y方向的位置、速度、加速度；% 觀測信息為距離和角度；%function 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;%過程噪

14、聲協(xié)方差陣R = 100 0; 0 0.0012;%量測噪聲協(xié)方差陣% 狀態(tài)方程目 錄第一章 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濾波在溫度測量中的應(yīng)用122.2.1 原理介紹122.2.1 matlab仿真程序132.3 Kalman濾波在自由落體中的應(yīng)用142.3.1 原理介紹142.3.2 Matlab仿真程序192.3.3 C語

15、言程序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)用312.6.1 視頻圖像處理的基本方法及程序312.6.2 Kalman濾波對自由下落的皮球跟蹤應(yīng)用332.6.3 目標(biāo)檢測程序332.6.4 Kalman濾波視頻跟蹤程序35第三章 擴(kuò)展卡爾曼濾波EKF373.1 擴(kuò)展Kalman濾波原理373.2 擴(kuò)展卡爾曼在一維非

16、線性系統(tǒng)中的應(yīng)用383.2.1 狀態(tài)方程和觀測方程都為非線性的通用系統(tǒng)383.2.2 EKF一維非線性系統(tǒng)仿真程序403.3 擴(kuò)展卡爾曼在二維非線性系統(tǒng)中的應(yīng)用413.3.1 狀態(tài)方程和觀測方程都為非線性的通用二維系統(tǒng)413.3.2 EKF二維非線性系統(tǒng)仿真程序433.4 擴(kuò)展卡爾曼在二維目標(biāo)跟蹤中的應(yīng)用443.4.1 基于觀測距離的EKF目標(biāo)跟蹤算法443.4.2跟蹤算法matlab程序443.4.3 基于純方位的EKF目標(biāo)跟蹤算法463.4.4純方位目標(biāo)跟蹤算法matlab程序473.5 EKF在純方位尋的導(dǎo)彈制導(dǎo)中的應(yīng)用483.5.1 三維尋的制導(dǎo)系統(tǒng)483.5.2 尋的制導(dǎo)matlab仿真程序52第四章 無跡卡爾曼濾波UKF554.1 無跡Kalman濾波原理554.1.1 UT變換554.1.2 無跡卡爾曼原理564.2 UKF在單站二維目標(biāo)跟蹤中的應(yīng)用574.2.1 原理介紹574.2.2 仿真程序574.3 UKF在六維CA目標(biāo)跟蹤模型中的應(yīng)用604.3.1 原理介紹604.3.2 仿真程序62第五章 交互多模型卡爾曼濾波655.1交互多模型Kalman濾波（IMM）655.1.1 交互多模原理655.1.2 仿真方法665.2交互多模型K