第七章濾波、推估與配置,kalman濾波- 復(fù)件

1、 第七章最小二乘濾波、推估與配置序貫平差、kalman濾波 一、濾波、推估及配置概述 經(jīng)典平差經(jīng)典平差-將參數(shù)定性為非隨機(jī)變量，不考慮其隨機(jī)性。 濾波濾波-參數(shù)考慮為正態(tài)隨機(jī)量，有先驗(yàn)統(tǒng)計(jì)性質(zhì)，且在估計(jì)這些參數(shù)時要考慮先驗(yàn)統(tǒng)計(jì)性質(zhì)。濾波得到的參數(shù)稱為信號。 信號分為兩種：信號分為兩種：1 1、濾波信號、濾波信號S S-已測點(diǎn)信號，與觀測向量L建立了函數(shù)模型；2 2、推估信號、推估信號SS-未測點(diǎn)信號，與觀測向量未建立函數(shù)模型。S與S統(tǒng)計(jì)相關(guān)。 當(dāng)平差模型中同時存在隨機(jī)參數(shù)（信號）隨機(jī)參數(shù)（信號）和非隨機(jī)參數(shù)（傾向參非隨機(jī)參數(shù)（傾向參數(shù)）數(shù)），且在估計(jì)這些參數(shù)時要考慮隨機(jī)參數(shù)的先驗(yàn)統(tǒng)計(jì)信息時，就

2、是最小二乘濾波、推估及配置濾波、推估及配置模型，配置也稱為擬合推估擬合推估。范圍外，稱外推或預(yù)報在當(dāng)；范圍內(nèi)，稱內(nèi)插或平滑在對靜態(tài)系統(tǒng)而言，當(dāng)?shù)倪^程稱為推估；值的過程稱為濾波；求估求估值向量。正態(tài)隨機(jī)參數(shù)觀測向量和觀測誤差。、濾波的函數(shù)模型：SSSSSSYLSSAYAL01濾波模型對設(shè)計(jì)矩陣濾波模型對設(shè)計(jì)矩陣B B是否滿秩，不作要求。是否滿秩，不作要求。濾波因考慮了參數(shù)的隨機(jī)性，故所得的估值比最小二乘平濾波因考慮了參數(shù)的隨機(jī)性，故所得的估值比最小二乘平差估值具有差估值具有更高更高的精度。的精度。將將G-MG-M模型推廣，可得最小二乘配置的函數(shù)模型：模型推廣，可得最小二乘配置的函數(shù)模型：。號意義

3、與濾波模型相同為隨機(jī)參數(shù)。其余各符為非隨機(jī)參數(shù)，YXSSAXBYAXBL01SALSXBYA0001，就成為，就成為項(xiàng)等于，就成為項(xiàng)等于上式中，當(dāng)間接平差模型；間接平差模型；濾波與推估模型；濾波與推估模型；濾波模型。濾波模型。虛擬觀測值虛擬觀測值-當(dāng)未知參數(shù)當(dāng)未知參數(shù)Y Y是正態(tài)隨機(jī)向量時，可以將它的先驗(yàn)是正態(tài)隨機(jī)向量時，可以將它的先驗(yàn)期望期望 Y Y當(dāng)作虛擬觀測值，虛擬觀測值的當(dāng)作虛擬觀測值，虛擬觀測值的方差方差為為Y Y 的先驗(yàn)方差的先驗(yàn)方差DDY Y，再按廣義最小二乘原理（或極大驗(yàn)后估計(jì)原理，二者計(jì)算公式再按廣義最小二乘原理（或極大驗(yàn)后估計(jì)原理，二者計(jì)算公式一致）求參數(shù)一致）求參數(shù)Y Y

