卡爾曼濾波對(duì)測(cè)量數(shù)據(jù)處理的程序?qū)崿F(xiàn)方法

本文格式為Word版，下載可任意編輯——卡爾曼濾波對(duì)測(cè)量數(shù)據(jù)處理的程序?qū)崿F(xiàn)方法黑龍江工程學(xué)院本科生畢業(yè)論文

第1章緒論

1.1研究的目的

自從1960年卡爾曼濾波提出以來(lái)，它已成為控制，信號(hào)處理與通信等領(lǐng)域最基本最重要的計(jì)算方法和工具之一，并已成功的應(yīng)用到航空，航天，工業(yè)過(guò)程及社會(huì)經(jīng)濟(jì)等不同領(lǐng)域，譬如，在雷達(dá)中，人們感興趣的是跟蹤目標(biāo)，但目標(biāo)的位置、速度、加速度的測(cè)量值往往在任何時(shí)候都有噪聲。卡爾曼濾波利用目標(biāo)的動(dòng)態(tài)信息,設(shè)法去掉噪聲的影響，得到一個(gè)關(guān)于目標(biāo)位置的好的估計(jì)。這個(gè)估計(jì)可以是對(duì)當(dāng)前目標(biāo)位置的估計(jì)（濾波），也可以是對(duì)于將來(lái)位置估計(jì)（預(yù)計(jì)），也可以是對(duì)過(guò)去位置的估計(jì)（差值或平滑）。但隨著微型計(jì)算機(jī)的普及應(yīng)用,對(duì)卡爾曼濾波的數(shù)值穩(wěn)定性、計(jì)算效率、實(shí)用性和有效性的要求越來(lái)越高，隨著微型計(jì)算機(jī)時(shí)代的來(lái)臨顯著地提高了科學(xué)計(jì)算的能力，濾波大量繁雜的計(jì)算在計(jì)算機(jī)種只需要幾分鐘就能算出，為此本文將對(duì)卡爾曼濾波進(jìn)行研究。

1.2研究的意義

卡爾曼濾波(Kalman,1960)是當(dāng)前應(yīng)用最廣的一種動(dòng)態(tài)數(shù)據(jù)處理方法,它具有最小無(wú)偏方差性.把變形體視為一個(gè)動(dòng)態(tài)系統(tǒng),將一組觀測(cè)值作為系統(tǒng)的輸出,可以用卡爾曼濾波模型來(lái)描述系統(tǒng)的狀態(tài).動(dòng)態(tài)系統(tǒng)由狀態(tài)方程和觀測(cè)方程描述,以監(jiān)測(cè)點(diǎn)的位置、速率和加速率參數(shù)為狀態(tài)向量,可構(gòu)造一個(gè)典型的運(yùn)動(dòng)模型.狀態(tài)方程中要加進(jìn)系統(tǒng)的動(dòng)態(tài)噪聲.其濾波方程是一組遞推計(jì)算公式,計(jì)算過(guò)程是一個(gè)不斷預(yù)計(jì)、修正的過(guò)程,在求解時(shí),優(yōu)點(diǎn)是不需保存用過(guò)的觀測(cè)值序列,并且當(dāng)?shù)玫叫碌挠^測(cè)數(shù)據(jù)時(shí),可隨時(shí)計(jì)算新的濾波值,便于實(shí)時(shí)處理觀測(cè)成果,把參數(shù)估計(jì)和預(yù)報(bào)有機(jī)地結(jié)合起來(lái).卡爾曼濾波特別適合變形監(jiān)測(cè)數(shù)據(jù)的動(dòng)態(tài)處理.

1.3研究的方法

1.4課題的主要內(nèi)容

