[发明专利]一种长时间导航系统全参数精度评估方法

权利要求书

1.一种长时间导航系统全参数精度评估方法，其特征在于：利用局部基准系统航行实验的原始数据及卫星信息，采用基于RTS平滑的离线处理方法，通过对存储的惯导数据及卫星数据的处理，得到基准系统高精度的位置、姿态及方位信息，作为基准系统的全参数精度评估的基准；

具体包括以下两个步骤以获取位置、速度及航姿数据：

（1)利用惯性/卫星数据信息进行前向的闭环卡尔曼滤波过程；

前向卡尔曼滤波算法采取GPS与惯性导航系统位置匹配模式，采用闭环校正方式进行；

（1.1）确定算法模型和状态误差量

算法采用18阶导航误差模型，选取18个状态误差量为：

X=ΔLΔhΔλΔVnΔVuΔVeΦnΦuΦe▿x▿y▿zϵxϵyϵzRxbRybRzb]]>

其中：

ΔL、Δh、Δλ：惯导的纬度、经度、高度误差；

ΔV_n、ΔV_u、ΔV_e：惯导北向、天向、东向速度误差；

Φ_n、Φ_u、Φ_e：惯导北向、天向、东向失准角误差；

惯导X、Y、Z轴加速度计零偏；

ε_x、ε_y、ε_z：惯导X、Y、Z轴陀螺漂移；

惯导与GPS间杆臂误差。

（1.2）确定系统状态方程

系统状态方程为：

其中：

F=F11F1203×303×303×303×3F21F22F23Cbn03×303×3F31F32F3303×3-Cbn03×303×303×303303×303×303×303×303×303×303×303×303×3]]>

F11=0-Vn(RM+h)20000VeRN+htanLsecL-VesecL(RN+h)20;F12=1RM+h0001000secLRN+h;]]>

F21=-(2VeωiecosL+Ve2sec2LRN+h)VnVu(RM+h)2+Ve2tanL(RN+h)20-2VeωiesinL-(Vn2(RM+h)2+Ve2(RN+h)2)02ωie(VusinL+VncosL)+VeVnsec2LRN+hVeVu-VeVntanL(RN+h)20;]]>

F22=-VuRM+h-VnRM+h-2(ωiesinL+VetanLRN+h)2VnRM+h02(ωiecosL+VeRN+h)2ωiesinL+VetanLRN+h-(2ωiecosL+VeRN+h)VntanL-VuRN+h;]]>

F23=0-fefufe0-fn-fufn0;]]>

F31=-ωiesinL-Ve(RN+h)20ωiecosL+Vesec2LRN+h-VetanL(RN+h)200Vn(RM+h)20;F32=001RN+h00tanLRN+h-1RM+h00;]]>

F33=0-VnRM+h-(ωiesinL+VetanLRN+h)VnRM+h0ωiecosL+VeRN+hωiesinL+VetanLRN+h-(ωiecosL+VeRN+h)0]]>

其中：Vn、Vu、Ve表示惯导系统的北向、天向和东向速度；L表示纬度，h表示高度，R_M表示地球的子午圈半径，R_N表示地球的卯酉圈半径，ω_ie表示地球的自转角速度，表示惯导系统从载体坐标系到导航坐标系的姿态转换矩阵，f_n、f_u、f_e表示惯导系统加速度计敏感到的比力在导航坐标系内的北向、天向和东向表示值；

（1.3）确定量测方程

量测方程为：Z=HX+V

其中：Z为量测量，即惯导系统的位置信息与GPS位置信息的差值，并考虑两者间杆臂误差：Z=[L-L^GPS-R_n h-h^GPS-R_u λ-λ^GPS-R_e]^T；h^GPS、λ^GPS、L^GPS分别表示GPS系统输出的高度信息、经度信息和纬度信息；R_n、R_u、R_e分别表示惯导系统的位置信息与GPS位置信息之间的北向杆臂误差、天向杆臂误差和东向杆臂误差；

H为量测阵，H=I3×303×12-Cbn]]>

（1.4）确定卡尔曼滤波方程

状态一步预测：其中：表示由k-1时刻到k时刻的状态一步预测值，表示k-1时刻的状态估计值，Φ_k,k-1表示离散化的卡尔曼滤波状态转移矩阵；

一步预测均方误差阵：其中：表示由k-1时刻到k时刻的一步预测均方误差阵，表示k-1时刻的估计均方误差阵，Q_k表示离散化后的系统噪声矩阵；

滤波增益矩阵：其中：表示k时刻的滤波增益阵，H_k表示k时刻的系统量测阵，R_k表示离散化后的量测噪声矩阵；

状态估计：其中：表示k时刻的状态估计值，Z_k表示系统观测量矩阵；

估计均方误差阵：PkF=(I-KkFHk)Pk,k-1F(I-KkFHk)T+KkFRk(KkF)T,]]>其中：表示k时刻的估计均方误差阵，F_i表示未离散化的系统状态转移矩阵对应元素值，I表示对应维数的单位矩阵，T_n表示惯导系统导航周期，T_f表示滤波周期；

（2)在步骤（1）卡尔曼滤波的基础上，进行反向最优固定区间平滑；

设整个导航时间区间为[0 N]，t表示此时间间隔中的任一时刻，0≤t≤N，固定区间平滑估值表示为

（2.1）首先进行0→N的正向Kalman滤波，同时存储滤波估计出的状态转移矩阵Φ_k,k-1、状态一步预测一步预测均方误差阵状态估计估计均方误差阵

（2.2）滤波结束后，将正向滤波最后时刻N的估计值作为反向R-T-S平滑起始时刻的初始值，令k=N，进行N→0的R-T-S平滑过程，其中表示N时刻的平滑估计值，表示N时刻的卡尔曼滤波状态估计值，表示N时刻的平滑估计均方误差阵，表示k时刻的卡尔曼滤波估计均方误差阵，方程如下：

平滑增益阵：其中，表示k时刻的平滑增益阵，Φ_k+1,k表示离散化的卡尔曼滤波状态转移矩阵，表示由k时刻到k+1时刻的一步预测均方误差阵；

平滑的状态估计：X^k/NS=X^kF+Kk/NS(X^k+1/NS-X^k+1,kF),]]>其中：表示第k时刻的平滑估计量，表示第k+1时刻的平滑估计量，表示由k时刻到k+1时刻的状态一步预测值；

平滑的估计均方误差阵：Pk/NS=PkF+Kk/NS(Pk+1/NS-Pk+1,kS)(Kk/NS)T,]]>其中：表示k时刻的平滑估计均方误差阵，表示k+1时刻的平滑估计均方误差阵，表示由k时刻到k+1时刻的一步预测均方误差阵，表示k时刻的平滑增益阵；

从公式可见，第k时刻的平滑估计量是在正向Kalman滤波估计量的基础上线性补偿了一个修正量得到的，该修正量是第k+1时刻的平滑估计量与正向一步预测估计量的差值；

由上面的分析过程得到，k时刻的R-T-S平滑估计量是k时刻的正向Kalman滤波估计量和k+1时刻的平滑估计量的线性融合，k+1时刻的平滑估计量利用了全局数据。