[發(fā)明專(zhuān)利]一種機(jī)載分布式POS的實(shí)時(shí)傳遞對(duì)準(zhǔn)的方法及裝置

有效

• 申請(qǐng)?zhí)枺?/td>: 201810153926.2
• 申請(qǐng)日: 2018-02-22
• 公開(kāi)（公告）號(hào): CN108303120B
• 公開(kāi)（公告）日: 2020-03-24
• 發(fā)明（設(shè)計(jì)）人: 宮曉琳;劉剛;陳隆君;房建成
• 申請(qǐng)（專(zhuān)利權(quán)）人: 北京航空航天大學(xué)
• 主分類(lèi)號(hào): G01C25/00
• 分類(lèi)號(hào): G01C25/00
• 代理公司: 北京挺立專(zhuān)利事務(wù)所(普通合伙) 11265
• 代理人: 李鑫

1.一種機(jī)載分布式POS的實(shí)時(shí)傳遞對(duì)準(zhǔn)的方法，其特征在于，包括：

建立大失準(zhǔn)角條件下的機(jī)載分布式POS傳遞對(duì)準(zhǔn)的誤差模型和數(shù)學(xué)模型；所述誤差模型包括子系統(tǒng)的慣導(dǎo)誤差模型、主系統(tǒng)和子系統(tǒng)間的角誤差模型；所述數(shù)學(xué)模型包括系統(tǒng)狀態(tài)方程和系統(tǒng)量測(cè)方程；其中，所述系統(tǒng)狀態(tài)方程包括線性狀態(tài)方程和非線性狀態(tài)方程；所述非線性狀態(tài)方程由與姿態(tài)狀態(tài)變量有關(guān)的微分方程組成，所述線性狀態(tài)方程為由與速度誤差、位置誤差、陀螺儀常值誤差、加速度計(jì)常值偏置、安裝誤差角、彈性變形角、彈性變形角速率中的至少一個(gè)狀態(tài)變量有關(guān)的微分方程組成；

所述建立大失準(zhǔn)角條件下的機(jī)載分布式POS傳遞對(duì)準(zhǔn)的誤差模型，包括：建立大失準(zhǔn)條件下的機(jī)載分布式POS的子系統(tǒng)的慣導(dǎo)誤差模型和主子系統(tǒng)間的角誤差模型，所述慣導(dǎo)誤差模型包括姿態(tài)誤差微分方程、速度誤差微分方程、位置誤差微分方程和慣性儀表誤差微分方程，所述主子系統(tǒng)間的角誤差模型包括安裝角誤差模型和彈性變形角模型；

所述姿態(tài)誤差微分方程為：

其中，為子系統(tǒng)姿態(tài)失準(zhǔn)角，φ_E、φ_N和φ_U分別為東向、北向、天向失準(zhǔn)角，為子系統(tǒng)真實(shí)導(dǎo)航坐標(biāo)系相對(duì)慣性坐標(biāo)系的角速度；為的誤差角速度；為子系統(tǒng)載體坐標(biāo)系到其計(jì)算導(dǎo)航坐標(biāo)系的方向余弦矩陣；I_3×3為3行、3列的單位矩陣，ε^b為子系統(tǒng)陀螺儀誤差，ε^b＝ε^c+w_ε，其中，ε^c為子系統(tǒng)陀螺儀常值漂移，w_ε為子系統(tǒng)陀螺儀隨機(jī)誤差，和分別為子系統(tǒng)載體坐標(biāo)系x軸、y軸和z軸陀螺儀常值漂移，和分別為子系統(tǒng)載體坐標(biāo)系x軸、y軸和z軸陀螺儀隨機(jī)誤差；為子系統(tǒng)真實(shí)導(dǎo)航坐標(biāo)系到其計(jì)算導(dǎo)航坐標(biāo)系的方向余弦矩陣；其中，

所述速度誤差微分方程為：

