Inertial integrated navigation method based on time series analysis to construct covariance matrix

Abstract

The invention discloses an inertial combined navigation method for constructing a covariance matrix based on time series analysis, belongs to the field of navigation technology, and is used for accurately setting an observation noise covariance matrix, including performing time series analysis on an observation noise vector, calculating the measurement variance value of the shortest sampling interval, performing a difference between the measurement data at the current moment and the measurement data at the previous moment to obtain an average, and considering the observation noise covariance matrix at the previous two moments, constructing an adaptive observation noise covariance matrix; constructing a state equation and a measurement equation according to a combination of a strapdown inertial navigation system and a GNSS system; according to the state equation and the observation equation, using an adaptive observation noise covariance matrix to perform an extended Kalman filter, obtaining a filtering result and feeding it back to the strapdown inertial navigation system, and finally outputting a combined navigation positioning result. The invention analyzes the observation noise based on the time series, adaptively obtains the observation noise covariance matrix under different environments, and improves the positioning accuracy.

Description

Technical Field

[0001] The invention discloses an inertial integrated navigation method for constructing a covariance matrix based on time series analysis, and belongs to the technical field of navigation. Background Art

[0002] Inertial navigation has the advantages of strong autonomy, high short-term accuracy, high output bandwidth, and complete navigation information, but navigation errors accumulate over time, making it difficult to independently perform long-term high-precision navigation tasks. The Global Satellite Navigation Positioning System (GNSS) has the advantages of globality and all-weather, but satellite signals are easily affected by external environmental interference, carrier maneuvers, occlusions and other factors, resulting in the receiver being unable to output continuous high-precision navigation positioning and attitude information. Inertial navigation and GNSS are highly complementary in navigation principles, error characteristics and other methods. Combining the two can overcome their shortcomings.

[0003] When fusing inertial navigation and GNSS data, the Kalman filter method is usually used, but the normal operation of the Kalman filter depends on the correct definition of the random model and the dynamic model. The random model describes the random characteristics of the system process noise and observation noise, and the state model describes the state change of the system state over time. The uncertainty of the covariance matrix of the system process noise and observation noise has a significant impact on the performance of the Kalman filter. Usually, good empirical analysis is required to determine the covariance matrix of the system process noise and observation noise, and in practice, the covariance matrix of the system process noise and observation noise is usually fixed and lacks flexibility.

[0004] The existing adaptive model using adaptive models and covariance directly estimates the covariance matrix of system process noise and observation noise, realizes the change of innovation covariance and residual covariance, and adjusts it in time; the observation noise covariance matrix is ​​constructed based on Allan variance estimation and fading factor, but the construction of fading factor needs to be determined based on empirical values. Previous studies usually use variance or empirical values ​​to construct the observation noise covariance matrix. Therefore, how to construct an adaptive observation noise covariance matrix under different environments is still a topic that needs to be studied. Summary of the invention

[0005] The purpose of the present invention is to provide an inertial integrated navigation method for constructing a covariance matrix based on time series analysis, so as to solve the problems in the prior art of low positioning accuracy, the need to use prior knowledge or empirical values ​​to obtain fading parameters, and inaccurate estimation of the measurement noise covariance matrix in traditional Kalman filtering affecting the filtering accuracy.

[0006] The inertial integrated navigation method based on time series analysis to construct the covariance matrix includes:

[0007] S1. Perform time series analysis on the observed noise vector by calculating the shortest sampling interval t0 The variance value of the measurement at the time is obtained, the difference between the measurement data at the current moment and the measurement data at the previous moment is averaged, and the observation noise covariance matrix of the previous two moments is considered to construct an adaptive observation noise covariance matrix;

[0008] S2. Construct state equations and measurement equations based on the combination of strapdown inertial navigation system and GNSS system;

[0009] S3. According to the state equation and measurement equation, the adaptive observation noise covariance matrix is ​​used to perform extended Kalman filtering, and the filtering results are fed back to the strapdown inertial navigation system, and finally the combined navigation positioning results are output.

[0010] S1 includes the shortest sampling interval t 0 The variance of the measurement at for:

[0011]

[0012] Z i =[P lati P loni P highi V ni V ei V di ] T ;

[0013] Where, t 0 represents the shortest sampling interval, i represents the data at the i-th moment, k is the total length of the measured data, and Z i Represents the measurement data of the 3D position and 3D velocity at the i-th moment; the 3D position includes longitude, latitude and altitude, and the 3D velocity includes the velocity in the east, north and ground directions. lati , P loni , P highi Respectively represent latitude, longitude, and altitude data, V ni 、V ei 、V di Represents the speed in the east, north and ground directions respectively.

