Adaptive navigation error prediction and compensation for integrated navigation

Abstract

This invention discloses an adaptive satellite navigation error prediction and compensation integrated navigation method, system, and device. The method includes a pure inertial navigation calculation thread, a satellite navigation prediction and compensation thread, and an integrated navigation fusion thread. The initial position, initial velocity, and initial attitude of the pure inertial navigation calculation thread are the integrated navigation results output by the integrated navigation fusion thread at the previous moment. The satellite navigation prediction and compensation thread constructs an adaptive observation noise matrix based on weights, and constructs an extended Kalman filter by combining the output pure inertial navigation position and velocity, outputting corrected satellite navigation position and velocity. The integrated navigation fusion thread constructs a loosely coupled Kalman filter using the difference between the corrected satellite navigation position and velocity and the output pure inertial navigation position and velocity as the observation vector, obtaining the final navigation output. This invention is applied in the field of navigation and effectively solves the positioning jump problem caused by poor satellite navigation signals due to interference such as obstruction and reflection.

Description

Technical Field

[0001] This invention relates to the field of navigation technology, specifically to an adaptive satellite navigation error prediction and compensation combined navigation method, system, and device. Background Technology

[0002] In applications of integrated GNSS and INS navigation technologies, interference factors such as obstruction and reflection, and multipath effects can easily lead to a deterioration in the quality of GNSS signals, causing problems such as navigation and positioning jumps and sharp drops in accuracy, severely restricting the reliability of integrated navigation systems in complex environments. Existing technologies primarily focus on the following three categories of solutions to this problem: System signal processing optimization: By improving the signal algorithm inside the satellite navigation terminal, the impact of multipath interference and noise on the original observation values ​​is suppressed. However, this type of method is limited by the computing power of the satellite navigation hardware and has limited adaptability to strong interference environments, making it difficult to fundamentally eliminate the risk of positioning jumps.

[0003] Antenna and signal source processing combined optimization: Hardware such as array antennas is used to receive satellite navigation signals, and antenna-side signal processing algorithms are used to separate interference signals from valid navigation signals. However, this approach requires additional hardware costs, and antenna deployment scenarios are limited, preventing its widespread adoption in miniaturized navigation devices such as vehicle-mounted and portable systems.

[0004] Integrated navigation fusion strategy optimization: Introducing an inertial navigation system (INS) and combining it with satellite navigation signals for calculation, there are two main technical approaches: tightly coupled and loosely coupled. The tightly coupled approach requires acquiring raw observations such as satellite pseudorange and carrier phase, and dynamically adjusting filter parameters based on satellite status. However, it relies on high-precision raw satellite navigation observations, and the filter adjustment exhibits significant lag when the signal is completely lost or severely distorted. The loosely coupled approach directly extracts the calculated position and velocity results from the satellite navigation output as observations. By comparing these results with the INS output data, a threshold is set to determine if a positioning jump has occurred. If an anomaly is detected, the satellite navigation weight in the filter is reduced, relying on the inertial sensor output to maintain navigation accuracy.

[0005] While the loosely coupled combination method described above is simple in structure and low in implementation cost, it has core technical defects: First, the threshold judgment is a static mechanism, which can only passively identify the deterioration of satellite navigation accuracy and cannot predict the trend of error change in advance, and its ability to identify gradual accuracy decline is insufficient; Second, error compensation is only achieved by statically adjusting the filter weights, without actively predicting and correcting the original output results of the satellite navigation, which cannot effectively offset the systematic errors caused by multipath effects. As a result, the output accuracy of the integrated navigation system will still show a significant decline when the satellite navigation signal continues to deteriorate, making it difficult to meet the application scenarios such as vehicle navigation and UAV aerial surveying that have strict requirements for long-term stability and high-precision navigation. Summary of the Invention

[0006] To address the shortcomings of the existing technologies, this invention provides an adaptive satellite navigation error prediction and compensation integrated navigation method, system, and device. By predicting, judging, and compensating for satellite navigation output results, it aims to compensate for multipath effects, thereby minimizing the loss of accuracy in integrated navigation results. This effectively solves the positioning jump problem caused by poor satellite navigation signals due to interference such as obstruction and reflection.

