Kalman濾波原理及程序手冊

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

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

3、1s，觀測噪聲的均值為0，方差為100。過程噪聲越小，目標越接近勻速直線運動，反之，則為曲線運動。仿真得到以下結(jié)果：圖3-1 跟蹤軌跡圖 圖3-2 跟蹤誤差圖仿真程序% Kalman濾波在目標跟蹤中的應用實例%function Kalmanclc;clear;T=1;%雷達掃描周期,N=80/T; %總的采樣次數(shù)X=zeros(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ù)，目標真實軌跡就是

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

5、標檢測，一定要找到重心，與雷達目標跟蹤中觀測目標位置是一回事。圖像目標檢測跟蹤程序% 目標檢測函數(shù)，這個函數(shù)主要完成將目標從背景中提取出來%function detectclear,clc; %清除所有內(nèi)存變量、圖形窗口% 計算背景圖片數(shù)目Imzero = zeros(240,320,3);for i = 1:5 % 將圖像文件 i.jpg 的圖像像素數(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% 讀取所有幀運行程序得到的x,y方向的位置跟蹤偏差分析Y方向的位置偏差X方向的位置偏差三、通用非線性系統(tǒng)的EKF實現(xiàn)例子：所謂的非線性方程，就是因變量與自變量的關(guān)系不是線性的，這類方程很多，例如平方關(guān)系，對數(shù)關(guān)系，指數(shù)關(guān)系，三角函數(shù)關(guān)系等等。這些方程可分為兩類，一類是多項式方程，一種是非多項式方程。為了便于說明非線性卡爾曼濾波擴展Kalman濾波的原理，我們選用以下系統(tǒng)，系統(tǒng)狀態(tài)為，它僅包含一維變量，即，系統(tǒng)狀態(tài)方程為 (3-2-1)觀測方程為 (3-2-2)其中，式(3-1-1)是包含分式，平方，三角函數(shù)在內(nèi)的嚴重

7、非線性的方程，為過程噪聲，其均值為0，方差為Q，觀測方程中，觀測信號與狀態(tài)的關(guān)系也是非線性的，也是均值為0，方差為R的高斯白噪聲。因此關(guān)于(3-1-1)和(3-2-2)是一個狀態(tài)和觀測都為非線性的一維系統(tǒng)。以此為通用的非線性方程的代表，接下來講述如何用擴展Kalman濾波來處理噪聲問題。第一步：初始化初始狀態(tài)，協(xié)防差矩陣。第二步：狀態(tài)預測 (3-2-3)第三步：觀測預測 (3-2-4)第九步：協(xié)方差更新 (3-2-10)以上九步為擴展卡爾曼濾波的一個計算周期，如此循環(huán)下去就是各個時刻EKF對非線性系統(tǒng)的處理過程。其他參數(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)擴展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; % 總時間Q=10;R=1;% 產(chǎn)生過程噪聲w=sqrt(Q)*randn(1,T);% 產(chǎn)生觀測噪聲v=sqrt(R)*randn(1,T

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

10、向角，實際測量中雷達具有加性測量噪聲，則在笛卡爾坐標系下，觀測方程為式中，為測量噪聲，他也是高斯型白色隨機向量序列，而且，對于，其定義為其中，顯然在笛卡爾坐標系下，該模型運動觀測方程為非線性的。仿真結(jié)果為： 軌跡跟蹤圖 位置誤差 速度誤差 加速度誤差尋的制導matlab仿真程序% 程序說明：目標跟蹤程序，實現(xiàn)運動彈頭對運動物體的三維跟蹤，主函數(shù)% 狀態(tài)方程： x(t)=Ax(t-1)+Bu(t-1)+w(t）% 參考資料：尋的導彈新型導引第5.5和5.6節(jié)中仿真參數(shù)設置%function main%delta_t=0.01; % 測量周期,采樣周期longa=1; % 機動時間常數(shù)的倒數(shù)，即機

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

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

14、聲協(xié)方差陣R = 100 0; 0 0.0012;%量測噪聲協(xié)方差陣% 狀態(tài)方程目 錄第一章 Kalman濾波概述11.1 kalman濾波的背景11.2 kalman濾波的發(fā)展過程31.3 kalman濾波應用領(lǐng)域5第二章 線性卡爾曼濾波62.1 Kalman濾波原理62.1.1 射影定理62.1.2 Kalman濾波原理推導82.1.3 Kalman濾波過程描述102.2 Kalman濾波在溫度測量中的應用122.2.1 原理介紹122.2.1 matlab仿真程序132.3 Kalman濾波在自由落體中的應用142.3.1 原理介紹142.3.2 Matlab仿真程序192.3.3 C語

15、言程序202.4 Kalman濾波在船舶GPS導航定位系統(tǒng)中的應用222.4.1 原理介紹222.4.2 matlab仿真程序252.5 Kalman濾波在石油地震勘探中的應用262.5.1 石油地震勘探白噪聲反卷積濾波原理262.5.2 石油地震勘探白噪聲反卷積濾波仿真程序282.6 Kalman濾波在視頻圖像目標跟蹤中的應用312.6.1 視頻圖像處理的基本方法及程序312.6.2 Kalman濾波對自由下落的皮球跟蹤應用332.6.3 目標檢測程序332.6.4 Kalman濾波視頻跟蹤程序35第三章 擴展卡爾曼濾波EKF373.1 擴展Kalman濾波原理373.2 擴展卡爾曼在一維非

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