無(wú)人機(jī)PX4狀態(tài)估計(jì)

1、1.1 組合導(dǎo)航從上章介紹飛行器需要獲得自己的十五個(gè)狀態(tài)信息，需要通過(guò)傳感器測(cè)量并推算。單獨(dú)的慣性測(cè)量元件由于誤差累積，導(dǎo)致隨時(shí)間增長(zhǎng)發(fā)發(fā)散。因此需要通過(guò)額外的傳感器信息來(lái)進(jìn)行互補(bǔ)修正。組合導(dǎo)航技術(shù)結(jié)合GNSS慣性測(cè)量元件、磁力計(jì)、氣壓計(jì)和超聲波等傳感設(shè)備，利用各自的有點(diǎn)互補(bǔ)，采用數(shù)字信號(hào)處理技術(shù)融合多傳感器的測(cè)量值，從而獲得較為準(zhǔn)確的飛行器的十五個(gè)狀 態(tài)空間量，通過(guò)一定的算法和規(guī)律盡可能的估計(jì)出與真實(shí)狀態(tài)量接近的量。慣性測(cè)量元件容易發(fā)散，從而這個(gè)發(fā)散可以通過(guò)GNS驗(yàn)球衛(wèi)星定位系統(tǒng)來(lái)獲取修正和抑制，然而 GNSSR有在室外無(wú)遮擋的環(huán)境下才能得到較好的信號(hào)強(qiáng) 度和定位信息，當(dāng)信號(hào)質(zhì)量較差或者位于

2、室內(nèi)導(dǎo)致無(wú)法定位的時(shí)候，系統(tǒng)則需要由慣性元件來(lái)短時(shí)間抑制和修正。 GNSS勺高度信息通常誤差較大，這時(shí)候可 以通過(guò)超聲波傳感器和氣壓傳感器進(jìn)行一定程度的修正。組合導(dǎo)航技術(shù)中傳感器互補(bǔ)的思想原理是由克勞德-香農(nóng)在1948年的信息論中提出。信息論的線性估計(jì)理論讓數(shù)字處理工程師在系統(tǒng)設(shè)計(jì)時(shí)能夠把多個(gè)具有誤差的傳感器通過(guò)數(shù)學(xué)方程組合矩陣運(yùn)算融合起來(lái)，利用傳感器信息估計(jì)特定的狀態(tài)量，并且理論上提出了更多的傳感器狀態(tài)信息參與互補(bǔ)運(yùn)算，就能 夠得到更好的狀態(tài)估計(jì)（更接近真值，并且方差更?。?。1.2 飛行器的坐標(biāo)系三維笛卡爾支教坐標(biāo)系? 三維笛卡爾直角坐標(biāo)系是指三條相互垂直的坐標(biāo)軸（正交）相交形成的坐標(biāo)系，其

3、中相交的點(diǎn)稱為原點(diǎn)O（ origin ）。? 每個(gè)軸都指向特定的方向，兩個(gè)不同的坐標(biāo)軸決定一個(gè)平面，也叫笛卡 爾平面。? 通常 3 個(gè)軸采用右手系標(biāo)定，即右手的拇指方向指向X 軸的正方向，食指則指向Y軸方向，中指指向的是Z軸方向。? 此外對(duì)于圍繞每個(gè)軸的正旋轉(zhuǎn)方向，則是用右手大拇指對(duì)應(yīng)每個(gè)軸的正方向，則其他四指自然彎曲的方向?yàn)閲@該軸旋轉(zhuǎn)的正方向。本文所介紹的軟件和算法均采用右手定則剛體剛體（Rigid Body ）的意思是指物體在任何外力的作用下，它的體積和形 狀都不會(huì)發(fā)生改變。這是力學(xué)范疇的一個(gè)理想抽象模型概念，實(shí)際情況任何物 體在受外力作用的時(shí)候都會(huì)或多或少的發(fā)生形變，并不存在真正意義上

