CN104949687A

CN104949687A - Whole parameter precision evaluation method for long-time navigation system

Info

Publication number: CN104949687A
Application number: CN201410125919.3A
Authority: CN
Inventors: 刘冲; 徐海刚; 李海军; 裴玉锋
Original assignee: Beijing Automation Control Equipment Institute BACEI
Current assignee: Beijing Automation Control Equipment Institute BACEI
Priority date: 2014-03-31
Filing date: 2014-03-31
Publication date: 2015-09-30

Abstract

The invention belongs to the field of inertial technology, and specifically relates to a method for evaluating the accuracy of all parameters of a long-time navigation system, which utilizes the original data and satellite information of the navigation experiment of the local reference system, adopts an off-line processing method based on RTS smoothing, and passes the stored inertial navigation data And satellite data processing, to obtain the high-precision position, attitude and orientation information of the reference system, which is used as the benchmark for the evaluation of the accuracy of all parameters of the reference system. In the technical solution of the present invention, the RTS smoothing estimator at time k is a linear fusion of the forward Kalman filter estimator at k time and the smoothing estimator at k+1 time, and the smoothing estimator at k+1 time effectively utilizes the global Therefore, the RTS smoothing of the Kalman filter is higher than the navigation accuracy obtained by simply using the Kalman filter.

Description

A method for evaluating the accuracy of all parameters of a long-duration navigation system

技术领域technical field

本发明属于惯性技术领域，具体涉及一种长时间导航系统全参数精度评估方法。The invention belongs to the technical field of inertia, and in particular relates to a method for evaluating the accuracy of all parameters of a long-time navigation system.

背景技术Background technique

POS（Position Orientation System）系统应用于现代测绘领域，能够实现高精度的位置、姿态测量。高精度后处理技术是POS系统的核心技术，主要是采用R-T-S（Rauch-Tung-Striebel）最优固定区间平滑处理方法。经过后处理，POS的定位精度可达0.05m～0.3m，航向和水平姿态测量精度分别达到了0.008°（RMS）和0.005°（RMS）。POS (Position Orientation System) system is applied in the field of modern surveying and mapping, which can realize high-precision position and attitude measurement. High-precision post-processing technology is the core technology of the POS system, mainly using the R-T-S (Rauch-Tung-Striebel) optimal fixed interval smoothing method. After post-processing, the positioning accuracy of POS can reach 0.05m ~ 0.3m, and the heading and horizontal attitude measurement accuracy can reach 0.008° (RMS) and 0.005° (RMS) respectively.

现阶段，局部基准系统为了提供给导弹惯导准确的位置、速度、姿态和方位信息，本身系统的输出信息需要达到精度要求。为了考核局部基准系统的各参数精度需要额外准备一套高精度的基准惯导系统。At this stage, in order to provide accurate position, velocity, attitude and orientation information to the missile inertial navigation system, the output information of the local reference system needs to meet the accuracy requirements. In order to assess the accuracy of each parameter of the local reference system, it is necessary to prepare an additional set of high-precision reference inertial navigation system.

因此，亟需研制一种长时间导航系统全参数精度评估方法，利用存储的惯导系统信息及卫星数据，通过处理得到高精度的位置、方位及姿态信息，作为精度评估的基准。Therefore, it is urgent to develop a long-term navigation system full-parameter accuracy assessment method, using stored inertial navigation system information and satellite data, through processing to obtain high-precision position, orientation and attitude information, as a benchmark for accuracy assessment.

发明内容Contents of the invention

本发明要解决的技术问题是经过惯导系统与卫星的组合处理，实现对长时间导航系统的全参数进行精度评估的目的。The technical problem to be solved by the present invention is to realize the purpose of evaluating the accuracy of all parameters of the long-time navigation system through the combined processing of the inertial navigation system and the satellite.

为了实现这一目的，本发明采取的技术方案是：In order to realize this object, the technical scheme that the present invention takes is:

一种长时间导航系统全参数精度评估方法，利用局部基准系统航行实验的原始数据及卫星信息，采用基于RTS平滑的离线处理方法，通过对存储的惯导数据及卫星数据的处理，得到基准系统高精度的位置、姿态及方位信息，作为基准系统的全参数精度评估的基准；A method for assessing the accuracy of all parameters of a long-term navigation system, using the original data and satellite information of local reference system navigation experiments, using an off-line processing method based on RTS smoothing, and obtaining the reference system by processing the stored inertial navigation data and satellite data High-precision position, attitude and orientation information, as the benchmark for the full parameter accuracy evaluation of the reference system;

具体包括以下两个步骤以获取位置、速度及航姿数据：It specifically includes the following two steps to obtain position, speed and attitude data:

（1)利用惯性/卫星数据信息进行前向的闭环卡尔曼滤波过程；(1) utilize inertial/satellite data information to carry out forward closed-loop Kalman filtering process;

前向卡尔曼滤波算法采取GPS与惯性导航系统位置匹配模式，采用闭环校正方式进行；The forward Kalman filter algorithm adopts the position matching mode of GPS and inertial navigation system, and adopts the closed-loop correction method;

（1.1）确定算法模型和状态误差量(1.1) Determine the algorithm model and state error amount

