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 the inertial technology and in particular relates to a whole parameter precision evaluation method for a long-time navigation system. Original data and satellite information of navigation testing of a partial reference system are utilized, an RTS (radar tracking station) smoothing based off-line processing method is adopted for processing stored inertial navigation data and satellite data to acquire high-precision position, posture and azimuth information of the reference system, and the high-precision position, posture and azimuth information is used as the criterion for whole parameter precision evaluation of the reference system. In the technical scheme, the R-T-S smoothing estimator of k moment is the linear fusion of the forward Kalman filtering estimator of k moment and the smoothing estimator of the k+1 moment, and the smoothing estimator of the k+1 moment effectively utilizes the global data, therefore, the navigation precision by use of Kalman filtering for R-T-S smoothing is higher than that adopting the Kalman filtering singly.

Description

Full-parameter precision evaluation method for long-time navigation system

Technical Field

The invention belongs to the technical field of inertia, and particularly relates to a method for evaluating full-parameter precision of a long-time navigation system.

Background

The POS (position Orientation System) system is applied to the field of modern surveying and mapping and can realize high-precision position and attitude measurement. The high-precision post-processing technology is the core technology of the POS system, and mainly adopts an optimal fixed interval smoothing processing method of R-T-S (Rauch-Tung-Streebel). After post-processing, the positioning accuracy of the POS can reach 0.05 m-0.3 m, and the measurement accuracy of the course and the horizontal attitude respectively reach 0.008 degrees (RMS) and 0.005 degrees (RMS).

At the present stage, in order to provide accurate position, speed, attitude and azimuth information for guided missile inertial navigation, the output information of the local reference system needs to meet the precision requirement. In order to assess the precision of each parameter of the local reference system, an additional set of high-precision reference inertial navigation system needs to be prepared.

Therefore, it is necessary to develop a method for evaluating the precision of the full-parameter of the long-time navigation system, in which the stored inertial navigation system information and satellite data are processed to obtain the position, orientation and posture information with high precision as the reference for evaluating the precision.

Disclosure of Invention

The invention aims to solve the technical problem of realizing the purpose of carrying out precision evaluation on the full parameters of a long-time navigation system through the combined processing of an inertial navigation system and a satellite.

In order to realize the purpose, the invention adopts the technical scheme that:

a full-parameter precision evaluation method of a long-time navigation system utilizes original data and satellite information of a local reference system navigation experiment, adopts an off-line processing method based on RTS smoothing, and obtains high-precision position, attitude and azimuth information of a reference system by processing stored inertial navigation data and satellite data, and the high-precision position, attitude and azimuth information is used as a reference for full-parameter precision evaluation of the reference system;

the method specifically comprises the following two steps of obtaining position, speed and attitude data:

(1) carrying out a forward closed loop Kalman filtering process by using inertia/satellite data information;

the forward Kalman filtering algorithm is carried out by adopting a position matching mode of a GPS and an inertial navigation system and adopting a closed loop correction mode;

(1.1) determining an algorithmic model and a state error quantity

The algorithm adopts an 18-order navigation error model, and 18 state error quantities are selected as follows:

wherein:

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

ΔV_n、ΔV_u、ΔV_e: north, sky, and east speed errors of inertial navigation;

Φ_n、Φ_u、Φ_e: inertial navigation north direction, sky direction and east direction misalignment angle errors;

inertial navigation X, Y, Z axis accelerometer zero offset;

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

and the lever arm error between inertial navigation and GPS.

(1.2) determining the equation of state of the System

The system state equation is:

wherein:

F_{11} = [\begin{matrix} 0 & - \frac{V_{n}}{{(R_{M} + h)}^{2}} & 0 \\ 0 & 0 & 0 \\ \frac{V_{e}}{R_{N} + h} \tan L \sec L & - \frac{V_{e} \sec L}{{(R_{N} + h)}^{2}} & 0 \end{matrix}]; F_{12} = [\begin{matrix} \frac{1}{R_{M} + h} & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \frac{\sec L}{R_{N} + h} \end{matrix}];

F_{23} = [\begin{matrix} 0 & {- f}_{e} & f_{u} \\ f_{e} & 0 & {- f}_{n} \\ {- f}_{u} & f_{n} & 0 \end{matrix}];

