[发明专利]一种闭环正向滤波结合反向平滑的POS后处理方法

摘要

本发明属于POS后处理方法，具体涉及一种闭环正向滤波结合反向平滑的POS后处理方法。目的是提供一种高精度的位置姿态后处理方法。包括：卡尔曼滤波的步骤，和反向R-T-S平滑的步骤，其中，将正向Kalman滤波估计出的水平速度误差和高度误差，通过反馈控制参数实时反馈到惯性导航解算的输入端，进而抑制惯性速度和位置的发散，提高误差模型的线性度，并在此基础上对滤波估计量进行反向R-T-S平滑，从而得到POS的高精度位置姿态后处理结果。优点在于：满足POS的应用需求，提供高精度的位置和姿态信息给相关图像处理系统(例如包括雷达，或者相机)等等。

主权项

1.一种闭环正向滤波结合反向平滑的POS后处理方法，是对POS的位置姿态数据进行后处理的方法，包括：卡尔曼滤波的步骤，和反向R-T-S平滑的步骤，其特征在于：在卡尔曼滤波的过程中，建立卡尔曼滤波模型的过程如下：步骤1、使用二阶系统模型对惯导系统进行描述；设计惯导系统水平通道成典型二阶系统；▿1′(s)=s2+K1s+(K2+1Re)g0---(1)]]>式中：K₁、K₂是水平通道的两个实时反馈控制系数；设计惯导系统垂向通道成典型二阶系统；▿2′(s)=s2+C1s+(C2-2g0Re)---(2)]]>式中：C₁、C₂是垂向通道的两个实时反馈控制系数；典型二阶系统的特征方程为▿(s)=s2+2ξwns+wn2---(3)]]>根据POS系统事后处理过程对滤波调节周期和超调量的要求，确定ξ、w_n；接着按如下过程计算反馈控制系数；1、根据系统的性能要求确定滤波调节周期和超调量；2、根据调节周期和超调量，确定ξ、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分别为惯导系统的北向速度、垂向速度、东向速度，单位：米/秒，为上一时刻的量；δv_N、δv_U、δv_E分别为惯导系统的北向速度误差、垂向速度误差、东向速度误差，单位：米/秒，为上一时刻的量；为惯导系统的纬度，单位：弧度，为上一时刻的量；为惯导系统的纬度误差，单位：弧度，为上一时刻的量；h为惯导系统的高度，单位：米，为上一时刻的量；δh为惯导系统的高度误差，单位：米，为上一时刻的量；φ_U、φ_E分别为惯导系统的垂向失调角、东向失调角，单位：弧度，为上一时刻的量；f_U、f_E分别为垂向加速度、东向加速度，单位：米/秒²，为当前时刻的量；分别为载体系x轴加速度计零偏误差、y轴加速度计零偏误差、z轴加速度计零偏误差，单位：米/秒²，为当前时刻的量；C_1j(j＝1，2，3)分别为姿态矩阵的元素，为上一时刻的量；R_M、R_N分别为地球子午圈、卯酉圈半径，单位：米，为上一时刻的量；ω_ie为地球自转角速率，单位：弧度/秒，为常量；为Kalman滤波估计的系统北速误差，单位：米/秒，为当前时刻的量；K₁为水平通道的实时反馈控制系数，由上述过程计算得到；式中：f_N为北向加速度，单位：米/秒²，为当前时刻的量；φ_N为惯导系统的北向失调角，单位：弧度，为上一时刻的量；C_2j(j＝1，2，3)分别为姿态矩阵的元素，为上一时刻的量；为Kalman滤波估计的系统高度误差，单位：米，为当前时刻的量；C₂为垂向通道的实时反馈控制系数，由上述过程计算得到；式中：C_3j(j＝1，2，3)分别为姿态矩阵的元素，为上一时刻的量；为Kalman滤波估计的系统东速误差，单位：米/秒，为当前时刻的量；式中：C₁为垂向通道的实时反馈控制系数，由上述过程计算得到；(6g)式中：分别为处理成一阶马尔可夫过程的载体系x轴陀螺漂移误差、y轴陀螺漂移误差、z轴陀螺漂移误差，单位：弧度/秒；(6h)(6i)步骤3、建立卡尔曼滤波模型；取状态变量观测量其中上标G表示GPS提供的信息；λ^G分别为GPS提供的纬度、经度，单位：弧度；h^G为GPS提供的高度，单位：米；分别为GPS提供的北向速度、垂向速度、东向速度，单位：米/秒；λ为惯导系统的经度，单位：弧度；根据给出的误差方程，确定系统的状态方程为式中：A(t)为15×15维系统参数矩阵，根据式(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]]>为系统控制向量；W(t)为15×1维系统激励噪声向量；量测方程为：Z＝HX(t)+V(t)                                                (8)式中：H为6×15维量测矩阵；V(t)为量测噪声向量；将状态方程和量测方程离散化为：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_d为系统的滤波周期，单位：秒；N为惯导系统的解算频率；带控制量的卡尔曼滤波器滤波模型为：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为滤波器的系统噪声方差阵；K_k为滤波器的增益阵；R_k为滤波器的量测噪声方差阵。