算法采用18阶导航误差模型，选取18个状态误差量为：The algorithm uses an 18-order navigation error model, and selects 18 state error quantities as:

$X x = = [\begin{matrix} ΔL ΔL & Δh Δh & Δλ Δλ & {ΔV ΔV}_{n no} & {ΔV ΔV}_{u u} & {ΔV ΔV}_{e e} & {Φ Φ}_{n no} & {Φ Φ}_{u u} & {Φ Φ}_{e e} & {&dtri; &dtri;}_{x x} & {&dtri; &dtri;}_{y the y} & {&dtri; &dtri;}_{z z} & {ϵ ϵ}_{x x} & {ϵ ϵ}_{y the y} & {ϵ ϵ}_{z z} & {R R}_{x x}^{b b} & {R R}_{y the y}^{b b} & {R R}_{z z}^{b b} \end{matrix}]$

其中：in:

ΔL、Δh、Δλ：惯导的纬度、经度、高度误差；ΔL, Δh, Δλ: latitude, longitude, height error of inertial navigation;

ΔV_n、ΔV_u、ΔV_e：惯导北向、天向、东向速度误差；ΔV _n , ΔV _u , ΔV _e : Inertial navigation north, sky, east speed error;

Φ_n、Φ_u、Φ_e：惯导北向、天向、东向失准角误差；Φ _n , Φ _u , Φ _e : Inertial navigation north, sky, east misalignment angle error;

惯导X、Y、Z轴加速度计零偏； Inertial navigation X, Y, Z axis accelerometer zero bias;

ε_x、ε_y、ε_z：惯导X、Y、Z轴陀螺漂移；ε _x , ε _y , ε _z : inertial navigation X, Y, Z axis gyro drift;

惯导与GPS间杆臂误差。 Lever arm error between inertial navigation and GPS.

（1.2）确定系统状态方程(1.2) Determine the system state equation

系统状态方程为： The state equation of the system is:

其中：in:

$F f = = [\begin{matrix} {F f}_{1111} & {F f}_{1212} & 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} \\ {F f}_{21 twenty one} & {F f}_{22 twenty two} & {F f}_{23 twenty three} & {C C}_{b b}^{n no} & 00_{33 \times \times 33} & 00_{33 \times \times 33} \\ {F f}_{3131} & {F f}_{3232} & {F f}_{3333} & 00_{33 \times \times 33} & {- - C C}_{b b}^{n no} & 00_{33 \times \times 33} \\ 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{3333} & 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} \\ 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} \end{matrix}]$

${F f}_{1111} = = [\begin{matrix} 00 & - - \frac{{V V}_{n no}}{{(({R R}_{M m} + + h h))}^{22}} & 00 \\ 00 & 00 & 00 \\ \frac{{V V}_{e e}}{{R R}_{N N} + + h h} tan the tan L L sec sec L L & - - \frac{{V V}_{e e} sec sec L L}{{(({R R}_{N N} + + h h))}^{22}} & 00 \end{matrix}];; {F f}_{1212} = = [\begin{matrix} \frac{11}{{R R}_{M m} + + h h} & 00 & 00 \\ 00 & 11 & 00 \\ 00 & 00 & \frac{sec sec L L}{{R R}_{N N} + + h h} \end{matrix}];;$

${F f}_{21 twenty one} = = [\begin{matrix} - - (({22 V V}_{e e} {ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{e e}^{22} {sec sec}^{22} L L}{{R R}_{N N} + + h h})) & \frac{{V V}_{n no} {V V}_{u u}}{{(({R R}_{M m} + + h h))}^{22}} + + \frac{{V V}_{e e}^{22} tan the tan L L}{{(({R R}_{N N} + + h h))}^{22}} & 00 \\ {- - 22 V V}_{e e} {ω ω}_{ie ie} sin sin L L & - - ((\frac{{V V}_{n no}^{22}}{{(({R R}_{M m} + + h h))}^{22}} + + \frac{{V V}_{e e}^{22}}{{(({R R}_{N N} + + h h))}^{22}})) & 00 \\ {22 ω ω}_{ie ie} (({V V}_{u u} sin sin L L + + {V V}_{n no} cos cos L L)) + + \frac{{V V}_{e e} {V V}_{n no} {sec sec}^{22} L L}{{R R}_{N N} + + h h} & \frac{{V V}_{e e} {V V}_{u u} - - {V V}_{e e} {V V}_{n no} tan the tan L L}{{(({R R}_{N N} + + h h))}^{22}} & 00 \end{matrix}];;$

${F f}_{22 twenty two} = = [\begin{matrix} - - \frac{{V V}_{u u}}{{R R}_{M m} + + h h} & - - \frac{{V V}_{n no}}{{R R}_{M m} + + h h} & - - 22 (({ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{e e} tan the tan L L}{{R R}_{N N} + + h h})) \\ \frac{{22 V V}_{n no}}{{R R}_{M m} + + h h} & 00 & 22 (({ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{e e}}{{R R}_{N N} + + h h})) \\ {22 ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{e e} tan the tan L L}{{R R}_{N N} + + h h} & - - (({22 ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{e e}}{{R R}_{N N} + + h h})) & \frac{{V V}_{n no} tan the tan L L - - {V V}_{u u}}{{R R}_{N N} + + h h} \end{matrix}];;$