4、的剛體。 但如果形變本身相對(duì)于物體的運(yùn)動(dòng)微乎其微，可以忽略，或者并不影響我們分 析物體的運(yùn)動(dòng)結(jié)果，則可以在特定的情況下有條件的忽略這種形變，即將物體 假設(shè)成剛體。其中質(zhì)心運(yùn)動(dòng)的分析范疇屬于牛頓第二定律定義的質(zhì)點(diǎn)運(yùn)動(dòng)與受力之間的 關(guān)系，即主要研究飛行器質(zhì)心的位置和速度信息。而轉(zhuǎn)動(dòng)的建模方程則屬于歐 拉方程所分析的模型運(yùn)動(dòng)關(guān)系，即主要研究飛行器的姿態(tài)信息。要研究以上兩種運(yùn)動(dòng)的模型和信息，需要定義兩個(gè)三維笛卡爾直角坐標(biāo)系:大地坐標(biāo)系-（或慣性坐標(biāo)系，用e或者G表示）用于研究飛行器相對(duì)大 地的運(yùn)動(dòng)狀態(tài)以及空間位置坐標(biāo)。通常飛控都是把地面起飛點(diǎn)選取為該坐標(biāo)系 的坐標(biāo)原點(diǎn)，即初始化x,y,z坐標(biāo)為0o通常

5、大地坐標(biāo)系的X-Y-Z方向選取為 大地的北-東-地。機(jī)體坐標(biāo)系-（用b或B表示）坐標(biāo)原點(diǎn)取機(jī)體的重心,用于研究飛行器 相對(duì)于重心的旋轉(zhuǎn)運(yùn)動(dòng)。通常機(jī)體坐標(biāo)系的X-Y-Z方向選取為機(jī)身的前-右-下。機(jī)體坐標(biāo)系是固定在機(jī)體上隨著機(jī)體的運(yùn)動(dòng)而運(yùn)動(dòng)。1.3方向余弦矩陣和歐拉角63.1方向余雄陣及啊般翻住個(gè)靴沃1就際蜥觸耳對(duì)刊地嘛Me,三恤胞fi嬋瞅1邠提,業(yè)也嗎于機(jī)體金標(biāo)系b,三個(gè)軸對(duì)應(yīng)的單位矢1分別是|i|cos（A（川。呵匕01心。舊）CO$(/.i) cos(/，0 cowKi)co$(/J)co$U/)co$(K./)CQSil.k) cosy.*) cos(K, k)瞰蝸拗胭嬋岐1瓢酸理樹(shù)如t

6、,帆醞成|i|/|co$(/J), m,i)=川 IMcos(K“三融融脆-這僥獻(xiàn)Ml體坐屆收劇嘛歌班嫣標(biāo)63.1方向余蜩大地坐赧的3郵位矢也可以初體坐標(biāo)系沖標(biāo)出來(lái),MHcoXU) cosi/i) cw(L力 co«/J) co$(/,k) cosfj.k)cos(Ki) CO5(K,/) cos(KJt)I i Ji Ki K*= l j I I K I Ik Ik K k這兩使陣定義了從Y坐標(biāo)系的靴廨螺到另-世標(biāo)系的轉(zhuǎn)換法:,或都用防式.這兩餞 陣即稱為方向余魏陣（Direction Cosine Matrix）632姿態(tài)與歐拉角利用艙公式來(lái)研究洲WB統(tǒng)質(zhì)情鬧的時(shí)悅主即究的對(duì)象就

7、題體的戢.無(wú)人機(jī)fi啜杰（或者翻角）指的是飛行黜端仙 椒/!詞的你見(jiàn)是用瀛斯黝固連機(jī)體坐標(biāo)系與大地幽新系之間的角位1版飛謂需要了解這個(gè)當(dāng)前的姿態(tài)角位1關(guān)系, 從而蝴鹿預(yù)娜搦惻睞計(jì)Utt®繳蒯L從而實(shí)觀一定蹴作.對(duì)于三堆空間里的Y豺系，任碉體摘披魏頤用三個(gè)鼬角果標(biāo),鹿的射系是mt不動(dòng)的，即前酬謎的大嬲歌 顆耐腕挫頒囑翻懶喻鐮,四確粉刷蒯陋ima岫詢機(jī)偏航角1.3.2姿態(tài)與歐拉角在定義歐拉角的時(shí)候，相同的姿態(tài)不同的取法會(huì)得到不同的歐拉角。本書(shū) 采用的取法順序?yàn)閆-Y-X,即繞三個(gè)軸的旋轉(zhuǎn)順序是先繞 Z軸，再繞Y軸，再 繞X軸，最終達(dá)到一定的姿態(tài)。而對(duì)于同樣的姿態(tài)，如果繞軸的順序不同，則得

