CN103364817A - 一种基于r-t-s平滑的pos系统双捷联解算后处理方法 - Google Patents

Abstract

一种基于R-T-S平滑的POS系统双捷联解算后处理方法，该方法首先利用载波相位差分GPS数据与惯性数据进行捷联解算及Kalman滤波，得到GPS采样点的组合滤波值及相应滤波参数，然后对滤波参数进行反向R-T-S固定区间平滑，得到Kalman滤波参数的误差校正值，利用该值对GPS采样点的捷联解算值进行校正可以得到GPS采样点的平滑输出。在每相邻GPS采样点之间采用双向捷联解算的方法，提高GPS采样点之间的POS系统输出精度。本发明具有同时提高POS系统GPS采样点输出精度以及滤波区间输出精度的特点，尤其在GPS信号失锁的情况下，能有效提高POS系统事后处理输出精度。

Description

一种基于R-T-S平滑的POS系统双捷联解算后处理方法

技术领域

本发明涉及一种提高POS系统输出精度的事后处理方法，该方法利用R-T-S平滑方法修正Kalman滤波参数，提高了各GPS采样点的POS输出精度，在此基础上，利用双向捷联解算，提高滤波区间内的POS输出精度。

背景技术

位置姿态系统（Position&Orientation System,POS）为遥感载荷提供高精度的位置、速度、姿态信息以进行运动误差补偿。POS通常由惯性测量单元（Inertial MeasurementUnit,IMU）、全球卫星定位系统（Global Position System,GPS）组成，其本质上是一种基于SINS(Strapdown Inertial Navigation System)和GPS（Global Positioning System）的组合导航系统，利用GPS长期定位精度高及SINS短期定位定向精度高而实现的互补运动信息测量系统。提高POS系统输出精度对载荷运动误差补偿具有重要意义。

通常采用Kalman滤波技术实现SINS/GPS组合运动测量。Kalman滤波对误差参数的估计精度取决于系统误差的可观测度，而系统误差的可观测度取决于载体的机动形式。载机在整个飞行作业过程中经历了静止、起飞、前“S”机动及匀速平飞，后“S”机动、下降及降落后静止等阶段，因此在整个飞行作业中，时间越靠后，机动越充分，系统误差可观测度越好，POS系统误差估计精度越高。因此如果能利用所有机动数据对某一时刻的误差参数进行估计，必然能提高该时刻系统误差的可观测度，从而提高POS系统输出精度。

R-T-S平滑方法在Kalman滤波的基础上，进行反向平滑，对每一GPS采样点的误差估计均利用到了所有量测数据，因此能使得在一次作业过程中，每个GPS采样点处的误差状态可观测度均达到最高，从而实现GPS采样点处系统误差的最精确估计。当前事后处理方法为利用R-T-S平滑方法获取GPS采样点处得POS输出，对于两相邻GPS采样点内以正向捷联解算作为POS输出。该方法存在以下不足：其只能实现GPS采样点处的最优估计，而对于GPS采样点区间内没有GPS量测信息时，采用捷联解算的方法计算系统输出。捷联解算误差主要由惯性器件误差引起，随计算时间迅速变大。

发明内容

本发明的技术解决的问题是：针对Kalman滤波在系统误差状态可观测度不高时对误差参数估计精度低的问题，采用R-T-S平滑方法，利用整个飞行作业的所有观测数据提高系统误差状态的可观测度，获取GPS采样点处的最优估计。针对滤波区间内采用单向捷联解算，其误差随时间迅速增大的问题，提出了在滤波区间内采用双向捷联解算代替单向捷联解算的方法，以降低POS系统在滤波区间内的输出误差。

本发明的技术解决方案为：一种基于R-T-S平滑的POS系统双捷联解算后处理方法，包括下列步骤：

（1）采集飞行试验或车载试验过程中POS系统输出的惯性测量数据及GPS基站数据及GPS流动站数据；

（2）对步骤（1）中获取的惯性测量数据进行温度误差补偿，补偿后的惯性数据包括三轴陀螺仪数据

和三轴加速度计数据x,y,z轴的陀螺仪数据分别为

x,y,z轴的加速度计数据分别为

对步骤（1）中获取的GPS数据进行载波相位差分处理，处理后其位置包括纬度L_gps，经度λ_gps及高度H_gps，速度包括东向速度V_Egps，北向速度V_Ngps和天向速度V_Ugps；

（3）利用步骤（2）补偿后的惯性数据及载波相位差分GPS数据进行捷联解算及Kalman滤波，记录每个GPS采样点的捷联解算输出和Kalman滤波参数，捷联解算输出包括位置、速度、姿态，其中位置包括纬度L_i，经度λ_i及高度H_i，速度包括东向速度V_Ei，北向速度V_Ni，天向速度V_Ui,姿态包括航向角ψ_i，俯仰角θ_i，滚动角γ_i；Kalman滤波参数包括离散化状态转移矩阵

预测协方差矩阵

状态预测值

协方差矩阵状态估计值

其中，带有下标kf的参数是由Kalman滤波计算估计得到的结果，带有上标’-‘和’+’的参数分别为预测结果和估计结果，k和k-1分别表示第k和第k-1个GPS采样点；