[0014] S1 includes the time series analysis of the observed noise vector as:

[0015]

[0016] In the formula, Z k Represents the measurement data at time k.

[0017] S1 includes setting the initial observation noise covariance matrix, and the adaptive observation noise covariance matrix is:

[0018]

[0019] In the formula, represents the observation noise covariance matrix at time k, represents the observation noise covariance matrix at time k-1, represents the observation noise covariance matrix at time k-1.

[0020] S1 includes that the adaptive observation noise covariance matrix is ​​only related to different time k, the observation value at the current time and the observation value at the previous time, and the observation noise covariance matrix under different environments is adaptively obtained.

[0021] S2 includes constructing the state equation as:

[0022] σx k+1 =Φ k+1 / k σx k +ω k ;

[0023] In the formula, Φ k+1 / k represents the state transfer matrix, σx k+1 represents the 21-dimensional state error vector at time k+1, ω k represents the system state noise vector;

[0024]

[0025] In the formula, Represents the position error vector of the position error in the east, north and sky directions in the navigation coordinate system n. Represents the velocity error vector of the velocity error in the east, north and sky directions in the navigation coordinate system n. Represents the attitude error vector in pitch angle, roll angle and heading angle, It represents the transposition of the gyro bias of the strapdown inertial navigation system in the right, front and top directions of the carrier coordinate system. It represents the transposition of the accelerometer zero bias of the strapdown inertial navigation system in the right, front and top directions of the carrier coordinate system. It represents the transposition of the gyroscope scale factor of the strapdown inertial navigation system in the right, front and top directions of the carrier coordinate system. Represents the transposition of the accelerometer scale factor of the strapdown inertial navigation system in the right, forward, and up directions of the carrier coordinate system.

[0026] S2 includes the three-dimensional position and three-dimensional velocity measurement equations:

[0027]

[0028] Where Z(t) represents the measurement vector, L INS ,INS 、h INS They represent the latitude, longitude and altitude obtained by the strapdown inertial navigation system INS through mechanical arrangement, L GNSS , GNSS 、h GNSS Respectively represent the latitude, longitude and altitude obtained by GNSS, V NINS 、V EINS 、V DINS They represent the speeds in the north, east, and ground directions obtained by INS through mechanical arrangement, respectively. NGNSS 、V EGNSS 、V DGNSS They represent the north, east, and ground speeds obtained by GNSS, respectively. H(t) represents the measurement matrix. V(t) represents the measurement noise covariance vector. R M , R N They represent the meridian curvature radius and the meridian curvature radius respectively, L represents the local latitude, and X(t) represents the state vector at time t.

[0029] S3 includes making one-step predictions of the state:

[0030]

[0031] In the formula, represents the state prediction value at time k-1 for time k, Φ k / k-1 represents the state transfer matrix from time k-1 to time k, Represents the state quantity at time k-1;

[0032] The state one-step prediction mean square error matrix is:

[0033]

[0034] Where P k / k-1 represents the state prediction covariance matrix, P k-1 represents the state covariance matrix at time k-1, Γ k-1 represents the system noise allocation matrix at time k-1, Q k-1 Represents the state noise covariance matrix at time k-1.

[0035] S3 includes the gain matrix:

[0036]

[0037] In the formula, K k represents the gain matrix of the extended Kalman filter, Represents the transpose of the observation matrix at time k;

[0038] The state estimation equation is:

[0039]

[0040] S3 includes the state estimation mean square error matrix:

[0041] P k =(IK k H k ) k / k-1 ;

[0042] Where P k represents the state estimation covariance matrix of the extended Kalman filter at time k, and I represents the identity matrix;

[0043] The position, velocity, attitude, gyroscope and accelerometer errors are obtained through state estimation and fed back to the position, velocity and attitude of the INS to perform position, velocity and attitude corrections and output navigation and positioning results.

[0044] Compared with the prior art, the present invention has the following beneficial effects: analyzing the observation noise based on the time series, adaptively acquiring the observation noise covariance matrix under different environments, and improving the positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 The adaptive observation noise covariance matrix method of GNSS / INS integrated navigation based on time series analysis designed by the present invention;

[0046] Figure 2 The process of obtaining an adaptive observation noise covariance matrix based on time series analysis of observation noise designed by the present invention;

[0047] Figure 3 The east direction position error result and RMS value of the present invention;

[0048] Figure 4 The north position error result and RMS value of the present invention;

[0049] Figure 5 It is the direction position error result and RMS value of the present invention;

[0050] Figure 6 The east direction speed error result and RMS value of the present invention;