8、到的三個(gè)歐拉角也不相同。 因此按Z-Y-X的定義進(jìn)一步來(lái)說(shuō)，本書(shū)所定義的歐拉角 舶角（觸）0 -機(jī)的水平面般也抬頭為正 犒航角（方位角耀仁f -機(jī)頭方向在他平面的嬲與林M軸加缺配北偏東為正 源角（雕角-搟顆體斕覷樹(shù)植姐 極為正姿態(tài)解算(attitude algorithm )也叫姿態(tài)分析或者姿態(tài)估計(jì)，即指飛控 系統(tǒng)或者慣導(dǎo)系統(tǒng)根據(jù)慣性測(cè)量單元(IMUU陀螺儀，加速度計(jì)，磁力計(jì)等傳感 器的實(shí)時(shí)信息求解飛行器的歐拉角的算法過(guò)程，因此也叫 IMU數(shù)據(jù)融合(IMU Data fusing )。633歐拉南的定軸轉(zhuǎn)動(dòng)表示矩陣物物一切中.瞋歐檢由的定此可中粗的分制就次性系的jmx三地旃轉(zhuǎn)也*.明爵到機(jī)體坐

9、標(biāo)率上但文三次腐輛的的第 為。兒幾兀力JT津津i.小兒匕& 0片九時(shí)】.第一次國(guó)境旋轉(zhuǎn)雹軸旋您5變成顯然此次旋轉(zhuǎn)新的坐標(biāo)系與老坐標(biāo)系之間的關(guān)系(投影)cos(W) sin(W)-sin(W) cos(W)00-cos(W) sin(W) 0庵=-sin(“)cos(W) 0-001第二次旋轉(zhuǎn)圍繞匕軸旋轉(zhuǎn)仇旋轉(zhuǎn)到OXzPzZz顯然此次旋轉(zhuǎn)新的坐標(biāo)系與老坐標(biāo)系之間的關(guān)系（投影）sin(6)0cos(0)匕zcos(8) 001sin(») 0Fcos(0) 0 sin(0)' 偌=010sin(J) 0 cos(6) 一第三次旋轉(zhuǎn)圍繞汗2軸旋轉(zhuǎn)巾，旋轉(zhuǎn)到OX/&Z

10、b顯然此次旋轉(zhuǎn)新的坐標(biāo)系與老坐標(biāo)系之間的關(guān)系（投影）1100Rg = 0 cos ） sing01 sin（p） cos（p）1d-疝間Q期陰雖然飛行器的姿態(tài)是隨機(jī)轉(zhuǎn)動(dòng)，而并不是依次沿三軸旋轉(zhuǎn)而成，我們?nèi)匀?可以依靠歐拉角的三次旋轉(zhuǎn)來(lái)還原出飛行器的姿態(tài)信息。而且如果旋轉(zhuǎn)的三軸 順序不同，得到的旋轉(zhuǎn)矩陣也不同，并且歐拉角也不相等，三次旋轉(zhuǎn)先后組合 在一起即可以形成從慣性大地坐標(biāo)系 e到機(jī)體坐標(biāo)系b的旋轉(zhuǎn)矩陣：1 Q 。眄。用"即牌=Q哪的$瞰）0 I。-$i曬 cm（啾-M*期仲）網(wǎng)0卜疝喇-疝劭sin®*如口）»o（f） - ch（旬州（昉 如仲）如砌+如仲）（財(cái)