（4）从最后一个GPS采样点开始，利用步骤（3）记录的与其相邻的前一GPS采样点处的捷联解算输出及Kalman滤波参数，进行反向R-T-S平滑解算，得到其相邻的前一GPS采样点平滑后的误差状态估计

和误差状态协方差矩阵P_s，利用平滑的误差状态估计

对前一GPS采样点处的捷联解算结果进行修正，得到其平滑位置、速度、姿态，其中位置包括纬度L_s，经度λ_s及高度H_s，速度包括东向速度V_E,s，北向速度V_N,s，天向速度V_U,s,姿态包括航向角ψ_s，俯仰角θ_s，滚动角γ_s，其中带有下标s的参数表示是经过平滑后的结果；依次向前迭代进行R-T-S平滑解算，直至计算至第一个GPS采样点，得到每一个GPS采样点的平滑结果。

（5）从第一个GPS采样点开始，在相邻两个GPS采样点的滤波区间内执行双捷联解算，以左GPS采样点的R-T-S平滑位置、速度、姿态做为正向捷联解算的初始值，从左GPS采样点至滤波区间中点执行正向捷联解算；以右GPS采样点的R-T-S平滑位置、速度、姿态做为反向捷联解算的初始值，从右GPS采样点至滤波区间中点，按时间反向顺序执行反向捷联解算，得到每相邻两个GPS采样点构成的滤波区间的POS系统输出。依次向后迭代计算至最后一个GPS采样点。

本发明的原理是：传统POS后处理方法基于载波相位差分GPS数据与惯性数据进行组合Kalman滤波。首先利用惯性数据进行递推的捷联解算，当运行至GPS采样点时，采用Kalman滤波技术将GPS量测与捷联解算结果进行组合滤波，根据滤波得到的参数对捷联解算结果进行误差修正，从而得到不随时间发散的组合滤波结果。然而由于滤波过程是逐渐收敛的，Kalman滤波结果只利用了部分量测数据对某一时刻的捷联解算结果进行误差估计，滤波结果在GPS采样点处存在跳跃现象，误差较大。R-T-S平滑技术在Kalman滤波的基础上进行反向平滑解算，利用整个飞行时段的全部量测数据对所有GPS采样点的滤波参数进行估计，提升了各GPS采样点的滤波参数估计精度。利用R-T-S估计的高精度的滤波参数对各GPS采样点的捷连解算结果进行修正，得到各GPS采样点的POS系统输出，依次得到整个飞行任务的所有GPS采样点POS输出。在此基础上，在相邻GPS采样点内，将整个滤波区间分为两部分，从左GPS采样点开始的1/2个滤波区间内，进行正向捷连解算，从右GPS采样点开始反向的1/2个滤波区间内，进行反向递推的捷连解算，使得各个滤波区间内的捷联解算误差最低。将各滤波区间内双向捷联解算输出结果当做POS输出。

本发明与现有技术相比的优点在于：该方法在GPS采样点，通过R-T-S平滑算法对Kalman滤波参数进行误差修正，获得优于Kalman滤波精度的POS输出；在相邻GPS量测区间内，采用双向捷联解算代替单向捷联解算，降低了单向捷联解算随计算时间增大的误差，使得滤波区间内的POS精度提高约75%左右。

附图说明

图1为本发明的基于R-T-S平滑的POS系统双捷联解算后处理方法流程图；

图2为本发明的步骤3中离散化Kalman滤波示意图；

图3为本发明的步骤4中R-T-S平滑方法原理图。

具体实施方式

一种基于R-T-S平滑的POS系统双捷联解算后处理方法主要分为三部分：第一部分是进行Kalman滤波解算，获取每个GPS采样点处平滑所需要的捷联解算结果和Kalman滤波参数。第二部分是利用R-T-S平滑方法估计每个GPS采样点处的状态误差的平滑输出。第三部分是在GPS采样点输出平滑结果的基础上，利用双向捷联解算实现相邻滤波区间内的POS输出。

本发明的具体实施步骤如下：

（1）采集飞行试验或车载试验过程中POS系统输出的惯性测量数据及GPS基站数据及GPS流动站数据；

（2）对步骤（1）中获取的惯性测量数据进行温度误差补偿，补偿后的惯性数据包括三轴陀螺仪数据

和三轴加速度计数据

x,y,z轴的陀螺仪数据分别为

x,y,z轴的加速度计数据分别为

对步骤（1）中获取的GPS数据进行载波相位差分处理，处理后其位置包括纬度L_gps，经度λ_gps及高度H_gps，速度包括东向速度V_Egps，北向速度V_Ngps和天向速度V_Ugps；

（3）利用步骤（2）补偿后的惯性数据及载波相位差分GPS数据进行捷联解算及Kalman滤波，记录每个GPS采样点的捷联解算输出和Kalman滤波参数，捷联解算输出包括位置、速度、姿态，其中位置包括纬度L_i，经度λ_i及高度H_i，速度包括东向速度V_Ei，北向速度V_Ni，天向速度V_Ui,姿态包括航向角ψ_i，俯仰角θ_i，滚动角γ_i；Kalman滤波参数包括离散化状态转移矩阵

预测协方差矩阵

状态预测值

协方差矩阵

状态估计值