本文先從現(xiàn)代測(cè)量誤差處理理論基礎(chǔ)開(kāi)始講解，細(xì)致的寫出現(xiàn)代測(cè)量誤差都有那些函數(shù)，并詳細(xì)分析講解這些函數(shù)，在繼續(xù)講解最小二乘與卡爾曼濾波的關(guān)系，如量測(cè)值越多，只要處理得適合，最小二乘估計(jì)的均方誤差就越小。采用批處理實(shí)現(xiàn)的最小二乘算法，需存儲(chǔ)所有的量測(cè)值。若量測(cè)值數(shù)量十分巨大，則計(jì)算機(jī)必需具備巨大的存儲(chǔ)容量，這顯然是不經(jīng)濟(jì)的。遞推最小二乘估計(jì)從每次獲得的最小量測(cè)值中提取出被估計(jì)量信息，用于修正上一步所得的估計(jì)。獲得量測(cè)的次數(shù)越多，修正的次數(shù)也越多，估計(jì)的精讀也越高。這和卡爾曼濾波原理十分相像，本文在詳細(xì)講解了卡爾曼濾波，寫出其原理性質(zhì)，在根據(jù)C++進(jìn)行編程，使其應(yīng)用于測(cè)量領(lǐng)域。

第2章現(xiàn)代測(cè)量誤差處理理論基礎(chǔ)

1

黑龍江工程學(xué)院本科生畢業(yè)論文

2.1概述

在測(cè)量、通信和控制等學(xué)科中，為了求得某些未知參數(shù)，往往要進(jìn)行一系列的觀測(cè)．由于測(cè)量上的局限性，往往只能觀測(cè)未知量的某些函數(shù)，且觀測(cè)值中必然含有誤差(或稱為噪聲)．這就產(chǎn)生了根據(jù)含有誤差的觀測(cè)值求定未知參數(shù)估值的問(wèn)題．下面舉幾個(gè)例子．

(1)為了確定平面或三維控制網(wǎng)中各點(diǎn)的坐標(biāo)，對(duì)控制網(wǎng)的邊長(zhǎng)和方向(或坐標(biāo)差)進(jìn)行了觀測(cè)，當(dāng)然，觀測(cè)值包含有誤差．設(shè)各點(diǎn)的坐標(biāo)為未知參數(shù)向量x，而包括邊長(zhǎng)和方向的觀測(cè)值向量為L(zhǎng)，則L和x之間有函數(shù)關(guān)系

L?F(X)??

式中?表示誤差向量．通過(guò)含有誤差?的觀測(cè)向量L來(lái)求定待定點(diǎn)坐標(biāo)的最正確估值，就是一個(gè)估計(jì)問(wèn)題．在測(cè)量中，就是一個(gè)平差問(wèn)題．

(2)通信理論中的一個(gè)重要問(wèn)題是從接收到的信號(hào)中，提取被發(fā)送的信號(hào)設(shè)被發(fā)送的信息調(diào)制成信號(hào)S(t)，而接收到的信號(hào)也就是信號(hào)的觀測(cè)值L(t)，由于大氣噪聲和電路噪聲的干擾，因此有

L(t)?S(t)?n(t)

其中n(t)是噪聲，t表示時(shí)間．通信中的主要問(wèn)題就是從L(t)中將有用的信號(hào)S(t)分開(kāi)出來(lái)，也就是由L(t)求定S(t)的最正確估值．信號(hào)S(t)也是一種未知參數(shù)．

(3)生產(chǎn)過(guò)程的自動(dòng)化可以達(dá)到高效率和高精度在實(shí)現(xiàn)生產(chǎn)過(guò)程的控制中，需要通過(guò)對(duì)生產(chǎn)系統(tǒng)進(jìn)行狀態(tài)的不斷測(cè)量，得到與系統(tǒng)運(yùn)行狀態(tài)有關(guān)的觀測(cè)值；然后對(duì)觀測(cè)值進(jìn)行分析處理得到控制信號(hào)，實(shí)時(shí)地控制生產(chǎn)系統(tǒng)按要求運(yùn)行．但由于觀測(cè)值中存在誤差，所以，為了得到控制信號(hào)，就要求由觀測(cè)值來(lái)估計(jì)系統(tǒng)的運(yùn)行狀態(tài)

(4)衛(wèi)星(或其他運(yùn)動(dòng)體)的軌道往往可以由如下微分方程確定

?(t)?f(X(t),U(t),?(t))X式中f表示時(shí)間；x(t)表示衛(wèi)星的軌道參數(shù)，在此處稱為狀態(tài)向量；U(t)為控制向量；力(t)是隨

