一種基于范數(shù)約束容積卡爾曼濾波的飛行器姿態(tài)估計(jì)方法
【專利摘要】本發(fā)明公開了一種基于范數(shù)約束容積卡爾曼濾波的飛行器姿態(tài)估計(jì)方法，步驟如下：建立飛行器導(dǎo)航系統(tǒng)的狀態(tài)方程；建立飛行器導(dǎo)航系統(tǒng)的非線性量測(cè)方程；離散化狀態(tài)方程和量測(cè)方程；對(duì)離散后的狀態(tài)方程和量測(cè)方程采用范數(shù)約束容積卡爾曼濾波算法，輸出飛行器的姿態(tài)。本發(fā)明所采用的范數(shù)約束容積卡爾曼濾波，將描述飛行器姿態(tài)的四元數(shù)的范數(shù)約束引入了姿態(tài)估計(jì)中，有效地解決了姿態(tài)估計(jì)過(guò)程中不考慮范數(shù)約束的協(xié)方差奇異問(wèn)題，僅增加了少許計(jì)算量，有效地修正了四元數(shù)的范數(shù)約束條件對(duì)飛行器姿態(tài)估計(jì)的影響，提高了姿態(tài)估計(jì)精度，進(jìn)而提高了飛行器的導(dǎo)航精度，增強(qiáng)了導(dǎo)航過(guò)程的穩(wěn)定性。
【專利說(shuō)明】
一種基于范數(shù)約束容積卡爾曼濾波的飛行器姿態(tài)估計(jì)方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于非線性約束狀態(tài)估計(jì)和姿態(tài)估計(jì)的技術(shù)領(lǐng)域，具體涉及一種基于范數(shù) 約束容積卡爾曼濾波的飛行器姿態(tài)估計(jì)方法，可用于提高飛行器姿態(tài)估計(jì)的精度。
【背景技術(shù)】
[0002] 眾所周知，卡爾曼濾波得到的是線性隨機(jī)估計(jì)問(wèn)題的無(wú)約束最優(yōu)解。但是，當(dāng)狀態(tài) 變量或者部分狀態(tài)變量滿足一定的約束時(shí)，卡爾曼濾波得到的估計(jì)結(jié)果就不一定是最優(yōu) 的。研究狀態(tài)具有約束的情況下卡爾曼濾波的估計(jì)問(wèn)題是必須要解決的難題，近年來(lái)越來(lái) 越引起學(xué)者們的關(guān)注。
[0003] 姿態(tài)估計(jì)是對(duì)飛行器導(dǎo)航和控制的一項(xiàng)關(guān)鍵技術(shù)，也是一種典型的非線性濾波問(wèn) 題。由于大多數(shù)飛行器的姿態(tài)用四元數(shù)進(jìn)行描述，因此解決具有四元數(shù)描述的姿態(tài)非線性 估計(jì)問(wèn)題是一個(gè)熱點(diǎn)問(wèn)題。用四元數(shù)描述飛行器的姿態(tài)時(shí)，四元數(shù)變量具有范數(shù)約束，即四 元數(shù)四個(gè)變量的平方和的范數(shù)為一，若不考慮此范數(shù)約束條件進(jìn)行非線性濾波，得到的姿 態(tài)估計(jì)的精度較差，甚至?xí)?dǎo)致協(xié)方差奇異問(wèn)題。經(jīng)典的擴(kuò)展卡爾曼濾波方法通過(guò)時(shí)間更 新和量測(cè)更新對(duì)狀態(tài)進(jìn)行估計(jì)，但擴(kuò)展卡爾曼濾波方法是利用一階線性近似非線性狀態(tài)方 程和非線性量測(cè)方程，因此濾波過(guò)程中會(huì)產(chǎn)生較大的誤差，特別是當(dāng)模型不精確的時(shí)候，甚 至?xí)?dǎo)致濾波系統(tǒng)發(fā)散。近年來(lái)，Arasaratnam提出了一種容積卡爾曼濾波，利用一組等權(quán) 值容積點(diǎn)解決貝葉斯濾波積分方程，從而來(lái)傳播系統(tǒng)狀態(tài)的均值和方差，具有較高的濾波 精度（Arasaratnam I. ,Haykin S. Cubature Kalman Filters .Automatic Control, IEEE Transactions on,2009,54(6): 1254-1269.)。容積卡爾曼濾波提供了一種新的實(shí)現(xiàn)方式用 以解決非線性估計(jì)問(wèn)題?？紤]到范數(shù)約束問(wèn)題，本發(fā)明將范數(shù)約束引入到容積卡爾曼濾波 中，提出具有范數(shù)約束的容積卡爾曼濾波方法，用以提高飛行器的姿態(tài)估計(jì)精度。