其中，带下标kf的参数是由Kalman滤波计算估计的结果，带上标’-‘和’+’的参数分别为预测结果和估计结果，k和k-1分别表示第k和第k-1个GPS采样点。

①进行捷联解算，捷联解算输出包括位置、速度、和姿态。其中，位置包括纬度L_i、经度λ_i和高度H_i，速度包括东向速度V_Ei、北向速度V_Ni及天向速度V_Ui及姿态：航向角ψ_i，俯仰角θ_i，滚动角γ_i；

②当运行到GPS采样点，对同一时刻的捷联解算输出和GPS信息进行Kalman滤波解算。

a.建立Kalman滤波系统系统方程

$\stackrel{\·}{x}\left(t\right)=F\left(t\right)x\left(t\right)+G\left(t\right)w\left(t\right)$

其中，x(t)为t时刻的系统误差状态矢量，w(t)为t时刻的系统噪声矢量，F(t)为t时刻的系统转移矩阵，G(t)为t时刻的噪声转换矩阵。

x＝[x_f x_a]^T

x_f＝[δL_kf δλ_kf δH_kf δV_E,kf δV_N,kf δV_U,kf φ_E,kf φ_N,kf φ_U,kf]^T

$x_a={\left[\begin{array}{cccccc}{\mathrm{\ϵ}}_{x,\mathrm{kf}}& {\mathrm{\ϵ}}_{y,\mathrm{kf}}& {\mathrm{\ϵ}}_{z,\mathrm{kf}}& {\▿}_{x,\mathrm{kf}}& {\▿}_{y,\mathrm{kf}}& {\▿}_{z,\mathrm{kf}}\end{array}\right]}^T$

$w={\left[\begin{array}{cccccc}{w}_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_y}& w_{{\mathrm{\ϵ}}_z}& w_{{\▿}_x}& w_{{\▿}_y}& w_{{\▿}_z}\end{array}\right]}^T$

其中，δL_kf、δλ_kf、δH_kf分别为由Kalman滤波估计的捷联解算纬度误差、经度误差、高度误差；δV_E,kf、δV_N,kf、δV_U,kf分别为Kalman滤波估计的捷联解算东向速度误差、北向速度误差、天向速度误差；

分别为Kalman滤波估计的捷联解算东向失准角、北向失准角、天向失准角。ε_x,kf、ε_y，kf、ε_z,kf分别为Kalman滤波估计的x、y、z轴陀螺的零漂，

分别为Kalman滤波估计的x、y、z轴加速度计的零偏。分别为x、y、z轴陀螺的噪声，分别为加速度计x、y、z的噪声。

$F=\left[\begin{array}{ccccc}{F}_{11}& F_{12}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ F_{21}& F_{22}& F_{23}& 0_{3\×3}& C_b^n\\ F_{31}& F_{32}& F_{33}& C_b^n& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{33\×}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$ $G=\left[\begin{array}{cc}{0}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ C_b^n& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]$

是载体系到导航系的资态变换阵。

$C_b^n=\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_i\cos {\mathrm{\ψ}}_i-\sin {\mathrm{\γ}}_i\sin {\mathrm{\θ}}_i\sin {\mathrm{\ψ}}_i& -\cos {\mathrm{\θ}}_i\sin {\mathrm{\ψ}}_i& \sin {\mathrm{\γ}}_i\cos {\mathrm{\ψ}}_i+\cos {\mathrm{\γ}}_i\sin {\mathrm{\θ}}_i\sin {\mathrm{\ψ}}_i\\ \cos {\mathrm{\γ}}_i\sin {\mathrm{\ψ}}_i+\sin {\mathrm{\γ}}_i\sin {\mathrm{\θ}}_i\cos {\mathrm{\ψ}}_i& \cos {\mathrm{\θ}}_i\cos {\mathrm{\ψ}}_i& \sin {\mathrm{\γ}}_i\sin {\mathrm{\ψ}}_i-\cos {\mathrm{\γ}}_i\sin {\mathrm{\θ}}_i\mathrm{cso}{\mathrm{\ψ}}_i\\ -\sin {\mathrm{\γ}}_i\cos {\mathrm{\θ}}_i& \sin {\mathrm{\θ}}_i& \cos {\mathrm{\γ}}_i\cos {\mathrm{\θ}}_i\end{array}\right]$

系统转移矩阵各子矩阵表示如下：

$F_{11}=\left[\begin{array}{ccc}0& 0& \frac{-V_{\mathrm{Ni}}}{{(R_M+H_i)}^2}\\ \frac{V_{\mathrm{Ei}}\·\sec L_i\·\tan L_i}{R_N+H_i}& 0& \frac{-V_{\mathrm{Ei}}\sec L_i}{{(R_N+H_i)}^2}\\ 0& 0& 0\end{array}\right]$ $F_{12}=\left[\begin{array}{ccc}0& \frac{1}{R_M+H_i}& 0\\ \frac{\sec L_i}{R_N+H_i}& 0& 0\\ 0& 0& 1\end{array}\right]$

