A GNSS signal quality adaptive integrated navigation method

Abstract

The application relates to a GNSS signal quality adaptive combination navigation method and relates to the technical field of vehicle navigation. IMU and GNSS positioning information and a positioning state are read, combination navigation is realized through an ESKF loose combination algorithm, the measurement covariance matrix is changed in real time according to the GNSS positioning state and a DOP factor, finally, the value of the error state is estimated and fed back to the combination navigation system, the position and speed information error in the navigation coordinate system and the attitude Euler angle error are fed back to the INS mechanical arrangement, the angular acceleration zero bias error of the IMU is fed back to the IMU itself, and the update of the state quantity is completed. According to the ESKF fusion algorithm of the loose combination of the IMU and GNSS data, the measurement covariance matrix is changed in real time according to the GNSS positioning state and the DOP factor, the error state is estimated and fed back to the combination navigation system, and the positioning precision can be effectively improved.

Description

Technical Field

[0001] This invention relates to the field of vehicle navigation technology, specifically a GNSS signal quality adaptive integrated navigation method. Background Technology

[0002] The widespread use of Global Navigation Satellite Systems (GNSS) provides real-time positioning and navigation services in the automotive field. To achieve accurate positioning, a GNSS receiver needs to receive signals from at least four satellites. These satellites should be distributed to cover different directions as much as possible to ensure high positioning accuracy and reliability. The quality of GNSS signals can be affected by various factors, such as the natural environment, satellite geometry, and electromagnetic interference. Different GNSS positioning modes directly affect positioning accuracy. Specifically: Single Point Positioning (SPS) is the most basic GNSS positioning mode, using only signals from GNSS satellites for positioning, with relatively low accuracy, typically within a few meters; Real-Time Kinematics (RTK) can provide centimeter-level positioning accuracy; Differential GPS (DGPS) corrects for satellite signal errors by using known position information from one or more ground reference stations.

[0003] An inertial measurement unit (IMU), composed of accelerometers and gyroscopes, is used to measure and report the vehicle's physical properties in real time, including velocity, orientation, and acceleration. In a vehicle's strapdown inertial navigation system, dead reckoning (DR) can be performed based on the raw IMU data. The basic idea is to obtain velocity information by integrating the IMU's acceleration data, then integrate the velocity data again to estimate displacement, and finally correct or update the object's orientation using gyroscope data to improve the accuracy of displacement calculation. However, due to the accumulation of IMU errors, small measurement errors amplify over time, affecting positioning accuracy. Furthermore, the raw IMU data is affected by factors such as bias and scale factor, requiring appropriate calibration and compensation mechanisms to ensure data accuracy.

[0004] Current loosely coupled fusion strategies for IMU and GNSS data, assuming initial pose is known, process IMU and GNSS data separately and then fuse the results. Navigation solutions are calculated using IMU dead reckoning, and GNSS measurements are used as observations, combined using error-state Kalman filtering to improve overall system positioning accuracy. However, in practical applications, GNSS positioning accuracy is affected by positioning modes and satellite geometry, increasing noise in the fusion algorithm. In unreliable GNSS positioning information, the integrated navigation system is affected. Current navigation strategies typically ignore this effect and the variations in GNSS positioning modes in real-world applications, indirectly introducing noise into the integrated navigation system and consequently reducing accuracy. Summary of the Invention

[0005] To address the shortcomings of the prior art, this invention provides a GNSS signal quality adaptive integrated navigation method. This method uses a loosely combined ESKF fusion algorithm to adjust the measurement covariance matrix in real time based on the GNSS positioning status and DOP factor for IMU and GNSS data, and estimates the error status to feed back to the integrated navigation system, which can effectively improve positioning accuracy.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: a GNSS signal quality adaptive integrated navigation method, comprising the following steps:

[0007] Step 1: Read IMU and GNSS positioning information and positioning status