4、的估值。的估值。二、最小二乘濾波與推估SSAYAL01濾波的函數(shù)模型： 相互獨(dú)立。、與隨機(jī)參數(shù)，表示觀測噪聲，若協(xié)方差：的先驗(yàn)期望和方差：）隨機(jī)模型：（設(shè)的先驗(yàn)統(tǒng)計(jì)信息構(gòu)成的和SSDDDSDSDDDDPDYDYEYPDDEYSSSSSSSSSSYYSS00,cov,cov,011120 SSYYYSSSSSSYYSSYYYYSSLYVLDDDDDPYELPDPYE111對應(yīng)的誤差方程為，其權(quán)為，令：，方差為為作為虛擬觀測值，其權(quán)將虛擬觀測值的誤差方程：最小二乘濾波的總誤差方程：。，則：的權(quán)陣。約定：觀測值，權(quán)陣：或?qū)懗桑?20100DPLPPPLLYIAVVLYVLYAVYYYYY廣義最小二乘

5、原理：為對應(yīng)的權(quán)陣。及測值的改正數(shù)，分別為觀測值及虛擬觀及YYYYTYTPPVVVPVVPVmin SYTSTYTSSTSTYTYTYTYTYTYTYYYTYTYYTYTLAALADAAADADADADDAADADDADADAADDAADADDDADADADALDLDADADAY111111111111111111111120,1又由于：，有將矩陣反演公式用于得法方程：，并考慮入濾波的總誤差方程，將廣義最小二乘原理代SSTSTSSSSSSSTSTSSSSSTSTSSSYTYSTSTSSSSTSTSSDADADAADDDDADADAADDDDADADAADDDDADADALDADAADSALDA

6、DAADS1111111111111111111111111111方差陣：得濾波參數(shù)的方差及協(xié)再由法方程得知得濾波參數(shù)解： 的情況下成立。，以上公式是假設(shè)在001SSDD 的解決方法。測量平差時，可參考教材廣義之間存在線性關(guān)系，即與推估信號參數(shù)當(dāng)濾波信號參數(shù)方法；的解決平差可參考教材廣義測量時而之間有函數(shù)關(guān)系時，即當(dāng)濾波信號參數(shù)協(xié)方差。的值及其它們的方差和及同理，可得變?yōu)闀r，廣義最小二乘原理，當(dāng)45P444P,3min002012211SSSSHHSSSSSSSSVVDDDDVVDDYYYYTYTSS。及其方差求推估信號的估值上例中，若還已知：例。及其方差求濾波信號的估值觀測方程為：設(shè)已知例S

7、SSSSSSSSSDSDDDDSSSLLDDDL 21212100,21,2,1201110010,20022002,00,111濾波的單位權(quán)方差估值：nVPVVPVDDYYTYTSS2000時，有，當(dāng)最小二乘濾波的應(yīng)用舉例：兩個相鄰控制網(wǎng)中的公共點(diǎn)有兩套坐標(biāo)時，可利用濾波的原理，使兩個控制網(wǎng)的所有坐標(biāo)統(tǒng)一在一個最佳坐標(biāo)狀態(tài)。.00000000000000000000IBXBXLDXXXXDDDXXXXDDDXXXXXXXXXXXXXXX其中。其觀測方程為的新觀測值，方差是是對于甲網(wǎng)，。對應(yīng)的方差和協(xié)方差陣、分別是和、參數(shù)估值，是非公共點(diǎn)的點(diǎn)的參數(shù)估值，是乙網(wǎng)單獨(dú)平差時公共乙網(wǎng)中，設(shè)。對應(yīng)的方

8、差和協(xié)方差陣、分別是和、參數(shù)估值，是非公共點(diǎn)的點(diǎn)的參數(shù)估值，是甲網(wǎng)單獨(dú)平差時公共甲網(wǎng)中，設(shè)似。濾波公式與上面內(nèi)容相是單位陣。其中。其觀測方程為的新觀測值，方差是是對于乙網(wǎng)，及精度：得甲網(wǎng)中濾波后的參數(shù)接上頁。根據(jù)濾波公式IBXBXLDXXDDDDDDDDDDDDDDDDDDXXDDXXXXDDDXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX000111010001000000000000000000000000000計(jì)算結(jié)果，將兩個控制網(wǎng)連接成一個控制網(wǎng)，使兩網(wǎng)坐標(biāo)統(tǒng)一起來。三、最小二乘配置。隨機(jī)模型與濾波的相同的先驗(yàn)統(tǒng)計(jì)信息構(gòu)成的和YSSYAAYXLYAXBL,01

