[发明专利]一种闭环正向滤波结合反向平滑的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为滤波器的量测噪声方差阵。