[0008] The raw data read from the IMU includes triaxial acceleration, triaxial angular velocity, and timestamp information. The GNSS connects to the Cors base station via a 4G module to achieve RTK positioning mode. After convergence, the positioning mode, positioning accuracy factor (DOP), location, and timestamp information are obtained. The positioning mode information includes RTK fixed solution, RTK floating-point solution, DGPS, and GPS. The location information includes longitude, latitude, and altitude. The microcontroller saves the IMU and GNSS information data to an SD card for subsequent offline data processing.

[0009] Step 2: Implementing Integrated Navigation using the ESKF Scattered Combination Algorithm

[0010] The error state Kalman filter method is designed, using the North-East-Ground coordinate system as the navigation coordinate system and the Front-Right-Down coordinate system as the vehicle coordinate system. The error state vector model is defined as follows:

[0011]

[0012] In the formula, This refers to the position information error in the navigation coordinate system. For the velocity information error in the navigation coordinate system, For attitude Euler angle error, For the angular velocity zero bias error of the IMU, The acceleration bias error of the IMU;

[0013] The rotation matrix from the vehicle coordinate system to the navigation coordinate system is defined as follows:

[0014]

[0015] In the formula, This represents the calculated estimate of the rotation matrix. This is the roll angle. The pitch angle, For heading angle;

[0016] INS mechanical arrangement from Time's up The update of the state at any given moment, where:

[0017] The attitude update equation is expressed as follows:

[0018]

[0019] In the formula, express Euler angles of the moment express The triaxial angular velocity information measured by the IMU at any given time. express The estimated value of the rotation matrix at time t. and These represent the projection components of the Earth's rotational angular velocity and the entrainment angular velocity at the current geographical location in the navigation coordinate system. Indicates the sampling time interval;

[0020] The speed update equation is as follows:

[0021]

[0022] In the formula, express Velocity information in the navigation coordinate system at any time. express Time's up The projection component of the velocity increment at time intervals in the carrier coordinate system. This represents the gravitational acceleration at the current geographical location;

[0023] Position update, expressed by the equation as follows:

[0024]

[0025] In the formula, express The height of time, express The latitude of time express Longitude of time Indicates ground velocity, Indicates northbound speed. Indicates eastward speed. and These represent the radii of the Earth's circumference and meridian, respectively.

[0026] The attitude error model is represented as follows: The error state vector of the navigation coordinate system is modeled.

[0027]

[0028] In the formula, and These represent the estimated position and velocity values ​​in the navigation coordinate system, respectively. and These represent the position and velocity information in the navigation coordinate system calculated by INS mechanical orchestration, respectively. Represents the identity matrix. The antisymmetric matrix representing the deviation angles between the actual Euler angles and the calculated Euler angles. Represents the true value of the rotation matrix;

[0029] Considering the zero bias and white noise of the IMU, the error models for the IMU accelerometer and angular velocity meter are expressed as follows:

[0030]

[0031] in,

[0032]

[0033] In the formula, and These represent the errors of the angular velocity meter and accelerometer in the carrier coordinate system output by the IMU, respectively. and Modeled as a first-order Gaussian Markov model. and Represents the white noise of the IMU. and The time constant of the model, and For the bias of the model;

[0034] The state transition matrix is ​​obtained by constructing a state propagation model from error states. as follows:

[0035]

[0036] In the formula, express The antisymmetric matrix formed by the projection components of the IMU output at any given time onto the navigation coordinate system. This represents the antisymmetric matrix formed by the Earth's rotational angular velocity and its entrainment angular velocity. , , , , They are as follows:

[0037]

[0038]

[0039]

[0040]

[0041]

[0042] In the formula, This represents the Earth's rotational speed constant;

[0043] The state prediction equation for the integrated navigation system is as follows:

[0044]

[0045] in,

[0046]

[0047] In the formula, express Discrete state transition matrix at time t. express The continuous state transition matrix at time t. express The IMU noise matrix at time t. This represents the IMU noise driving matrix. This represents the noise covariance matrix of the IMU measurement. Indicates in Time prediction Error state vector at time step [time]. Indicates in Time prediction The state covariance matrix at time t;