9、為隨機(jī)參數(shù)。數(shù)）；為非隨機(jī)參數(shù)（傾向參為觀測向量；配置的函數(shù)模型：1.最小二乘配置的誤差方程 SSYYYSSSSSSYYSSSSSSLYVLDDDDDPVVYESSYAALYAXBV,011對應(yīng)的誤差方程為。與，其權(quán)為和其相應(yīng)的改正數(shù)為作為虛擬觀測值，理，將，按照廣義最小二乘原式中：誤差方程：最小二乘配置應(yīng)用舉例 。的則是隨機(jī)量（信號）的擾動重力場所引起量（傾向參數(shù)），地球是不具有隨機(jī)性質(zhì)的變其中，的函數(shù)，于是重力場所引起的擾動以及由地球是衛(wèi)星正常軌道參數(shù)觀測值：在衛(wèi)星觀測中，距離例。則是隨機(jī)量，亦即信號數(shù)），重力異常機(jī)性質(zhì)的變量（傾向參隨個基本參數(shù)，是不具有統(tǒng)的取決于所采用的參考系正常重力之

10、和，即和觀測誤差重力異常、都是正常重力個點(diǎn)上的重力觀測值：在重力測量中，每一例TXTXXXSTXSgXgXgggXgu,24121同。的計(jì)算公式與濾波時相與可見式中”時的參數(shù)估計(jì)公式：，陣反演公式，在“根據(jù)矩陣分塊求逆和矩。平差問題。權(quán)陣為：題已轉(zhuǎn)化成一般的間接此時，最小二乘配置問的方差陣為、上觀測值將上式寫為SSDADAMAXBLMADSAXBLMADSALMBMBBXDDDPDDDDDDDDDDLLSSXIIABVVVLYVLYAXBVTSSTSSSSTSSSTTSSSSSSSSSSSSSSSSSSYY,000000011111111111 最小二乘配置的總誤差方程為的個數(shù)。為非隨機(jī)參數(shù)估

11、值為時，驗(yàn)后單位權(quán)方差的當(dāng)式中方差及協(xié)方差陣：最小二乘配置各參數(shù)的XttnVPVVPVDDDADAMMBDADDMBDADDDAMBBDIMADDDDAMBBDIMADDDDAMBBDIMADDDMBBDxxYYTYTSSTSXTSSXSXTSXSSSTXTSSSSSSSTXTSSSSSTXTSSSTX0, 0,20111111111111四、序貫平差靜態(tài)卡爾曼濾波特點(diǎn)：若對同一個控制網(wǎng)進(jìn)行了分期重復(fù)觀測，只需將上一次平差的結(jié)果作一些修改，就可得到聯(lián)合平差的效果。是一種遞推的間接平差法。（也稱為靜態(tài)逐次濾波）未知參數(shù)不改變的情況矩陣反演公式：（要求P、R兩矩陣有逆陣存在）12211221222