11、同 珈剛 ）CO心”疝的* ODE砌+ 5X1（。卜疝響 酬（*）*酬砌疝砌網(wǎng)+詞的 期明*儂伯T11 r12 r13, 房觸阻哪魏隔D.R” %hl儂，裁沱喻旗硯儺IMMRI r31 r32 r33,巾=arctan p = - arcsin(r13)仲=arctan(rn/r33)方向余弦矩陣的幾大缺陷抑S7閥，會(huì)注直蝴歷向親密電螭蝴算眥t因?yàn)榕笥嘞覍儆迋€(gè)統(tǒng)取部個(gè)元爵腰酬漉新勺曲奇異點(diǎn)這眄庭間上萩為萬(wàn)向節(jié)滕佝曲皿叫隨域是當(dāng)三個(gè)雕中的中間這個(gè)雌達(dá)到641四元數(shù)的定義醞觸XI用于頓縊8 (及郵圖像分析)上標(biāo)E蝴的踞引他糊于另懿嘯 O,器誦，娥)，四元嬲靛魴面的優(yōu)虹 強(qiáng)更快，提供平滑雕、布姆免萬(wàn)

12、向蛹題、存網(wǎng)西畋小”.四無(wú)數(shù)(Quaternions)是由愛(ài)爾蘭數(shù)學(xué)家漢韻強(qiáng) (WilliamRowan Hamilton, 18052865)在1843年提出來(lái)的數(shù)學(xué)枇念它將復(fù)數(shù)所湎幽三鯉間拓展到間，可以郭蝗酸更睚斷他然也可以以腳助.強(qiáng)尊捌螃!, < 聘運(yùn)算醞fi主要包含四個(gè)元索qoqqq，其中%均為蝮四元數(shù)的表達(dá)網(wǎng):Q(flo-?r?2»?3)= % + ?ii + + ?3fc四元數(shù)艇算題J:i*i = j*j = k*k = -li*j = k,j*k = t,k*i = jy*i = -k,k*/ = -i,i*k = -j6.4.1四元數(shù)的定義醞州眄OMf”.典也是

13、o3ToMUk到3B,財(cái)哂崢*酚，任中勿易的幾何廉義可以3瞟成為一種直轉(zhuǎn).而四方由懶義為Qy,q 旗陣形式可以寫(xiě)為Q =此外定義四旅的+ q/+q/ + qj. aftCHta四元,這表示單位四元數(shù).四酒的逆QT = Lt對(duì)于單位四元數(shù)Q-l = Q運(yùn)n為相應(yīng)部位的加派，例如假設(shè)Q = q0+qj+qj+q3kP = P0+Pj WP31fQ±P = (%±Po)+(Qt ± P” + (/ 土 P2)J + 83 土 p3 M而p Q =(Po+p j+p j+/)&”+q?" q同=OVb - PNi - PW J +(Po% + PN( +

14、 Pz/ -23奧)'(Pi/h +%q。+PW| -%)/ +(Po +PWo +P1% -P2*)k= r0 + rj r“ r5t641四元數(shù)的我研形確以=0 1 I J q q q q. 3 1 -P%; 2 3-P-P&A pl詼巴-P2 1 4 p p p p>=m 八 nlnI=Q p或者IB 4 1J p p p p 3 CS3 0 1 2 3 H q q qb642四元數(shù)與旋轉(zhuǎn)的關(guān)系四元我是可以描述三間中腑的旋轉(zhuǎn)醫(yī)中三維空間(R3)中削咻上的任意一點(diǎn)Pk以)點(diǎn)要圍境通過(guò)原點(diǎn)的矢用抽y旋舞帷度,轉(zhuǎn)期(匕)點(diǎn)上.將P點(diǎn)用齊次坐標(biāo)來(lái)表示為醞 數(shù):®M