$F_{21}=\left[\begin{array}{ccc}2{\mathrm{\ω}}_{\mathrm{ie}}{V}_{\mathrm{Ni}}\cos L_i+\frac{V_{\mathrm{Ei}}{V}_{\mathrm{Ni}}{\sec }^2{L}_i}{R_N+H_i}+2{\mathrm{\ω}}_{\mathrm{ie}}{V}_{\mathrm{Ui}}\sin L_i& 0& \frac{V_{\mathrm{Ei}}{V}_{\mathrm{Ui}}-V_{\mathrm{Ei}}{V}_{\mathrm{Ni}}\tan L_i}{{(R_N+H_i)}^2}\\ (2{\mathrm{\ω}}_{\mathrm{ie}}\cos L_i+\frac{V_{\mathrm{Ei}}{\sec }^2{L}_i}{R_N+H_i})V_{\mathrm{Ei}}& 0& \frac{V_{\mathrm{Ni}}{V}_{\mathrm{Ui}}-V_{\mathrm{Ei}}^2\tan L_i}{{(R_N+H_i)}^2}\\ -2{\mathrm{\ω}}_{\mathrm{ie}}{V}_{\mathrm{Ei}}\sin L_i& 0& \frac{V_{\mathrm{Ei}}^2+V_{\mathrm{Ni}}^2}{{(R_N+H_i)}^2}\end{array}\right]$

$F_{22}=\left[\begin{array}{ccc}\frac{V_{\mathrm{Ni}}\tan L_i-V_{\mathrm{Ui}}}{R_N+H_i}& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L_i+\frac{V_{\mathrm{Ei}}\tan L_i}{R_N+H_i}& -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L_i+\frac{V_{\mathrm{Ei}}}{R_N+H_i})\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L_i+\frac{V_{\mathrm{Ei}}\mathrm{tna}{L}_i}{R_N+H_i})& -\frac{V_{\mathrm{Ui}}}{R_M+H_i}& -\frac{V_{\mathrm{Ni}}}{R_M+H_i}\\ 2({\mathrm{\ω}}_{\mathrm{ie}}\cos L_i+\frac{V_{\mathrm{Ei}}}{R_N+H_i})& 2\frac{V_{\mathrm{Ni}}}{R_M+H_i}& 0\end{array}\right]$

$F_{23}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\\ -f_N& f_E& 0\end{array}\right]$ $F_{31}=\left[\begin{array}{ccc}0& 0& \frac{V_{\mathrm{Ni}}}{{(R_M+H_i)}^2}\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L_i& 0& \frac{-V_E}{{(R_N+H_i)}^2}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L_i+\frac{V_{\mathrm{Ei}}{\sec }^2{L}_i}{R_N+H_i}& 0& \frac{-V_{\mathrm{Ei}}\tan L_i}{{(R_N+H_i)}^2}\end{array}\right]$

$F_{32}=\left[\begin{array}{ccc}0& \frac{-1}{R_M+H_i}& 0\\ \frac{1}{R_N+H_i}& 0& 0\\ \frac{\tan L_i}{R_N+H_i}& 0& \frac{-V_{\mathrm{Ei}}\tan L_i}{{(R_N+H_i)}^2}\end{array}\right]$

$F_{33}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L_i+\frac{V_{\mathrm{Ei}}\tan L_i}{R_N+H_i}& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L_i+\frac{V_{\mathrm{Ei}}}{R_N+H_i})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L_i+\frac{V_{\mathrm{Ei}}\tan L_i}{R_N+H_i})& 0& \frac{-V_{\mathrm{Ni}}}{R_M+H_i}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L_i+\frac{V_{\mathrm{Ei}}}{R_N+H_i}& \frac{V_{\mathrm{Ni}}}{R_M+H_i}& \frac{-V_{\mathrm{Ei}}\tan L_i}{{(R_N+H_i)}^2}\end{array}\right]$

$\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]=C_b^n{f}_{\mathrm{ib}}^b$

上式中，R_M、R_N分别为地球的子午圈主曲率半径，卯酉圈主曲率半径，ω_ie为地球自转角速度。

b.建立Kalman滤波量测方程

z(t)＝H(t)x(t)+v(t)

其中：z(t)、H(t)分别为t时刻的观测矢量和观测矩阵，v(t)为量测噪声：

$z={\left[\begin{array}{cccccc}{\mathrm{\δL}}^{\′}& {\mathrm{\δ\λ}}^{\′}& {\mathrm{\δH}}^{\′}& {\mathrm{\δV}}_E^{\′}& {\mathrm{\δV}}_N^{\′}& {\mathrm{\δV}}_U^{\′}\end{array}\right]}^T$