[0007] This invention aims to achieve dynamic prediction and real-time compensation of satellite navigation output results by constructing an independent satellite navigation error prediction and compensation mechanism. On the one hand, it breaks through the limitation of existing technologies that passively deal with errors by adjusting filter weights, and identifies the trend of satellite navigation accuracy changes in advance, effectively compensating for systematic errors caused by interference such as multipath effects. On the other hand, it combines the pure inertial navigation solution results with the original satellite navigation observation values ​​to construct an adaptive observation noise matrix and an extended Kalman filter, achieving accurate prediction and pre-compensation of satellite navigation errors, and then completing the final navigation solution through loosely coupled fusion filtering.

[0008] To achieve the above objectives, the present invention provides an adaptive satellite navigation error prediction and compensation integrated navigation method, including a pure inertial navigation solution thread, a satellite navigation prediction and compensation thread, and an integrated navigation fusion thread; The pure inertial navigation solution thread operates in the first cycle. The system executes the process, and its initial position, initial velocity, and initial attitude are the combined navigation results output by the combined navigation fusion thread at the previous moment. Based on the initial position, initial velocity, and initial attitude, it performs inertial navigation calculation and outputs pure inertial navigation position and pure inertial navigation velocity. The satellite navigation prediction compensation thread operates in the first cycle. The system executes a process that uses the original position and velocity of the satellite navigation system as observation vectors, the position error and velocity error of the satellite navigation system as state vectors, and constructs an adaptive observation noise matrix based on weights. It then combines the output pure inertial navigation system position and velocity to construct an extended Kalman filter, and outputs the corrected satellite navigation system position and velocity. The integrated navigation fusion thread operates in a second cycle. The system executes a process that uses the difference between the corrected guard position and the corrected guard velocity and the output pure inertial navigation position and the pure inertial navigation velocity as the observation vector, and uses the position error, velocity error, attitude error and IMU error as the state vector to construct a loosely coupled Kalman filter to obtain the final navigation output.

[0009] To achieve the above objectives, the present invention also provides an adaptive satellite navigation error prediction and compensation integrated navigation system, which uses the above-described method for navigation. The integrated navigation system includes: The inertial navigation calculation module is used to take the combined navigation result output in the previous moment as the initial position, initial velocity and initial attitude, and perform inertial navigation calculation based on the initial position, initial velocity and initial attitude to output pure inertial navigation position and pure inertial navigation velocity; The satellite navigation correction module is used to construct an adaptive observation noise matrix based on the original satellite navigation position and original satellite navigation velocity as observation vectors, and the satellite navigation position error and satellite navigation velocity error as state vectors. It also combines the output pure inertial navigation position and pure inertial navigation velocity to construct an extended Kalman filter and output the corrected satellite navigation position and corrected satellite navigation velocity. The integrated navigation module is used to construct a loosely coupled Kalman filter with the difference between the corrected guard position and the corrected guard velocity and the output pure inertial navigation position and the pure inertial navigation velocity as the observation vector, and with the position error, velocity error, attitude error and IMU error as the state vector, to obtain the final navigation output.

[0010] To achieve the above objectives, the present invention also provides a terminal device, wherein the terminal device is provided with: Memory, used to store programs; A processor is configured to execute the program stored in the memory, and when the program is executed, the processor is configured to perform the method as described above.

[0011] Compared with the prior art, the present invention has the following beneficial technical effects: 1. This invention overcomes the technical bottleneck of existing loosely coupled integrated navigation systems that rely solely on static thresholds and passively adjust filter weights to address errors by constructing an independent satellite navigation error prediction and compensation thread. By combining the pure inertial navigation solution results with the original satellite navigation observations, an adaptive observation noise matrix and an extended Kalman filter are constructed to achieve dynamic prediction and real-time pre-compensation of satellite navigation errors. This allows for early identification of accuracy degradation trends and effectively offsets systematic errors caused by multipath effects, obstruction, reflection, and other interferences, providing more reliable corrected satellite navigation data for subsequent fusion filtering.

[0012] 2. This invention employs a three-thread collaborative architecture (pure inertial navigation solution + satellite navigation prediction compensation + integrated navigation solution) to proactively identify and compensate for error changes when satellite navigation signals are interfered with or their accuracy deteriorates. This avoids the problem of sudden drops in navigation accuracy caused by traditional solutions relying solely on weight adjustments. Even in scenarios where satellite navigation signal quality continues to decline, it can still maintain the stability of integrated navigation output accuracy for a certain period, significantly reducing the impact of satellite navigation accuracy loss on the overall navigation results. This effectively addresses the core pain points of positioning jumps and accuracy decay in existing technologies, significantly improving the adaptability and navigation reliability of integrated navigation systems in complex environments.