15、.定義Q為任意單位四元數(shù)，則(月"“6/2)o5(8/2)代表的旋轉(zhuǎn)可以將喋期%y/M5w.首先，對(duì)于單位四元數(shù)Q如釉行標(biāo)乘法的話，并不影響旋轉(zhuǎn)本身.圍境Q與$00為標(biāo)卻所對(duì)應(yīng)的旋轉(zhuǎn)是一樣的，顯然(sQ)P(sQ)XQPQ«4s 1QAQ :=QPQ :=QPQ耿,任何四元數(shù)的實(shí)數(shù)郃分可以寫(xiě)成% = (Q + Q.)/2,因此QPQ,的標(biāo)實(shí)酬(QPQ，)="二等匚=也誓2二絲產(chǎn)=QR(P)Q=R(P).因此對(duì)于四元數(shù)P二p，M的實(shí)部在及過(guò)旋轉(zhuǎn)Q后不會(huì)改變，此外旋轉(zhuǎn)本身不會(huì)改變虛部矢部分的大小，因此|P| = |P'|.642四元數(shù)與旋轉(zhuǎn)的關(guān)系旋轉(zhuǎn),為事直嚙

16、與，點(diǎn)的共面目垂直于旋轉(zhuǎn)軸V.定義砒四元數(shù)唯反量部分底旋相畫(huà)上的投影矢曲0是四元即的矢量部分喈旋鼾面I影燈L 蝴當(dāng)我量圍頗電唱專渝度到2 謹(jǐn)餞設(shè)yl是該碓統(tǒng)姮能言獻(xiàn)1.譴歸蛛量均為單位 #矢艮這洋魏vOvl = es1由于應(yīng)喻垂直于旋轉(zhuǎn)投影面.低特柏0位矢量為v =窗孑現(xiàn)在我們定義用于描述該底喻四元BlQ = vl Z|EV1|vOa = (vOx vl,vO vl| = |vsuij.cos此外令v2 = QvOQ只要證明與“的夾角是JW裊用Oft從到，的俺裴四元ft ”到以的旋轉(zhuǎn)EjcRvZ vl, =QvO(?r = (vl vOJ>r = vOvO(vrvr),由于單25dtvO

17、vO = T, vlvl= -1,因itv2*v=Q：卜叫叫M，vO二這a聊2與八的相椒匕與幽平面重合，并映角醐,均筆 網(wǎng)以與幽夾角為e.酬3雅義的v2 = (MQ ,即是P知;傳平面上的投影，是VCS過(guò)Q所通述的貨轉(zhuǎn)四元數(shù)旋轉(zhuǎn)后的矢！(vO X vl)II VO X vlj|既然四元數(shù)可用來(lái)描述旋轉(zhuǎn),那么就可以用它來(lái)描述從大地坐標(biāo)系e到機(jī)體坐標(biāo)系b之間的轉(zhuǎn)換，也就是用它來(lái)描述6.3.1所介紹的方向余龍炬陣.在參考文獻(xiàn)2臬永元的慣性導(dǎo)航）一文中922節(jié)給出了四元數(shù)與方向余弦矩陣的對(duì)應(yīng)關(guān)系和詳細(xì)的推導(dǎo)過(guò)也這里簿要的給出一下結(jié)論機(jī)體以卬=3J+叼+恤A,旋轉(zhuǎn)運(yùn)動(dòng).從QA/到（MW; u為垂直于臼的

18、單位眼時(shí)軸.自小二m經(jīng)運(yùn)算可胤r，= （/ + UsE& +而嗎UU）d = DJ,從而得到網(wǎng)為機(jī)體坐標(biāo)系b到大地坐標(biāo)系的方向余弦矩陣,即:ggg=/+ l/2sin-cos- + 2sin*-47y2220 -nsln-msin j,， i0-(m2 +rr)sinx-0 1 0 + 2cos-0 Rsin-e-msin j/msinInsin/msin2 y-(f2 + n2)sin2 j* mnsm -Insin -一mnsm -» a 2°-(m2 + r)sin2 -RBUn n 0燈醐 勘Y叫 ybi* q產(chǎn)而叫 %=對(duì)Q 二 “ + 仇.。*.外心=3

19、；.(%.町.+ 成0)疝-二61. /血； 代A幅用會(huì)費(fèi)鉗瑞城+q”4一諳2值曲-命%)2®%+q。4)=28兇2+用3)2%-研1)2(如也-qoq2) 2(和外 + QoQi) % -d-d + 4醞數(shù)與懶劃砌應(yīng)關(guān)系,因此采用醞期飛行圖姿態(tài)進(jìn)行估計(jì)，值需要每個(gè)時(shí)鐘采樣硼腌環(huán)迭代運(yùn)H出新的四元 數(shù)為444,就可以獲得方向余弦矩陣，從祓得實(shí)時(shí)的歐拉角信義如何在每個(gè)時(shí)鐘采樣周期通過(guò)新的傳感器信息送代出新的四元教信息呢？這就是采用K格庫(kù)塔法對(duì)四元嬲行粉運(yùn)鼠由于匹元數(shù)Q = cos g + (如 + m/0 +*)sin % cos ； + u( sin 鵬期邊進(jìn)行酚可以得到dQdti

20、9 de ,1 e de due e-sin - - + ue - - cos - - + sin 22dt22dtdt21/06= -6)e-l cos-4- ue-sin-j乙乙乙/11h= Q = 3Q 3b 乙乙1 , 、=5 (qO + qli + q2j + q3k) (a)xi + a)yj 4- a)zk)=。匕 Q 乙Jt'p因此采用一階龍格庫(kù)塔法,q(t + r)= q+ T *=q(E)+ 孫。Q展開(kāi)后可以得到四元數(shù)每部分的更新(一山彳91 R尸 3蕓93)丁2(3,9。一 qa + 破)丁2(q3 + a)yqa -/qj T03= 93 +卡爾曼濾波卡爾曼濾波