12、22112111111.DQPLXBVDQPLXBVDQPLXBVkkkkkHPHPHRPHPHRHPTTT1111解法 1:1111121122211222111222211222211122211111111111111)()(:,.,)()()()(:,)(;:,kkTkkXkXkkTkkkXkXkkkTXTTXTXXTTXXTXTXBPBPQLPBXPQXLLLLBPBPBPBBPBQLPBXPQLPBLPBQXLLPBPBQLPBQXL可得聯(lián)合求解由可得聯(lián)合求解由可得求解由解法解法 2:11111111112122122112122122122112)()()()()()(1kXkk

13、TkkXkTkkXkXkXkkkkTkXkTkkXkkXTXTXXXTXTXQBQBQBBQQQXBLQBQBBQXXQBQBQBBQQQXBLQBQBBQXXk五、卡爾曼濾波1、kalman濾波理論特點(diǎn)：1） Kalman濾波是 利用觀測向量來重構(gòu)、估計(jì)隨時間不斷變化的狀態(tài)向量。順序是：預(yù)測實(shí)測修正。2） Kalman濾波得到的狀態(tài)向量估值具有無偏性和最小方差性，是一種基于線性最小方差估計(jì)上的最優(yōu)估計(jì)方法。3） Kalman濾波的優(yōu)點(diǎn)：是一種計(jì)算量小，存儲量低，實(shí)時性高的遞推解算法。4）在經(jīng)歷了初始濾波的過渡狀態(tài)后，濾波效果會很好。Kalman濾波應(yīng)用濾波應(yīng)用舉例舉例 在在雷達(dá)跟蹤目標(biāo)系統(tǒng)中

14、，觀察到的是帶有噪聲的雷達(dá)跟蹤目標(biāo)系統(tǒng)中，觀察到的是帶有噪聲的目標(biāo)位置、運(yùn)動速度、加速度等，目標(biāo)位置、運(yùn)動速度、加速度等， Kalman濾波就是濾波就是能利用目標(biāo)的動態(tài)信息，減小或去除噪聲的影響，得能利用目標(biāo)的動態(tài)信息，減小或去除噪聲的影響，得到關(guān)于目標(biāo)位置的最佳到關(guān)于目標(biāo)位置的最佳估計(jì)。估計(jì)。 Kalman濾波利用濾波利用過去直到當(dāng)前過去直到當(dāng)前的觀測信息：的觀測信息：1)、對目標(biāo)當(dāng)前位置估計(jì)（對目標(biāo)當(dāng)前位置估計(jì)（濾波濾波）；）；2)、對目標(biāo)將來位置估計(jì)（對目標(biāo)將來位置估計(jì)（預(yù)測預(yù)測）；）；3)、重新、重新估計(jì)過去的狀態(tài)（估計(jì)過去的狀態(tài)（插值插值或稱或稱平滑平滑）。）。Kalman濾波分為：

15、濾波分為：濾波、預(yù)測、濾波、預(yù)測、平滑平滑三個主要部分。三個主要部分。的形式（和、上面三部分常記為：)0jkjkXkjkXkkX 21,0,0111111, 1111kDNnrnmnnkDNppmrrmmmmkkkkkkkkkkkkkkkkUGXBLUXX維觀測噪聲階系數(shù)矩陣階系數(shù)矩陣維觀測向量聲維動態(tài)噪系數(shù)陣階動態(tài)噪聲或稱輸入向量維控制向量向量系數(shù)陣階控制階轉(zhuǎn)移矩陣維狀態(tài)向量，2、 狀態(tài)方程（動態(tài)方程）和觀測方程 下面為下面為 基礎(chǔ)基礎(chǔ)Kalman濾波的濾波的狀態(tài)方程狀態(tài)方程(1) 和和觀測方程觀測方程(2) （該模（該模型對應(yīng)的是離散線性系統(tǒng)）：型對應(yīng)的是離散線性系統(tǒng)）：以上兩方程也常稱為

16、動力學(xué)模型和測量模型。以上兩方程也常稱為動力學(xué)模型和測量模型。Kalman濾波的應(yīng)用舉例：導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在濾波的應(yīng)用舉例：導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等。近年來更被應(yīng)用于計(jì)算機(jī)軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等。近年來更被應(yīng)用于計(jì)算機(jī)圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。隨機(jī)模型 的前二階矩。和、已知）函數(shù)。是克羅內(nèi)爾（的正定方差陣；是的非負(fù)定方差陣；是其中：協(xié)方差：方差。；，有且對于一切；kkkjkjkkjkjkjkxxjkkjjkkjjkkkXjkjkKronekDkDLXXk

