[發(fā)明專利]一種閉環(huán)正向濾波結合反向平滑的POS后處理方法

無效

• 申請?zhí)枺?/td>: 201110273046.7
• 申請日: 2011-09-15
• 公開（公告）號: CN102997915A
• 公開（公告）日: 2013-03-27
• 發(fā)明（設計）人: 周東靈;李文耀;尚克軍;張勤拓;扈光鋒;周祖洋;劉輝;謝仕民;邱宏波;劉峰
• 申請（專利權）人: 北京自動化控制設備研究所
• 主分類號: G01C21/16
• 分類號: G01C21/16
• 代理公司: 核工業(yè)專利中心 11007
• 代理人: 李燁

1.一種閉環(huán)正向濾波結合反向平滑的POS后處理方法，是對POS的位置姿態(tài)數據進行后處理的方法，包括：卡爾曼濾波的步驟，和反向R-T-S平滑的步驟，其特征在于：

在卡爾曼濾波的過程中，建立卡爾曼濾波模型的過程如下：

步驟1、使用二階系統(tǒng)模型對慣導系統(tǒng)進行描述；

設計慣導系統(tǒng)水平通道成典型二階系統(tǒng)；

▿1′(s)=s2+K1s+(K2+1Re)g0---(1)]]>

式中：K₁、K₂是水平通道的兩個實時反饋控制系數；

設計慣導系統(tǒng)垂向通道成典型二階系統(tǒng)；

▿2′(s)=s2+C1s+(C2-2g0Re)---(2)]]>

式中：C₁、C₂是垂向通道的兩個實時反饋控制系數；

典型二階系統(tǒng)的特征方程為

▿(s)=s2+2ξwns+wn2---(3)]]>

根據POS系統(tǒng)事后處理過程對濾波調節(jié)周期和超調量的要求，確定ξ、w_n；接著按如下過程計算反饋控制系數；

1、根據系統(tǒng)的性能要求確定濾波調節(jié)周期和超調量；

2、根據調節(jié)周期和超調量，確定ξ、w_n；

3、分別將式(1)、(2)與式(3)進行同系數對比，即得到公式(4)、(5)，按照公式(4)、(5)解算得反饋控制系數K₁、K₂、C₁、C₂；

K₁＝2ξw_n

(4)

K2+1Re=wn2]]>

C₁＝2ξw_n

(5)

C2-2g0Re=wn2]]>

步驟2、建立POS的誤差方程：

式中：

v_N、v_U、v_E分別為慣導系統(tǒng)的北向速度、垂向速度、東向速度，單位：米/秒，為上一時刻的量；

δv_N、δv_U、δv_E分別為慣導系統(tǒng)的北向速度誤差、垂向速度誤差、東向速度誤差，單位：米/秒，為上一時刻的量；

為慣導系統(tǒng)的緯度，單位：弧度，為上一時刻的量；

為慣導系統(tǒng)的緯度誤差，單位：弧度，為上一時刻的量；

h為慣導系統(tǒng)的高度，單位：米，為上一時刻的量；

δh為慣導系統(tǒng)的高度誤差，單位：米，為上一時刻的量；

φ_U、φ_E分別為慣導系統(tǒng)的垂向失調角、東向失調角，單位：弧度，為上一時刻的量；

f_U、f_E分別為垂向加速度、東向加速度，單位：米/秒²，為當前時刻的量；

分別為載體系x軸加速度計零偏誤差、y軸加速度計零偏誤差、z軸加速度計零偏誤差，單位：米/秒²，為當前時刻的量；

C_1j(j＝1，2，3)分別為姿態(tài)矩陣的元素，為上一時刻的量；

R_M、R_N分別為地球子午圈、卯酉圈半徑，單位：米，為上一時刻的量；

ω_ie為地球自轉角速率，單位：弧度/秒，為常量；

為Kalman濾波估計的系統(tǒng)北速誤差，單位：米/秒，為當前時刻的量；

K₁為水平通道的實時反饋控制系數，由上述過程計算得到；

式中：

f_N為北向加速度，單位：米/秒²，為當前時刻的量；

φ_N為慣導系統(tǒng)的北向失調角，單位：弧度，為上一時刻的量；

C_2j(j＝1，2，3)分別為姿態(tài)矩陣的元素，為上一時刻的量；

為Kalman濾波估計的系統(tǒng)高度誤差，單位：米，為當前時刻的量；

C₂為垂向通道的實時反饋控制系數，由上述過程計算得到；

式中：

C_3j(j＝1，2，3)分別為姿態(tài)矩陣的元素，為上一時刻的量；

為Kalman濾波估計的系統(tǒng)東速誤差，單位：米/秒，為當前時刻的量；

式中：

C₁為垂向通道的實時反饋控制系數，由上述過程計算得到；

(6g)

式中：

分別為處理成一階馬爾可夫過程的載體系x軸陀螺漂移誤差、y軸陀螺漂移誤差、z軸陀螺漂移誤差，單位：弧度/秒；

(6h)

(6i)

步驟3、建立卡爾曼濾波模型；

取狀態(tài)變量觀測量其中上標G表示GPS提供的信息；

λ^G分別為GPS提供的緯度、經度，單位：弧度；h^G為GPS提供的高度，單位：米；分別為GPS提供的北向速度、垂向速度、東向速度，單位：米/秒；λ為慣導系統(tǒng)的經度，單位：弧度；

根據給出的誤差方程，確定系統(tǒng)的狀態(tài)方程為

式中：

A(t)為15×15維系統(tǒng)參數矩陣，根據式(6)計算；

L(t)為15×3維控制系數矩陣，L(t)＝[0_3×2?L_3×7?0_3×6]^T，其中L3×7=-C10-C200000-K10000-K2000-K1K200;]]>

U(t)=δh^(t)δv^N(t)δv^E(t)T]]>為系統(tǒng)控制向量；

W(t)為15×1維系統(tǒng)激勵噪聲向量；

量測方程為：

Z＝HX(t)+V(t)

????????????????????????????????????????????????(8)

式中：

H為6×15維量測矩陣；

V(t)為量測噪聲向量；

將狀態(tài)方程和量測方程離散化為：

X_k＝Φ_k，k-1X_k-1+L_kU_k-1+W_k-1????????????????????(9)

Z_k＝HX_k+V_k??????????????????????????????????????(10)

Φk,k-1=I+TdΣi=0N-1Ai---(11)]]>

其中Φ_k，k-1為系統(tǒng)的狀態(tài)轉移陣；T_d為系統(tǒng)的濾波周期，單位：秒；N為慣導系統(tǒng)的解算頻率；

帶控制量的卡爾曼濾波器濾波模型為：

X^k,k-1=Φk,k-1X^k-1+Lk-1Uk-1]]>

???????????????????????????????????????(12a)

Pk,k-1=Φk,k-1Pk-1Φk,k-1T+Qk-1]]>

???????????????????????????????????????(12b)

K_k＝P_k，k-1H^T(HP_k，k-1H^T+R_k)^-1

???????????????????????????????????????(12c)

X^k=X^k,k-1+Kk(Zk-HX^k,k-1)]]>

???????????????????????????????????????(12d)

P_k＝(I-K_kH)P_k，k-1(I-K_kH)^T+K_kR_k(K_k)^T

???????????????????????????????????????(12e)

式中：

P_k，k-1為濾波器的預測均方誤差陣；

P_k為濾波器的估計均方誤差陣；

Q_k為濾波器的系統(tǒng)噪聲方差陣；

K_k為濾波器的增益陣；

R_k為濾波器的量測噪聲方差陣。