21、是一種系統(tǒng)狀態(tài)觀測(cè)估計(jì)器，用來(lái)描述一個(gè)系統(tǒng)的輸入、輸出 和內(nèi)部遞歸迭代及重構(gòu)的方式?？柭鼮V波作為一個(gè)估計(jì)算法，主要基于系統(tǒng) 狀態(tài)空間的方法，因此也屬于第七章所談到的現(xiàn)代控制理論?？柭鼮V波使用 系統(tǒng)模型的迭代方程(或者微分方程)計(jì)算出理論輸出模型在于實(shí)際觀測(cè)相比 較，乘上比例因子形成狀態(tài)反饋，將系統(tǒng)狀態(tài)重構(gòu)偏差迭代消除，濾波器不但 對(duì)系統(tǒng)進(jìn)行模型抽象，同時(shí)建立量測(cè)噪聲和系統(tǒng)噪聲模型，并通過(guò)測(cè)量與理論 的比較結(jié)果估計(jì)出噪聲能量?？柭鼮V波器是一種遞推線性最小方差估計(jì)的算法。對(duì)于解決大部分的問(wèn) 題，是最優(yōu)，效率最高，也是最有用的?？柭鼮V波的廣泛應(yīng)用超過(guò)30年，包括機(jī)器人控制、導(dǎo)航，傳感器數(shù)據(jù)融

22、合，信號(hào)處理，計(jì)算機(jī)圖像處理等等。它 的特點(diǎn)主要是 無(wú)儲(chǔ)性EG)二心 即系統(tǒng)狀態(tài)的怙計(jì)值的數(shù)學(xué)期望等真徒 估計(jì)方差。(£) =以尸)一雙為工最小，即任何一個(gè)其他的估計(jì).所對(duì)應(yīng)的 方差都大于濾波器的估計(jì)值的方差 實(shí)時(shí)性，采用遞推方式，減小運(yùn)尊量，實(shí)時(shí)輸出運(yùn)克濾波結(jié)果卡爾曼濾波公式:系統(tǒng)狀態(tài)方程.X# =巾*#7*4 +限-1 + Wr-iWjcT觀測(cè)公式：入="*見(jiàn)+/燈上7為索線狀態(tài)空間轉(zhuǎn)尊矩陣.為上時(shí)刻的凝統(tǒng)輸入量*為索統(tǒng)哄自嘉ft力系統(tǒng)啜聲，乩內(nèi)說(shuō)刈翅陣，也為堀網(wǎng)噬聲-索統(tǒng)噪聲與羯嚏聲的統(tǒng)計(jì)將性為高斯白喉聲.= G,且些聲方壁姐 降為系統(tǒng)喚聲節(jié)差短庫(kù)： 丸 =QAi =