${F f}_{23 twenty three} = = [\begin{matrix} 00 & {- - f f}_{e e} & {f f}_{u u} \\ {f f}_{e e} & 00 & {- - f f}_{n no} \\ {- - f f}_{u u} & {f f}_{n no} & 00 \end{matrix}];;$

${F f}_{3131} = = [\begin{matrix} {- - ω ω}_{ie ie} sin sin L L & - - \frac{{V V}_{e e}}{{(({R R}_{N N} + + h h))}^{22}} & 00 \\ {ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{e e} {sec sec}^{22} L L}{{R R}_{N N} + + h h} & - - \frac{{V V}_{e e} tan the tan L L}{{(({R R}_{N N} + + h h))}^{22}} & 00 \\ 00 & \frac{{V V}_{n no}}{{(({R R}_{M m} + + h h))}^{22}} & 00 \end{matrix}];; {F f}_{3232} = = [\begin{matrix} 00 & 00 & \frac{11}{{R R}_{N N} + + h h} \\ 00 & 00 & \frac{tan the tan L L}{{R R}_{N N} + + h h} \\ - - \frac{11}{{R R}_{M m} + + h h} & 00 & 00 \end{matrix}];;$

${F f}_{3333} = = [\begin{matrix} 00 & - - \frac{{V V}_{n no}}{{R R}_{M m} + + h h} & - - (({ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{e e} tan the tan L L}{{R R}_{N N} + + h h})) \\ \frac{{V V}_{n no}}{{R R}_{M m} + + h h} & 00 & {ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{e e}}{{R R}_{N N} + + h h} \\ {ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{e e} tan the tan L L}{{R R}_{N N} + + h h} & - - (({ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{e e}}{{R R}_{N N} + + h h})) & 00 \end{matrix}]$

其中：Vn、Vu、Ve表示惯导系统的北向、天向和东向速度；L表示纬度，h表示高度，R_M表示地球的子午圈半径，R_N表示地球的卯酉圈半径，ω_ie表示地球的自转角速度，表示惯导系统从载体坐标系到导航坐标系的姿态转换矩阵，f_n、f_u、f_e表示惯导系统加速度计敏感到的比力在导航坐标系内的北向、天向和东向表示值；Among them: Vn, Vu, Ve represent the northward, celestial and eastward speeds of the inertial navigation system; L represents the latitude, h represents the height, R _M represents the earth’s meridian circle radius, R _N represents the earth’s maoyou circle radius, ω _ie is the angular velocity of the earth's rotation, Represents the attitude transformation matrix of the inertial navigation system from the carrier coordinate system to the navigation coordinate system, f _n , _fu , f _e represent the north, sky and east directions of the specific force sensitive to the accelerometer of the inertial navigation system in the navigation coordinate system value;

（1.3）确定量测方程(1.3) Determine the measurement equation

量测方程为：Z=HX+VThe measurement equation is: 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位置信息之间的北向杆臂误差、天向杆臂误差和东向杆臂误差；Among them: Z is the quantity measurement, that is, the difference between the position information of the inertial navigation system and the GPS position information, and the error of the lever arm between the two is considered: Z=[LL ^GPS -R _n hh ^GPS -R _u λ-λ ^GPS -R _e ] ^T ; h ^GPS , λ ^GPS , L ^GPS represent the height information, longitude information and latitude information output by the GPS system respectively; R _n , R _u , _Re represent the distance between the position information of the inertial navigation system and the GPS position information North-direction lever-arm error, sky-direction lever-arm error and east-direction lever-arm error;

H为量测阵， $H = [\begin{matrix} I_{3 \times 3} & 0_{3 \times 12} & {- C}_{b}^{n} \end{matrix}]$ H is the measurement array, $h = [\begin{matrix} I_{3 \times 3} & 0_{3 \times 12} & {- C}_{b}^{no} \end{matrix}]$

（1.4）确定卡尔曼滤波方程(1.4) Determine the Kalman filter equation

状态一步预测：其中：表示由k-1时刻到k时刻的状态一步预测值，表示k-1时刻的状态估计值，Φ_k,k-1表示离散化的卡尔曼滤波状态转移矩阵；State one-step prediction: in: Indicates the one-step predicted value of the state from k-1 time to k time, Indicates the estimated value of the state at time k-1, Φ _k,k-1 represents the discretized Kalman filter state transition matrix;

一步预测均方误差阵：其中：表示由k-1时刻到k时刻的一步预测均方误差阵，表示k-1时刻的估计均方误差阵，Q_k表示离散化后的系统噪声矩阵；One-step forecast mean square error matrix: in: Represents the one-step forecast mean square error matrix from time k-1 to time k, Represents the estimated mean square error matrix at time k-1, and Q _k represents the discretized system noise matrix;

滤波增益矩阵：其中：表示k时刻的滤波增益阵，H_k表示k时刻的系统量测阵，R_k表示离散化后的量测噪声矩阵；Filter gain matrix: in: Represents the filter gain matrix at time k, H _k represents the system measurement matrix at time k, and R _k represents the discretized measurement noise matrix;