其中，Vⁿ＝[V_E V_N V_U]^T和δVⁿ＝[δV_E δV_N δV_U]^T分別為子系統(tǒng)速度和速度誤差，其中V_E、V_N和V_U分別為東向、北向和天向速度，δV_E、δV_N和δV_U分別為東向、北向和天向速度誤差；f^b＝[f_xf_y f_z]^T是子系統(tǒng)的比力，其中f_x、f_y和f_z分別為載體坐標(biāo)系x方向、y方向和z方向比力；和分別為地球坐標(biāo)系相對(duì)慣性坐標(biāo)系的角速度ω_ie及其誤差δω_ie在子系統(tǒng)真實(shí)導(dǎo)航坐標(biāo)系下的表示；和分別為子系統(tǒng)真實(shí)導(dǎo)航坐標(biāo)系相對(duì)地球坐標(biāo)系的角速度及其誤差；為子系統(tǒng)加速度計(jì)誤差，其中，為子系統(tǒng)加速度計(jì)常值偏置，為系統(tǒng)加速度計(jì)隨機(jī)誤差，和分別為子系統(tǒng)載體坐標(biāo)系x軸、y軸和z軸加速度計(jì)常值偏置，和分別為子系統(tǒng)載體坐標(biāo)系x軸、y軸和z軸加速度計(jì)隨機(jī)誤差；

所述位置誤差微分方程為：

其中，L、λ、h分別為子系統(tǒng)的緯度、經(jīng)度、高度，δL、δλ、δh分別為緯度誤差、經(jīng)度誤差、高度誤差；為緯度的一階導(dǎo)數(shù)，為經(jīng)度的一階導(dǎo)數(shù)，為高度的一階導(dǎo)數(shù)；R_M和R_N分別為沿子午圈和卯酉圈的主曲率半徑；

所述慣性儀表誤差微分方程為：

其中，ε^c為子系統(tǒng)陀螺儀常值漂移，為子系統(tǒng)加速度計(jì)常值偏置；

所述建立主系統(tǒng)和子系統(tǒng)間的角誤差模型，包括：

建立子系統(tǒng)固定安裝誤差角ρ的微分方程：

其中，ρ＝[ρ_x ρ_y ρ_z]^T為子系統(tǒng)固定安裝誤差角，ρ_x、ρ_y和ρ_z分別為子系統(tǒng)在載體坐標(biāo)系x軸、y軸和z軸的安裝誤差角；

建立子系統(tǒng)彈性變形角θ的微分方程：

其中，θ_j為子系統(tǒng)載體坐標(biāo)系第j軸上的彈性變形角，j＝x,y,z，θ＝[θ_x θ_y θ_z]^T為彈性變形角；β_j＝2.146/τ_j，τ_j為二階馬爾科夫過(guò)程相關(guān)時(shí)間；η_j為零均值白噪聲，其方差滿足：

其中，σ_j²為彈性變形角θ_j的方差，β_j和為描述彈性變形角θ的二階馬爾科夫過(guò)程的參數(shù)；

利用UKF對(duì)t_k-1時(shí)刻所述非線性狀態(tài)方程中的非線性狀態(tài)變量x^NL進(jìn)行時(shí)間更新，并利用KF對(duì)t_k-1時(shí)刻所述線性狀態(tài)方程中的線性狀態(tài)變量x^L進(jìn)行時(shí)間更新；

所述非線性狀態(tài)變量x^NL和線性狀態(tài)變量x^L分別定義為：

x^NL＝[x¹]^T

x^L＝[x² x³]^T

其中，

x¹＝[φ_E φ_N φ_U],

所述非線性狀態(tài)方程為：

所述線性狀態(tài)方程為：