$\left\{\begin{array}{c}{\mathrm{\δL}}^{\′}=L_i-L_{\mathrm{gps}}\\ {\mathrm{\δ\λ}}^{\′}={\mathrm{\λ}}_i-{\mathrm{\λ}}_{\mathrm{gps}}\\ {\mathrm{\δH}}^{\′}=H_i-H_{\mathrm{gps}}\\ {\mathrm{\δV}}_E^{\′}=V_{\mathrm{Ei}}-V_{\mathrm{Egps}}\\ {\mathrm{\δV}}_N^{\′}=V_{\mathrm{Ni}}-V_{\mathrm{Ngps}}\\ {\mathrm{\δV}}_U^{\′}=V_{\mathrm{Ui}}-V_{\mathrm{Ugps}}\end{array}\right.$

量测矩阵H＝[H_P H_V]^T

H_P＝[diag(R_M+H_i(R_N+H_i)cosL_i 1)0_3×6 0_3×6]

H_V＝[0_3×3 diag([1 1 1])0_3×9]

$v={\left[\begin{array}{cccccc}{v}_{{\mathrm{\δL}}^{\′}}& v_{{\mathrm{\δ\λ}}^{\′}}& v_{{\mathrm{\δH}}^{\′}}& v_{{\mathrm{\δV}}_E^{\′}}& v_{{\mathrm{\δV}}_N^{\′}}& v_{{\mathrm{\δV}}_U^{\′}}\end{array}\right]}^T$

w(t)和v(t)均为零均值的高斯白噪声，满足

$\left\{\begin{array}{c}E\left[w\left(t\right)\right]=0,E\left[w\left(t\right)w^T(t+\mathrm{\Δt})\right]=\mathrm{Q\δ}\left(\mathrm{\Δt}\right)\\ E\left[v\left(t\right)\right]=0,E\left[v\left(t\right)v^T(t+\mathrm{\Δt})\right]=\mathrm{R\δ}\left(\mathrm{\Δt}\right)\\ E\left[w\left(t\right)v^T(t+\mathrm{\Δt})\right]=0\end{array}\right.$

式中：Q∈R^6×6是过程噪声方差阵，为非负定阵；R∈R^6×6为量测噪声方差阵，为正定阵；Δt为采样时间间隔。

c.Kalman滤波系统方程和量测方程离散化

令t＝t_k-1，t+Δt＝t_k。t_k时刻的线性离散型系统方程可表示为

$x_k={\mathrm{\Φ}}_{k/k-1}{x}_{k-1}+G_{k-1}^w{w}_{k-1}$

y_k＝H_kx_k+v_k

式中：Φ_k/k-1为状态转移矩阵F的离散化形式。当Δt（即滤波周期）较短时，F(t)可近似看作常阵，即

F(t)≈F(t_k-1)   t_k-1≤t＜t_k

此时，状态转移矩阵Φ_k/k-1有如下计算式：

${\mathrm{\Φ}}_{k/k-1}=I+F_{k-1}\mathrm{\Δt}+F_{k-1}^2\frac{{\mathrm{\Δt}}^2}{2!}+F_{k-1}^3\frac{{\mathrm{\Δt}}^3}{3!}\·\·\·$

如果POS系统中的惯性测量单元（IMU）与GPS天线之间存在杆臂R_I，还需要进行杆臂补偿，量测方程离散化表示如下：

$\left[\begin{array}{c}{\mathrm{\δ\λ}}^{\′}\\ {\mathrm{\δ\λ}}^{\′}\\ {\mathrm{\δH}}^{\′}\end{array}\right]=\left[\begin{array}{c}{L}_i-L_{\mathrm{gps}}\\ {\mathrm{\λ}}_i-{\mathrm{\λ}}_{\mathrm{gps}}\\ H_i-H_{\mathrm{gps}}\end{array}\right]+\mathrm{\Π}{C}_b^n{R}_I$

Π＝diag([(R_M+H_i)^-1,secL/(R_N+H_i),1])

$\left[\begin{array}{c}{\mathrm{\δV}}_E^{\′}\\ {\mathrm{\δV}}_N^{\′}\\ {\mathrm{\δV}}_U^{\′}\end{array}\right]=\left[\begin{array}{c}{V}_{\mathrm{Ei}}-V_{\mathrm{Egps}}\\ V_{\mathrm{Ni}}-V_{\mathrm{Ngps}}\\ V_{\mathrm{Ui}}-V_{\mathrm{Ugps}}\end{array}\right]+C_b^n({\mathrm{\ω}}_{\mathrm{nb}}^b\×R_I)$

离散化的卡尔曼滤波基本算法编排:

状态一步预测方程

${\hat{X}}_{k,\mathrm{kf}}^{-}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1,\mathrm{kf}}^{+}$

状态估值计算方程

${\hat{X}}_{k,\mathrm{kf}}^{+}={\hat{X}}_{k,\mathrm{kf}}^{-}+K_k(Z_k-H_k{\hat{X}}_{k,\mathrm{kf}}^{-})$

滤波增量方程

$K_{k,\mathrm{kf}}=P_{k,\mathrm{kf}}^{-}{H}_k^T{(H_k{P}_{k,\mathrm{kf}}^{-}{H}_k^T+R_k)}^{-1}$

一步预测均方误差方程

估计均方误差方程

$P_{k,\mathrm{kf}}^{+}=(I-K_{k,\mathrm{kf}}{H}_k)P_{k,\mathrm{kf}}^{-}{(I-K_{k,\mathrm{kf}}{H}_k)}^T+K_{k,\mathrm{kf}}{R}_k{K}_{k,\mathrm{kf}}^T$

卡尔曼滤波流程图如图2所示，左右侧回路分别为增益计算回路及滤波计算回路。在增益计算回路中，根据系统状态转移矩阵

前一时刻的估计均方误差

系统噪声方差阵Q_k-1得到一步预测均方误差

由

及观测矩阵H_k和量测噪声方差阵R_k得到滤波增益阵K_k,kf。根据K_k,kf及

得到本次估计均方误差。在滤波计算回路中，由前一时刻状态估值

及

得到状态单步预测

由

量测Z_k及滤波增益K_k,kf得到状态估计

经过卡尔曼滤波，得到该时刻的位置误差估计值δL_kf、δλ_kf、δH_kf，速度误差估计值δV_E,kf、δV_N,kf、δV_U,kf，姿态误差估计值φ_E,kf、φ_N,kf、φ_U,kf。

d.反馈校正

将Kalman滤波估计的系统状态误差反馈至捷联解算输出，对捷联解算输出的位置、速度、姿态进行校正，得到POS系统在GPS采样点处的Kalman滤波输出，以其输出为POS输出继续进行迭代解算，直至最后一个惯性数据。

[L_kf λ_kf H_kf]^T＝[L_i λ_i H_i]^T-[δL_kf δλ_kf δH_kf]^T

IMU敏感中心速度修正：

[V_E,kf V_N,kf V_U,kf]^T＝[V_Ei V_Ui V_Ni]^T-[δV_E,kf δV_N,kf δV_U，kf]^T

Kalman滤波解算姿态修正：

$C_{b,\mathrm{kf}}^n=\left[\begin{array}{ccc}1& -{\mathrm{\Φ}}_{U,\mathrm{kf}}& {\mathrm{\Φ}}_{N,\mathrm{kf}}\\ 0& 1& -{\mathrm{\Φ}}_{E,\mathrm{kf}}\\ -{\mathrm{\Φ}}_{N,\mathrm{kf}}& {\mathrm{\Φ}}_{E,\mathrm{kf}}& 1\end{array}\right]C_{b,i}^n$

由修正后的

可解得POS系统Kalman滤波解算输出的航向角Ψ_kf、俯仰角θ_kf和滚动角γ_kf。

${\mathrm{\Ψ}}_{\mathrm{kf}}=\arctan [-C_{b,\mathrm{kf}}^n\left(\mathrm{1,2}\right)/C_{b,\mathrm{kf}}^n\left(\mathrm{2,2}\right)]$

${\mathrm{\θ}}_{\mathrm{kf}}=\arcsin \left[C_{b,\mathrm{kf}}^n\left(\mathrm{3,2}\right)\right]$

${\mathrm{\γ}}_{\mathrm{kf}}=\arctan [-C_{b,\mathrm{kf}}^n\left(\mathrm{3,1}\right)/C_{b,\mathrm{kf}}^n\left(\mathrm{3,3}\right)]$

③存储滤波前的捷联解算输出和Kalman滤波信息。

存储的捷联解算输出包括：每当运行到GPS采样点时，将捷联解算计算得到的位置、速度、姿态记录在存储器中。

存储的Kalman滤波信息包括离散化状态转移矩阵预测协方差矩阵

状态预测值协方差矩阵

状态估计值

（4）从最后一个GPS采样点开始，利用步骤（3）记录的与其相邻的前一GPS采样点处的捷联解算输出及Kalman滤波参数，进行反向R-T-S平滑解算，得到其相邻的前一GPS采样点平滑后的误差状态估计

和误差状态协方差矩阵P_s，利用平滑的误差状态估计对前一GPS采样点处的捷联解算结果进行修正，得到其平滑位置、速度、姿态，其中位置包括纬度L_s，经度λ_s及高度H_s，速度包括东向速度V_E,s，北向速度V_N,s，天向速度V_U,s,姿态包括航向角ψ_s，俯仰角θ_s，滚动角γ_s，其中带有下标s的参数表示是经过平滑后的结果；依次向前迭代进行R-T-S平滑解算，直至计算至第一个GPS采样点，得到每一个GPS采样点的平滑结果。

①R-T-S平滑初值设置

分别以Kalman滤波最后一个GPS采样点的误差状态估计值

和误差状态协方差阵

做为平滑的误差状态初值和误差状态协方差阵。

${\hat{X}}_{s\_\mathrm{ini}}={\hat{X}}_{f\_\mathrm{last}}^{+};P_{s\_\mathrm{ini}}=P_{f\_\mathrm{final}}^{+}$

为初始的平滑状态估计值，P_{s_ini}为初始的误差状态协方差平滑矩阵。

②R-T-S平滑参数估计

从Kalman滤波的最后一个GPS采样点开始逆向递推计算每一个GPS采样点的平滑估计值，该计算过程用到了步骤（4）中存储的GPS采样点的Kalman滤波参数离散化状态转移矩阵

预测协方差矩阵

状态预测值协方差矩阵

状态估计值反向R-T-S平滑估计如下：

$P_k^s=P_{f,k}^{+}+K_k^s[P_{k+1}^s-P_{k+1,\mathrm{kf}}^{-}]{\left(K_k^s\right)}^T$

${\hat{x}}_k^s={\hat{x}}_{k,\mathrm{kf}}^{+}+K_k^s[{\hat{x}}_{k+1}^s-x_{k+1,\mathrm{kf}}^{-}]$

式中，下标k，k+1分别代表第k个GPS采样点，第k+1个GPS采样点。上标s代表平滑估计值。

为平滑估计的滤波增益阵。平滑计算原理图如图3所示。图3中，第k个GPS采样点处的平滑估计结果由两部分组成，第一部分为第k个GPS采样点的Kalman滤波估计值

第二部分为第k+1个GPS采样点的平滑估计的新息结果

平滑新息增益矩阵为k时刻的平滑估计可以理解为利用k+1时刻平滑估计得到的新息对k时刻Kalman滤波的估计结果进行的修正。

③R-T-S平滑校正

利用步骤（4）的R-T-S平滑参数估计中计算得到的平滑参数对步骤（3）计算过程存储的捷联解算结果进行修正，修正方法与步骤（3）中Kalman滤波解算部分的反馈校正方法相同，只需将该步骤中的Kalman滤波误差状态估计值

替换为R-T-S平滑误差状态估计值

R-T-S平滑修正后的GPS采样点输出为POS系统平滑输出。

（5）从第一个GPS采样点开始，在相邻两个GPS采样点的滤波区间内执行双捷联解算，以左GPS采样点的R-T-S平滑位置、速度、姿态做为正向捷联解算的初始值，从左GPS采样点至滤波区间中点执行正向捷联解算；以右GPS采样点的R-T-S平滑位置、速度、姿态做为反向捷联解算的初始值，从右GPS采样点至滤波区间中点，按时间反向顺序执行反向捷联解算，得到每相邻两个GPS采样点构成的滤波区间的POS系统输出。依次向后迭代计算至最后一个GPS采样点。

①从左GPS采样点至1/2滤波区间进行正向捷联解算。

${\stackrel{\·}{L}}_i=V_{\mathrm{Ni}}/(R_m/H_i),$ ${\stackrel{\·}{\mathrm{\λ}}}_i=V_{\mathrm{Ei}}\sec L_i/(R_n+H_i),$ ${\stackrel{\·}{H}}_i=V_{\mathrm{Ui}}$

${\stackrel{\·}{V}}^n=f^n-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×V^n+g_1^n)$