状态估计：其中：表示k时刻的状态估计值，Z_k表示系统观测量矩阵；State estimation: in: Indicates the estimated value of the state at time k, and Z _k indicates the system observation matrix;

估计均方误差阵： $P_{k}^{F} = (I - K_{k}^{F} H_{k}) P_{k, k - 1}^{F} {(I - K_{k}^{F} H_{k})}^{T} + K_{k}^{F} R_{k} {(K_{k}^{F})}^{T},$ 其中：表示k时刻的估计均方误差阵，F_i表示未离散化的系统状态转移矩阵对应元素值，I表示对应维数的单位矩阵，T_n表示惯导系统导航周期，T_f表示滤波周期；Estimate the mean squared error matrix: $P_{k}^{f} = (I - K_{k}^{f} h_{k}) P_{k, k - 1}^{f} {(I - K_{k}^{f} h_{k})}^{T} + K_{k}^{f} R_{k} {(K_{k}^{f})}^{T},$ in: Represents the estimated mean square error matrix at time k, F _i represents the corresponding element value of the undiscretized system state transition matrix, I represents the identity matrix of the corresponding dimension, T _n represents the navigation cycle of the inertial navigation system, and T _f represents the filter cycle;

（2)在步骤（1）卡尔曼滤波的基础上，进行反向最优固定区间平滑；(2) On the basis of step (1) Kalman filter, perform reverse optimal fixed interval smoothing;

设整个导航时间区间为[0 N]，t表示此时间间隔中的任一时刻，0≤t≤N，固定区间平滑估值表示为 Assuming that the entire navigation time interval is [0 N], t represents any moment in this time interval, 0≤t≤N, and the fixed interval smoothing estimation is expressed as

（2.1）首先进行0→N的正向Kalman滤波，同时存储滤波估计出的状态转移矩阵Φ_k,k-1、状态一步预测一步预测均方误差阵状态估计估计均方误差阵 (2.1) First, carry out 0→N forward Kalman filtering, and store the state transition matrix Φ _k,k-1 estimated by filtering at the same time, and the state one-step prediction One-step prediction mean square error matrix state estimation Estimated mean square error matrix

（2.2）滤波结束后，将正向滤波最后时刻N的估计值作为反向R-T-S平滑起始时刻的初始值，令k=N，进行N→0的R-T-S平滑过程，其中表示N时刻的平滑估计值，表示N时刻的卡尔曼滤波状态估计值，表示N时刻的平滑估计均方误差阵，表示k时刻的卡尔曼滤波估计均方误差阵，方程如下：(2.2) After the filtering is finished, the estimated value of the last time N of the forward filtering is used as the initial value of the starting time of reverse RTS smoothing, let k=N, Carry out N→0 RTS smoothing process, where Indicates the smoothed estimated value at time N, Indicates the estimated value of the Kalman filter state at time N, Represents the smooth estimated mean square error matrix at time N, Represents the Kalman filter estimated mean square error matrix at time k, the equation is as follows:

平滑增益阵：其中，表示k时刻的平滑增益阵，Φ_k+1,k表示离散化的卡尔曼滤波状态转移矩阵，表示由k时刻到k+1时刻的一步预测均方误差阵；Smooth gain matrix: in, Represents the smooth gain matrix at time k, Φ _k+1,k represents the discretized Kalman filter state transition matrix, Indicates the one-step prediction mean square error matrix from time k to time k+1;

平滑的状态估计： ${\hat{X}}_{k / N}^{S} = {\hat{X}}_{k}^{F} + K_{k / N}^{S} ({\hat{X}}_{k + 1 / N}^{S} - {\hat{X}}_{k + 1, k}^{F}),$ 其中：表示第k时刻的平滑估计量，表示第k+1时刻的平滑估计量，表示由k时刻到k+1时刻的状态一步预测值；Smooth state estimation: ${\hat{x}}_{k / N}^{S} = {\hat{x}}_{k}^{f} + K_{k / N}^{S} ({\hat{x}}_{k + 1 / N}^{S} - {\hat{x}}_{k + 1, k}^{f}),$ in: Indicates the smoothing estimator at the kth time, Indicates the smoothing estimator at the k+1th moment, Indicates the one-step predicted value of the state from time k to time k+1;

平滑的估计均方误差阵： $P_{k / N}^{S} = P_{k}^{F} + K_{k / N}^{S} (P_{k + 1 / N}^{S} - P_{k + 1, k}^{S}) {(K_{k / N}^{S})}^{T},$ 其中：表示k时刻的平滑估计均方误差阵，表示k+1时刻的平滑估计均方误差阵，表示由k时刻到k+1时刻的一步预测均方误差阵，表示k时刻的平滑增益阵；Smoothed estimated mean squared error matrix: $P_{k / N}^{S} = P_{k}^{f} + K_{k / N}^{S} (P_{k + 1 / N}^{S} - P_{k + 1, k}^{S}) {(K_{k / N}^{S})}^{T},$ in: Represents the smooth estimated mean square error matrix at time k, Represents the smooth estimated mean square error matrix at time k+1, Indicates the one-step prediction mean square error matrix from time k to time k+1, Indicates the smooth gain matrix at time k;