其中，和分別為t_k時(shí)刻的非線性狀態(tài)變量和線性狀態(tài)變量，和分別為t_k-1時(shí)刻的非線性狀態(tài)變量和線性狀態(tài)變量，非線性狀態(tài)方程的系統(tǒng)噪聲為線性狀態(tài)方程的系統(tǒng)噪聲為其中分別為子系統(tǒng)載體坐標(biāo)系x軸、y軸、z軸陀螺儀的隨機(jī)誤差，分別為子系統(tǒng)載體坐標(biāo)系x軸、y軸、z軸加速度計(jì)的隨機(jī)誤差；非線性狀態(tài)方程的系統(tǒng)噪聲w^NL和線性狀態(tài)方程的w^L均為零均值高斯白噪聲，w^NL的方差陣Q^NL由陀螺儀隨機(jī)漂移確定，w^L的方差陣Q^L由加速度計(jì)隨機(jī)偏置和二階馬爾科夫過(guò)程參數(shù)確定；系統(tǒng)狀態(tài)方程中各系數(shù)矩陣的表達(dá)式如下：

所述建立大失準(zhǔn)角條件下的機(jī)載分布式POS傳遞對(duì)準(zhǔn)的數(shù)學(xué)模型包括：建立大失準(zhǔn)角條件下的機(jī)載分布式POS傳遞對(duì)準(zhǔn)的系統(tǒng)量測(cè)模型；

所述建立大失準(zhǔn)角條件下的機(jī)載分布式POS傳遞對(duì)準(zhǔn)的系統(tǒng)量測(cè)模型，包括：

系統(tǒng)量測(cè)量定義為：

z＝[δψ δθ δγ δV′_E δV′_N δV′_U δL′ δλ′ δh′]^T

建立系統(tǒng)量測(cè)方程：

其中，z_k為t_k時(shí)刻的量測(cè)量，δψ、δθ、δγ分別為子系統(tǒng)與主系統(tǒng)的航向角、俯仰角、橫滾角之差，δV′_E、δV′_N、δV′_U分別為子系統(tǒng)與主系統(tǒng)東向、北向、天向速度之差，δL′、δλ′、δh′分別為子系統(tǒng)與主系統(tǒng)的緯度、經(jīng)度、高度之差；量測(cè)噪聲其中v_δψ、v_δθ、v_δγ分別為主系統(tǒng)航向角、俯仰角、橫滾角的量測(cè)噪聲，分別為主系統(tǒng)東向、北向、天向速度的量測(cè)噪聲，v_δL、v_δλ、v_δh分別為主系統(tǒng)緯度、經(jīng)度、高度的量測(cè)噪聲；v為量測(cè)噪聲，取為零均值高斯白噪聲，v_k為v在t_k時(shí)刻的值；

所述利用UKF對(duì)t_k-1時(shí)刻所述非線性狀態(tài)方程中的非線性狀態(tài)變量進(jìn)行時(shí)間更新，包括：

計(jì)算t_k-1時(shí)刻的2n_NL+1個(gè)樣本點(diǎn)其中，n_NL為非線性狀態(tài)變量的維數(shù)：

其中，10^-4≤α≤1，κ＝3-n_NL，和分別為t_k-1時(shí)刻非線性狀態(tài)變量的估計(jì)值和估計(jì)協(xié)方差矩陣；表示矩陣的下三角分解平方根的第i列；

利用UKF計(jì)算t_k時(shí)刻非線性狀態(tài)變量的一步預(yù)測(cè)值

其中，為的一步預(yù)測(cè)模型值，為補(bǔ)償項(xiàng)；W_i為權(quán)值；

所述利用KF對(duì)t_k-1時(shí)刻所述線性狀態(tài)方程中的線性狀態(tài)變量進(jìn)行時(shí)間更新，包括：

其中，為補(bǔ)償項(xiàng)；