機(jī)的狀態(tài)噪聲．為了確切估計(jì)或預(yù)計(jì)衛(wèi)星的軌道，就需要對(duì)衛(wèi)星進(jìn)行觀測(cè)，從而得到大量的觀測(cè)數(shù)據(jù)L(t)，然后實(shí)時(shí)地由含有誤差的觀測(cè)值L(t)來(lái)估計(jì)衛(wèi)星的軌道，即估計(jì)衛(wèi)星的軌道參數(shù)．以上例中所述的信號(hào)或狀態(tài)都可以說(shuō)是一種未知參數(shù)．在測(cè)量平差中，尋常稱非隨機(jī)的未知參數(shù)向量為參數(shù)，而稱隨機(jī)參數(shù)向量為信號(hào)，而稱隨時(shí)間t變化的動(dòng)態(tài)系統(tǒng)中的未知參數(shù)向量為狀態(tài)向量，或筒稱為狀態(tài)．可以看到，在上面的例子中，都存在一個(gè)對(duì)未知參數(shù)進(jìn)行估計(jì)的問(wèn)題．一般說(shuō)來(lái)，若設(shè)x為t階未知參數(shù)向量(簡(jiǎn)稱為參數(shù))，L為n階觀測(cè)向量(或稱觀測(cè)值)，?表示n維誤差(或噪聲)向量．那么，所謂估計(jì)問(wèn)題，就是根據(jù)含有誤差?的觀測(cè)值L，構(gòu)造一個(gè)函數(shù)

?(L)，使X?(L)成為未知參數(shù)向量X的最正確估計(jì)量，其具體數(shù)值稱為最正確估值(以后一般不區(qū)分X?，并記?(L)簡(jiǎn)記為X其含義)．尋常將X???X??X?X(L)?X?X

?稱?X?；為X(L)的估計(jì)誤差．

可以看到，當(dāng)△；的數(shù)學(xué)期望等于零時(shí)，?X??X?)；而當(dāng)X為非隨機(jī)量?；的方差就等于E(?X

2

T黑龍江工程學(xué)院本科生畢業(yè)論文

時(shí)，未知參數(shù)的估值工的方差DX?；也就等于其誤差方差D(?X?)．在估計(jì)理論中，尋常是用估計(jì)

?的誤差方差D(?)來(lái)衡量其精度的．量X但在經(jīng)典的最小二乘平差中，由于X一般都是非隨機(jī)參?X數(shù)，所以習(xí)慣上都用估值(平差值)的方差衡量精度．

?(L)時(shí)，在根據(jù)觀測(cè)值L求未知參數(shù)x的估值X總是希望所得到的估值是最優(yōu)的．由估計(jì)理論

知道，最優(yōu)估計(jì)量主要應(yīng)具有以下幾特性質(zhì)：

?(L)尋常與其真值是不同的，我們希望當(dāng)觀測(cè)值個(gè)數(shù)n增(1)一致性．由觀測(cè)值得到的估值X加時(shí)，估計(jì)量變得更好些；當(dāng)n無(wú)限增大時(shí)，估計(jì)量向被估計(jì)的參數(shù)趨近的概率等于1．即假使對(duì)于任意??0，有

??X??)?1(1-1-1)limP(X???Xn???具有一致性；若有則稱估計(jì)量X?)(X?X?))?0(1-1-2)lim((X?XTn??則稱此估計(jì)量是均方一致的．估計(jì)量的一致性是從它的極限性質(zhì)來(lái)看的．

?的數(shù)學(xué)期望等于被估計(jì)量x的數(shù)學(xué)期望，即(2)無(wú)偏性．若估計(jì)量X?)?E(X)(1-1-3)E(X假使丑是非隨機(jī)量，上式即為

?)?X(1-1-4)E(X?為漸近無(wú)偏．?)?X(n??)，則稱X則稱丘為無(wú)偏估計(jì)量．假使E(X?的誤差方差E((X?X?)(X?X?)T)，小于由L(3)有效性．若由觀測(cè)向量L得到無(wú)偏估計(jì)量X得到的任何其他無(wú)偏估計(jì)量X的誤差方差E((X?X)(X?X))，即

***T?)(X?X?)T)<E((X?X)(X?X))E((X?X**T或?qū)憺镈(?X?)?D(?X)(1-1-5)

*?是有效估計(jì)量，也稱X?具有有效性或方差最小性．則稱X以不同的準(zhǔn)則來(lái)求定未知參數(shù)的最正確估值，可得到不同的估計(jì)方法．估計(jì)方法主要有極大似然估計(jì)，最小二乘估計(jì)，極大驗(yàn)后估計(jì)，最小方差估計(jì)和線性最小方差估計(jì)等；經(jīng)典的測(cè)量平差法都是以最小二乘估計(jì)或極大似然估計(jì)為根據(jù)導(dǎo)出的；而濾波、配置和動(dòng)態(tài)系統(tǒng)的卡爾曼濾波等，最初是以極大驗(yàn)后估計(jì)或最小方差估計(jì)為根據(jù)導(dǎo)出的．因此，概率統(tǒng)計(jì)中的估計(jì)理論是廣義測(cè)量平差的理論基礎(chǔ)．

3

黑龍江工程學(xué)院本科生畢業(yè)論文

2.2多維正態(tài)分布

正態(tài)分布是測(cè)量平差理論中最常用的誤差分布，是最小二乘平差誤差理論的基礎(chǔ)．本節(jié)在已學(xué)過(guò)的一元正態(tài)分布的基礎(chǔ)上，對(duì)多維正態(tài)分布做全面闡述．廣義測(cè)量平差理論中還涉及其他分布，則將分別在相應(yīng)章節(jié)中一一介紹．

2.2.1多維正態(tài)分布的定義和性質(zhì)

已知隨機(jī)變量X的正態(tài)分布概率密度為f(x)?1?1?exp??2(x??X)2?(1-2-1)

?2??2??式中兩個(gè)參數(shù)?X和?2分別為隨機(jī)變量X的數(shù)學(xué)期望和方差．當(dāng)?X=0，?2=1時(shí)，X為標(biāo)準(zhǔn)正態(tài)分布變量．記為X~N(0,1)，其概率密度為f(x)?1?1?exp??x2?(1-2-2)2??2?Z2?Zm?它們

T設(shè)有m個(gè)相互獨(dú)立的標(biāo)準(zhǔn)正態(tài)隨機(jī)變量構(gòu)成的隨機(jī)向量Z??Z1的有限個(gè)線性函數(shù)

?X1??Z1?????XZ22X????A???A0n?1???n?m???n?1????X?n??Zm?為n維正態(tài)隨機(jī)向量．此時(shí)，X的數(shù)學(xué)期望和方差陣為

E(X)???T?DX?AA?X的分布函數(shù)和概率密度都簡(jiǎn)稱為n維(或n元)正態(tài)分布，簡(jiǎn)記為X~Nn(?,AAT)，或?qū)憺?/p>