从公式可见，第k时刻的平滑估计量是在正向Kalman滤波估计量的基础上线性补偿了一个修正量得到的，该修正量是第k+1时刻的平滑估计量与正向一步预测估计量的差值；It can be seen from the formula that the smoothing estimator at the kth moment is the estimator in the forward Kalman filter It is obtained by linearly compensating a correction amount on the basis of , which is the smoothing estimator at the k+1th moment and the forward one-step forecast estimator the difference;

由上面的分析过程得到，k时刻的R-T-S平滑估计量是k时刻的正向Kalman滤波估计量和k+1时刻的平滑估计量的线性融合，k+1时刻的平滑估计量利用了全局数据。From the above analysis process, the R-T-S smoothing estimator at time k is a linear fusion of the forward Kalman filter estimator at time k and the smoothing estimator at time k+1, and the smoothing estimator at time k+1 utilizes global data.

进一步的，如上所述的一种长时间导航系统全参数精度评估方法，其中：惯导系统导航周期T_n=0.05s。Further, a method for evaluating the accuracy of all parameters of a long-term navigation system as described above, wherein: the navigation period of the inertial navigation system T _n =0.05s.

进一步的，如上所述的一种长时间导航系统全参数精度评估方法，其中：滤波周期T_f=1s。Further, a method for evaluating the accuracy of all parameters of a long-duration navigation system as described above, wherein: the filtering period T _f =1s.

由上面的分析过程可知，k时刻的R-T-S平滑估计量是k时刻的正向Kalman滤波估计量和k+1时刻的平滑估计量的线性融合，而k+1时刻的平滑估计量有效利用了全局数据，所以对Kalman滤波进行R-T-S平滑比单纯采用Kalman滤波获得的导航精度高。From the above analysis process, it can be seen that the R-T-S smoothing estimator at time k is a linear fusion of the forward Kalman filter estimator at time k and the smoothing estimator at time k+1, and the smoothing estimator at time k+1 effectively utilizes the global Therefore, the R-T-S smoothing of the Kalman filter is higher than the navigation accuracy obtained by simply using the Kalman filter.

本发明的有益效果在于：提出了一种长时间导航系统的全参数评估方法，通过对存储的惯导数据及卫星数据的处理，得到基准系统高精度的位置、姿态及方位信息，可作为基准系统的全参数精度评估的基准，其中方位精度达到0.25′，姿态精度达到0.025′。经试验验证发现，此技术能够满足局部基准系统精度评价需求，具有很高的实用价值。The beneficial effect of the present invention is that: a full-parameter evaluation method of a long-time navigation system is proposed, by processing the stored inertial navigation data and satellite data, the high-precision position, attitude and orientation information of the reference system can be obtained, which can be used as a reference The benchmark of the system's full parameter accuracy evaluation, where the azimuth accuracy reaches 0.25' and the attitude accuracy reaches 0.025'. It is verified by experiments that this technology can meet the accuracy evaluation requirements of the local reference system and has high practical value.

附图说明Description of drawings

图1是本发明算法实现流程图；Fig. 1 is the flow chart of algorithm realization of the present invention;

图2是后处理位置精度曲线；Figure 2 is the post-processing position accuracy curve;

图3是后处理速度精度曲线；Figure 3 is the post-processing speed accuracy curve;

图4是后处理姿态精度曲线；Figure 4 is the post-processing attitude accuracy curve;

图5是后处理航向精度曲线。Figure 5 is the post-processing heading accuracy curve.

具体实施方式Detailed ways

下面结合附图和具体实施例对本发明技术方案进行详细说明。The technical solution of the present invention will be described in detail below in conjunction with the accompanying drawings and specific embodiments.

本发明算法实现流程如图1所示，具体包括以下两个步骤以获取位置、速度及航姿数据：The algorithm implementation process of the present invention is shown in Figure 1, specifically includes the following two steps to obtain position, speed and attitude data:

$X x = = [\begin{matrix} ΔL Δ L & Δh Δh & Δλ Δλ & {ΔV ΔV}_{n no} & {ΔV ΔV}_{u u} & {ΔV ΔV}_{e e} & {Φ Φ}_{n no} & {Φ Φ}_{u u} & {Φ Φ}_{e e} & {&dtri; &dtri;}_{x x} & {&dtri; &dtri;}_{y the y} & {&dtri; &dtri;}_{z z} & {ϵ ϵ}_{x x} & {ϵ ϵ}_{y the y} & {ϵ ϵ}_{z z} & {R R}_{x x}^{b b} & {R R}_{y the y}^{b b} & {R R}_{z z}^{b b} \end{matrix}]$

其中：in:

（1.2）确定系统状态方程(1.2) Determine the system state equation

系统状态方程为： The state equation of the system is:

其中：in:

（1.3）确定量测方程(1.3) Determine the measurement equation

量测方程为：Z=HX+VThe measurement equation is: Z=HX+V

（1.4）确定卡尔曼滤波方程(1.4) Determine the Kalman filter equation