[0048] Step 3: Adjust the measurement covariance matrix in real time based on the GNSS positioning status and DOP factor.

[0049] Read the GNSS positioning mode information at each moment, initialize the measurement covariance matrix according to preset values ​​based on different positioning mode information, and calculate the values ​​based on the HDOP factor and VDOP factor. The measurement covariance matrix at time t is represented as follows:

[0050]

[0051] In the formula, This represents the value of the initial measurement covariance matrix;

[0052] The position information of the navigation coordinate system output by GNSS is used as the observation value of the ESKF algorithm. The observation equation of the integrated navigation system is expressed as follows:

[0053]

[0054] In the formula, This represents the estimated position information in the navigation coordinate system after compensating for lever arm effects. Vectors representing the spatial location of GNSS and IMU. This represents the deviation vector between the predicted position information in the navigation coordinate system and the position information measured by GNSS. This represents the position vector information in the navigation coordinate system measured by GNSS. This represents the observation matrix of the integrated navigation system. The transformation matrix representing latitude and longitude to meters is shown below:

[0055]

[0056] Construct the Kalman filter equation and apply it to the error state vector model. and state covariance matrix An estimate is made, expressed as follows:

[0057]

[0058] In the formula, express Kalman gain at time step;

[0059] Step 4: Estimate the error state value and feed it back to the integrated navigation system.

[0060] exist The position information error in the navigation coordinate system is calculated in the error state vector at each time step. and speed information error and attitude Euler angle error The navigation error is fed back to the INS mechanical orchestration, and the IMU's angular velocity zero bias error is also included. and acceleration zero bias error The error is fed back to the IMU itself to update the state variables, and finally the error state vector is cleared to zero.

[0061] Compared with existing technologies, the beneficial effects of this invention are as follows: This invention first acquires raw data from IMU and GNSS, then uses a loosely combined ESKF fusion algorithm, combined with the accuracy factor DOP, to calculate the measurement covariance matrix of the ESKF filter based on the GNSS positioning quality. The calculated error state vector is fed back to the integrated navigation system, achieving adaptive integrated navigation through real-time changes in GNSS signal quality. This invention has the following characteristics:

[0062] 1. Only GNSS location measurement information is required; no additional GNSS measurement information is needed.

[0063] 2. Applicable to different GNSS positioning systems, not just RTK positioning implemented by Cors base stations;

[0064] 3. By considering the relationship between satellite geometric position and positioning accuracy, the impact of GNSS positioning information with large errors on the system can be reduced, thereby indirectly improving positioning accuracy;

[0065] 4. It can switch according to different positioning modes and adaptively increase or decrease the fusion weight of GNSS measurement values;

[0066] 5. It can be implemented based on an embedded system, providing continuous positioning status. Attached Figure Description

[0067] Figure 1 This is a flowchart of the method of the present invention;

[0068] Figure 2 This is a system block diagram of the method of the present invention;

[0069] Figure 3 This is a schematic diagram of the positioning effect of the extended Kalman filter method and GNSS positioning points in the embodiment;

[0070] Figure 4 This is a schematic diagram of the positioning effect of the method of the present invention and GNSS positioning points in the embodiments;

[0071] Figure 5 yes Figure 3 A magnified view of a portion of the image;

[0072] Figure 6 yes Figure 4 A magnified view of a portion of the image. Detailed Implementation

[0073] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the invention, not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0074] like Figures 1-2 As shown, a GNSS signal quality adaptive integrated navigation method includes the following steps:

[0075] Step 1: Read IMU and GNSS positioning information and positioning status

[0076] The IMU's raw data is read at a frequency of 100Hz, including triaxial acceleration (specific force), triaxial angular velocity, and timestamp information.