將所述KF更新后的t_k-1時(shí)刻的線性狀態(tài)作為所述非線性狀態(tài)方程的參數(shù)，并利用UKF對(duì)所述非線性狀態(tài)方程進(jìn)行量測(cè)更新，得到t_k時(shí)刻的非線性狀態(tài)變量估計(jì)值將所述作為所述線性狀態(tài)方程的參數(shù)，并利用KF對(duì)所述線性狀態(tài)變量方程進(jìn)行量測(cè)更新，得到t_k時(shí)刻的線性狀態(tài)變量估計(jì)值

所述將所述KF更新后的t_k-1時(shí)刻的線性狀態(tài)作為所述非線性狀態(tài)方程的參數(shù)，并利用UKF對(duì)所述非線性狀態(tài)方程進(jìn)行量測(cè)更新，得到t_k時(shí)刻的非線性狀態(tài)變量估計(jì)值包括：

根據(jù)所述非線性狀態(tài)變量一步預(yù)測(cè)值計(jì)算非線性狀態(tài)變量的一步預(yù)測(cè)樣本點(diǎn)

其中，為的誤差，為協(xié)方差矩陣為的零均值高斯白噪聲，為t_k時(shí)刻量測(cè)變量一步預(yù)測(cè)值的自協(xié)方差矩陣，為t_k時(shí)刻非線性狀態(tài)變量一步預(yù)測(cè)值與量測(cè)量一步預(yù)測(cè)值的互協(xié)方差矩陣，和的計(jì)算過(guò)程如下：

其中，為補(bǔ)償項(xiàng)；

計(jì)算非線性狀態(tài)變量的增益矩陣

計(jì)算濾波估計(jì)值及其協(xié)方差矩陣：

將所述UKF更新后的t_k-1時(shí)刻的非線性狀態(tài)作為所述線性狀態(tài)方程的參數(shù)，并利用KF對(duì)所述線性狀態(tài)變量方程進(jìn)行量測(cè)更新，得到t_k時(shí)刻的線性狀態(tài)變量估計(jì)值包括：

其中，為線性狀態(tài)變量的增益矩陣，Y′為的雅可比矩陣，為補(bǔ)償項(xiàng)；I_21×21為21行21列的單位矩陣；

根據(jù)所述線性狀態(tài)變量估計(jì)值、非線性狀態(tài)變量估計(jì)值對(duì)所述子系統(tǒng)的捷聯(lián)解算結(jié)果進(jìn)行修正，分別得到修正后的t_k時(shí)刻的子系統(tǒng)的線性狀態(tài)變量值和非線性狀態(tài)變量值；

所述根據(jù)所述線性狀態(tài)變量估計(jì)值對(duì)所述子系統(tǒng)的捷聯(lián)解算結(jié)果進(jìn)行修正，得到修正后的t_k時(shí)刻的子系統(tǒng)的線性狀態(tài)變量值，包括：

其中，和分別為子IMU修正后的東向、北向和天向速度；和分別為子系統(tǒng)捷聯(lián)解算得到的東向、北向和天向速度；δV_E、δV_N和δV_U分別為t_k時(shí)刻KF估計(jì)出的子系統(tǒng)捷聯(lián)解算東向、北向和天向速度誤差；

L^new＝L^old-δL

λ^new＝λ^old-δλ

h^new＝h^old-δh

其中，L^old、λ^old和h^old分別為子IMU捷聯(lián)解算得到的緯度、經(jīng)度和高度；L^new、λ^new和h^new分別為子IMU修正后的緯度、經(jīng)度和高度；δL、δλ和δh分別為t_k時(shí)刻KF估計(jì)出的子IMU捷聯(lián)解算緯度、經(jīng)度和高度誤差；

所述根據(jù)所述非線性狀態(tài)變量估計(jì)值對(duì)所述子系統(tǒng)的捷聯(lián)解算結(jié)果進(jìn)行修正，得到修正后的t_k時(shí)刻的子系統(tǒng)的非線性狀態(tài)變量值，包括：

計(jì)算t_k時(shí)刻子系統(tǒng)真實(shí)導(dǎo)航坐標(biāo)系n與計(jì)算導(dǎo)航坐標(biāo)系n₁間的方向余弦矩陣