估计均方误差阵： $P_{k}^{F} = (I - K_{k}^{F} H_{k}) P_{k, k - 1}^{F} {(I - K_{k}^{F} H_{k})}^{T} + K_{k}^{F} R_{k} {(K_{k}^{F})}^{T},$ 其中：表示k时刻的估计均方误差阵，F_i表示未离散化的系统状态转移矩阵对应元素值，I表示对应维数的单位矩阵，T_n表示惯导系统导航周期（在本具体实施例中，T_n=0.05s），T_f表示滤波周期（在本具体实施例中，T_f=1s）；Estimate the mean squared error matrix: $P_{k}^{f} = (I - K_{k}^{f} h_{k}) P_{k, k - 1}^{f} {(I - K_{k}^{f} h_{k})}^{T} + K_{k}^{f} R_{k} {(K_{k}^{f})}^{T},$ in: Represents the estimated mean square error matrix at time k, F _i represents the corresponding element value of the non-discretized system state transition matrix, I represents the identity matrix of the corresponding dimension, T _n represents the navigation cycle of the inertial navigation system (in this specific embodiment, T _n =0.05s), T _f represents the filter period (in this specific embodiment, T _f =1s);

经过上面两步得到的高精度的位置、速度及航姿信息。为了验证此方法的可行性，对航行试验数据进行处理。The high-precision position, speed and attitude information obtained through the above two steps. In order to verify the feasibility of this method, the sailing test data are processed.

根据算法原理，估计均方误差阵的大小代表了后处理算法的精度。如图2～5所示。According to the principle of the algorithm, the estimated mean square error matrix The size of represents the accuracy of the post-processing algorithm. As shown in Figure 2-5.

由图2～5可以看出，该方法得到的水平姿态精度为0.025′，航向精度为0.25′，位置精度为1.3m，速度误差为0.006m/s，可以作为评判局部基准全参数精度的基准。It can be seen from Figures 2 to 5 that the horizontal attitude accuracy obtained by this method is 0.025′, the heading accuracy is 0.25′, the position accuracy is 1.3m, and the velocity error is 0.006m/s, which can be used as a benchmark for judging the accuracy of all parameters of the local benchmark .

对六条航行试验数据进行处理，得到局部基准系统各参数在线精度，其中航姿精度如表1所示。The six navigation test data are processed to obtain the online accuracy of each parameter of the local reference system, and the attitude accuracy is shown in Table 1.

表1各条次试验统计精度Table 1 Statistical accuracy of each test

由表中统计结果可知，各条次试验水平姿态误差的最大值为0.59′，航向误差最大值为2.96′，能够满足水平姿态误差1.5′，航向误差3.67′/cosφ的指标要求。It can be seen from the statistical results in the table that the maximum value of the horizontal attitude error of each test is 0.59′, and the maximum value of the heading error is 2.96′, which can meet the index requirements of the horizontal attitude error of 1.5′ and the heading error of 3.67′/cosφ.

Claims

1. A method for assessing the accuracy of all parameters of a long-time navigation system, characterized in that: utilize the original data and satellite information of the navigation experiment of the local reference system, adopt an off-line processing method based on RTS smoothing, and pass through the stored inertial navigation data and satellite data The high-precision position, attitude and orientation information of the reference system can be obtained through processing, which can be used as the reference for the evaluation of the accuracy of all parameters of the reference system;

It specifically includes the following two steps to obtain position, speed and attitude data:

(1) utilize inertial/satellite data information to carry out forward closed-loop Kalman filtering process;

The forward Kalman filter algorithm adopts the position matching mode of GPS and inertial navigation system, and adopts the closed-loop correction method;

(1.1) Determine the algorithm model and state error amount

The algorithm uses an 18-order navigation error model, and selects 18 state error quantities as:

X x = = [\begin{matrix} ΔL ΔL & Δh Δh & Δλ Δλ & {ΔV ΔV}_{n no} & {ΔV ΔV}_{u u} & {ΔV ΔV}_{e e} & {Φ Φ}_{n no} & {Φ Φ}_{u u} & {Φ Φ}_{e e} & {&dtri; &dtri;}_{x x} & {&dtri; &dtri;}_{y the y} & {&dtri; &dtri;}_{z z} & {ϵ ϵ}_{x x} & {ϵ ϵ}_{y the y} & {ϵ ϵ}_{z z} & {R R}_{x x}^{b b} & {R R}_{y the y}^{b b} & {R R}_{z z}^{b b} \end{matrix}]

in:

ΔL, Δh, Δλ: latitude, longitude, height error of inertial navigation;

ΔV _n , ΔV _u , ΔV _e : Inertial navigation north, sky, east speed error;

Φ _n , Φ _u , Φ _e : Inertial navigation north, sky, east misalignment angle error;

Inertial navigation X, Y, Z axis accelerometer zero bias;

ε _x , ε _y , ε _z : inertial navigation X, Y, Z axis gyro drift;

Lever arm error between inertial navigation and GPS.

(1.2) Determine the system state equation

The state equation of the system is:

in:

F f = = [\begin{matrix} {F f}_{1111} & {F f}_{1212} & 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} \\ {F f}_{21 twenty one} & {F f}_{22 twenty two} & {F f}_{23 twenty three} & {C C}_{b b}^{n no} & 00_{33 \times \times 33} & 00_{33 \times \times 33} \\ {F f}_{3131} & {F f}_{3232} & {F f}_{3333} & 00_{33 \times \times 33} & {- - C C}_{b b}^{n no} & 00_{33 \times \times 33} \\ 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{3333} & 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} \\ 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} \end{matrix}]

{F f}_{1111} = = [\begin{matrix} 00 & - - \frac{{V V}_{n no}}{{(({R R}_{M m} + + h h))}^{22}} & 00 \\ 00 & 00 & 00 \\ \frac{{V V}_{e e}}{{R R}_{N N} + + h h} tan the tan L L sec sec L L & - - \frac{{V V}_{e e} sec sec L L}{{(({R R}_{N N} + + h h))}^{22}} & 00 \end{matrix}];; {F f}_{1212} = = [\begin{matrix} \frac{11}{{R R}_{M m} + + h h} & 00 & 00 \\ 00 & 11 & 00 \\ 00 & 00 & \frac{sec sec L L}{{R R}_{N N} + + h h} \end{matrix}];;

{F f}_{21 twenty one} = = [\begin{matrix} - - (({22 V V}_{e e} {ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{e e}^{22} {sec sec}^{22} L L}{{R R}_{N N} + + h h})) & \frac{{V V}_{n no} {V V}_{u u}}{{(({R R}_{M m} + + h h))}^{22}} + + \frac{{V V}_{e e}^{22} tan the tan L L}{{(({R R}_{N N} + + h h))}^{22}} & 00 \\ {- - 22 V V}_{e e} {ω ω}_{ie ie} sin sin L L & - - ((\frac{{V V}_{n no}^{22}}{{(({R R}_{M m} + + h h))}^{22}} + + \frac{{V V}_{e e}^{22}}{{(({R R}_{N N} + + h h))}^{22}})) & 00 \\ {22 ω ω}_{ie ie} (({V V}_{u u} sin sin L L + + {V V}_{n no} cos cos L L)) + + \frac{{V V}_{e e} {V V}_{n no} {sec sec}^{22} L L}{{R R}_{N N} + + h h} & \frac{{V V}_{e e} {V V}_{u u} - - {V V}_{e e} {V V}_{n no} tan the tan L L}{{(({R R}_{N N} + + h h))}^{22}} & 00 \end{matrix}];;

{F f}_{22 twenty two} = = [\begin{matrix} - - \frac{{V V}_{u u}}{{R R}_{M m} + + h h} & - - \frac{{V V}_{n no}}{{R R}_{M m} + + h h} & - - 22 (({ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{e e} tan the tan L L}{{R R}_{N N} + + h h})) \\ \frac{{22 V V}_{n no}}{{R R}_{M m} + + h h} & 00 & 22 (({ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{e e}}{{R R}_{N N} + + h h})) \\ {22 ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{e e} tan the tan L L}{{R R}_{N N} + + h h} & - - (({22 ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{e e}}{{R R}_{N N} + + h h})) & \frac{{V V}_{n no} tan the tan L L - - {V V}_{u u}}{{R R}_{N N} + + h h} \end{matrix}];;

{F f}_{23 twenty three} = = [\begin{matrix} 00 & {- - f f}_{e e} & {f f}_{u u} \\ {f f}_{e e} & 00 & {- - f f}_{n no} \\ {- - f f}_{u u} & {f f}_{n no} & 00 \end{matrix}];;

{F f}_{3131} = = [\begin{matrix} {- - ω ω}_{ie ie} sin sin L L & - - \frac{{V V}_{e e}}{{(({R R}_{N N} + + h h))}^{22}} & 00 \\ {ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{e e} {sec sec}^{22} L L}{{R R}_{N N} + + h h} & - - \frac{{V V}_{e e} tan the tan L L}{{(({R R}_{N N} + + h h))}^{22}} & 00 \\ 00 & \frac{{V V}_{n no}}{{(({R R}_{M m} + + h h))}^{22}} & 00 \end{matrix}];; {F f}_{3232} = = [\begin{matrix} 00 & 00 & \frac{11}{{R R}_{N N} + + h h} \\ 00 & 00 & \frac{tan the tan L L}{{R R}_{N N} + + h h} \\ - - \frac{11}{{R R}_{M m} + + h h} & 00 & 00 \end{matrix}];;

{F f}_{3333} = = [\begin{matrix} 00 & - - \frac{{V V}_{n no}}{{R R}_{M m} + + h h} & - - (({ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{e e} tan the tan L L}{{R R}_{N N} + + h h})) \\ \frac{{V V}_{n no}}{{R R}_{M m} + + h h} & 00 & {ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{e e}}{{R R}_{N N} + + h h} \\ {ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{e e} tan the tan L L}{{R R}_{N N} + + h h} & - - (({ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{e e}}{{R R}_{N N} + + h h})) & 00 \end{matrix}]