[0077] The GNSS connects to the Cors base station via a 4G module using the Ntrip protocol. The 4G module sends the $GGA message in the NMEA0183 protocol to the Cors base station. The Cors base station returns RTCM differential data and forwards it to the GNSS receiver module to achieve RTK positioning mode. After convergence, the GNSS receiver module acquires the NMEA0183 message at a frequency of 1Hz, which includes positioning mode, positioning accuracy factor (DOP), location, and timestamp information. The positioning mode information includes RTK fixed solution, RTK floating solution, DGPS, and GPS, and the location information includes longitude, latitude, and altitude.

[0078] The microcontroller saves the IMU and GNSS information data to an SD card and performs subsequent offline data processing.

[0079] Step 2: Implementing Integrated Navigation using the ESKF Scattered Combination Algorithm

[0080] The Kalman filter method for error states is designed, which uses the North-East-Ground coordinate system as the navigation coordinate system (Navi system) and the Front-Right-Down coordinate system as the body coordinate system (Body system). The error state vector model is defined as follows:

[0081]

[0082] In the formula, This refers to the position information error (longitude, latitude, altitude) in the navigation coordinate system. For the velocity information error in the navigation coordinate system, For attitude Euler angle error, For the angular velocity zero bias error of the IMU, This represents the zero-bias error of the IMU's acceleration.

[0083] The rotation matrix from the vehicle coordinate system to the navigation coordinate system is defined as follows:

[0084]

[0085] In the formula, This represents the calculated estimate of the rotation matrix. This is the roll angle. The pitch angle, This is the heading angle.

[0086] INS mechanical arrangement indicates from Time's up The update of the state at any given moment, where:

[0087] In the attitude update of the integrated navigation system, based on the incremental angular velocity information output by the IMU, the Earth's rotation angular velocity at the current geographical location is subtracted, resulting in the attitude update equation as follows:

[0088]

[0089] In the formula, express Euler angles of the moment express The triaxial angular velocity information measured by the IMU at any given time. express The estimated value of the rotation matrix at time t. and These represent the projection components of the Earth's rotational angular velocity and the entrainment angular velocity at the current geographical location in the navigation coordinate system. Indicates the sampling time interval.

[0090] In the velocity update of the integrated navigation system, the gravitational acceleration at the current geographical location is considered, and its value changes with the geographical location. Based on the current acceleration information output by the IMU accelerometer, and after subtracting the gravitational component and the influence of the Coriolis force, the velocity update equation is obtained as follows:

[0091]

[0092] In the formula, express Velocity information in the navigation coordinate system at any time. express Time's up The projection component of the velocity increment at time intervals in the carrier coordinate system. This represents the gravitational acceleration at the current geographical location.

[0093] In the position update of the integrated navigation system, the latitude and longitude increments in the geographic coordinate system are obtained based on the velocity information of the navigation coordinate system at the current moment. The position update equation is obtained by adding the latitude and longitude increments from the previous moment to the latitude and longitude, as follows:

[0094]

[0095] In the formula, express Altitude at any given time (m) express Latitude (rad) of a given moment express Longitude (rad) at any given time Indicates ground velocity, Indicates northbound speed. Indicates eastward speed. and These represent the radii of the Earth's circumference and meridian, respectively.

[0096] The attitude error model is represented as follows: The error state vector of the navigation coordinate system is modeled.

[0097]

[0098] In the formula, and These represent the estimated position and velocity values ​​in the navigation coordinate system, respectively. and These represent the position and velocity information in the navigation coordinate system calculated by INS mechanical orchestration, respectively. Represents the identity matrix. The antisymmetric matrix representing the deviation angles between the actual Euler angles and the calculated Euler angles. This represents the true value of the rotation matrix.

[0099] Considering the zero bias and white noise of the IMU, the error models for the IMU accelerometer and angular velocity meter are expressed as follows:

[0100]

[0101] in,

[0102]

[0103] In the formula, and These represent the errors of the angular velocity meter and accelerometer in the carrier coordinate system output by the IMU, respectively. and Modeled as a first-order Gaussian Markov model. and Represents the white noise of the IMU. and The time constant of the model, and This is the bias of the model.