計(jì)算t_k時(shí)刻子系統(tǒng)載體坐標(biāo)系b與真實(shí)導(dǎo)航坐標(biāo)系n之間的方向余弦矩陣

其中為t_k時(shí)刻子系統(tǒng)捷聯(lián)解算得到的方向余弦矩陣；

由被更新后的計(jì)算t_k時(shí)刻子系統(tǒng)的航向角ψ_s、俯仰角θ_s和橫滾角γ_s，將記為：

其中，T_lm為矩陣中第l行、第m列的元素，則子IMU航向角ψ_s、俯仰角θ_s和橫滾角γ_s的主值，即ψ_s主、θ_s主和γ_s主分別為：

θ_s主＝arcsin(T₃₂)

θ_s＝θ_s主，

2.一種機(jī)載分布式POS的實(shí)時(shí)傳遞對(duì)準(zhǔn)的裝置，其特征在于，包括：

建立模塊，用于建立大失準(zhǔn)角條件下的機(jī)載分布式POS傳遞對(duì)準(zhǔn)的誤差模型和數(shù)學(xué)模型；所述誤差模型包括子系統(tǒng)的慣導(dǎo)誤差模型、主系統(tǒng)和子系統(tǒng)間的角誤差模型；所述數(shù)學(xué)模型包括系統(tǒng)狀態(tài)方程和系統(tǒng)量測(cè)方程；其中，所述系統(tǒng)狀態(tài)方程包括線性狀態(tài)方程和非線性狀態(tài)方程；所述非線性狀態(tài)方程由與姿態(tài)狀態(tài)變量有關(guān)的微分方程組成，所述線性狀態(tài)方程為由與速度誤差、位置誤差、陀螺儀常值誤差、加速度計(jì)常值偏置、安裝誤差角、彈性變形角、彈性變形角速率中的至少一個(gè)狀態(tài)變量有關(guān)的微分方程組成；

所述建立模塊具體用于建立大失準(zhǔn)條件下的機(jī)載分布式POS的子系統(tǒng)的慣導(dǎo)誤差模型和主子系統(tǒng)間的角誤差模型，所述慣導(dǎo)誤差模型包括姿態(tài)誤差微分方程、速度誤差微分方程、位置誤差微分方程和慣性儀表誤差微分方程，所述主子系統(tǒng)間的角誤差模型包括安裝角誤差模型和彈性變形角模型；

其中，所述姿態(tài)誤差微分方程為：

其中，為子系統(tǒng)姿態(tài)失準(zhǔn)角，φ_E、φ_N和φ_U分別為東向、北向、天向失準(zhǔn)角，為子系統(tǒng)真實(shí)導(dǎo)航坐標(biāo)系相對(duì)慣性坐標(biāo)系的角速度；為的誤差角速度；為子系統(tǒng)載體坐標(biāo)系到其計(jì)算導(dǎo)航坐標(biāo)系的方向余弦矩陣；I_3×3為3行、3列的單位矩陣，ε^b為子系統(tǒng)陀螺儀誤差，ε^b＝ε^c+w_ε，其中，ε^c為子系統(tǒng)陀螺儀常值漂移，w_ε為子系統(tǒng)陀螺儀隨機(jī)誤差，和分別為子系統(tǒng)載體坐標(biāo)系x軸、y軸和z軸陀螺儀常值漂移，和分別為子系統(tǒng)載體坐標(biāo)系x軸、y軸和z軸陀螺儀隨機(jī)誤差；為子系統(tǒng)真實(shí)導(dǎo)航坐標(biāo)系到其計(jì)算導(dǎo)航坐標(biāo)系的方向余弦矩陣；其中，

所述速度誤差微分方程為：