【發(fā)明內(nèi)容】

[0004] 為了改進(jìn)現(xiàn)有的飛行器姿態(tài)估計(jì)受范數(shù)約束的影響精度較低的問(wèn)題，本發(fā)明提出 了一種基于范數(shù)約束容積卡爾曼濾波的飛行器姿態(tài)估計(jì)方法，將范數(shù)約束引入到容積卡爾 曼濾波中，并將其應(yīng)用到飛行器的姿態(tài)估計(jì)問(wèn)題中，提高了飛行器的姿態(tài)估計(jì)精度。
[0005] 為了達(dá)到上述目的，本發(fā)明的技術(shù)方案是:一種范數(shù)約束容積卡爾曼濾波的飛行 器姿態(tài)估計(jì)方法，其步驟如下：
[0006] 步驟一、建立飛行器導(dǎo)航系統(tǒng)的非線性狀態(tài)方程
[0007] 在目標(biāo)坐標(biāo)系下，利用慣性測(cè)量單元提供的飛行器的加速度和角速度建立飛行器 的動(dòng)力學(xué)模型，其狀態(tài)方程為
[0011]其中，r= []^，1>山]1'和￥=[1，％，^]1'分別是飛行器的位置和速度，乂、丫、2是目標(biāo) 系的坐標(biāo)軸;q=[qo qi q2 93]?姿態(tài)四元數(shù);a=[ax，ay，az]T和 ω=[ωχ，ωγ，ωζ]%1?^ 陀螺的加速度和角速度;g是重力加速度;(6=(cfr1是從體坐標(biāo)系到目標(biāo)坐標(biāo)系的系數(shù)轉(zhuǎn)換 矩陣，系數(shù)轉(zhuǎn)換矩陣G為：
[0012]
(2)
[0013] 四元數(shù)矩陣Ω(ω)為：
[0014]
())
[0015]狀態(tài)向量為義=[1￥，1^々《4]7，則其狀態(tài)方程為
[0016]
(4)
[0017] 其中，S和?分別為加速度計(jì)和陀螺儀的輸出向量，a和ω分別為陀螺真實(shí)的加速 度和角速度，bjPbu分別為加速度的偏差和角速度的偏差，ξ^Ρξω分別為加速度和角速度 的零均值高斯白噪聲，η4Ρηω分別為加速度和角速度的漂移偏差的高斯白噪聲，系數(shù)矩陣 %為
[0018]
[0019] 步驟二:建立飛行器導(dǎo)航系統(tǒng)的非線性量測(cè)方程
[0020] 將慣性測(cè)量單元的速度和加速度，測(cè)距測(cè)速儀測(cè)得的高度和三軸速度和兩個(gè)導(dǎo)航 信標(biāo)的距離作為量測(cè)值引入到量測(cè)模型，則量測(cè)方程為：
[0021] z = [rz vc Ri R2]T+G = h(x)+G (6)
[0022] 其中，
[0023]
[0024]
[0025] 其中，Ri表示飛行器與導(dǎo)航信標(biāo)的距離，η是信標(biāo)的位置坐標(biāo)，（^是測(cè)距的噪聲， ζ?Μ為零均值高斯白噪聲；
[0026] 步驟三:離散化狀態(tài)方程和量測(cè)方程
[0027] 將狀態(tài)方程和量測(cè)方程進(jìn)行離散化，得到離散的飛行器的狀態(tài)方程和量測(cè)方程：
[0028] xk = f (xk-i)+wk-i (9)
[0029] zk = h(xk)+ik (10)
[0030] 其中，k時(shí)刻的狀態(tài)變量Xk具有范數(shù)約束|七『=λ，wk和ζι<是相互獨(dú)立的零均 值高斯白噪聲序列，且的方差分別為Qk和Rk，且滿足
[0031]
ill)
[0032] 其中，5kj為Kronecker3函數(shù)，當(dāng)k = j時(shí)，5kj = 1;當(dāng)k辛 j時(shí)，5kj = 0;
[0033] 步驟四：將離散后狀態(tài)方程和量測(cè)方程采用范數(shù)約束容積卡爾曼濾波，輸出飛行 器的姿態(tài)
[0034] 1.對(duì)離散的狀態(tài)方程和量測(cè)方程的狀態(tài)和誤差方差陣分別進(jìn)行初始化：
[0035]
[0036]
[0037] 2.設(shè)第k-Ι步tk-i時(shí)刻的狀態(tài)估計(jì)值和誤差方差陣分別為尤則第k步tk時(shí)刻 的容積點(diǎn)為：
[0038]
(14)
[0039] 其中，上標(biāo)"+"表示其后驗(yàn)估計(jì)，^為誤差方差距陣/^的平方根，滿足
j為2n個(gè)容積點(diǎn)的第j個(gè)值，其取值集合為
[0040] 75? {，e2，…。,-巧,-? A}. (15)
[0041] 其中，ej(i = l,2,··· ,η)為單位向量，其第j個(gè)值為1，其余元素均為零，即ej = [0,…，0，1，0，···，0]τ;
[0042] 經(jīng)過(guò)非線性函數(shù)傳遞后的容積點(diǎn)為；
[0043] χ；Λ = ), 7 = 12, · -, 2n (1.6)
[0044] 則其相應(yīng)的先驗(yàn)狀態(tài)估計(jì)和方差分別為
[0045]
[0046]
[0047] 式中，上標(biāo)"一"表示變量的先驗(yàn)估計(jì)；
[0048] 3.進(jìn)行量測(cè)方程的更新:首先，由先驗(yàn)狀態(tài)估計(jì)盡和方差茗重新計(jì)算容積點(diǎn)：
[0049]
(19)
[0050] 然后，計(jì)算經(jīng)過(guò)非線性量測(cè)函數(shù)傳遞的容積點(diǎn)位：
[0051 ]
(20)
[0052] 其次，計(jì)算量測(cè)先驗(yàn)估計(jì)值及先驗(yàn)估計(jì)值的方差、先驗(yàn)估計(jì)值協(xié)方差：
[0053] (21)
[0054] (22)
[0055](23) /=.1.
[0056] 4.計(jì)算范數(shù)約束的卡爾曼增益:首先，計(jì)算無(wú)約束的卡爾曼增益炙：
[0057]
(24>
[0058]和無(wú)約束的最優(yōu)狀態(tài)估計(jì)值
[0059]
[0060]
[0061 ] 1 、一
[0062]然后，計(jì)算具有范數(shù)約束的卡爾曼增益Kk:
[0063]
[0064]
[0065]
[0066]
[0067] 5.計(jì)算狀態(tài)估計(jì)值和相應(yīng)的方差陣:基于范數(shù)約束的后驗(yàn)狀態(tài)估計(jì)值為
[0068]
[0069]
[0070]
[0071]
[0072]
[0073]以上4步循環(huán)迭代，得到飛行器的實(shí)時(shí)狀態(tài)估計(jì)值，包含探測(cè)器的位置、速度和姿 ??τ 〇
[0074]所述步驟一中的目標(biāo)坐標(biāo)系是指以預(yù)定著陸點(diǎn)為原點(diǎn)，X軸指向東，y軸指向北，ζ 軸指向天;體坐標(biāo)系是指以飛行器的重心為原點(diǎn)，0X軸位于飛行器參考平面內(nèi)平行于飛行 器軸線并指向飛行器前方，0Y軸垂直于飛行器參考面并指向飛行器右方，0Z軸在參考面內(nèi) 垂直于Χ0Υ平面，并指向飛行器下方。
[0075]所述步驟一中姿態(tài)四元數(shù)的向量q=[qo qi q2 范數(shù)是q中各個(gè)元素平方之 和再開根號(hào)
亂滿足范數(shù)約束I |q| |2=1。
[0076]本發(fā)明的有益效果：與標(biāo)準(zhǔn)的容積卡爾曼濾波相比，本發(fā)明所采用的范數(shù)約束容 積卡爾曼濾波，將描述飛行器姿態(tài)的四元數(shù)的范數(shù)約束引入了姿態(tài)估計(jì)中，有效地解決了 姿態(tài)估計(jì)過(guò)程中不考慮范數(shù)約束的協(xié)方差奇異問(wèn)題，僅增加了少許計(jì)算量，有效地修正了 四元數(shù)的范數(shù)約束條件對(duì)飛行器姿態(tài)估計(jì)的影響，提高了姿態(tài)估計(jì)精度，進(jìn)而提高了飛行 器的導(dǎo)航精度，增強(qiáng)了導(dǎo)航過(guò)程的穩(wěn)定性。
【附圖說(shuō)明】
[0077]圖1為本發(fā)明的流程圖。
[0078]圖2為本發(fā)明中的范數(shù)約束容積卡爾曼濾波的原理圖。
【具體實(shí)施方式】
[0079] 下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明作詳細(xì)的說(shuō)明。
[0080] -種基于范數(shù)約束容積卡爾曼濾波的飛行器姿態(tài)估計(jì)方法，其計(jì)算流程圖如圖1 所示，數(shù)約束容積卡爾曼濾波算法流程圖如圖2所示，它包括以下四個(gè)步驟：
[0081] 步驟一:建立飛行器導(dǎo)航系統(tǒng)的狀態(tài)方程
[0082] 在目標(biāo)坐標(biāo)系下，利用慣性測(cè)量單元提供的飛行器的加速度和角速度，建立飛行 器的動(dòng)力學(xué)模型，即狀態(tài)方程為：
[0083]
[0084]
[0085]
[0086] 其中，r= [rx，ry，rz]T和￥=[￥\,￥，，￥ 2]1'分別是飛行器的位置和速度，乂、￥、2是目標(biāo) 系的坐標(biāo)軸;q=[qo qi q2 q3]T是姿態(tài)四元數(shù)，姿態(tài)四元數(shù)向量q的2范數(shù)是其中各個(gè)元素平 方之和再開根號(hào)，S
.且滿足范數(shù)約束I |q| |2 = l;a=[ax，ay，az]^ ω = [ ωχ, ωγ, ωζ]τ分別是陀螺的加速度和角速度；g是重力加速度，其值為8=[0,0，-3.69] T 是從體坐標(biāo)系到目標(biāo)坐標(biāo)系的系數(shù)轉(zhuǎn)換矩陣，系數(shù)矩陣C丨為：
[0087]
[0088]四元數(shù)矩陣Ω(ω)定義為
[0089]
[0090] 狀態(tài)向量取為1=匕^，1^，1^，(1」|，則相應(yīng)的狀態(tài)方程為
[0091]
[0092] 上瓦甲，S和》甘別73那迷皮訂和叱螺儀犧出的那迷皮和用迷度，a和ω分別為真 實(shí)的加速度和角速度，bjPbu分別為加速度偏差和角速度偏差，ξ^Ρξω分別為加速度和角速 度的零均值高斯白噪聲，n a和η ω分別為加速度和角速度的漂移偏差的高斯白噪聲，系數(shù)矩 陣％定義為
[0093]
[0094] 其中，目標(biāo)坐標(biāo)系是指以預(yù)定著陸點(diǎn)為原點(diǎn)，x軸指向東，y軸指向北，z軸指向天； 體坐標(biāo)系是指以飛行器的重心為原點(diǎn)，0X軸位于飛行器參考平面內(nèi)平行于飛行器軸線并指 向飛行器前方，0Y軸垂直于飛行器參考面并指向飛行器右方，0Z軸在參考面內(nèi)垂直于Χ0Υ平 面，指向飛行器下方。
[0095] 步驟二:建立飛行器導(dǎo)航系統(tǒng)的非線性量測(cè)方程
[0096] 在建立量測(cè)模型時(shí)，考慮將慣性測(cè)量單元的速度和加速度、測(cè)距測(cè)速儀測(cè)得的高 度和三軸速度、兩個(gè)導(dǎo)航信標(biāo)的距離作為量測(cè)值引入到量測(cè)模型，則相應(yīng)的量測(cè)方程為
[0097] z = [rz vc Ri Κ2]τ+ζ = 1ι(χ)+ζ (6)
[0098] 式中，
[0099]
[0100]
[0101] 式中，Ri表示飛行器與導(dǎo)航信標(biāo)的距離，rb是信標(biāo)的位置坐標(biāo)，（^是測(cè)距的噪聲， ζ?Μ為零均值高斯白噪聲。
[0102] 步驟三:離散化狀態(tài)方程和量測(cè)方程
[0103] 將上述式(4)的狀態(tài)方程和量測(cè)方程進(jìn)行離散化，得到離散的飛行器的狀態(tài)方程 和量測(cè)方程：
[0104]
[0105] -________ 、--，
[0106] 式中，設(shè)此時(shí)狀態(tài)變量Xk具有范數(shù)約束||x,|f ，wl^Kk為獨(dú)立的零均值高斯 白噪聲序列。wk和Gk的方差分別為Qk和Rk，且滿足：
[0107]
(11)
[0108] 式中，為Kronecker3函數(shù)，即有當(dāng)k = j時(shí)，5kj = 1;當(dāng)k辛 j時(shí)，5kj = 0〇
[0109] 其中，在本發(fā)明中范數(shù)約束即為姿態(tài)四元數(shù)的約束：I |q| |2 = 1，也就是說(shuō)上述算 法中的A =1 a
[0110] 步驟四：將離散后的狀態(tài)方程和量測(cè)方程采用范數(shù)約束容積卡爾曼濾波，輸出飛 行器的姿態(tài)
[0111] 步驟三中的離散化的式(9)狀態(tài)方程和式（10)量測(cè)方程，考慮其狀態(tài)滿足一定的 范數(shù)約束，將其引入到容積卡爾曼濾波算法中，形成新的范數(shù)約束容積卡爾曼濾波算法。本 發(fā)明的范數(shù)約束容積卡爾曼濾波算法的原理圖參見圖2,實(shí)現(xiàn)步驟為：
[0112] 1.對(duì)離散的狀態(tài)方程和量測(cè)方程的狀態(tài)和誤差方差陣進(jìn)行初始化:賦予狀態(tài)和 誤差方差陣初值
[0113]
[0114]
[0115] 2.設(shè)第k-1步即時(shí)刻的狀態(tài)估計(jì)值和誤差方差陣分別為A和◎，在此基礎(chǔ)之 上生成第k步即tk時(shí)刻的容積點(diǎn)
[0116]
(14)
[0117] 式中，"+"表示后驗(yàn)估計(jì)為方差匕的平方根，即滿足為 2n個(gè)容積點(diǎn)的第j個(gè)值，其取值集合為
[0118]
(15)
[0119] 其中，ej(i = 1，2，…，n)為單位向量，其第j個(gè)值為1，其余元素均為零，即ej = [0,…，0，1，0，···，0]τ。
[0120] 經(jīng)過(guò)非線性函數(shù)傳遞后的容積點(diǎn)分別為
[0121]
(16)
[0122] 則相應(yīng)的狀態(tài)和誤差方差陣的先驗(yàn)狀態(tài)估計(jì)和方差分別為
[0123]
[0124] -·* 7-1
[0125] 式中，上標(biāo)"一"表示先驗(yàn)估計(jì)。
[0126] 3.進(jìn)行量測(cè)更新:首先，由先驗(yàn)狀態(tài)估計(jì)劣和方差^重新計(jì)算容積點(diǎn)
[0127]
[0128] 然后，計(jì)算經(jīng)過(guò)非線性量測(cè)函數(shù)傳遞的容積點(diǎn)位
[0129]
[0130] 其次，計(jì)算量測(cè)先驗(yàn)估計(jì)值及先驗(yàn)估計(jì)值的的方差、協(xié)方差為
[0131]
[0132]
[0133] /-1
[0134] 4.計(jì)算范數(shù)約束的卡爾曼增益:首先，計(jì)算無(wú)約束的卡爾曼增益眾：
[0135]
(24)
[0136] 和無(wú)約束的最優(yōu)狀態(tài)估計(jì)值
[0137]
[0138]
[0139]
[0140]然后，計(jì)算具有范數(shù)約束的卡爾曼增益Kk:
[0141](27)
[0142] 式中，
[0143]
[0144]
[0145] 5.計(jì)算狀態(tài)估計(jì)值和相應(yīng)的方差陣:基于范數(shù)約束的后驗(yàn)狀態(tài)估計(jì)值為
[0146]
[0147]
[0148]
[0149]
[0150]
[0151] 通過(guò)以上4步循環(huán)進(jìn)行即可得到飛行器的實(shí)時(shí)狀態(tài)估計(jì)值夂包含探測(cè)器的位置 i5、速度?和姿態(tài)4 (見流程圖圖1)。
[0152] 本發(fā)明通過(guò)建立飛行器導(dǎo)航系統(tǒng)的非線性狀態(tài)方程和量測(cè)方程，然后利用范數(shù)約 束容積卡爾曼濾波算法將范數(shù)約束引入到容積卡爾曼濾波中，提高了飛行器的姿態(tài)估計(jì)精 度。
[0153] 以上所述，僅為本發(fā)明較佳的【具體實(shí)施方式】，但本發(fā)明的保護(hù)范圍并不局限于此， 任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi)，可輕易想到的變化或替換， 都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍之內(nèi)。
【主權(quán)項(xiàng)】
1. 一種基于范數(shù)約束容積卡爾曼濾波的飛行器姿態(tài)估計(jì)方法，其特征在于，其步驟如 下： 步驟一、建立飛行器導(dǎo)航系統(tǒng)的非線性狀態(tài)方程 在目標(biāo)坐標(biāo)系下，利用慣性測(cè)量單元提供的飛行器的加速度和角速度建立飛行器的動(dòng) 力學(xué)模型，其狀態(tài)方程為(1) 其中，r= [Γχ,Γγ,Γζ]哺v= [Vx，Vy，Vz]τ分別是飛行器的位置和速度，X、Y、Z是目標(biāo)系的 坐標(biāo)軸;q=[qo qi Q2 Q3]T是姿態(tài)四元數(shù);a=[ax，ay，az]哺ω = [ωχ,ωγ,ωζ]τ分別是巧螺 的加速度和角速度；g是重力加速度；ct=(c；r是從體坐標(biāo)系到目標(biāo)坐標(biāo)系的系數(shù)轉(zhuǎn)換矩 陣，系數(shù)轉(zhuǎn)換矩陣玲為：其中，6和&分別為加速度計(jì)和巧螺儀的輸出向量，a和ω分別為巧螺真實(shí)的加速度和角 速度，ba和b。分別為加速度的偏差和角速度的偏差，ξ。和ξ。分別為加速度和角速度的零均值 高斯白噪聲，％和η。分別為加速度和角速度的漂移偏差的高斯白噪聲，系數(shù)矩陣Θ q為(5)， 步驟二:建立飛行器導(dǎo)航系統(tǒng)的非線性量測(cè)方程 將慣性測(cè)量單元的速度和加速度，測(cè)距測(cè)速儀測(cè)得的高度和Ξ軸速度和兩個(gè)導(dǎo)航信標(biāo) 的距離作為量測(cè)值引入到量測(cè)模型，則量測(cè)方程為： z = [rz V。化化]τ+ζ = Η(χ)+ζ (6) 其中，其中，Ri表示飛行器與導(dǎo)航信標(biāo)的距離，η是信標(biāo)的位置坐標(biāo)，ζκ,ι是測(cè)距的噪聲，ζκ,ι為 零均值高斯白噪聲； 步驟Ξ:離散化狀態(tài)方程和量測(cè)方程 將狀態(tài)方程和量測(cè)方程進(jìn)行離散化，得到離散的飛行器的狀態(tài)方程和量測(cè)方程：其中，k時(shí)刻的狀態(tài)變量xk具有范數(shù)約束|.Ytf =-v>t =義，冊(cè)和Ck是相互獨(dú)立的零均值高 斯白噪聲序列，且wk和Ck的方差分別為Qk和Rk，且滿足(II) 其中，Skj為K;ronecke;rS函數(shù)，當(dāng)k= j時(shí)，Skj = l;當(dāng)k聲 j時(shí)，Skj = 0; 步驟四：將離散后狀態(tài)方程和量測(cè)方程采用范數(shù)約束容積卡爾曼濾波，輸出飛行器的 姿態(tài)1. 對(duì)離散的狀態(tài)方程和量測(cè)方程的狀態(tài)和誤差方差陣分別進(jìn)行初始化：2. 設(shè)第k-1步tk-i時(shí)刻的狀態(tài)估計(jì)值和誤差方差陣分別為，和巧_|，則第k步tk時(shí)刻的容 積點(diǎn)為：(14) 其中，上標(biāo)"表示其后驗(yàn)估計(jì)，^為誤差方差距陣巧+_，的平方根，滿足 柏=^/^(^/^f電為化個(gè)容積點(diǎn)的第^'個(gè)值，其取值集合為(15) 其中，ej(i = l，2，···，n)為單位向量，其第j個(gè)值為1，其余元素均為零，即ej = [0，···，0， 1，〇，...，〇]τ; 經(jīng)過(guò)非線性函數(shù)傳遞后的容積點(diǎn)為；(峭 則其相應(yīng)的先驗(yàn)狀態(tài)估計(jì)和方差分別為式中，上標(biāo)"一"表示變量的先驗(yàn)估計(jì)；3. 進(jìn)行量測(cè)方程的更新:首先，由先驗(yàn)狀態(tài)估計(jì)盡和方差《重新計(jì)算容積點(diǎn)：(19) 然后，計(jì)算經(jīng)過(guò)非線性量測(cè)函數(shù)傳遞的容積點(diǎn)位： .Z巧=賊K j.;k) 口 0) 其次，計(jì)算量測(cè)先驗(yàn)估計(jì)值及先驗(yàn)估計(jì)值的方差、先驗(yàn)估計(jì)值協(xié)方差：W上4步循環(huán)迭代，得到飛行器的實(shí)時(shí)狀態(tài)估計(jì)值?，包含探測(cè)器的位置i^速度(6和姿態(tài) 今(見流程圖圖1)。2. 根據(jù)權(quán)利要求1所示的基于范數(shù)約束容積卡爾曼濾波的飛行器姿態(tài)估計(jì)方法，其特 征在于，所述步驟一中的目標(biāo)坐標(biāo)系是指W預(yù)定著陸點(diǎn)為原點(diǎn)，X軸指向東，y軸指向北，Z軸 指向天;體坐標(biāo)系是指W飛行器的重屯、為原點(diǎn)，0X軸位于飛行器參考平面內(nèi)平行于飛行器 軸線并指向飛行器前方，0Y軸垂直于飛行器參考面并指向飛行器右方，0Z軸在參考面內(nèi)垂 直于X0Y平面，并指向飛行器下方。3. 根據(jù)權(quán)利要求1所示的基于范數(shù)約束容積卡爾曼濾波的飛行器姿態(tài)估計(jì)方法，其特 征在于，所述步驟一中姿態(tài)四元數(shù)的向量q=[q〇 qi Q2 q3]T的2范數(shù)是q中各個(gè)元素平方之 和再開根號(hào)：，且滿足范數(shù)約束II q I 12= 1。
【文檔編號(hào)】G01C21/20GK105973238SQ201610303633
【公開日】2016年9月28日
【申請(qǐng)日】2016年5月9日
【發(fā)明人】婁泰山, 賀振東, 楊小亮, 王妍, 吳青娥, 陳虎
【申請(qǐng)人】鄭州輕工業(yè)學(xué)院