17、jDXXuXEkDkDEE00001kercovvar0,cov0,cov0,cov00var0000,cov,cov,cov00在完全不相關(guān)白噪聲作用下，離散線性系統(tǒng)的隨機(jī)模型為：在完全不相關(guān)白噪聲作用下，離散線性系統(tǒng)的隨機(jī)模型為：Kalman濾波的前提條件： kkkkkkkkkkssskskkkkkskkkskkkkkkkkkkkkkkkkkkkkkkZXBlXXklZXBlXXlZXBlXXllllklXXklUklUGZZXBLUXX1112222212112111110100121111, 111111111, 111111021,1:221，觀測方程為：式可有、個時，觀測向量有可看

18、成是觀測向量。當(dāng)式中式變?yōu)閯t，。令由上兩式可得觀測方程）式中設(shè)（狀態(tài)方程和觀測方程統(tǒng)一轉(zhuǎn)化為觀測方程形式 的濾波公式。于上兩式，得標(biāo)準(zhǔn)形式再將矩陣反演公式作用公式）：估值及其方差陣（濾波一起平差，得與上面第三個誤差方程，方程為：作為虛擬觀測值，誤差將方差：個方程解得：、由上面第：方程轉(zhuǎn)變的誤差方程為。得到由觀測觀測值，方差陣為的先驗(yàn)期望看成是虛擬將隨機(jī)參數(shù)，111111111111111010101010110112222221121111110010010/11/110/10/110/11/11/10/11/10/10/1000/1,00/00/10/00/0211111000/000BDB

19、DDZlDBXDBDBDXXXXvXDDDlXXXXXXlZXBvklXXkvlZXBvlXXvlZXBvlXXvXXvDXTXXTXTXXTTXXskkkkkskkkksssssXX虛擬觀測方程可見kalman濾波，就是一種變參數(shù)的逐次平差法。推導(dǎo)時注意：P=D-13、kalman濾波器的5個核心公式）。此時并未產(chǎn)生，即預(yù)測，所以動態(tài)噪聲狀態(tài)方程得到，因?yàn)槭莾?yōu)濾波值。（上式可從是上一狀態(tài)的最的預(yù)測值，式中求即用）（，01/1,11/11/11111kkkkkkkkkkXXLLUkkXkkXkalman一步預(yù)測公式： 沒有方差）傳播得到。（控制項(xiàng)此式由狀態(tài)方程經(jīng)誤差，kTkkkkTkkXkkX

20、UkDkkDkkD211/11/1,1,11一步預(yù)測值的方差為： 。項(xiàng)去修正了預(yù)測值也可以說是用，估值，得到現(xiàn)在狀態(tài)的最優(yōu)時測量值和即用預(yù)測值1/1/1/31/1/kkXkkXBZLJkkXLkkkXkkXBZLJkkXkkXkkkkkkkkkKalman濾波公式：濾波增益矩陣： 的計(jì)算工作量。算出，減少實(shí)時處理時與觀測值無關(guān)，可預(yù)先可見kTkXkTkXkJkDBkkDBBkkDJ41/1/1濾波值的方差公式： 51/kkDBJIkkDXkkX 即可。，兩式中令、上面則項(xiàng)為零時，測方程中的非隨機(jī)控制注意：當(dāng)動態(tài)方程和觀0031kkZU 為了令卡爾曼濾波器開始工作，需要兩個零時刻的初始值為了令卡爾曼濾波器開始工作，需要兩個零時刻的初始值-X(0/0)和和DX(0/0)。他們的值可以任意給一個，因?yàn)殡S著卡爾曼的工作，。他們的值可以任意給一個，因?yàn)殡S著卡爾曼的工作，X會逐漸的收斂。會逐漸的收斂。但是對于但是對于DX(0/0) ，一般不要取，一般不要取0，因?yàn)檫@樣可能會令系統(tǒng)完全相信你給定的，因?yàn)檫@樣可能會令系統(tǒng)完全相信你給定的X(0/0)是系統(tǒng)最優(yōu)的，從而使算法不能收斂。是系統(tǒng)最優(yōu)的，從而使算法不能收斂。Kalman系統(tǒng)的可觀可控性可控性輸入能夠控制狀態(tài)的變化。 可觀性狀態(tài)的變化能由輸出反映出來。器