[0051] Figure 7 The north direction velocity error result and RMS value of the present invention;

[0052] Figure 8 It is the direction speed error result and RMS value of the present invention. DETAILED DESCRIPTION

[0053] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention is described clearly and completely below. Obviously, the described embodiments are part of the embodiments of the present invention, but not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0054] The inertial integrated navigation method based on time series analysis to construct the covariance matrix, the adaptive observation noise covariance matrix method based on time series analysis of GNSS / INS integrated navigation designed by the present invention is as follows Figure 1 As shown, the steps include:

[0055] S1. Perform time series analysis on the observed noise vector by calculating the shortest sampling interval t 0 The variance value of the measurement at the time is obtained, the difference between the measurement data at the current moment and the measurement data at the previous moment is averaged, and the observation noise covariance matrix of the previous two moments is considered to construct an adaptive observation noise covariance matrix;

[0056] S2. Construct state equations and measurement equations based on the combination of strapdown inertial navigation system and GNSS system;

[0057] S3. According to the state equation and measurement equation, the adaptive observation noise covariance matrix is ​​used to perform extended Kalman filtering, and the filtering results are fed back to the strapdown inertial navigation system, and finally the combined navigation positioning results are output.

[0058] S1 includes the shortest sampling interval t 0 The variance of the measurement at for:

[0059]

[0060] Z i =[P lati P loni P highi V ni V ei V di ] T ;

[0061] In the formula, t 0 represents the shortest sampling interval, i represents the data at the i-th moment, k is the total length of the measured data, and Z i Represents the measurement data of the 3D position and 3D velocity at the i-th moment; the 3D position includes longitude, latitude and altitude, and the 3D velocity includes the velocity in the east, north and ground directions. lati , P loni , P highiRespectively represent latitude, longitude, and altitude data, V ni 、V ei 、V di Represents the speed in the east, north and ground directions respectively.

[0062] S1 includes the time series analysis of the observed noise vector as:

[0063]

[0064] In the formula, Z k Represents the measurement data at time k.

[0065] S1 includes setting the initial observation noise covariance matrix, and the adaptive observation noise covariance matrix is:

[0066]

[0067] In the formula, represents the observation noise covariance matrix at time k, represents the observation noise covariance matrix at time k-1, represents the observation noise covariance matrix at time k-1.

[0068] S1 includes that the adaptive observation noise covariance matrix is ​​only related to different time k, the observation value at the current time and the observation value at the previous time, and the observation noise covariance matrix under different environments is adaptively obtained.

[0069] S2 includes constructing the state equation as:

[0070] σx k+1 =Φ k+1 / k σx k +ω k ;

[0071] In the formula, Φ k+1 / k represents the state transfer matrix, σx k+1 represents the 21-dimensional state error vector at time k+1, ω k represents the system state noise vector;

[0072]

[0073] In the formula, Represents the position error vector of the position error in the east, north and sky directions in the navigation coordinate system n. Represents the velocity error vector of the velocity error in the east, north and sky directions in the navigation coordinate system n. Represents the attitude error vector in pitch angle, roll angle and heading angle, It represents the transposition of the gyro bias of the strapdown inertial navigation system in the right, front and top directions of the carrier coordinate system. It represents the transposition of the accelerometer zero bias of the strapdown inertial navigation system in the right, front and top directions of the carrier coordinate system. It represents the transposition of the gyroscope scale factor of the strapdown inertial navigation system in the right, front and top directions of the carrier coordinate system. Represents the transposition of the accelerometer scale factor of the strapdown inertial navigation system in the right, forward, and up directions of the carrier coordinate system.

[0074] S2 includes the three-dimensional position and three-dimensional velocity measurement equations:

[0075]

[0076] Where Z(t) represents the measurement vector, L INS , INS 、h INS They represent the latitude, longitude and altitude obtained by the strapdown inertial navigation system INS through mechanical arrangement, L GNSS , GNSS 、h GNSS Respectively represent the latitude, longitude and altitude obtained by GNSS, V NINS 、V EINS 、V DINS They represent the speeds in the north, east, and ground directions obtained by INS through mechanical arrangement, respectively. NGNSS 、V EGNSS 、V DGNSS They represent the north, east, and ground speeds obtained by GNSS, respectively. H(t) represents the measurement matrix. V(t) represents the measurement noise covariance vector. R M , R N They represent the meridian curvature radius and the meridian curvature radius respectively, L represents the local latitude, and X(t) represents the state vector at time t.

[0077] S3 includes making one-step predictions of the state:

[0078]