[0104] The state transition matrix is ​​obtained by constructing a state propagation model from error states. as follows:

[0105]

[0106] In the formula, express The antisymmetric matrix formed by the projection components of the IMU output at any given time onto the navigation coordinate system. This represents the antisymmetric matrix formed by the Earth's rotational angular velocity and its entrainment angular velocity. , , , , They are as follows:

[0107]

[0108]

[0109]

[0110]

[0111]

[0112] In the formula, This represents the Earth's rotational speed constant.

[0113] The state prediction equation for the integrated navigation system is as follows:

[0114]

[0115] in,

[0116]

[0117] In the formula, express Discrete state transition matrix at time t. express The continuous state transition matrix at time t. express The IMU noise matrix at time t. This represents the IMU noise driving matrix. This represents the noise covariance matrix of the IMU measurement. Indicates in Time prediction Error state vector at time step [time]. Indicates in Time prediction The state covariance matrix at time t.

[0118] Step 3: Adjust the measurement covariance matrix in real time based on the GNSS positioning status and DOP factor.

[0119] The positioning mode information of GNSS at each moment is read. Different positioning modes directly determine the positioning accuracy. RTK fixed solution provides the highest accuracy, followed by RTK floating solution, DGPS, and SPS. Based on the different positioning mode information, the measurement covariance matrix is ​​initialized according to preset values. The value of this factor, based on which the magnitude of the HDOP factor reflects the sum of squared errors in horizontal latitude and longitude, is expressed as follows:

[0120]

[0121] In the formula, The standard deviation of latitude It represents the standard deviation of longitude.

[0122] The standard deviation of latitude and longitude directly reflects the accuracy of positioning. Therefore, the HDOP factor indirectly reflects the accuracy of horizontal positioning. Similarly, the VDOP factor indirectly reflects the accuracy of elevation positioning.

[0123] Calculated based on HDOP factor and VDOP factor The measurement covariance matrix at time t is represented as follows:

[0124]

[0125] In the formula, This represents the value of the initial measurement covariance matrix.

[0126] From this, we obtain The value changes in real time based on the GNSS positioning mode information and the DOP factor.

[0127] Considering the "pole arm effect" of the GNSS antenna and IMU installation positions—that is, the spatial positional deviation caused by the installation of the GNSS and IMU—the spatial positions of the GNSS and IMU are measured during installation as extrinsic parameters of the integrated navigation system. The three-dimensional position information of the navigation coordinate system output by the GNSS is used as the observation value of the ESKF algorithm. The observation equation of the integrated navigation system is expressed as follows:

[0128]

[0129] In the formula, This represents the estimated position information in the navigation coordinate system after compensating for lever arm effects. Vectors representing the spatial location of GNSS and IMU. This represents the deviation vector between the predicted position information in the navigation coordinate system and the position information measured by GNSS. This represents the position vector information in the navigation coordinate system measured by GNSS. This represents the observation matrix of the integrated navigation system. The transformation matrix representing latitude and longitude to meters is shown below:

[0130]

[0131] Construct the Kalman filter equation and apply it to the error state vector model. and state covariance matrix An estimate is made, expressed as follows:

[0132]

[0133] In the formula, express Kalman gain at time step.

[0134] Step 4: Estimate the error state value and feed it back to the integrated navigation system.

[0135] exist The position information error in the navigation coordinate system is calculated in the error state vector at each time step. and speed information error and attitude Euler angle error The navigation error is fed back to the INS mechanical orchestration, and the IMU's angular velocity zero bias error is also included. and acceleration zero bias error The error is fed back to the IMU itself to update the state variables, and finally the error state vector is cleared to zero.

[0136] Example