[0013] 3. This invention abandons the static processing method of relying on manual setting of error thresholds in traditional schemes. By introducing the standard deviation of satellite navigation error to construct an adaptive observation noise matrix, the filtering weights can be dynamically updated and adjusted. There is no need to preset fixed parameters for different environments. The fusion weights of inertial navigation and satellite navigation information can be automatically adjusted according to the current satellite navigation signal quality, which greatly enhances the universality and robustness of the system under different interference scenarios.

[0014] 4. This invention adopts a modular and lightweight technical architecture, which does not require additional hardware costs or complex computational overhead, and has low engineering implementation difficulty. At the same time, by using the parallel operation of pure inertial calculation, satellite navigation prediction compensation and integrated navigation, it ensures strict synchronization of inertial navigation information and satellite navigation information in terms of time sequence, avoids the problem of observation data mismatch caused by time difference, and ensures the accuracy and stability of fusion calculation. Attached Figure Description

[0015] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the structures shown in these drawings without creative effort.

[0016] Figure 1 This is a principle block diagram of the integrated navigation method for adaptive satellite navigation error prediction and compensation in an embodiment of the present invention; Figure 2 This is a structural block diagram of the integrated navigation system for adaptive satellite navigation error prediction and compensation in an embodiment of the present invention; Figure 3 This is a structural block diagram of the terminal device in an embodiment of the present invention.

[0017] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0018] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0019] Furthermore, the technical solutions of the various embodiments of the present invention can be combined with each other, but only if they are feasible for those skilled in the art. If the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention.

[0020] This embodiment discloses an adaptive satellite navigation error prediction and compensation integrated navigation method, which mainly includes a pure inertial navigation calculation thread, a satellite navigation prediction and compensation thread, and an integrated navigation fusion thread. By constructing an independent satellite navigation error prediction and compensation mechanism, dynamic prediction and real-time compensation of satellite navigation output results are achieved. On the one hand, it breaks through the limitation of existing technologies that only passively deal with errors by adjusting filter weights, and identifies the trend of satellite navigation accuracy changes in advance, effectively compensating for systematic errors caused by interference such as multipath effects. On the other hand, by combining the pure inertial navigation calculation results and the original satellite navigation observation values, an adaptive observation noise matrix and an extended Kalman filter are constructed to achieve accurate prediction and pre-compensation of satellite navigation errors. Finally, the navigation calculation is completed through loosely coupled fusion filtering.

[0021] In this embodiment, the integrated navigation system includes an inertial measurement unit (IMU) and a satellite navigation unit (GNSS).

[0022] The inertial measurement unit (IMU) acquires raw data from the gyroscope and accelerometer (plus meter) in real time, including: Gyroscope output: Three-axis angular velocity The unit is rad / s, where For inertial coordinate system, For the carrier coordinate system, The current moment; Output of the table: Three-axis force ratio , in m / s², characterizes the acceleration of a carrier relative to an inertial frame (excluding gravity).

[0023] The satellite navigation unit (GNSS) output carrier contains satellite navigation position information, satellite navigation velocity information, and positioning accuracy factors, including: Original location of the satellite guide: (ECEF coordinate system, unit: m); Satellite's original speed: (ECEF coordinate system, unit m / s); Positioning accuracy factors: position accuracy factor PDOP, horizontal accuracy factor HDOP, vertical accuracy factor VDOP, and user equivalent distance error UERE.