其中，Vⁿ＝[V_Ei V_Ni V_Ui]^T

$\stackrel{\·}{q}=\frac{1}{2}q\⊗{\mathrm{\ω}}_{\mathrm{nb}}^b$

$f^n={\left[\begin{array}{ccc}{f}_E& f_N& f_U\end{array}\right]}^T=C_b^n{f}_{\mathrm{ib}}^b$

${\mathrm{\ω}}_{\mathrm{ie}}^n={\left[\begin{array}{ccc}\mathrm{\Ω}\cos L_i& 0& \mathrm{\Ω}\sin L_i\end{array}\right]}^T$

${\mathrm{\ω}}_{\mathrm{en}}^n={[V_{\mathrm{Ei}}/(R_n+H_i)-V_{\mathrm{Ni}}/(R_m+H_i)-V_{\mathrm{Ei}}\tan L_i/(R_m+H_i)]}^T$

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)$

$g_1^n=g-\frac{{\mathrm{\Ω}}^2(R_0+H_i)}{2}\left(\begin{array}{c}\sin 2L_i\\ 0\\ 1+\cos 2L_i\end{array}\right)$

Ω为地球自转角速度,g为地球重力加速度，q＝[q₀ q₁ q₂ q₃]是描述载体转动的姿态四元数。

