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

1、Kalman濾波原理及仿真手冊(cè)KF/EKF/UKF原理+應(yīng)用實(shí)例+MATLAB程序本手冊(cè)的研究?jī)?nèi)容主要有Kalman濾波，擴(kuò)展Kalman濾波，無(wú)跡Kalman濾波等，包括理論介紹和MATLAB源程序兩部分。本手冊(cè)所介紹的線性濾波器，主要是Kalman濾波和-濾波，交互多模型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濾波處理問(wèn)題，相信讀者可以通過(guò)學(xué)習(xí)本文通用的非線性系統(tǒng)，能快速掌握

2、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)包含水平方向的位置和速度和縱向的位置和速度。則目標(biāo)跟蹤的系統(tǒng)方程可以用式（3.1）和（3.2）表示， （2-4-9） （2-4-10）其中，u,v為零均值的過(guò)程噪聲和觀測(cè)噪聲。T為采樣周期。為了便于理解，將狀態(tài)方程和觀測(cè)方程具體化：假定船舶在二維水平面上運(yùn)動(dòng)，初始位置為（-100m,200m），水平運(yùn)動(dòng)速度為2m/s，垂直方向的運(yùn)動(dòng)速度為20 m/s，GPS接收機(jī)的掃描周期為T=1s

3、，觀測(cè)噪聲的均值為0，方差為100。過(guò)程噪聲越小，目標(biāo)越接近勻速直線運(yùn)動(dòng)，反之，則為曲線運(yùn)動(dòng)。仿真得到以下結(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); % 傳感器對(duì)位置的觀測(cè)Z(:,1)=X(1,1),X(3,1); % 觀測(cè)初始化delta_w=1e-2; %如果增大這個(gè)參數(shù)，目標(biāo)真實(shí)軌跡就是曲線

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

5、測(cè)，一定要找到重心，與雷達(dá)目標(biāo)跟蹤中觀測(cè)目標(biāo)位置是一回事。圖像目標(biāo)檢測(cè)跟蹤程序% 目標(biāo)檢測(cè)函數(shù)，這個(gè)函數(shù)主要完成將目標(biāo)從背景中提取出來(lái)%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)系，對(duì)數(shù)關(guān)系，指數(shù)關(guān)系，三角函數(shù)關(guān)系等等。這些方程可分為兩類，一類是多項(xiàng)式方程，一種是非多項(xiàng)式方程。為了便于說(shuō)明非線性卡爾曼濾波擴(kuò)展Kalman濾波的原理，我們選用以下系統(tǒng)，系統(tǒng)狀態(tài)為，它僅包含一維變量，即，系統(tǒng)狀態(tài)方程為 (3-2-1)觀測(cè)方程為 (3-2-2)其中，式(3-1-1)是包含分式，平方，三角函數(shù)在內(nèi)的嚴(yán)重非線

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

8、態(tài)與真值之間的偏差如圖圖3-2-2所示。 圖3-2-1 EKF濾波處理后的狀態(tài)與真值對(duì)比 圖3-2-2 偏差分析EKF一維非線性系統(tǒng)仿真程序% 函數(shù)功能：一維非線性系統(tǒng)擴(kuò)展Kalman濾波問(wèn)題% 狀態(tài)函數(shù)：X(k+1)=0.5X(k)+2.5X(k)/(1+X(k)2)+8cos(1.2k) +w(k)% 觀測(cè)方程：Z（k）=X(k)2/20 +v(k)%function EKF_for_One_Div_UnLine_System% 初始化T=50; % 總時(shí)間Q=10;R=1;% 產(chǎn)生過(guò)程噪聲w=sqrt(Q)*randn(1,T);% 產(chǎn)生觀測(cè)噪聲v=sqrt(R)*randn(1,T);

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

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

11、率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)/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ū)動(dòng)矩陣gama五、UK

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

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

14、方差陣R = 100 0; 0 0.0012;%量測(cè)噪聲協(xié)方差陣% 狀態(tài)方程目 錄第一章 Kalman濾波概述11.1 kalman濾波的背景11.2 kalman濾波的發(fā)展過(guò)程31.3 kalman濾波應(yīng)用領(lǐng)域5第二章 線性卡爾曼濾波62.1 Kalman濾波原理62.1.1 射影定理62.1.2 Kalman濾波原理推導(dǎo)82.1.3 Kalman濾波過(guò)程描述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.3.3 C語(yǔ)言程

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濾波對(duì)自由下落的皮球跟蹤應(yīng)用332.6.3 目標(biāo)檢測(cè)程序332.6.4 Kalman濾波視頻跟蹤程序35第三章 擴(kuò)展卡爾曼濾波EKF373.1 擴(kuò)展Kalman濾波原理373.2 擴(kuò)展卡爾曼在一維非線性

16、系統(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)413.3.2 EKF二維非線性系統(tǒng)仿真程序433.4 擴(kuò)展卡爾曼在二維目標(biāo)跟蹤中的應(yīng)用443.4.1 基于觀測(cè)距離的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第四章 無(wú)跡卡爾曼濾波UKF554.1 無(wú)跡Kalman濾波原理554.1.1 UT變換554.1.2 無(wú)跡卡爾曼原理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 交互多模原