其中，Vⁿ＝[V_E V_N V_U]^T和δVⁿ＝[δV_E δV_N δV_U]^T分別為子系統(tǒng)速度和速度誤差，其中V_E、V_N和V_U分別為東向、北向和天向速度，δV_E、δV_N和δV_U分別為東向、北向和天向速度誤差；f^b＝[f_xf_y f_z]^T是子系統(tǒng)的比力，其中f_x、f_y和f_z分別為載體坐標(biāo)系x方向、y方向和z方向比力；和分別為地球坐標(biāo)系相對(duì)慣性坐標(biāo)系的角速度ω_ie及其誤差δω_ie在子系統(tǒng)真實(shí)導(dǎo)航坐標(biāo)系下的表示；和分別為子系統(tǒng)真實(shí)導(dǎo)航坐標(biāo)系相對(duì)地球坐標(biāo)系的角速度及其誤差；為子系統(tǒng)加速度計(jì)誤差，其中，為子系統(tǒng)加速度計(jì)常值偏置，為系統(tǒng)加速度計(jì)隨機(jī)誤差，和分別為子系統(tǒng)載體坐標(biāo)系x軸、y軸和z軸加速度計(jì)常值偏置，和分別為子系統(tǒng)載體坐標(biāo)系x軸、y軸和z軸加速度計(jì)隨機(jī)誤差；

所述位置誤差微分方程為：

其中，L、λ、h分別為子系統(tǒng)的緯度、經(jīng)度、高度，δL、δλ、δh分別為緯度誤差、經(jīng)度誤差、高度誤差；為緯度的一階導(dǎo)數(shù)，為經(jīng)度的一階導(dǎo)數(shù)，為高度的一階導(dǎo)數(shù)；R_M和R_N分別為沿子午圈和卯酉圈的主曲率半徑；

所述慣性儀表誤差微分方程為：

其中，ε^c為子系統(tǒng)陀螺儀常值漂移，為子系統(tǒng)加速度計(jì)常值偏置；

所述建立主系統(tǒng)和子系統(tǒng)間的角誤差模型，包括：

建立子系統(tǒng)固定安裝誤差角ρ的微分方程：

其中，ρ＝[ρ_x ρ_y ρ_z]^T為子系統(tǒng)固定安裝誤差角，ρ_x、ρ_y和ρ_z分別為子系統(tǒng)在載體坐標(biāo)系x軸、y軸和z軸的安裝誤差角；

建立子系統(tǒng)彈性變形角θ的微分方程：

其中，θ_j為子系統(tǒng)載體坐標(biāo)系第j軸上的彈性變形角，j＝x,y,z，θ＝[θ_x θ_y θ_z]^T為彈性變形角；β_j＝2.146/τ_j，τ_j為二階馬爾科夫過(guò)程相關(guān)時(shí)間；η_j為零均值白噪聲，其方差滿足：

其中，σ_j²為彈性變形角θ_j的方差，β_j和為描述彈性變形角θ的二階馬爾科夫過(guò)程的參數(shù)；

第一更新模塊，用于利用UKF對(duì)t_k-1時(shí)刻所述非線性狀態(tài)方程中的非線性狀態(tài)變量x^NL進(jìn)行時(shí)間更新，并利用KF對(duì)t_k-1時(shí)刻所述線性狀態(tài)方程中的線性狀態(tài)變量x^L進(jìn)行時(shí)間更新；

所述非線性狀態(tài)變量x^NL和線性狀態(tài)變量x^L分別定義為：

x^NL＝[x¹]^T

x^L＝[x² x³]^T

其中，

x¹＝[φ_E φ_N φ_U],

所述非線性狀態(tài)方程為：

所述線性狀態(tài)方程為：