[0079] In the formula, represents the state prediction value at time k-1 for time k, Φ k / k-1 represents the state transfer matrix from time k-1 to time k, Represents the state quantity at time k-1;

[0080] The state one-step prediction mean square error matrix is:

[0081]

[0082] Where Pk / k-1 represents the state prediction covariance matrix, P k-1 represents the state covariance matrix at time k-1, Γ k-1 represents the system noise allocation matrix at time k-1, Q k-1 Represents the state noise covariance matrix at time k-1.

[0083] S3 includes the gain matrix:

[0084]

[0085] In the formula, K k represents the gain matrix of the extended Kalman filter, Represents the transpose of the observation matrix at time k;

[0086] The state estimation equation is:

[0087]

[0088] S3 includes the state estimation mean square error matrix:

[0089] P k =(IK k H k ) k / k-1 ;

[0090] Where P k represents the state estimation covariance matrix of the extended Kalman filter at time k, and I represents the identity matrix;

[0091] The position, velocity, attitude, gyroscope and accelerometer errors are obtained through state estimation and fed back to the position, velocity and attitude of the INS to perform position, velocity and attitude corrections and output navigation and positioning results.

[0092] The process of obtaining the adaptive observation noise covariance matrix based on time series analysis observation noise designed by the present invention is as follows: Figure 2 As shown, the adaptive observation noise covariance matrix is ​​obtained by performing time series analysis and taking the average value based on the GNSS position and velocity observations at the previous moment and the GNSS position and velocity observations at the current moment.

[0093] The east direction position error result and RMS value of the present invention are as follows: Figure 3 As shown, the adaptive observation noise covariance matrix is ​​used to perform extended Kalman filtering to obtain the position of the carrier in the east direction, and the east direction position obtained by the IE software is subtracted. The position error result and RMS value in the east direction are 0.02m; the north direction position error result and RMS value of the present invention are as follows Figure 4As shown, the extended Kalman filter is used to obtain the position of the carrier in the north direction by using the adaptive observation noise covariance matrix, and the north direction position obtained by the IE software is subtracted. The north direction position error result and RMS value are 0.02m; the ground direction position error result of the present invention and RMS value are as follows Figure 5 As shown, the adaptive observation noise covariance matrix is ​​used to perform extended Kalman filtering to obtain the position of the carrier in the ground direction, which is subtracted from the local position obtained by the IE software. The local upward position error result and RMS value are 0.06m.

[0094] The east direction speed error result of the present invention is similar to the RMS value. Figure 6 As shown, the adaptive observation noise covariance matrix is ​​used to perform extended Kalman filtering to obtain the carrier's speed in the east direction, and the speed in the east direction obtained by the IE software is subtracted. The speed error result in the east direction and the RMS value are 0.02m / s; the speed error result in the north direction of the present invention and the RMS value are as follows Figure 7 As shown, the adaptive observation noise covariance matrix is ​​used to perform extended Kalman filtering to obtain the velocity of the carrier in the north direction, and the velocity in the north direction obtained by the IE software is subtracted. The velocity error result and RMS value in the north direction are 0.03m / s; the velocity error result and RMS value in the ground direction of the present invention are as follows Figure 8 As shown, the adaptive observation noise covariance matrix is ​​used to perform extended Kalman filtering to obtain the carrier's velocity in the ground direction, which is then subtracted from the local velocity obtained by the IE software. The local upward velocity error result and RMS value are 0.03m / s.

[0095] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, a person skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some or all of the technical features may be replaced by equivalents, and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. An inertial integrated navigation method for constructing a covariance matrix based on time series analysis, characterized in that: include: S1. Perform time series analysis on the observation noise vector, calculate the variance value of the measurement at the shortest sampling interval t0, average the difference between the measurement data at the current moment and the measurement data at the previous moment, and consider the observation noise covariance matrix of the previous two moments to construct an adaptive observation noise covariance matrix; S2. Construct state equations and measurement equations based on the combination of strapdown inertial navigation system and GNSS system; S3. According to the state equation and measurement equation, the adaptive observation noise covariance matrix is ​​used to perform extended Kalman filtering, and the filtering results are fed back to the strapdown inertial navigation system, and finally the combined navigation positioning results are output.

2. The inertial integrated navigation method for constructing a covariance matrix based on time series analysis according to claim 1, characterized in that: S1 includes the variance value of the measurement at the shortest sampling interval t0 for: With i =[P lati P loni P highi In ni In ei In di ] T ; In the formula, t0 represents the shortest sampling interval, i represents the data at the i-th moment, k is the total length of the measured data, and Z i Represents the measurement data of the 3D position and 3D velocity at the i-th moment; the 3D position includes longitude, latitude and altitude, and the 3D velocity includes the velocity in the east, north and ground directions. lati , P loni , P highi Respectively represent latitude, longitude, and altitude data, V ni 、V ei 、V di Represents the speed in the east, north and ground directions respectively.

