[发明专利]一种基于强跟踪SDRE滤波的组合导航系统及导航方法

摘要

本发明公开了一种基于强跟踪SDRE滤波的组合导航系统及导航方法，其中，所述系统包括GPS接收机、惯性导航子系统、和滤波器，其中，所述滤波器包括空间模块、SDRE分解模块、离散化处理模块和强跟踪滤波模块，并且，所述强跟踪滤波模块又包括初始化模块、一步预测子模块、增益矩阵获得子模块、实际状态估计子模块和误差方差阵更新子模块。所述方法包括以下步骤：先进行SDRE分解，然后进行离散化处理，最后进行强跟踪滤波，得到组合导航数据，并进行数据输出。本发明所述的系统和方法引入了自适应渐消因子，能够进行准确滤波，得到精确的导航数据。

主权项

1.一种基于强跟踪SDRE滤波的组合导航系统，其特征在于，所述系统包括：GPS接收机(1)，用于输出通过GPS技术得到的当前地理位置和当前速度信息；惯性导航子系统(2)，其包括加速度计(21)和陀螺仪(22)，用于检测并输出通过惯性导航技术得到的当前地理位置、当前速度和当前姿态角信息；和滤波器(3)，用于对GPS接收机(1)和惯性导航子系统(2)输出的数据进行数据处理，得到组合导航信息并进行输出；所述滤波器(3)包括空间模块(31)，在其内集成有状态空间和量测空间；SDRE分解模块(32)，用于对空间模块(31)内的状态空间和量测空间进行状态相关分解，分别得到状态分解空间和量测分解空间；离散化处理模块(33)，用于对SDRE分解模块(32)内的状态分解空间和量测分解空间进行离散化处理，分别得到状态离散空间和量测离散空间；和强跟踪滤波模块(34)，用于对离散化处理模块(33)内的状态离散空间和量测离散空间进行强跟踪滤波处理，得到组合导航信息；所述状态空间和量测空间分别如式(31‑1)和式(31‑2)所示：z＝h(x)+V    式(31‑2)；其中，在式(31‑1)中：其中，φ_e、φ_n和φ_u分别为数学平台误差角，实时进行更新；v_e、v_n和v_u分别为载体的东向、北向、天向的速度；λ和h分别为载体所在的纬度，经度和高度；ε_e、ε_n和ε_u分别为陀螺仪常值漂移，Δ_e、Δ_n和Δ_u分别为加速度计常值漂移；其中，和分别表示陀螺仪在东向、北向和天向的随机误差，和分别表示加速度计在东向、北向和天向的随机误差；其中，所述随机误差即为系统噪声；G为系统噪声转移矩阵，其中，I表示单位矩阵；在式(31‑2)中：量测信息z＝[v_ge,v_gn,v_gu,p_ge,p_gn,p_gu]^T，其中，v_ge表示GPS接收机测得的载体在东向的速度，v_gn表示GPS接收机测得的载体在北向的速度，v_gu表示GPS接收机测得的载体在天向的速度，p_ge表示GPS接收机测得的载体在东向的位置，p_gn表示GPS接收机测得的载体在北向的位置，p_gu表示GPS接收机测得的载体在天向的位置；V为量测噪声，V＝[V_vge,V_vgn,V_vgu,V_pge,V_pgn,V_pgu]^T，其中，V_vge表示GPS接收机测得的载体在东向速度的随机误差，V_vgn表示GPS接收机测得的载体在北向速度的随机误差，V_vgu表示GPS接收机测得的载体在天向速度的随机误差，P_pge表示GPS接收机测得的载体在东向位置的随机误差，P_pgn表示GPS接收机测得的载体在北向位置的随机误差，P_pgu表示GPS接收机测得的载体在天向位置的随机误差；h(x)是线性方程，h(x)＝Hx，其中，H为量测矩阵，其中：所述状态分解空间和量测分解空间分别如式(32‑1)和式(32‑2)所示:z＝Hx+V    式(32‑2)；其中，在式(32‑1)中，F(x)为状态转移矩阵，其中，F_N是9维矩阵，其中的非零元素为：F(4,2)＝‑f_u    F(4,3)＝f_nF(5,1)＝f_u    F(5,3)＝‑f_eF(6,1)＝‑f_n    F(6,2)＝f_eF(9,6)＝1；其中，ω_ie是地球自转角速率；和h分别为惯导系统测得的载体所在的纬度和高度；v_e、v_n分别为惯导系统测得的载体的东向、北向的速度；δv_e为惯导系统所测的东向速度与GPS接收机所测的东向速度之差，δv_n为惯导系统所测的东向速度与GPS接收机所测的北向速度之差；为惯导系统所测的纬度与GPS接收机所测的纬度之差，δh为惯导系统所测的高度与GPS接收机所测的高度之差；f_e、f_n和f_u分别为加速度计的测量值在东向、北向和天向三个方向的加速度分量；R_M是当地子午面内主曲率半径；R_N是卯酉面内主曲率半径；其中，地球长半径R_e＝6378245，地球扁率e＝1/298.257；F_s为：H为量测矩阵，其中：所述状态离散空间和量测离散空间分别如式(33‑1)和式(33‑2)所示：x_k+1＝Φ_k+1/kx_k+Γ_kw_k    式(33‑1)，z_k+1＝Θ_k+1x_k+1+V_k+1    式(33‑2)；其中，Φ_k+1/k是离散后的状态转移矩阵，Г_k是离散后的噪声转移矩阵，Θ_k+1是离散后的量测矩阵；w_k表示在k时刻的系统噪声向量；x_k表示k时刻的状态；x_k+1表示k+1时刻的实际状态估计值；z_k+1为k+1时刻的量测数据；所述强跟踪滤波模块(34)包括一步预测子模块(341)、增益矩阵获得子模块(342)、实际状态估计子模块(343)、误差方差阵更新子模块(344)；所述状态的一步预测如式(341‑1)所示进行处理：其中，在式(341‑1)中，表示根据k时刻的状态预测k+1时刻的状态预测值，x_k表示k时刻的状态，Δt表示滤波周期；所述误差方差阵的一步预测如式(341‑2)所示进行处理：其中，在式(341‑2)中，P_k+1/k表示根据k时刻的误差方差阵预测得到的k+1时刻的误差方差阵的预测值，β_k为自适应渐消因子，P_k表示k时刻的误差方差阵，Φ_k+1/k表示离散后的状态转移矩阵，表示Φ_k+1/k的转置矩阵，Q_k表示w_k的系统噪声方差阵，k表示当前时刻，k+1表示下一时刻，其中，k＝0,1,2,3,…，L，其中，L由滤波时间和滤波周期决定，L＝滤波时间/滤波周期；所述实际状态估计值x_k+1的获得如式(343‑1)所示：其中，表示根据k时刻的状态预测的k+1时刻的预测状态值，K_k+1表示k+1时刻的增益矩阵，z_k+1为k+1时刻的量测数据、根据GPS给出的k+1时刻的信息获得，表示根据一步预测状态值的量测值。