$C_b^n=\left[\begin{array}{ccc}1-2(q_2^2+q_3^2)& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_0{q}_3+q_1{q}_2)& 1-2(q_1^2+q_3^2)& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& 1-2(q_1^2+q_2^2)\end{array}\right]$

②从右GPS采样点至后1/2滤波区间进行反向捷联解算

反向捷联解算与正向捷联解算公式相同，但滤波时间Δt＝t_k-1-t_k。

Claims (4)

1.一种基于R-T-S平滑的POS系统双捷联解算后处理方法，其特征在于包括下列步骤：

（1）采集飞行试验或车载试验过程中POS系统输出的惯性测量数据及GPS基站数据及GPS流动站数据；

（2）对步骤（1）中获取的惯性测量数据进行温度误差补偿，补偿后的惯性数据包括三轴陀螺仪数据

和三轴加速度计数据

x,y,z轴的陀螺仪数据分别为

x,y,z轴的加速度计数据分别为；对步骤（1）中获取的GPS数据进行载波相位差分处理，处理后其位置包括纬度L_gps，经度λ_gps及高度H_gps，速度包括东向速度V_Egps，北向速度V_Ngps和天向速度V_Ugps；

（3）利用步骤（2）补偿后的惯性数据及载波相位差分GPS数据进行捷联解算及Kalman滤波，记录每个GPS采样点的捷联解算输出和Kalman滤波参数，捷联解算输出包括位置、速度、姿态，其中位置包括纬度L_i，经度λ_i及高度H_i，速度包括东向速度V_Ei，北向速度V_Ni，天向速度V_Ui,姿态包括航向角ψ_i，俯仰角θi，滚动角γi；Kalman滤波参数包括离散化状态转移矩阵预测协方差矩阵

状态预测值

协方差矩阵

状态估计值

其中，带有下标kf的参数是由Kalman滤波计算估计得到的结果，带有上标’-‘和’+’的参数分别为预测结果和估计结果，k和k-1分别表示第k和第k-1个GPS采样点；

（4）从最后一个GPS采样点开始，利用步骤（3）记录的与其相邻的前一GPS采样点处的捷联解算输出及Kalman滤波参数，进行反向R-T-S平滑解算，得到其相邻的前一GPS采样点平滑后的误差状态估计和误差状态协方差矩阵P_s，利用平滑的误差状态估计