[0024] refer to Figure 1 This is a block diagram illustrating the principle of the adaptive satellite navigation error prediction and compensation integrated navigation method in this embodiment. Specifically: The pure inertial navigation solution thread is in the first cycle. The initial position, initial velocity, and initial attitude are the combined navigation results output by the combined navigation fusion thread at the previous moment. Based on the initial position, initial velocity, and initial attitude, inertial navigation calculation is performed to output pure inertial navigation position and pure inertial navigation velocity. Satellite guidance predicts compensation thread in the first cycle The execution method uses the original position and velocity of the satellite navigation system as observation vectors, the position error and velocity error of the satellite navigation system as state vectors, and an adaptive observation noise matrix constructed based on weights. It combines the output pure inertial navigation system position and pure inertial navigation system velocity to construct an extended Kalman filter, and outputs the corrected satellite navigation system position and corrected satellite navigation system velocity. Integrated navigation fusion thread in the second cycle The system executes a process that uses the difference between the corrected guard position and velocity and the output pure inertial navigation position and velocity as the observation vector, and uses the position error, velocity error, attitude error, and IMU error as the state vector to construct a loosely coupled Kalman filter to obtain the final navigation output.

[0025] In the specific implementation process, the pure inertial navigation calculation thread completes the pure inertial navigation calculation based on the raw IMU data, and outputs position and velocity information in the first cycle. Before execution and calculation begin, the initial value is updated by the combined navigation result output by the combined navigation fusion thread at the previous moment to ensure calculation accuracy and avoid pure inertia drift accumulation.

[0026] Let the pure inertial navigation solution thread be the first... The next solution time is At the start of the solution process, the initial position, velocity, and attitude of the pure inertial navigation solution thread are determined from the previous moment. The output update of the integrated navigation fusion thread is as follows: Initial position update: ; Initial speed update: ; Initial attitude update: ; in, For the first The moment before the next solution is started (i.e., the instant after the initial value is updated). For the pure inertial navigation solution thread The initial position vector for this solution (Geocentric Earth-Fixed Coordinate System ECEF, unit m) has a dimension of 3×1, i.e. ; For the pure inertial navigation solution thread The initial velocity vector for this solution (ECEF coordinate system, unit m / s) has a dimension of 3×1, i.e. ; For the pure inertial navigation solution thread The initial attitude quaternion (representing the carrier coordinate system) is calculated in the second solution. Relative to inertial frame (the posture), dimension 4×1, that is ,satisfy ; , , For the previous moment The integrated navigation fusion thread outputs the position, velocity, and attitude.

[0027] In this embodiment, the calculation process of the pure inertial navigation solution thread is based on inertial navigation mechanics orchestration, starting from the initial value. , , Starting from there, combining the three-axis angular velocities collected by the IMU... Compared with triaxial force The solution completion time is obtained through integration. The position, velocity, and attitude, and the specific implementation process includes: 1. Attitude update based on quaternion differential equations: The attitude quaternion differential equation describes the change in attitude over time, and is solved based on the angular velocity output by the gyroscope, as follows: ; in, The antisymmetric matrix of angular velocity has a dimension of 4×4 and is expressed as follows: ; in, , , Triaxial angular velocity The three-axis components; Then, the Runge-Kutta method or the trapezoidal integral is used for integration to ensure accuracy, resulting in: ; 2. Speed ​​of updates: The velocity update requires converting the specific force in the carrier's coordinate system to the inertial frame, subtracting the Earth's gravity in the inertial frame, and then integrating to obtain the velocity, which is: ; in, The velocity vector in the inertial frame has a dimension of 3×1; This is the Earth's gravitational vector in the inertial frame, with dimensions 3×1, which can be determined based on the current position. Real-time calculation (using a spherical harmonic gravity model or a simplified gravity model); The attitude transformation matrix from the carrier coordinate system to the inertial frame is obtained by attitude quaternion transformation, with dimensions of 3×3. The transformation process is as follows: ; Then, by integration, we get: ; in, The pure inertial navigation velocity output by the pure inertial navigation solution thread; 3. Location update: The differential of the position vector is equal to the velocity vector in the inertial frame. Integrating these components yields the position, which is: ; Then, by integration, we get: ; in, This is the pure inertial navigation position output by the pure inertial navigation solution thread.

[0028] The first cycle in the specific implementation process That is, the pure inertial navigation solution thread performs a complete pure inertial navigation solution every 0.5 seconds and outputs the pure inertial navigation position. Pure inertial navigation velocity with posture .

[0029] In this embodiment, the satellite navigation prediction and compensation thread receives the output of the pure inertial navigation solution thread and the original satellite navigation output. It then sets weights, constructs observation vectors, calculates an adaptive observation noise matrix, executes an extended Kalman filter prediction-update process, and outputs corrected satellite navigation information as input to the integrated navigation fusion thread. The weight of the pure inertial navigation output information is greater than that of the satellite navigation information, and the weight of the satellite navigation information is updated in real time based on its position and velocity error standard deviation.