wherein: vn, Vu and Ve represent the north, the sky and the east speed of the inertial navigation system; l represents latitude, h represents altitude, R_MRepresents the radius of the meridian of the earth, R_NRepresenting radius of the prime circle, omega, of the earth_ieWhich represents the rotational angular velocity of the earth,representing the attitude transformation matrix, f, of the inertial navigation system from the carrier coordinate system to the navigation coordinate system_n、f_u、f_eRepresenting north, sky and east representation values of specific force sensed by an accelerometer of the inertial navigation system in a navigation coordinate system;

(1.3) determination of measurement equation

The measurement equation is as follows: z = HX + V

Wherein: and measuring the Z component, namely the difference value between the position information of the inertial navigation system and the GPS position information, and considering the lever arm error between the two: z = [ L-L =^GPS-R_n h-h^GPS-R_u λ-λ^GPS-R_e]^T；h^GPS、λ^GPS、L^GPSRespectively representing altitude information, longitude information and latitude information output by a GPS system; r_n、R_u、R_eRespectively representing a north lever arm error, a sky lever arm error and an east lever arm error between the position information of the inertial navigation system and the GPS position information;

h is a measurement array,

(1.4) determining Kalman Filter equation

And (3) state one-step prediction:wherein:representing the one-step predicted value of the state from the time k-1 to the time k,represents the state estimate at time k-1, phi_k,k-1Representing a discretized Kalman filtering state transition matrix;

one-step prediction of mean square error matrix:wherein:representing a one-step prediction mean square error matrix from time k-1 to time k,estimated mean square error matrix, Q, representing time k-1_kRepresenting a discretized system noise matrix;

a filter gain matrix:wherein:filter gain matrix representing time k, H_kSystematic measurement array representing time k, R_kRepresenting the discretized measurement noise matrix;

and (3) state estimation:wherein:to representState estimate at time k, Z_kRepresenting a system observation measurement matrix;

estimating a mean square 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},

wherein:estimated mean square error matrix, F, representing time k_iRepresenting the corresponding element values of the non-discretized system state transition matrix, I representing the identity matrix of the corresponding dimension, T_nRepresenting the navigation period, T, of the inertial navigation system_fRepresents a filtering period;

(2) performing reverse optimal fixed interval smoothing on the basis of the Kalman filtering in the step (1);

setting the whole navigation time zoneIs meta [ 0N]T represents any time in the time interval, t is more than or equal to 0 and less than or equal to N, and the smooth estimation value of the fixed interval is expressed as

(2.1) first, forward Kalman filtering of 0 → N is performed, and the estimated state transition matrix phi of the filtering is stored_k,k-1State one-step predictionOne-step prediction mean square error arrayState estimationEstimating mean square error matrix

(2.2) after the filtering is finished, taking the estimation value of the last time N of the forward filtering as the initial value of the starting time of the backward R-T-S smoothing, and making k = N,performing an N → 0R-T-S smoothing process, whereinA smoothed estimate of the time instance N is represented,representing the kalman filter state estimate at time N,representing a smoothed estimated mean square error matrix at N time instants,card for indicating k timeThe mean square error matrix is estimated by the Kalman filtering, and the equation is as follows:

smoothing gain array:wherein,a smoothing gain matrix representing the time k, #_k+1,kRepresenting a discretized kalman filter state transition matrix,representing a one-step prediction mean square error matrix from the k moment to the k +1 moment;

smoothed 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}),

wherein:representing the smoothed estimate at the time of the k-th instant,representing the smoothed estimate at time k +1,representing a state one-step predicted value from the k moment to the k +1 moment;

smoothed estimated mean square 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},

wherein:representing the smoothed estimated mean square error matrix at time k,representing the smoothed estimated mean square error matrix at time k +1,represents a one-step prediction mean square error matrix from time k to time k +1,a smoothing gain array representing time k;

from the formula, the smoothed estimate at time kIs a forward Kalman filter estimatorIs linearly compensated by a correction amount which is a smoothed estimate at the (k + 1) th timeAnd forward one-step prediction estimatorA difference of (d);

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

