[發明專利]火星動力下降段減弱動力學系統誤差的兩步濾波方法

有效

• 申請號: 201310287572.8
• 申請日: 2013-07-10
• 公開（公告）號: CN103344246A
• 公開（公告）日: 2013-10-09
• 發明（設計）人: 傅惠民;吳云章;張勇波;王治華;婁泰山;肖強
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G01C21/24
• 分類號: G01C21/24
• 代理公司: 北京慧泉知識產權代理有限公司 11232
• 代理人: 王順榮;唐愛華

1.一種火星動力下降段減弱動力學系統誤差的兩步濾波方法，其特征在于：它包括以下步驟：

步驟一、建立工程實際方程：離散的動力學系統和量測系統

xk+1=f(xk,uk)+Bkbgk+wkx---(1)]]>

bgk+1=Akbgk+wkb---(2)]]>

z_k=h(x_k)+v_k??(3)

其中x_k表示系統狀態量，z_k是測量系統測量值,b_gk是未知的動力學系統誤差即火星重力加速度誤差；非線性方程f(·)和h(·)分別是狀態轉移方程和量測方程并且關于可x_k微；矩陣B_k為未知的動力學系統誤差即火星重力加速度誤差對動力系統的驅動矩陣，矩陣A_k為未知的動力學系統誤差即火星重力加速度誤差轉移矩陣，矩陣B_k,A_k具有恰當的維數；是動力學系統誤差的噪聲，和v_k分別是動力學系統噪聲和量測噪聲，它們是不相關的高斯白噪聲滿足以下式子：

E[wkxwkbvkwkxwkbvkT]=Qkx000Qkb000Rkδkj---(3)]]>

其中R_k≥0，δ_kj是δ函數,當k=j時δ_kj=1當k≠j時δ_kj=0.

步驟二、給定初始值：

E[x0]=x^0,]]>E[(x0-x^0)(x0-x^0)T]=P0x≥0]]>

E[bg0]=b^g0,]]>E[(bg0-b^g0)(bg0-b^g0)T]=P0bg≥0]]>

E[(x0-x^0)(bg0-b^g0)T]=P0xbg≥0]]>

為初始狀態的估計值，為初始狀態估計均方誤差，為初始誤差的估計值通常取0，為初始誤差的估計均方誤差，為初始狀態和初始誤差的估計均方誤差；將這些以上初始值通過以下式子計算得到濾波初始條件：

x‾0(+)=x^0-V0b^g0,]]>b‾g0(+)=b^g0,]]>V0=P0xbg(P0bg)-1,]]>P‾0x(+)=P0x-V0P0bgV0T,]]>P‾0bg(+)=P0bg]]>

其中，為濾波初始狀態的估計值，為濾波初始狀態估計均方誤差，為濾波初始誤差的估計值通常取0，為濾波初始誤差的估計均方誤差陣，V₀為濾波初始狀態和初始誤差的相關系數；

步驟三、狀態量濾波

x^k(-)=f(x^k-1,uk-1)++u‾k-1---(4)]]>

P‾kx(-)=Φk-1P‾k-1x(+)Φk-1T+Q‾k-1x---(5)]]>

K‾kx=P‾kx(-)HkT[HkP‾kx(-)HkT+Rk]-1---(6)]]>

P‾kx(+)=(I-K‾kxHk)P‾kx(-)---(7)]]>

η‾kx=zk-Hkx‾k(-),]]>x‾k(+)=x‾k(-)+K‾kxη‾kx---(8)]]>

其中

Φk=∂f(x)∂x|x=x^k(+)]]>Hk=∂h(x)∂x|x=x^k(-).---(10)]]>

式中：為t_k-1時刻的狀態量，u_k-1為t_k-1時刻的控制輸入量，為t_k-1時刻的濾波校正輸入量；為狀態的濾波一步預測；為t_k-1時刻的濾波狀態估計均方誤差，Φ_k-1為t_k-1時刻到t_k時刻的一步轉移矩陣；為系統的噪聲的濾波校正方差陣，為一步預測均方誤差；H_k為量測陣，為狀態增益；I為單位陣，為t_k時刻的濾波狀態估計均方誤差；為量測新息，為濾波狀態估計；為狀態的一步預測由式（17）計算得到；