其中，和分別為t_k時(shí)刻的非線性狀態(tài)變量和線性狀態(tài)變量，和分別為t_k-1時(shí)刻的非線性狀態(tài)變量和線性狀態(tài)變量，非線性狀態(tài)方程的系統(tǒng)噪聲為線性狀態(tài)方程的系統(tǒng)噪聲為其中分別為子系統(tǒng)載體坐標(biāo)系x軸、y軸、z軸陀螺儀的隨機(jī)誤差，分別為子系統(tǒng)載體坐標(biāo)系x軸、y軸、z軸加速度計(jì)的隨機(jī)誤差；非線性狀態(tài)方程的系統(tǒng)噪聲w^NL和線性狀態(tài)方程的w^L均為零均值高斯白噪聲，w^NL的方差陣Q^NL由陀螺儀隨機(jī)漂移確定，w^L的方差陣Q^L由加速度計(jì)隨機(jī)偏置和二階馬爾科夫過(guò)程參數(shù)確定；系統(tǒng)狀態(tài)方程中各系數(shù)矩陣的表達(dá)式如下：

所述建立模塊具體用于建立大失準(zhǔn)角條件下的機(jī)載分布式POS傳遞對(duì)準(zhǔn)的系統(tǒng)量測(cè)模型；

所述建立大失準(zhǔn)角條件下的機(jī)載分布式POS傳遞對(duì)準(zhǔn)的系統(tǒng)量測(cè)模型，包括：

系統(tǒng)量測(cè)量定義為：

z＝[δψ δθ δγ δV′_E δV′_N δV′_U δL′ δλ′ δh′]^T

建立系統(tǒng)量測(cè)方程：

其中，z_k為t_k時(shí)刻的量測(cè)量，δψ、δθ、δγ分別為子系統(tǒng)與主系統(tǒng)的航向角、俯仰角、橫滾角之差，δV′_E、δV′_N、δV′_U分別為子系統(tǒng)與主系統(tǒng)東向、北向、天向速度之差，δL′、δλ′、δh′分別為子系統(tǒng)與主系統(tǒng)的緯度、經(jīng)度、高度之差；量測(cè)噪聲其中v_δψ、v_δθ、v_δγ分別為主系統(tǒng)航向角、俯仰角、橫滾角的量測(cè)噪聲，分別為主系統(tǒng)東向、北向、天向速度的量測(cè)噪聲，v_δL、v_δλ、v_δh分別為主系統(tǒng)緯度、經(jīng)度、高度的量測(cè)噪聲；v為量測(cè)噪聲，取為零均值高斯白噪聲，v_k為v在t_k時(shí)刻的值；

所述第一更新模塊具體用于，計(jì)算t_k-1時(shí)刻的2n_NL+1個(gè)樣本點(diǎn)其中，n_NL為非線性狀態(tài)變量的維數(shù)：

其中，10^-4≤α≤1，κ＝3-n_NL，和分別為t_k-1時(shí)刻非線性狀態(tài)變量的估計(jì)值和估計(jì)協(xié)方差矩陣；表示矩陣的下三角分解平方根的第i列；

利用UKF計(jì)算t_k時(shí)刻非線性狀態(tài)變量的一步預(yù)測(cè)值

其中，為的一步預(yù)測(cè)模型值，為補(bǔ)償項(xiàng)；W_i為權(quán)值；

所述第一更新模塊還具體用于根據(jù)以下公式利用KF對(duì)t_k-1時(shí)刻所述線性狀態(tài)方程中的線性狀態(tài)變量進(jìn)行時(shí)間更新：

其中，為補(bǔ)償項(xiàng)；

第二更新模塊，用于將所述KF更新后的t_k-1時(shí)刻的線性狀態(tài)作為所述非線性狀態(tài)方程的參數(shù)，并利用UKF對(duì)所述非線性狀態(tài)方程進(jìn)行量測(cè)更新，得到t_k時(shí)刻的非線性狀態(tài)變量估計(jì)值以及將所述作為所述線性狀態(tài)方程的參數(shù)，并利用KF對(duì)所述線性狀態(tài)變量方程進(jìn)行量測(cè)更新，得到t_k時(shí)刻的線性狀態(tài)變量估計(jì)值