Further, the method for evaluating the full-parameter accuracy of the long-time navigation system as described above, wherein: navigation period T of inertial navigation system_n=0.05s。

Further, the method for evaluating the full-parameter accuracy of the long-time navigation system as described above, wherein: filter period T_f=1s。

As can be known from the analysis process, the R-T-S smoothing estimator at the time k is linear fusion of the forward Kalman filtering estimator at the time k and the smoothing estimator at the time k +1, and the smoothing estimator at the time k +1 effectively utilizes global data, so that the R-T-S smoothing of the Kalman filtering is higher than the navigation precision obtained by purely adopting the Kalman filtering.

The invention has the beneficial effects that: the method comprises the steps of processing stored inertial navigation data and satellite data to obtain high-precision position, posture and azimuth information of a reference system, wherein the high-precision position, posture and azimuth information can be used as a reference for full-parameter precision evaluation of the reference system, the azimuth precision reaches 0.25 ', and the posture precision reaches 0.025'. Experiments prove that the technology can meet the requirement of evaluating the precision of the local reference system and has high practical value.

Drawings

FIG. 1 is a flow chart of an algorithm implementation of the present invention;

FIG. 2 is a post-processing position accuracy plot;

FIG. 3 is a post-processing speed accuracy curve;

FIG. 4 is a post-processing pose accuracy curve;

FIG. 5 is a post-processing course accuracy curve.

Detailed Description

The technical scheme of the invention is explained in detail in the following by combining the drawings and the specific embodiment.

the algorithm implementation flow of the invention is shown in fig. 1, and specifically comprises the following two steps to obtain position, speed and attitude data:

(1.1) determining an algorithmic model and a state error quantity

wherein:

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

inertial navigation X, Y, Z axis accelerometer zero offset;

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

and the lever arm error between inertial navigation and GPS.

(1.2) determining the equation of state of the System

The system state equation is:

wherein:

F_{11} = [\begin{matrix} 0 & - \frac{V_{n}}{{(R_{M} + h)}^{2}} & 0 \\ 0 & 0 & 0 \\ \frac{V_{e}}{R_{N} + h} \tan L \sec L & - \frac{V_{e} \sec L}{{(R_{N} + h)}^{2}} & 0 \end{matrix}]; F_{12} = [\begin{matrix} \frac{1}{R_{M} + h} & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \frac{\sec L}{R_{N} + h} \end{matrix}];

F_{23} = [\begin{matrix} 0 & {- f}_{e} & f_{u} \\ f_{e} & 0 & {- f}_{n} \\ {- f}_{u} & f_{n} & 0 \end{matrix}];

(1.3) determination of measurement equation

The measurement equation is as follows: z = HX + V

Wherein: and measuring the Z component, namely the difference value between the position information of the inertial navigation system and the GPS position information, and considering the lever arm error between the two: z = [ L-L =^GPS-R_n h-h^GPS-R_u λ-λ^GPS-R_e]^T；h^GPS、λ^GPS、L^GPSRespectively representing altitude information, longitude information and latitude information output by a GPS system; r_n、R_u、R_eRespectively representing position information and GPS position of inertial navigation systemSetting north lever arm errors, zenith lever arm errors and east lever arm errors among the information;

h is a measurement array,

(1.4) determining Kalman Filter equation

and (3) state estimation:wherein:state estimation value, Z, at time k_kRepresenting a system observation measurement matrix;

estimating a mean square 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},

wherein:estimated mean square error matrix, F, representing time k_iRepresenting the corresponding element values of the non-discretized system state transition matrix, I representing the identity matrix of the corresponding dimension, T_nRepresents the navigation cycle of the inertial navigation system (in this embodiment, T)_n=0.05s），T_fIndicates the filter period (in this particular embodiment, T_f=1s）；

let the whole navigation time interval be [ 0N ]]T represents any time in the time interval, t is more than or equal to 0 and less than or equal to N, and the smooth estimation value of the fixed interval is expressed as

(2.2) after the filtering is finished, taking the estimation value of the last time N of the forward filtering as the initial value of the starting time of the backward R-T-S smoothing, and making k = N,performing an N → 0R-T-S smoothing process, whereinA smoothed estimate of the time instance N is represented,representing the kalman filter state estimate at time N,representing a smoothed estimated mean square error matrix at N time instants,and expressing a Kalman filtering estimation mean square error matrix at the k moment, and the equation is as follows:

smoothing gain array:wherein,a smoothing gain matrix representing the time k, #_k+1,kRepresenting a discretized kalman filter state transition matrix,one-step prediction mean square error matrix from k time to k +1 time；

Smoothed 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}),

smoothed estimated mean square 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},

And obtaining high-precision position, speed and attitude information through the two steps. To verify the feasibility of this method, the voyage test data was processed.

Estimating a mean square error matrix according to algorithmic principlesThe size of (d) represents the accuracy of the post-processing algorithm. As shown in fig. 2-5.

As can be seen from FIGS. 2 to 5, the horizontal attitude precision obtained by the method is 0.025 ', the heading precision is 0.25', the position precision is 1.3m, and the speed error is 0.006m/s, and the horizontal attitude precision can be used as a reference for judging the precision of the local reference full parameter.

And processing the six pieces of navigation test data to obtain the online precision of each parameter of the local reference system, wherein the navigation attitude precision is shown in table 1.

TABLE 1 statistical accuracy of the tests

The statistical results in the table show that the maximum value of the horizontal attitude error of each test is 0.59 ', the maximum value of the heading error is 2.96', and the index requirements of the horizontal attitude error of 1.5 'and the heading error of 3.67'/cos phi can be met.

Claims

1. A full-parameter precision evaluation method of a long-time navigation system is characterized by comprising the following steps: obtaining high-precision position, attitude and azimuth information of a reference system by using original data and satellite information of a local reference system navigation experiment and adopting an off-line processing method based on RTS smoothing through processing stored inertial navigation data and satellite data, wherein the high-precision position, attitude and azimuth information is used as a reference for full-parameter precision evaluation of the reference system;

(1.1) determining an algorithmic model and a state error quantity

wherein:

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

inertial navigation X, Y, Z axis accelerometer zero offset;

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

and the lever arm error between inertial navigation and GPS.

(1.2) determining the equation of state of the System

The system state equation is:

wherein:

F_{11} = [\begin{matrix} 0 & - \frac{V_{n}}{{(R_{M} + h)}^{2}} & 0 \\ 0 & 0 & 0 \\ \frac{V_{e}}{R_{N} + h} \tan L \sec L & - \frac{V_{e} \sec L}{{(R_{N} + h)}^{2}} & 0 \end{matrix}]; F_{12} = [\begin{matrix} \frac{1}{R_{M} + h} & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \frac{\sec L}{R_{N} + h} \end{matrix}];

F_{23} = [\begin{matrix} 0 & {- f}_{e} & f_{u} \\ f_{e} & 0 & {- f}_{n} \\ {- f}_{u} & f_{n} & 0 \end{matrix}];

wherein: vn, Vu, Ve represent north, sky and east speeds of the inertial navigation systemDegree; l represents latitude, h represents altitude, R_MRepresents the radius of the meridian of the earth, R_NRepresenting radius of the prime circle, omega, of the earth_ieWhich represents the rotational angular velocity of the earth,representing the attitude transformation matrix, f, of the inertial navigation system from the carrier coordinate system to the navigation coordinate system_n、f_u、f_eRepresenting north, sky and east representation values of specific force sensed by an accelerometer of the inertial navigation system in a navigation coordinate system;

(1.3) determination of measurement equation

The measurement equation is as follows: z = HX + V

h is a measurement array,

(1.4) determining Kalman Filter equation

estimating a mean square 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},

wherein:estimated mean square error matrix, F, representing time k_iRepresenting the values of the corresponding elements of the non-discretized system state transition matrix, I representing the unit of the corresponding dimensionBit matrix, T_nRepresenting the navigation period, T, of the inertial navigation system_fRepresents a filtering period;

smoothed 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}),

smoothed estimated mean square 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},

2. The full-parameter accuracy assessment method of the long-time navigation system as claimed in claim 1, wherein: navigation period T of inertial navigation system_n=0.05s。

3. The full-parameter accuracy assessment method of the long-time navigation system as claimed in claim 1, wherein: filter period T_f=1s。