Among them: Vn, Vu, Ve represent the northward, celestial and eastward speeds of the inertial navigation system; L represents the latitude, h represents the height, R _M represents the earth’s meridian circle radius, R _N represents the earth’s maoyou circle radius, ω _ie is the angular velocity of the earth's rotation, Represents the attitude transformation matrix of the inertial navigation system from the carrier coordinate system to the navigation coordinate system, f _n , _fu , f _e represent the north, sky and east directions of the specific force sensitive to the accelerometer of the inertial navigation system in the navigation coordinate system value;

(1.3) Determine the measurement equation

The measurement equation is: Z=HX+V

Among them: Z is the quantity measurement, that is, the difference between the position information of the inertial navigation system and the GPS position information, and the error of the lever arm between the two is considered: Z=[LL ^GPS -R _n hh ^GPS -R _u λ-λ ^GPS -R _e ] ^T ; h ^GPS , λ ^GPS , L ^GPS represent the height information, longitude information and latitude information output by the GPS system respectively; R _n , R _u , _Re represent the distance between the position information of the inertial navigation system and the GPS position information North-direction lever-arm error, sky-direction lever-arm error and east-direction lever-arm error;

H is the measurement array,

h = [\begin{matrix} I_{3 \times 3} & 0_{3 \times 12} & {- C}_{b}^{no} \end{matrix}]

(1.4) Determine the Kalman filter equation

State one-step prediction: in: Indicates the one-step predicted value of the state from k-1 time to k time, Indicates the estimated value of the state at time k-1, Φ _k,k-1 represents the discretized Kalman filter state transition matrix;

One-step forecast mean square error matrix: in: Represents the one-step forecast mean square error matrix from time k-1 to time k, Represents the estimated mean square error matrix at time k-1, and Q _k represents the discretized system noise matrix;

Filter gain matrix: in: Represents the filter gain matrix at time k, H _k represents the system measurement matrix at time k, and R _k represents the discretized measurement noise matrix;

State estimation: in: Indicates the estimated value of the state at time k, and Z _k indicates the system observation matrix;

Estimate the mean squared error matrix:

P_{k}^{f} = (I - K_{k}^{f} h_{k}) P_{k, k - 1}^{f} {(I - K_{k}^{f} h_{k})}^{T} + K_{k}^{f} R_{k} {(K_{k}^{f})}^{T},

in: Represents the estimated mean square error matrix at time k, F _i represents the corresponding element value of the undiscretized system state transition matrix, I represents the identity matrix of the corresponding dimension, T _n represents the navigation cycle of the inertial navigation system, and T _f represents the filter cycle;

(2) On the basis of step (1) Kalman filter, perform reverse optimal fixed interval smoothing;

Assuming that the entire navigation time interval is [0 N], t represents any moment in this time interval, 0≤t≤N, and the fixed interval smoothing estimation is expressed as

(2.1) First, carry out 0→N forward Kalman filtering, and store the state transition matrix Φ _k,k-1 estimated by filtering at the same time, and the state one-step prediction One-step prediction mean square error matrix state estimation Estimated mean square error matrix

(2.2) After the filtering is finished, the estimated value of the last time N of the forward filtering is used as the initial value of the starting time of reverse RTS smoothing, let k=N, Carry out N→0 RTS smoothing process, where Indicates the smoothed estimated value at time N, Indicates the estimated value of the Kalman filter state at time N, Represents the smooth estimated mean square error matrix at time N, Represents the Kalman filter estimated mean square error matrix at time k, the equation is as follows:

Smooth gain matrix: in, Represents the smooth gain matrix at time k, Φ _k+1,k represents the discretized Kalman filter state transition matrix, Indicates the one-step prediction mean square error matrix from time k to time k+1;

Smooth state estimation:

{\hat{x}}_{k / N}^{S} = {\hat{x}}_{k}^{f} + K_{k / N}^{S} ({\hat{x}}_{k + 1 / N}^{S} - {\hat{x}}_{k + 1, k}^{f}),

in: Indicates the smoothing estimator at the kth time, Indicates the smoothing estimator at the k+1th moment, Indicates the one-step predicted value of the state from time k to time k+1;

Smoothed estimated mean squared error matrix:

P_{k / N}^{S} = P_{k}^{f} + K_{k / N}^{S} (P_{k + 1 / N}^{S} - P_{k + 1, k}^{S}) {(K_{k / N}^{S})}^{T},

in: Represents the smooth estimated mean square error matrix at time k, Represents the smooth estimated mean square error matrix at time k+1, Indicates the one-step prediction mean square error matrix from time k to time k+1, Indicates the smooth gain matrix at time k;

It can be seen from the formula that the smoothing estimator at the kth moment is the estimator in the forward Kalman filter It is obtained by linearly compensating a correction amount on the basis of , which is the smoothing estimator at the k+1th moment and the forward one-step forecast estimator the difference;

From the above analysis process, the R-T-S smoothing estimator at time k is a linear fusion of the forward Kalman filter estimator at time k and the smoothing estimator at time k+1, and the smoothing estimator at time k+1 utilizes global data.

2. A method for evaluating the accuracy of all parameters of a long-time navigation system as claimed in claim 1, wherein the navigation period of the inertial navigation system is T _n =0.05s.

3. A method for evaluating the accuracy of all parameters of a long-duration navigation system as claimed in claim 1, characterized in that: the filtering period T _f =1s.