所述第二更新模塊具體用于，根據(jù)所述非線性狀態(tài)變量一步預(yù)測(cè)值計(jì)算非線性狀態(tài)變量的一步預(yù)測(cè)樣本點(diǎn)

其中，為的誤差，為協(xié)方差矩陣為的零均值高斯白噪聲，為t_k時(shí)刻量測(cè)變量一步預(yù)測(cè)值的自協(xié)方差矩陣，為t_k時(shí)刻非線性狀態(tài)變量一步預(yù)測(cè)值與量測(cè)量一步預(yù)測(cè)值的互協(xié)方差矩陣，和的計(jì)算過(guò)程如下：

其中，為補(bǔ)償項(xiàng)；

計(jì)算非線性狀態(tài)變量的增益矩陣

計(jì)算濾波估計(jì)值及其協(xié)方差矩陣：

以及，所述第二更新模塊具體用于根據(jù)以下獲得時(shí)刻的t_k時(shí)刻的非線性狀態(tài)變量估計(jì)值

其中，為線性狀態(tài)變量的增益矩陣，Y′為的雅可比矩陣，為補(bǔ)償項(xiàng)；I_21×21為21行21列的單位矩陣；

修正模塊，用于根據(jù)所述線性狀態(tài)變量估計(jì)值、非線性狀態(tài)變量估計(jì)值對(duì)所述子系統(tǒng)的捷聯(lián)解算結(jié)果進(jìn)行修正，分別得到修正后的t_k時(shí)刻的子系統(tǒng)的線性狀態(tài)變量值和非線性狀態(tài)變量值；

所述修正模塊具體用于，根據(jù)以下公式對(duì)所述子系統(tǒng)的捷聯(lián)解算結(jié)果進(jìn)行修正得到修正后t_k時(shí)刻的子系統(tǒng)的線性狀態(tài)變量值：

其中，和分別為子IMU修正后的東向、北向和天向速度；和分別為子系統(tǒng)捷聯(lián)解算得到的東向、北向和天向速度；δV_E、δV_N和δV_U分別為t_k時(shí)刻KF估計(jì)出的子系統(tǒng)捷聯(lián)解算東向、北向和天向速度誤差；

L^new＝L^old-δL

λ^new＝λ^old-δλ

h^new＝h^old-δh

其中，L^old、λ^old和h^old分別為子IMU捷聯(lián)解算得到的緯度、經(jīng)度和高度；L^new、λ^new和h^new分別為子IMU修正后的緯度、經(jīng)度和高度；δL、δλ和δh分別為t_k時(shí)刻KF估計(jì)出的子IMU捷聯(lián)解算緯度、經(jīng)度和高度誤差；

所述修正模塊還具體用于，根據(jù)以下對(duì)所述子系統(tǒng)的捷聯(lián)解算結(jié)果進(jìn)行修正得到修正后t_k時(shí)刻的子系統(tǒng)的非線性狀態(tài)變量值：

計(jì)算t_k時(shí)刻子系統(tǒng)真實(shí)導(dǎo)航坐標(biāo)系n與計(jì)算導(dǎo)航坐標(biāo)系n₁間的方向余弦矩陣

計(jì)算t_k時(shí)刻子系統(tǒng)載體坐標(biāo)系b與真實(shí)導(dǎo)航坐標(biāo)系n之間的方向余弦矩陣

其中為t_k時(shí)刻子系統(tǒng)捷聯(lián)解算得到的方向余弦矩陣；

由被更新后的計(jì)算t_k時(shí)刻子系統(tǒng)的航向角ψ_s、俯仰角θ_s和橫滾角γ_s，將記為：

其中，T_lm為矩陣中第l行、第m列的元素，則子IMU航向角ψ_s、俯仰角θ_s和橫滾角γ_s的主值，即ψ_s主、θ_s主和γ_s主分別為：

θ_s主＝arcsin(T₃₂)

θ_s＝θ_s主，