对前一GPS采样点处的捷联解算结果进行修正，得到其平滑位置、速度、姿态，其中位置包括纬度L_s，经度λ_s及高度H_s，速度包括东向速度V_E,s，北向速度V_N,s，天向速度V_U,s，姿态包括航向角ψ_s，俯仰角θ_s，滚动角γ_s，其中带有下标s的参数表示是经过平滑后的结果；依次向前迭代进行R-T-S平滑解算，直至计算至第一个GPS采样点，得到每一个GPS采样点的平滑结果。

（5）从第一个GPS采样点开始，在相邻两个GPS采样点的滤波区间内执行双捷联解算，以左GPS采样点的R-T-S平滑位置、速度、姿态作为正向捷联解算的初始值，从左GPS采样点至滤波区间中点执行正向捷联解算；以右GPS采样点的R-T-S平滑位置、速度、姿态作为反向捷联解算的初始值，从右GPS采样点至滤波区间中点，按时间反向顺序执行反向捷联解算，得到每相邻两个GPS采样点构成的滤波区间的POS系统输出，依次向后迭代计算至最后一个GPS采样点。

2.根据权利要求1所述的一种基于R-T-S平滑的POS系统双捷联解算后处理方法，其特征在于：步骤（4）所述的反向R-T-S平滑算法步骤为：

A、取Kalman滤波最后GPS采样点的滤波结果作为反向R-T-S平滑初始值；

B、从最后一个GPS采样点反向递推计算每一个GPS采样点的R-T-S平滑值；

C、计算至第一个GPS采样点平滑值后，用每个GPS采样点的平滑状态估计值

对步骤（2）中相同时刻的捷联解算结果进行误差校正，得到每一个GPS采样点处的POS平滑输出。

3.根据权利要求1所述的一种基于R-T-S平滑的POS系统双捷联解算后处理方法，其特征在于：步骤（5）所述的反向捷联解算计算方法为：

（A）对于相邻的两个GPS采样点，从时间顺序上在前的GPS采样点称之为左GPS采样点，在时间顺序上靠后的GPS采样点称为右GPS采样点；反向捷联解算以右GPS采样点计算得到的平滑位置、速度、姿态作为反向捷联解算的初始位置、速度、姿态；

（B）反向捷联解算的位置、速度、姿态计算公式如下：

位置方程：

$\stackrel{\·}{L}=V_N/(R_m+H),$ $\stackrel{\·}{\mathrm{\λ}}=V_E\sec L/(R_n+H),$ $\stackrel{\·}{H}=V_U$

其中，L,λ,H代表地理系下的纬度，经度，高度，V_E,V_N,V_U代表东向速度，北向速度，天向速度，R_m,R_n分别代表地球的子午圈半径，卯酉圈半径；

速度方程：

${\stackrel{\·}{V}}^n=f^n-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×V^n+g_1^n)$

其中，Vⁿ＝[V_E V_N V_U]^T

$f^n={\left[\begin{array}{ccc}{f}_E& f_N& f_U\end{array}\right]}^T=C_b^n{f}_{\mathrm{ib}}^b$

${\mathrm{\ω}}_{\mathrm{ie}}^n={\left[\begin{array}{ccc}\mathrm{\Ω}\cos L& 0& \mathrm{\Ω}\sin L\end{array}\right]}^T$

${\mathrm{\ω}}_{\mathrm{en}}^n={[V_E/(R_n+H)-V_N/(R_m+H)-V_E\tan L/(R_m+H)]}^T$

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)$

$g_1^n=g-\frac{{\mathrm{\Ω}}^2(R_0+H)}{2}\left(\begin{array}{c}\sin 2L\\ 0\\ 1+\cos 2L\end{array}\right)$

Ω为地球自转角速度,g为地球重力加速度；

四元数方程为：

$\stackrel{\·}{q}=\frac{1}{2}q\⊗{\mathrm{\ω}}_{\mathrm{nb}}^b$

q＝[q₀ q₁ q₂ q₃]是描述载体转动的姿态四元数；

$C_b^n=\left[\begin{array}{ccc}1-2(q_2^2+q_3^2)& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_0{q}_3+q_1{q}_2)& 1-2(q_1^2+q_3^2)& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& 1-2(q_1^2+q_2^2)\end{array}\right]$

$C_n^b={\left(C_b^n\right)}^T;$

（C）按时间逆向过程，从右GPS采样点开始进行反向捷联解算，反向捷联解算的时间增量为Δt＝t_n-1-t_n，n-1、n分别为第n-1和第n个惯性数据采样时刻的UTC时间，即Δt为负值。

4.根据权利要求1所述的一种基于R-T-S平滑的POS系统双捷联解算后处理方法，其特征在于：步骤（5）所述的双捷联解算计算方法为：

①以左GPS采样点的平滑位置、速度、姿态为正向捷联解算的初始位置、速度、姿态，在前1/2个滤波区间内按时间正向顺序执行正向捷联解算；

②以右GPS采样点的平滑位置、速度、姿态为反向捷联解算的初始位置、速度、姿态，在后1/2个滤波区间内按时间反向顺序执行反向捷联解算。

Images