3. The inertial integrated navigation method for constructing a covariance matrix based on time series analysis according to claim 2, characterized in that: S1 includes the time series analysis of the observed noise vector as: In the formula, Z k Represents the measurement data at time k.

4. The inertial integrated navigation method for constructing a covariance matrix based on time series analysis according to claim 4, characterized in that: S1 includes setting the initial observation noise covariance matrix, and the adaptive observation noise covariance matrix is: In the formula, represents the observation noise covariance matrix at time k, represents the observation noise covariance matrix at time k-1, represents the observation noise covariance matrix at time k-1.

5. The inertial integrated navigation method for constructing a covariance matrix based on time series analysis according to claim 4, characterized in that: S1 includes that the adaptive observation noise covariance matrix is ​​only related to different time k, the observation value at the current time and the observation value at the previous time, and the observation noise covariance matrix under different environments is adaptively obtained.

6. The inertial integrated navigation method for constructing a covariance matrix based on time series analysis according to claim 5, characterized in that: S2 includes constructing the state equation as: sx k+1 =Φ k+1 / k sx k +oh k ; In the formula, Φ k+1 / k represents the state transfer matrix, σx k+1 represents the 21-dimensional state error vector at time k+1, ω k represents the system state noise vector; In the formula, Represents the position error vector of the position error in the east, north and sky directions in the navigation coordinate system n. Represents the velocity error vector of the velocity error in the east, north and sky directions in the navigation coordinate system n. Represents the attitude error vector in pitch angle, roll angle and heading angle, It represents the transposition of the gyro bias of the strapdown inertial navigation system in the right, front and top directions of the carrier coordinate system. It represents the transposition of the accelerometer zero bias of the strapdown inertial navigation system in the right, front and top directions of the carrier coordinate system. It represents the transposition of the gyroscope scale factor of the strapdown inertial navigation system in the right, front and top directions of the carrier coordinate system. Represents the transposition of the accelerometer scale factor of the strapdown inertial navigation system in the right, forward, and up directions of the carrier coordinate system.

7. The inertial integrated navigation method for constructing a covariance matrix based on time series analysis according to claim 6, characterized in that: S2 includes the three-dimensional position and three-dimensional velocity measurement equations: Where Z(t) represents the measurement vector, L INS , INS 、h INS They represent the latitude, longitude and altitude obtained by the strapdown inertial navigation system INS through mechanical arrangement, L GNSS , GNSS 、h GNSS Respectively represent the latitude, longitude and altitude obtained by GNSS, V NINS 、V EINS 、V DINS They represent the speeds in the north, east, and ground directions obtained by INS through mechanical arrangement, respectively. NGNSS 、V EGNSS 、V DGNSS They represent the north, east, and ground speeds obtained by GNSS, respectively. H(t) represents the measurement matrix. V(t) represents the measurement noise covariance vector. R M , R N They represent the meridian curvature radius and the meridian curvature radius respectively, L represents the local latitude, and X(t) represents the state vector at time t.

8. The inertial integrated navigation method for constructing a covariance matrix based on time series analysis according to claim 7, characterized in that: S3 includes making one-step predictions of the state: In the formula, represents the state prediction value at time k-1 for time k, Φ k / k-1 represents the state transfer matrix from time k-1 to time k, Represents the state quantity at time k-1; The state one-step prediction mean square error matrix is: Where P k / k-1 represents the state prediction covariance matrix, P k-1 represents the state covariance matrix at time k-1, Γ k-1 represents the system noise allocation matrix at time k-1, Q k-1 Represents the state noise covariance matrix at time k-1.

9. The inertial integrated navigation method for constructing a covariance matrix based on time series analysis according to claim 8, characterized in that: S3 includes the gain matrix: In the formula, K k represents the gain matrix of the extended Kalman filter, Represents the transpose of the observation matrix at time k; The state estimation equation is:

10. The inertial integrated navigation method for constructing a covariance matrix based on time series analysis according to claim 9, characterized in that: S3 includes the state estimation mean square error matrix: P k =(I-K k H k )P k / k-1 ; Where P k represents the state estimation covariance matrix of the extended Kalman filter at time k, and I represents the identity matrix; The position, velocity, attitude, gyroscope and accelerometer errors are obtained through state estimation and fed back to the position, velocity and attitude of the INS to perform position, velocity and attitude corrections and output navigation and positioning results.

Images