步驟四、動力學系統偏差濾波

bg‾k(-)=Ak-1bg‾k-1(+)---(11)]]>

P‾kbg(-)=Ak-1P‾k-1bg(+)Ak-1T+Qk-1bg---(12)]]>

K‾kbg=P‾kbg(-)NkT[HkP‾kx(-)HkT+Rk+NkP‾kbg(-)NkT]-1---(13)]]>

P‾kbg(+)=(I-K‾kbgNk)P‾kbg(-)---(14)]]>

η‾kbg=zk-Hkx‾k(-)-Nkbg‾k(-),]]>bg‾k(+)=bg‾k(-)+K‾kbgη‾kbg---(15)]]>

其中步驟三步驟四中的對應式子為

NkT=HkUk,]]>Uk=U‾k[I-Qk-1bg[P‾kbg(-)]-1],]]>U‾k=(Φk-1Vk-1+Bk-1)Ak-1-1]]>

Vk=Uk-K‾kxNk,]]>u‾k=(U‾k+1-Uk+1)Akbg‾k(+)andQ‾kx=Qkx+Uk+1QkbgU‾k+1T]]>

式中，A_k-1為t_k-1時刻動力學系統誤差的轉移矩陣，為t_k-1時刻動力學系統誤差即火星重力加速度誤差的濾波估計，為動力學系統誤差即火星重力加速度誤差的一步預測，為t_k-1時刻動力學系統誤差的估計均方誤差陣，為動力學系統誤差的一步預測均方誤差，為動力學系統誤差的噪聲的方差陣，為動力學系統誤差狀態增益，R_k為量測噪聲的方差陣；為t_k時刻動力學系統誤差的估計均方誤差陣，為動力學系統誤差即火星重力加速度誤差的量測新息，為動力學系統誤差的狀態估計；N_k為t_k時刻動力學系統偏差對量測系統校正量測陣；為系統噪聲方差陣；U_k，V_k為狀態估計均方誤差陣U-V分解的對應陣；

步驟五、更新相關系數、校正狀態估計和動力學偏差估計：情況如下

Uk=U‾k[I-Qk-1bg[P‾kbg(-)]-1],]]>U‾k=(Φk-1Vk-1+Bk-1)Ak-1-1,]]>Vk=Uk-K‾kxNk---(16)]]>

x^k(-)=x‾k(-)+Ukbg‾k(-),]]>x^k(+)=x‾k(+)+Vkbg‾k(+)---(17)]]>

Pkx(-)=P‾kx(-)+UkP‾kb(-)UkT,]]>Pkx(+)=P‾kx(+)+UkP‾kb(+)UkT---(18)]]>

為t_k時刻校正后的狀態量，為t_k時刻校正后的狀態估計均方誤差，為t_k時刻校正后的狀態量，為t_k時刻校正后的狀態估計均方誤差；

步驟六、令k=k+1，返回步驟三往下進行，直到k等于火星動力下降段時間截止對應的時刻T時，即火星著陸器著陸為止；至此完成火星動力下降段減弱動力學系統誤差的兩步濾波方法。

2.根據權利要求1所述的一種火星動力下降段減弱動力學系統誤差的兩步濾波方法，其特征在于：在步驟一中所述的建立工程實際方程，其步驟如下：

a、分析動力學不確定性之間的相互關系，并進行相應的數值計算分析；

b、得到這些不確定性因素對動力學模型的影響是通過哪些量對動力學模型產生影響從而引起誤差的傳播；

c、將動力學系統x_k+1=f(x_k,u_k)改寫為考慮動力學系統偏差的動力學系統xk+1=f(xk,uk)+Bkbgk+wkx.]]>