X~Nn(?,DX)．

由相互獨(dú)立的標(biāo)準(zhǔn)正態(tài)隨機(jī)變量組成的隨機(jī)向量Z，可寫為Z~N(0,En).En為n階單位陣．

多維正態(tài)分布具有以下性質(zhì)：

(1)正態(tài)隨機(jī)向量的線性函數(shù)還是正態(tài)的.例如,設(shè)X~Nn(?,AAT),Y?BX?b則

Y~N(B??b,BAATBT)

(2)設(shè)X~Nn(?,AAT),，記

4

黑龍江工程學(xué)院本科生畢業(yè)論文

?X?????DX??1?,???1?,DX?AAT??11?X2???2??D21r?1n?rD12??D22?則X1~Nr(?1,D11),X2~N(?2,D22).2.2.2多維正態(tài)分布

設(shè)有n維正態(tài)隨機(jī)向量X—N。(p。，Dx)，其中方差陣D，為可逆陣，即det(Dx)≠0，則它的概

率密度為

f(x)?2(?)??2DX?12?1??1exp??(x??X)TDX(x??X)?

?2?式中DX表示DX的行列式．對(duì)于二維正態(tài)隨機(jī)向量?XY?，若它有可逆方差陣和數(shù)學(xué)期望為

2??X???XYT?XY???X?和??2??Y???Y?則由(1-2-3)式可得其概率密度為

f(x,y)?12?????2X2Y2XY?

22?(x??X)2?Y?2(x??X)(y??Y)?XY?(y??Y)2?Xexp??2222?X?Y??XY????因相關(guān)系數(shù)?XY?1?XY，所以上式可寫為?X?Y???(x??X)2(x??X)(y??Y)(y??Y)2??1?exp???2?????222?X?Y?Y???2(