[0137] Raw data is read from the IMU and GNSS: The IMU module selected is the WheelTec-N100, which communicates with the microcontroller via CAN bus. The microcontroller reads the IMU's specific force and angular velocity information at a frequency of 100Hz. The GNSS module selected is the TAU1308 positioning module, which, together with the 4G module, connects to the CORS base station for RTK positioning. It communicates with the microcontroller via serial port and reads the GNSS position, DOP factor, and positioning status information at a frequency of 1Hz. The raw data from the IMU and GNSS modules is stored on an SD card for subsequent offline data processing.

[0138] ESKF loose combination algorithm implements integrated navigation: the acquired IMU raw data is processed offline, and INS mechanical arrangement is performed at each time step to recursively obtain the attitude, velocity, and position information of the integrated navigation system; at the same time, the state transition matrix is ​​obtained according to the state propagation model. .

[0139] The measurement covariance matrix is ​​changed in real time based on GNSS positioning status and DOP factor. GNSS information is used to obtain the initial measurement covariance matrix based on different positioning mode information. And based on the DOP factor at each time step, according to the proposed Matrix calculation method for real-time calculation of measurement covariance matrix .

[0140] The initial measurement covariance matrix set in this embodiment As shown in Table 1:

[0141]

[0142] Construct the Kalman filter equation and apply it to the error state vector. and state covariance matrix Make an estimate.