23、汽，測(cè)量嚶聲方差矩陣：/ =網(wǎng)品產(chǎn)離散卡爾曼濾波器的目的就是要通過(guò)系統(tǒng)上一時(shí)刻的狀態(tài)和輸入，以及當(dāng) 前時(shí)刻的觀測(cè)來(lái)估計(jì)出當(dāng)前時(shí)刻的狀態(tài)，并且它是最小方差的無(wú)偏估計(jì)。對(duì)于 線性系統(tǒng)來(lái)說(shuō)，它也是這幾部分的線性疊加，即式父 = K潑kfk-i + K向t + Kk uk-i 杵曼源波式中曲幽,巾4崛魏獨(dú)騎趟趟醐酎蝴1帆蛔B.跚嗨茄城1加田I.下夙aii端晶地蟠tX.小,社局切械章空肥計(jì).掰工憫的倒耨靛導(dǎo)出三個(gè)孤蜥”.人 更嘉晟九戰(zhàn)觸捺比幽Mb 卸3 -“ = %An =加-加=穴.*-1麻=| + %-1 +- 6L-1|*-L & %(心卜“內(nèi)T + Mt-1 + 口上-例1k-J + “

24、) -=隗上7 -此為中土b1 - £；%T + K；依=1 -八=斤1)+ Q -即%)%.1嗨=1 + 口 -兒加-端Uhl - &嶺要求E(品，二0.EPE/G =*N*Th J- Mi 凡H也上T -3F7)+人聞h.滴帆t) +(»凡瓦- (；)的-)卜口因?yàn)闀r(shí)111)=。. EWi);。，E(Vjt) = 0 且£(*1)¥0, Ed-1)中 o所助須足除卜1 -及必軋山7 - K =。設(shè)二外/T 一&此,心】.ft即f -KkHk-Kk = 0K； = 1- KkHk 將式口泗代同式中.得到均上二(外/一1 一 K春見(jiàn)/慮_

25、)鼠_情_(kāi)i + KkZk +。一 K/Quri=巾*品-1他-1 + *k 體1 %他/一國(guó)-ill + uk-l) + ufc-l= fc|fc-l +雁f -之桃7)(式子4 )其中幾41 =中門(mén)-1心71 +見(jiàn).它農(nóng)小的占山日葉制的系統(tǒng)狀態(tài)例通過(guò)條統(tǒng)建華來(lái)的汁3的勺前時(shí)刻向狀 志化iHk %1 = &輸1，它仍油當(dāng)前時(shí)刻估計(jì)值腦過(guò)觀瀾鹿也矩陣估計(jì)出來(lái)當(dāng)前時(shí)刻的雙窩量，它號(hào)力 到實(shí)際也割哂之間的差即為當(dāng)前時(shí)見(jiàn)的晚洲浙皂系統(tǒng)狀態(tài)估計(jì)言預(yù)測(cè)狀態(tài)+卡爾曼濾波系數(shù)*觀測(cè)新息這個(gè)卡爾曼澹被系數(shù)均是年次法代根指系統(tǒng)咕計(jì)和煤由佑計(jì)動(dòng)態(tài)調(diào)整.根心堪中 T計(jì)算它，使得岫*是打法的最小方趨估計(jì)，即使將先席的方差矩陣生夬的值最小將式（式子4） ftUu中.得到:=£埠即丫廣備（小后十 口廣冊(cè)/* J=U- 4見(jiàn)&* -%或市一 i - v=i-小用 乂上中一 %n j -=（，一即跖）%tT - & ,為了計(jì)?？诒痊F(xiàn)幾出的方氈飾/加的