[0030] In practice, the observation vector of the extended Kalman filter is selected from the position and velocity of the original satellite navigation output, i.e.: ; in, The calculation time is consistent with that of the pure inertial navigation calculation thread, that is, once every 0.5 seconds, to ensure that the observation vector is synchronized with the time of the pure inertial navigation output.

[0031] In the extended Kalman filter, the standard deviation of the satellite navigation position error is calculated in real time based on the positioning accuracy factor of the satellite navigation output. Standard deviation of vertical position error With the standard deviation of speed error Then, by combining the preset weights, an adaptive observation noise covariance matrix is ​​constructed. To achieve adaptive adjustment of satellite guidance weights, the specific implementation process is as follows: First, calculate the standard deviation of the error, including the standard deviation of the position error. Standard deviation of vertical position error With the standard deviation of speed error They are respectively: ; ; ; in, This is the position accuracy factor output in real time by the satellite navigation system. This refers to the vertical accuracy factor output in real time by the satellite navigation system. The user equivalent distance error is output in real time by the satellite navigation system. This is a scaling factor, which can be calibrated according to the actual scenario, and is usually taken as... ; Then, the weights are set, including the weights for pure inertial information. Basic weight of satellite navigation information ,in, ,and For example, it can be set , ; Finally, the adaptive observation noise matrix is ​​calculated as follows: ; in, It is a diagonal matrix with dimensions 6×6. The positional error of the first 3 diagonal elements is ( , Direction is measured by the standard deviation of position error , Direction is measured by the standard deviation of vertical position error The velocity error corresponding to the last three diagonal elements () , , Direction is expressed using the standard deviation of velocity error. ); This is the adaptive observation noise matrix.

