[發明專利]一種基于非線性映射自適應混合Kalman/H∞濾波器的組合導航方法

有效

• 申請號: 201310000851.1
• 申請日: 2013-01-04
• 公開（公告）號: CN103063212A
• 公開（公告）日: 2013-04-24
• 發明（設計）人: 張勇剛;黃玉龍;李寧;郜中星;王程程;王國臣;高偉;周廣濤
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/00
• 分類號: G01C21/00
• 代理公司: 北京永創新實專利事務所 11121
• 代理人: 趙文利

1.一種基于非線性映射自適應混合Kalman/H∞濾波器的組合導航方法，包括以下幾個步驟：

步驟一：在組合導航混合濾波器中，建立描述組合導航系統的狀態方程和觀測方程；

具體為，建立組合導航系統模型如下：

xk+1=Fk+1xk+Gk+1wkyk+1=Hk+1xk+1+vk+1zk+1=Lk+1xk+1---(1)]]>

其中，狀態方程為x_k+1＝F_k+1x_k+G_k+1w_k，觀測方程為y_k+1＝H_k+1x_k+1+v_k+1，待估計向量為z_k+1＝L_k+1x_k+1，k表示第k步，k+1表示第k+1步，x_k為第k步的n維狀態向量，y_k+1為第k+1步的m維觀測向量，z_k+1為第k+1步的q維待估計的輸出向量，F_k+1為第k+1步系統狀態轉移矩陣，Η_k+1為第k+1步系統觀測矩陣，G_k+1為第k+1步系統狀態噪聲矩陣，L_k+1為第k+1步估計矩陣，w_k為第k步n維系統噪聲，v_k+1為第k+1步m維觀測噪聲，z_k+1為第k+1步待估計向量；

步驟二：在組合導航混合濾波器中同時運行卡爾曼濾波器和H∞濾波器；

卡爾曼濾波方程為：

假設w_k和v_k+1是不相關的零均值高斯白噪聲，它們的方差分別為Q和R；組合導航系統的卡爾曼濾波器初始狀態估計為表示初始狀態估計，x₀表示組合導航系統真實的初始狀態，0表示第0步，E表示期望，初始狀態估計誤差協方差為P₂₀表示初始狀態估計誤差協方差，卡爾曼濾波按照以下迭代方式進行：

x^2(k+1/k)=Fk+1x^2(k)]]>

P2(k+1/k)=Fk+1P2(k)Fk+1T+Gk+1QGk+1T]]>

K2(k+1)=P2(k+1/k)Hk+1T(Hk+1P2(k+1/k)Hk+1T+R)-1]]>

(2)

x^2(k+1)=x^2(k+1/k)+K2(k+1)(yk+1-Hk+1x^2(k+1/k))]]>

rk+1=yk+1-Hk+1x^2(k+1/k)]]>

z^2(k+1)=Lk+1x^2(k+1)]]>

P_2(k+1)＝(I-K_2(k+1)H_k+1)P_2(k+1/k)

其中：為卡爾曼濾波器第k步狀態估計值，F_k+1為系統第k+1步狀態轉移矩陣，為卡爾曼濾波器第k+1步狀態預測值，G_k+1為系統第k+1步狀態噪聲矩陣，P_2(k)為卡爾曼濾波器第k步狀態估計誤差協方差矩陣，P_2(k+1/k)為卡爾曼濾波器第k+1步狀態預測誤差協方差矩陣，H_k+1為系統第k+1步觀測矩陣，K_2(k+1)為卡爾曼濾波器第k+1步增益矩陣，為卡爾曼濾波器第k+1步狀態估計值，r_k+1為卡爾曼濾波器第k+1步新息，為待估計向量的第k+1步卡爾曼濾波估計值，I為n階單位矩陣，P_2(k+1)為卡爾曼濾波器第k+1步狀態估計誤差協方差矩陣；當假設成立且系統模型準確時，卡爾曼濾波具有最小方差性能，可以獲得對向量z_k+1精度較高的估計結果

H∞濾波方程為：

組合導航系統的H∞濾波器的初始濾波參數為：初始狀態估計為初始估計廣義協方差為P_∞0，對于給定的魯棒參數γ＞0，如果[F_k+1G_k+1]行滿秩，則H∞濾波器存在當且僅當對于所有的k有：

P∞(k)-1+HkTHk-γ-2LkTLk>0---(3)]]>

其中：P_∞(k)為H∞濾波第k步迭代矩陣，其迭代方式如下：

P∞(k+1)=FkP∞(k)FkT+GkGkT]]>

-FkP∞(k)HkTLkTRe,k-1HkLkP∞(k)FkT---(4)]]>

上式中，Re,k=I00-γ2I+HkLkP∞(k)HkTLkT,]]>I表示單位矩陣；

通過對式（4）進行矩陣代數運算，等價變換為：

P∞(k+1)=FkP∞k[I-γ-2LkTLkP∞k+HkTHkP∞k]-1FkT+GkGkT---(5)]]>

H∞濾波方程如下：

K∞(k+1)=P∞(k+1)Hk+1T(Hk+1P∞(k+1)Hk+1T+I)-1]]>

x^∞(k+1)=Fk+1x^∞(k)+Kk+1(yk+1-Hk+1Fk+1x^∞(k))---(6)]]>

z^∞(k+1)=Lk+1x^∞(k+1)]]>

其中：K_∞(k+1)為H∞濾波器第k+1步增益矩陣，和分別為H∞濾波器第k步和k+1步狀態估計，P_∞(k+1)為H∞濾波器第k+1步迭代矩陣，為待估計向量z_k+1的第k+1步H∞濾波估計值；

步驟三：利用卡爾曼濾波器輸出新息序列自相關矩陣的時間平均值和濾波器參數，獲取卡爾曼濾波器性能量化指標；

具體為：

根據卡爾曼濾波器方程(2)，獲取第i+1步新息r_i+1的函數值J_i+1：

Ji+1=ri+1Tri+1tr(Hi+1Pi+1/iHi+1T+R)---(7)]]>

其中，tr(·)表示矩陣的跡，對于卡爾曼濾波器的第k+1步迭代，建立以下卡爾曼濾波器性能量化指標：

J‾k+1=1MΣi=k-M+1k+1Ji+1---(8)]]>

其中：J_i+1由(7)式給出，為時間窗口寬度M內J_i+1的平均值，M為時間窗口寬度；

步驟四：建立卡爾曼濾波器性能量化指標與混合濾波器加權參數間的非線性映射關系，自適應地調整加權參數；

卡爾曼濾波器性能量化指標與加權參數d_k+1間的非線性映射函數如下：

dk+1=1(J‾k+1≤J2)be-J‾k+1a(J2<J‾k+1≤J∞)0(J‾k+1>J∞)---(9)]]>

其中：由式(8)計算給出，J₂和J_∞分別為卡爾曼濾波器上界運行參數和下界運行參數，a,b為參數；

步驟五：通過加權參數，將卡爾曼濾波器和H∞濾波器輸出的加權和作為整個混合濾波器的輸出，完成組合導航信息處理；

具體混合濾波器輸出如下：

x^k+1=dk+1x^2(k+1)+(1-dk+1)x^∞(k+1)]]>

(10)

z^k+1=Lk+1x^k+1]]>

其中：為導航誤差的估計值，為待求的導航誤差值，通過公式（10）得到待求的導航誤差值，通過對導航定位位置進行修正。