[0143] The error state value is estimated and fed back to the integrated navigation system: the integrated navigation system error (including position information error, velocity information error and attitude error) calculated according to the ESKF method is fed back to the INS mechanical arrangement, and the calculated IMU error (including the IMU's angular velocity zero bias and acceleration zero bias) is fed back to the IMU error compensation.

[0144] The design's operational parameters and filter parameters are as follows:

[0145] IMU spatial position vector relative to GNSS module ;

[0146] Initial NE-GV of Integrated Navigation System ;

[0147] Initial position of integrated navigation system ;

[0148] Initial Euler angles of integrated navigation system ;

[0149] IMU module parameters:

[0150] , ;

[0151] , ;

[0152] , .

[0153] This embodiment verifies the effectiveness of the method of the present invention under different GNSS positioning environments. Comparative experiments are conducted using the extended Kalman filter method and the method of the present invention, and the positioning results of both are combined with the GNSS positioning points. Figure 3 and Figure 4 As shown, prominent positions are magnified and displayed in combination with... Figure 5 and Figure 6 As shown, the method of the present invention can effectively make rapid adjustments according to changes in GNSS positioning quality, and the positioning accuracy is not significantly affected by bad GNSS positioning signals, thus verifying the superiority of the method of the present invention.

[0154] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered illustrative and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of the equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

[0155] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This narrative style is merely for clarity. Those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A GNSS signal quality adaptive integrated navigation method, characterized in that: Includes the following steps: Step 1: Read IMU and GNSS positioning information and positioning status The raw data read from the IMU includes triaxial acceleration, triaxial angular velocity, and timestamp information. The GNSS connects to the Cors base station via a 4G module to achieve RTK positioning mode. After convergence, the positioning mode, positioning accuracy factor (DOP), location, and timestamp information are obtained. The positioning mode information includes RTK fixed solution, RTK floating-point solution, DGPS, and GPS. The location information includes longitude, latitude, and altitude. The microcontroller saves the IMU and GNSS information data to an SD card for subsequent offline data processing. Step 2: Implementing Integrated Navigation using the ESKF Scattered Combination Algorithm The error state Kalman filter method is designed, using the North-East-Ground coordinate system as the navigation coordinate system and the Front-Right-Down coordinate system as the vehicle coordinate system. The error state vector model is defined as follows: In the formula, This refers to the position information error in the navigation coordinate system. For the velocity information error in the navigation coordinate system, For attitude Euler angle error, For the angular velocity zero bias error of the IMU, The acceleration bias error of the IMU; The rotation matrix from the vehicle coordinate system to the navigation coordinate system is defined as follows: In the formula, This represents the calculated estimate of the rotation matrix. This is the roll angle. The pitch angle, For heading angle; INS mechanical arrangement from Time's up The update of the state at any given moment, where: The attitude update equation is expressed as follows: In the formula, express Euler angles of the moment express The triaxial angular velocity information measured by the IMU at any given time. express The estimated value of the rotation matrix at time t. and These represent the projection components of the Earth's rotational angular velocity and the entrainment angular velocity at the current geographical location in the navigation coordinate system. Indicates the sampling time interval; The speed update equation is as follows: In the formula, express Velocity information in the navigation coordinate system at any time. express Time's up The projection component of the velocity increment at time intervals in the carrier coordinate system. This represents the gravitational acceleration at the current geographical location; Position update, expressed by the equation as follows: In the formula, express The height of time, express The latitude of time express Longitude of time Indicates ground velocity, Indicates northbound speed. Indicates eastward speed. and These represent the radii of the Earth's circumference and meridian, respectively. The attitude error model is represented as follows: The error state vector of the navigation coordinate system is modeled. In the formula, and These represent the estimated position and velocity values ​​in the navigation coordinate system, respectively. and These represent the position and velocity information in the navigation coordinate system calculated by INS mechanical orchestration, respectively. Represents the identity matrix. The antisymmetric matrix representing the deviation angles between the actual Euler angles and the calculated Euler angles. Represents the true value of the rotation matrix; Considering the zero bias and white noise of the IMU, the error models for the IMU accelerometer and angular velocity meter are expressed as follows: in, In the formula, and These represent the errors of the angular velocity meter and accelerometer in the carrier coordinate system output by the IMU, respectively. and Modeled as a first-order Gaussian Markov model. and Represents the white noise of the IMU. and The time constant of the model, and For the bias of the model; The state transition matrix is ​​obtained by constructing a state propagation model from error states. as follows: In the formula, express The antisymmetric matrix formed by the projection components of the IMU output at any given time onto the navigation coordinate system. This represents the antisymmetric matrix formed by the Earth's rotational angular velocity and its entrainment angular velocity. , , , , They are as follows: In the formula, This represents the Earth's rotational speed constant; The state prediction equation for the integrated navigation system is as follows: in, In the formula, express Discrete state transition matrix at time t. express The continuous state transition matrix at time t. express The IMU noise matrix at time t. This represents the IMU noise driving matrix. This represents the noise covariance matrix of the IMU measurement. Indicates in Time prediction Error state vector at time step [time]. Indicates in Time prediction The state covariance matrix at time t; Step 3: Adjust the measurement covariance matrix in real time based on the GNSS positioning status and DOP factor. Read the GNSS positioning mode information at each moment, initialize the measurement covariance matrix according to preset values ​​based on different positioning mode information, and calculate the values ​​based on the HDOP factor and VDOP factor. The measurement covariance matrix at time t is represented as follows: In the formula, This represents the value of the initial measurement covariance matrix; The position information of the navigation coordinate system output by GNSS is used as the observation value of the ESKF algorithm. The observation equation of the integrated navigation system is expressed as follows: In the formula, This represents the estimated position information in the navigation coordinate system after compensating for lever arm effects. Vectors representing the spatial location of GNSS and IMU. This represents the deviation vector between the predicted position information in the navigation coordinate system and the position information measured by GNSS. This represents the position vector information in the navigation coordinate system measured by GNSS. This represents the observation matrix of the integrated navigation system. The transformation matrix representing latitude and longitude to meters is shown below: Construct the Kalman filter equation and apply it to the error state vector model. and state covariance matrix An estimate is made, expressed as follows: In the formula, express Kalman gain at time step; Step 4: Estimate the error state value and feed it back to the integrated navigation system. exist The position information error in the navigation coordinate system is calculated in the error state vector at each time step. and speed information error and attitude Euler angle error The navigation error is fed back to the INS mechanical orchestration, and the IMU's angular velocity zero bias error is also included. and acceleration zero bias error The error is fed back to the IMU itself to update the state variables, and finally the error state vector is cleared to zero.

Images