[0032] This embodiment updates the adaptive observation noise matrix in real time based on the positioning accuracy factor output by the satellite navigation system. When the quality of the satellite navigation deteriorates ( , When (increase), As the observation noise covariance increases, according to the Kalman filter principle, an increase in the observation noise covariance will reduce the weight of satellite navigation observations in the update step, thereby reducing the impact of satellite navigation errors on the fusion result; conversely, when the satellite navigation quality improves, Decrease, increase the weight of satellite navigation.

[0033] In this embodiment, the extended Kalman filter adopts a standard prediction-update two-step process. The prediction step is based on the mechanical arrangement equations of the pure inertial navigation solution thread, and the update step is based on the constructed observation vector and the adaptive observation noise matrix. The specific implementation process is as follows: The satellite navigation position and velocity errors are selected as the state vector (6×1 dimension) of the extended Kalman filter, focusing on the correction of satellite navigation information: ; in, For satellite positioning error, For satellite speed error, , For the true values ​​of satellite position and satellite velocity; The state prediction equation and covariance prediction equation of the extended Kalman filter are as follows: ; ; in, For the first The initial moment of the next solution, For the first The time after the next solution , These are the state vector and the state covariance matrix, respectively. This is the transpose of the matrix; The state transition matrix is ​​used because the initial values ​​have been updated by the integrated navigation fusion thread through the pure inertial navigation solution thread, resulting in a small error. Therefore, the state transition matrix in this embodiment is... The derivative is obtained from the differential equations in the pure inertial navigation solution thread, and is approximately the identity matrix. The specific process is as follows: According to the theory of linear time-varying systems, the state transition matrix... The rigorous analytical solution is in series expansion form: ; In the formula It is the identity matrix, and the subsequent terms are first-order, second-order and higher-order correction terms, respectively. Analysis of the series expansion of the state transition matrix: First-order correction term The first-order integral terms are small quantities, while the second-order and higher-order integral terms are higher-order infinitesimals. Considering the dual conditions of extremely small initial error and extremely short single-step period, the values ​​of all first-order and higher-order correction terms are much smaller than the order of magnitude of the identity matrix and can be approximately truncated and ignored. After truncating all perturbation terms, the state transition matrix can be simplified to: ; The system noise covariance matrix is ​​preset as a diagonal matrix in this embodiment. The diagonal elements are the position and velocity error variances of the pure inertial navigation calculation thread, and are calibrated according to IMU accuracy, specifically: ; in, , , The standard deviation of the process noise for the eastward attitude angle error, the northward attitude angle error, and the upward attitude angle error is given. , , The standard deviation of process noise for eastward velocity error, northward velocity error, and celestial velocity error. , , The standard deviation of process noise for latitude error, longitude error, and altitude error. , , The standard deviation of the random walk noise for the zero bias of the x-axis accelerometer, the zero bias of the y-axis accelerometer, and the zero bias of the z-axis accelerometer.

[0034] In this embodiment, the update process of the extended Kalman filter is as follows: First, calculate the observation residuals. ,for: ; in, The observation matrix is ​​taken as the identity matrix. (The observation vector directly corresponds to the state vector); Then, calculate the Kalman gain. ,for: ; Finally, based on the observation residuals Kalman gain Perform state update and covariance update as follows: ; ; in, For the first The time after the next solution It is an identity matrix.

[0035] After the extended Kalman filter is updated, the updated state error vector can be used to correct the original satellite navigation output, resulting in the corrected satellite navigation position and velocity. This eliminates some of the error in the original satellite navigation output, as follows: ; ; in, , To correct the position and speed of the guards.

[0036] In this embodiment, the integrated navigation fusion thread receives the corrected guard position, corrected guard velocity, and raw IMU data output from the extended Kalman filter, and completes information fusion through an internal loosely coupled Kalman filter, in the second cycle. The final navigation result is output, which is also used for the next initial value update of the pure inertial navigation solution thread (updated every 0.5 seconds). The second cycle... That is, the corrected guard position output by the guard prediction compensation thread. Correcting the speed of the guards This data is used for the subsequent five integrated navigation fusion thread calculations. At the same time, the raw IMU data input to the integrated navigation fusion thread and the pure inertial navigation calculation thread share the same sensor data.

[0037] In this embodiment, the state vector of the loosely coupled Kalman filter is selected from position error, velocity error, attitude error, and IMU error (gyroscope zero bias, added table zero bias), with a dimension of 15×1, as follows: ; in, , , These are the position error vector, velocity error vector, and attitude error angle vector (each with a dimension of 3×1). , , For position true value, velocity true value, and attitude true value, , , This is the solution result from the pure inertial navigation solution thread.

[0038] Based on the propagation characteristics of inertial navigation errors, the state equation of the loosely coupled Kalman filter is a linear time-varying equation, as follows: ; in, The state transition matrix (15×15) is derived from the propagation relationship of position, velocity, and attitude errors. The core submatrix includes the coefficients of velocity error with respect to position error and attitude error with respect to velocity error. This is the noise driving matrix (15×6 dimensions), used to introduce IMU random noise into the state equation; The system noise vector (6×1 dimension) includes gyroscope random noise and table-adding random noise, and follows a Gaussian distribution. , The system noise covariance matrix (dimension 6×6); To adapt to the execution frequency of T2=0.1s, the discretized state equation is as follows: ; in, The discrete state transition matrix is ​​(by...) (obtained through points) For discrete noise driving matrix, , For the first The moment after the next merge update For the first The moment before the next fusion prediction; The observation vector selection for the loosely coupled Kalman filter involves correcting the difference between the guard position, correcting the guard velocity, and the output pure inertial navigation position and pure inertial navigation velocity. The observation equation is as follows: ; in, For observation vectors; The observation matrix is ​​specifically a sparse matrix, where only the positions corresponding to position errors and velocity errors are non-zero. , It is a 3×3 identity matrix. It is a zero matrix; To observe the noise, it follows a Gaussian distribution. , To observe the noise covariance matrix, it is determined by the error standard deviation of the corrected back guidance information, specifically: ; in, , , To correct the standard deviation of the noise from the GNSS eastward, northward, and celestial position observations, , , This is the standard deviation of the noise in the eastward, northward, and skyward velocity observations of the GNSS after correction.

[0039] In practical implementation, the Kalman filter in the loosely coupled Kalman filter is updated as follows: Covariance prediction: ; Kalman gain calculation: ; Status Update: ; Covariance update: ; in, , These are the state vector and the state covariance matrix, respectively. It is an identity matrix.

[0040] By correcting the pure inertial navigation position and velocity using the updated error vector, the final navigation output can be obtained as follows: ; ; ; in, , , The final navigation output includes position, velocity, and attitude. , , The position, velocity, and attitude calculated by the pure inertial navigation solution thread. For attitude error angle The resulting error quaternion, This is quaternion multiplication.

[0041] In summary, the adaptive satellite navigation error prediction and compensation method in this embodiment effectively suppresses pure inertial drift and satellite navigation random errors through the closed-loop coordination of integrated navigation correction of pure inertial navigation, pure inertial navigation correction of satellite navigation, and pure inertial navigation-assisted integrated navigation, thereby improving the accuracy and stability of the entire integrated navigation system.

[0042] Example 2 Based on the adaptive satellite navigation error prediction and compensation integrated navigation method in Embodiment 1, this embodiment discloses an adaptive satellite navigation error prediction and compensation integrated navigation system, referencing... Figure 2 The integrated navigation system includes an inertial navigation calculation module, a satellite navigation correction module, and an integrated navigation module, specifically: The inertial navigation solution module is used to take the combined navigation results output in the previous moment as the initial position, initial velocity and initial attitude, and to perform inertial navigation solution based on the initial position, initial velocity and initial attitude, and output pure inertial navigation position and pure inertial navigation velocity; The satellite navigation correction module is used to construct an adaptive observation noise matrix based on the original satellite navigation position and original satellite navigation velocity as observation vectors, and the satellite navigation position error and satellite navigation velocity error as state vectors. It also combines the output pure inertial navigation position and pure inertial navigation velocity to construct an extended Kalman filter, and outputs the corrected satellite navigation position and corrected satellite navigation velocity. The integrated navigation module uses the difference between the corrected guard position and corrected guard velocity and the output pure inertial navigation position and pure inertial navigation velocity as the observation vector, and the position error, velocity error, attitude error and IMU error as the state vector to construct a loosely coupled Kalman filter to obtain the final navigation output.

[0043] In this embodiment, the specific working process and working principle of the inertial navigation calculation module, satellite navigation correction module, and integrated navigation module are the same as those in Embodiment 1, so they will not be described again in this embodiment. Each module can be implemented entirely or partially through software, hardware, or a combination thereof. Each module can be embedded in the processor of the computer device in hardware form or independent of it, or it can be stored in the memory of the computer device in software form, so that the processor can call and execute the operations corresponding to the above modules.

[0044] Example 3 like Figure 3 The diagram illustrates a terminal device disclosed in this embodiment, comprising a transmitter, a receiver, a memory, and a processor. The transmitter transmits instructions and data, the receiver receives instructions and data, the memory stores computer-executed instructions, and the processor executes the computer-executed instructions stored in the memory to implement the method described in Embodiment 1 above.

[0045] It is important to note that the aforementioned memory can be either standalone or integrated with the processor. When the memory is set up independently, the terminal device also includes a bus for connecting the memory and the processor.

[0046] The above description is only a preferred embodiment of the present invention and does not limit the scope of protection of the present invention. All equivalent structural transformations made under the inventive concept of the present invention using the contents of the present invention specification and drawings, or direct / indirect applications in other related technical fields, are included within the scope of protection of the present invention.

Claims

1. A combined navigation method for adaptive satellite navigation error prediction and compensation, characterized in that, It includes a pure inertial navigation solution thread, a satellite navigation prediction and compensation thread, and an integrated navigation fusion thread; The pure inertial navigation solution thread operates in the first cycle. The system executes the process, and its initial position, initial velocity, and initial attitude are the combined navigation results output by the combined navigation fusion thread at the previous moment. Based on the initial position, initial velocity, and initial attitude, it performs inertial navigation calculation and outputs pure inertial navigation position and pure inertial navigation velocity. The satellite navigation prediction compensation thread operates in the first cycle. The system executes a process that uses the original position and velocity of the satellite navigation system as observation vectors, the position error and velocity error of the satellite navigation system as state vectors, and constructs an adaptive observation noise matrix based on weights. It then combines the output pure inertial navigation system position and velocity to construct an extended Kalman filter, and outputs the corrected satellite navigation system position and velocity. The integrated navigation fusion thread operates in a second cycle. The system executes a process that uses the difference between the corrected guard position and the corrected guard velocity and the output pure inertial navigation position and the pure inertial navigation velocity as the observation vector, and uses the position error, velocity error, attitude error and IMU error as the state vector to construct a loosely coupled Kalman filter to obtain the final navigation output.

2. The adaptive satellite navigation error prediction and compensation integrated navigation method according to claim 1, characterized in that, The calculation process of the adaptive observation noise matrix is ​​as follows: First, calculate the standard deviation of the error, including the standard deviation of the position error. Standard deviation of vertical position error With the standard deviation of speed error They are respectively: ； ； ； in, This is the position accuracy factor output in real time by the satellite navigation system. This refers to the vertical accuracy factor output in real time by the satellite navigation system. The user equivalent distance error is output in real time by the satellite navigation system. This is the proportionality coefficient; Then, the weights are set, including the weights for pure inertial information. Basic weight of satellite navigation information ,in, ,and ; Finally, the adaptive observation noise matrix is ​​calculated as follows: ； in, It is a diagonal matrix. This is the adaptive observation noise matrix.

3. The adaptive satellite navigation error prediction and compensation integrated navigation method according to claim 2, characterized in that, In the prediction process of the extended Kalman filter, the state prediction equation and the covariance prediction equation are respectively: ； ； in, For the first The initial moment of the next solution, For the first The time after the next solution , These are the state vector and the state covariance matrix, respectively. Here is the state transition matrix. This is the transpose of the matrix. Let be the system noise covariance matrix.

4. The adaptive satellite navigation error prediction and compensation integrated navigation method according to claim 3, characterized in that, The system noise covariance matrix It is a diagonal matrix, and its diagonal elements are the position and velocity error variances of the pure inertial navigation solution thread.

5. The adaptive satellite navigation error prediction and compensation integrated navigation method according to claim 3, characterized in that, The update process of the extended Kalman filter is as follows: First, calculate the observation residuals. ,for: ； in, For the observation vector, The observation matrix; Then, calculate the Kalman gain. ,for: ； Finally, based on the observation residuals Kalman gain Perform state update and covariance update as follows: ； ； in, For the first The time after the next solution It is an identity matrix.

6. The adaptive satellite navigation error prediction and compensation integrated navigation method according to any one of claims 1 to 5, characterized in that, The Kalman filter in the loosely coupled Kalman filter is updated as follows: Covariance prediction: ; Kalman gain calculation: ; Status Update: ; Covariance update: ; in, For the first The moment before the next fusion prediction For the first The moment after the next merge update , These are the state vector and the state covariance matrix, respectively. The discrete state transition matrix, This is the transpose of the matrix. For discrete noise driving matrix, Let be the system noise covariance matrix. For Kalman gain, For the observation matrix, To observe the noise covariance matrix, For the first The observation vector during the second fusion It is an identity matrix.

7. The adaptive satellite navigation error prediction and compensation integrated navigation method according to claim 6, characterized in that, The observation noise covariance matrix It is determined by the standard deviation of the error of the corrected guidance information.

8. The adaptive satellite navigation error prediction and compensation integrated navigation method according to any one of claims 1 to 5, characterized in that, First cycle The second cycle .

9. A combined navigation system for adaptive satellite navigation error prediction and compensation, characterized in that, Navigation is performed using the method described in any one of claims 1 to 8, wherein the integrated navigation system comprises: The inertial navigation calculation module is used to take the combined navigation result output in the previous moment as the initial position, initial velocity and initial attitude, and perform inertial navigation calculation based on the initial position, initial velocity and initial attitude to output pure inertial navigation position and pure inertial navigation velocity; The satellite navigation correction module is used to construct an adaptive observation noise matrix based on the original satellite navigation position and original satellite navigation velocity as observation vectors, and the satellite navigation position error and satellite navigation velocity error as state vectors. It also combines the output pure inertial navigation position and pure inertial navigation velocity to construct an extended Kalman filter and output the corrected satellite navigation position and corrected satellite navigation velocity. The integrated navigation module is used to construct a loosely coupled Kalman filter with the difference between the corrected guard position and the corrected guard velocity and the output pure inertial navigation position and the pure inertial navigation velocity as the observation vector, and with the position error, velocity error, attitude error and IMU error as the state vector, to obtain the final navigation output.

10. A terminal device, characterized in that, The terminal device is equipped with: Memory, used to store programs; A processor for executing the program stored in the memory, wherein when the program is executed, the processor is configured to perform the method as described in any one of claims 1 to 8.

Images