Sequential invariant extended Kalman filtering method for navigation positioning task

Abstract

The invention provides a sequential invariant extended Kalman filtering method for a navigation positioning task, and belongs to the field of automobile navigation. The problems that efficient fusion of multi-coordinate system observation is difficult to achieve under an IEKF framework, and geometric consistency of covariance transfer cannot be guaranteed are solved. The method comprises the following steps: dividing observed quantities of a sensor according to different coordinate systems of the navigation sensor; constructing a state vector in a Lie group form; modeling the output of the inertial measurement unit; performing state propagation according to a Lie group state equation; calculating propagation of a covariance matrix; establishing a left invariant observation equation and a right invariant observation equation according to the two types of divided matrixes; and synchronously updating state estimation through the left invariant extended Kalman filter and the right invariant extended Kalman filter, and mapping the left invariant covariance matrix and the right invariant covariance matrix by using the common state covariance matrix to complete sequential fusion. The method is mainly used in the automatic driving field.

Description

Technical Field

[0001] The present invention belongs to the field of vehicle navigation, and particularly relates to a sequential invariant extended Kalman filtering method for navigation and positioning tasks. Background Art

[0002] High-precision positioning of vehicles is the core foundation of autonomous driving systems, and its reliability directly affects decision-making, planning, and driving safety. In complex urban environments, due to factors such as high-rise building occlusion, multipath effects, and signal interference, the positioning accuracy of the Global Navigation Satellite System (GNSS) significantly decreases (usually with an error exceeding 4 meters), making it difficult to meet the centimeter-level positioning requirements of autonomous driving. Although the Inertial Measurement Unit (IMU) can provide short-term continuous pose estimation through dead reckoning, its error accumulates over time. Especially in dynamic scenarios, the rapid changes in vehicle attitude and motion state further exacerbate the positioning drift problem.

[0003] To overcome the limitations of single sensors, multi-sensor fusion technology has become a research hotspot. The Kalman Filter (KF) and its extended form (EKF) are widely used in GNSS / IMU integrated positioning. By fusing the global position constraint of GNSS and the high-frequency motion measurements of IMU, the positioning robustness is improved. However, traditional EKF introduces errors due to linearization approximation in nonlinear systems (such as rotation operations in vehicle attitude estimation), and is prone to state estimation distortion during dynamic coordinate system transformation (such as the mapping between the vehicle body coordinate system and the navigation coordinate system). To solve this problem, the Invariant Extended Kalman Filter (IEKF) was proposed. By utilizing the Lie group structure characteristics of the state space and maintaining the geometric invariance of the error dynamics, it significantly reduces the linearization error and demonstrates superior performance in fields such as inertial navigation and SLAM.

[0004] However, there are still key bottlenecks in existing IEKF methods when fusing multi-coordinate system observation data. First, the observation model of IEKF usually only adapts to a single coordinate system (such as the navigation system or the vehicle body system), resulting in the inability to directly fuse observations from different coordinate systems. For example, GNSS provides position observations in the navigation system, while wheel speed sensors, zero velocity detection, etc. output speed or attitude constraints in the vehicle body system. Traditional IEKF is difficult to synchronously utilize these two types of information. Second, the Left Invariant IEKF (L-IEKF) and the Right Invariant IEKF (R-IEKF) are designed for observations in different coordinate systems respectively, and their error propagation and covariance update mechanisms are incompatible. If directly cascaded, the state covariance matrix cannot correctly reflect the actual error distribution, thereby reducing the filtering stability.

[0005] Therefore, how to achieve efficient fusion of multi-coordinate system observations within the IEKF framework and ensure the geometric consistency of covariance transfer has become a key challenge for improving vehicle positioning accuracy in complex scenarios. Summary of the Invention

[0006] In view of this, the present invention aims to propose a sequential invariant extended Kalman filtering method for navigation and positioning tasks, so as to solve the problems that it is difficult to achieve efficient fusion of multi-coordinate system observations under the IEKF framework and the geometric consistency of covariance transfer cannot be guaranteed.

[0007] To achieve the above object, the present invention adopts the following technical solutions: A sequential invariant extended Kalman filtering method for navigation and positioning tasks, the method comprising:

[0008] Step S1: According to the different coordinate systems where the navigation sensors are located, the observed quantities of the sensors are divided into two categories, one is the observed quantities in the vehicle body coordinate system, and the other is the observed quantities in the navigation coordinate system;

[0009] Step S2: Construct a state vector in the form of a Lie group:

[0010] Step S3: Model the output of the inertial measurement unit;

[0011] Step S4: Perform state propagation according to the Lie group state equation;

[0012] Step S5: Calculate the propagation of the covariance matrix;

[0013] Step S6: Establish a left-invariant observation equation and a right-invariant observation equation according to the two types of matrices divided in Step S1;

[0014] Step S7: Synchronously update the state estimate through a left-invariant extended Kalman filter and a right-invariant extended Kalman filter, and map the left-invariant covariance matrix and the right-invariant covariance matrix by using a common state covariance matrix to complete sequential fusion.

[0015] Further, a preferred method is also proposed. The observed quantities in the vehicle body coordinate system in Step S1 include the vehicle zero-speed hypothesis, and the observed quantities in the navigation coordinate system include the position provided by the satellite navigation system.

[0016] Further, a preferred method is also proposed. The state vector in the form of a Lie group in Step S2 is:

[0017]

[0018] Wherein, represents the attitude estimation quantity of the vehicle body in the navigation coordinate system at time k, represents the speed estimation quantity of the vehicle body in the navigation coordinate system at time k, represents the position estimation quantity of the vehicle body in the navigation coordinate system at time k, represents the estimated value of the gyro zero bias of the IMU at time k, Represents the estimated value of the accelerometer bias at time k, Represents the estimated value of the rotation amount of the IMU and the vehicle center lever arm at time k, Represents the estimated value of the translation amount of the IMU and the vehicle center lever arm at time k.

[0019] Furthermore, a preferred method is also proposed. In step S3, modeling the output of the inertial measurement unit includes:

[0020]

[0021] where ω k Represents the true value of the IMU angular velocity at time k, a k Represents the true value of the IMU acceleration at time k, Represents the output value of the IMU at time k, Is the angular velocity bias at time k, Is the acceleration bias at time k, Is the random error of the angular velocity at time k, Is the random error of the acceleration at time k.

[0022] Furthermore, a preferred method is also proposed. In step S4, state propagation according to the Lie group state equation includes:

[0023]

[0024] where the subscript k+1|k represents estimating the data at time k+1 using the data at time k, N represents the observation variance matrix, g represents the gravitational acceleration, Represents the random error of the angular velocity bias, Represents the random error of the acceleration bias, Represents the lever arm rotation amount, Represents the random error of the lever arm translation.

[0025] Furthermore, a preferred method is also proposed. In step S5, calculating the propagation of the covariance matrix includes:

[0026]

[0027] where F k Represents the Jacobian matrix of the error Lie algebra with respect to the state equation, G k Represents the noise driving matrix, P k|k Represents the covariance matrix of the error Lie algebra, Q k Represents the noise covariance matrix.

[0028] Furthermore, a preferred method is also proposed. In step S6, establishing the left-invariant observation equation and the right-invariant observation equation includes:

[0029]

[0030] Among them, is the GNSS observation error, represents the vehicle speed at time k + 1, is the forward speed, is the lateral speed, is the vertical speed. represents the lever arm rotation matrix between the IMU and the vehicle, represents the displacement part of the lever arm.

[0031] Furthermore, a preferred method is also proposed. In step S7, the left-invariant covariance matrix and the right-invariant covariance matrix are mapped using the common state covariance matrix, including:

[0032]

[0033] Among them, P State is the common state covariance matrix, is the left-invariant error Lie algebra, is the right-invariant error Lie algebra, is the Jacobian matrix of the error Lie algebra with respect to the left-invariant state equation, is the Jacobian matrix of the error Lie algebra with respect to the right-invariant state equation.

[0034] Based on the same inventive concept, the present invention also provides a computer device, including a memory and a processor. A computer program is stored in the memory. When the processor runs the computer program stored in the memory, the processor executes a sequential invariant extended Kalman filtering method for a navigation and positioning task described in any one of the above.

[0035] Based on the same inventive concept, the present invention also provides a computer-readable storage medium. A computer program is stored on the computer-readable storage medium. When the computer program is run by the processor, it executes the steps of a sequential invariant extended Kalman filtering method for a navigation and positioning task described in any one of the above.

[0036] Compared with the prior art, the beneficial effects of the present invention are:

[0037] A sequential invariant extended Kalman filtering method for a navigation and positioning task proposed by the present invention effectively processes the observation data between multiple coordinate systems by dividing the sensor observations into two types of observations in the body coordinate system and the navigation coordinate system, enabling more efficient fusion of observations in different coordinate systems in the IEKF framework, thereby improving the fusion accuracy and efficiency of multi-source information.

[0038] In traditional Extended Kalman Filter (EKF), there is a problem of geometric consistency in the propagation of covariance. By using the state vector in the form of Lie group to describe the geometric relationship in the navigation system, the geometric consistency during the covariance propagation is ensured, and the error accumulation and inconsistency problems in the traditional method are avoided. Invariant property: The construction methods of left-invariant and right-invariant observation equations are adopted, and state estimation is carried out within the invariant framework to enhance the stability of the filtering process. In the navigation and positioning tasks in a dynamic environment, it can also better cope with complex changes and measurement noise.

[0039] Furthermore, this method realizes sequential fusion by synchronously updating the state estimations of the left-invariant and right-invariant Extended Kalman filters and using the common state covariance matrix for covariance mapping. This sequential fusion method can balance state estimation and covariance propagation among multiple filters, improving the estimation accuracy and real-time performance of the entire system.

[0040] The present invention can effectively handle information fusion tasks from multiple different sources (such as inertial measurement units, external positioning sensors, etc.), especially in complex navigation environments, such as the conversion between multiple coordinate systems and the comprehensive utilization of multi-source data. This method has strong adaptability and can handle navigation and positioning problems in high-dynamic and complex environments. Brief Description of the Drawings

[0041] The accompanying drawings that form a part of the present invention are used to provide a further understanding of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation to the present invention. In the drawings:

[0042] Figure 1 It is a flowchart of a sequential invariant Extended Kalman filtering method for navigation and positioning tasks described in Embodiment 1. Detailed Embodiment

[0043] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. It should be noted that, without conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other. The described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments.

[0044] Embodiment 1. Refer to Figure 1 This embodiment is described. A sequential invariant Extended Kalman filtering method for navigation and positioning tasks described in this embodiment includes:

[0045] Step S1: According to the different coordinate systems where the navigation sensors are located, the observed quantities of the sensors are divided into two categories. One category is the observed quantities in the body coordinate system, and the other category is the observed quantities in the navigation coordinate system;

[0046] Step S2: Construct a state vector in Lie group form:

[0047] Step S3: Model the output of the inertial measurement unit;

[0048] Step S4: Perform state propagation according to the Lie group state equation;

[0049] Step S5: Calculate the propagation of the covariance matrix;

[0050] Step S6: Establish left-invariant and right-invariant observation equations based on the two types of matrices divided in Step S1;

[0051] Step S7: Synchronously update the state estimation through the left-invariant extended Kalman filter and the right-invariant extended Kalman filter, and map the left-invariant covariance matrix and the right-invariant covariance matrix using the common state covariance matrix to complete sequential fusion.

[0052] The method proposed in this embodiment effectively processes the observation data between multiple coordinate systems by dividing the sensor observations into two types of observations in the body coordinate system and the navigation coordinate system, enabling more efficient fusion of observations in different coordinate systems under the IEKF framework, thereby improving the fusion accuracy and efficiency of multi-source information.

[0053] In traditional extended Kalman filtering (EKF), there is a problem of geometric consistency in the transmission of covariance. Using a state vector in Lie group form to describe the geometric relationship in the navigation system ensures geometric consistency during the covariance propagation process and avoids the error accumulation and inconsistency problems in traditional methods. Invariant property: By adopting the construction methods of left-invariant and right-invariant observation equations, state estimation is carried out in the invariant framework, enhancing the stability of the filtering process and better coping with complex changes and measurement noise in the navigation and positioning tasks in a dynamic environment.

[0054] Furthermore, this method realizes sequential fusion by synchronously updating the state estimations of the left-invariant and right-invariant extended Kalman filters and using the common state covariance matrix for covariance mapping. This sequential fusion method can balance state estimation and covariance propagation among multiple filters, improving the estimation accuracy and real-time performance of the entire system.

[0055] The method proposed in this embodiment can effectively handle information fusion tasks from multiple different sources (such as inertial measurement units, external positioning sensors, etc.), especially in complex navigation environments, such as coordinate system conversions between multiple coordinate systems and comprehensive utilization of multi-source data. This method has strong adaptability and can handle navigation and positioning problems in high-dynamic and complex environments.

[0056] Embodiment 2. This embodiment further defines a sequential invariant extended Kalman filtering method for navigation and positioning tasks described in Embodiment 1. In step S1, the observed quantities in the vehicle coordinate system include the vehicle zero-velocity assumption, and the observed quantities in the navigation system include the position provided by the satellite navigation system.

[0057] Embodiment 3. This embodiment further defines a sequential invariant extended Kalman filtering method for navigation and positioning tasks described in Embodiment 1. The state vector in Lie group form in step S2 is:

[0058]

[0059] where, represents the attitude estimation of the vehicle in the navigation coordinate system at time k, represents the velocity estimation of the vehicle in the navigation coordinate system at time k, represents the position estimation of the vehicle in the navigation coordinate system at time k, represents the estimated value of the gyro bias of the IMU at time k, represents the estimated value of the accelerometer bias at time k, represents the estimated value of the rotation amount between the IMU and the vehicle center boom at time k, represents the estimated value of the translation amount between the IMU and the vehicle center boom at time k.

[0060] In this embodiment, by introducing the estimation of the rotation amount and translation amount between the IMU and the vehicle center boom, the influence of the change in the position of the IMU on navigation and positioning is considered in the filtering process, avoiding the problem that the change in the position and direction of the IMU relative to the vehicle center significantly affects the positioning accuracy. By accurately estimating the relative position between the IMU and the vehicle center boom, the error can be effectively reduced, and the accuracy and reliability of navigation and positioning can be improved.

[0061] In this embodiment, the attitude change and position transformation are described by the state vector in Lie group form, thereby improving the performance of the extended Kalman filter (EKF) in a highly nonlinear system and avoiding the approximation error caused by the traditional linearization method.

[0062] Embodiment 4. This embodiment further defines a sequential invariant extended Kalman filtering method for navigation and positioning tasks described in Embodiment 1. Modeling the output of the inertial measurement unit in step S3 includes:

[0063]

[0064] where, ω k represents the true value of the IMU angular velocity at time k, a krepresents the true value of the IMU acceleration at time k, represents the output value of the IMU at time k, is the gyroscope bias at time k, is the accelerometer bias at time k, is the random error of the gyroscope at time k, is the random error of the accelerometer at time k.

[0065] In this embodiment, by considering the bias errors of the IMU (such as gyroscope bias and accelerometer bias) during the modeling process, the influence of these error sources on the filtering result can be effectively reduced. Bias errors often affect the accuracy of inertial sensors. Without compensation, long-term error accumulation may occur. However, in this embodiment, by modeling and compensating these errors, the accuracy of filtering can be significantly improved. This enables the filter to better adapt to the uncertainties in actual measurements, thereby providing more accurate positioning and navigation results.

[0066] Embodiment 5: This embodiment further limits a sequential invariant extended Kalman filtering method for navigation and positioning tasks described in Embodiment 1. In step S4, state propagation according to the Lie group state equation includes:

[0067]

[0068] where the subscript k + 1|k represents estimating the data at time k + 1 using the data at time k, N represents the observation variance matrix, g represents the gravitational acceleration, represents the random error of the gyroscope bias, represents the random error of the accelerometer bias, represents the lever arm rotation amount, represents the random error of the lever arm translation.

[0069] Embodiment 6: This embodiment further limits a sequential invariant extended Kalman filtering method for navigation and positioning tasks described in Embodiment 5. In step S5, the propagation of the covariance matrix is calculated, including:

[0070]

[0071] where F k represents the Jacobian matrix of the error Lie algebra with respect to the state equation, G k represents the noise driving matrix, P k|k represents the covariance matrix of the error Lie algebra, Q k represents the noise covariance matrix.

[0072] Embodiment 7. This embodiment further limits a sequential invariant extended Kalman filtering method for navigation and positioning tasks described in Embodiment 6. In step S6, establishing the left-invariant observation equation and the right-invariant observation equation includes:

[0073]

[0074] Among them, is the GNSS observation error, represents the vehicle speed at time k + 1, is the forward speed, is the lateral speed, is the vertical speed. represents the lever-arm rotation matrix between the IMU and the vehicle, represents the displacement part of the lever arm.

[0075] In this embodiment, considering the design of the left-invariant observation equation and the right-invariant observation equation, it effectively processes the spatial transformation relationship between the vehicle and the IMU (inertial measurement unit), and through the left-invariant observation equation and the right-invariant observation equation, better describes the constraint conditions in vehicle motion, making the filtering process have better invariance to the transformation and rotation in motion, thereby reducing the error propagation problem that may exist in the traditional EKF method.

[0076] Furthermore, this embodiment also improves the overall positioning accuracy through the optimization of error modeling and observation update. Information such as the vehicle's forward speed, lateral speed, and vertical speed is introduced into the filtering equation to provide more accurate kinematic information for state estimation, thereby improving the accuracy of state estimation.

[0077] Embodiment 8. This embodiment further limits a sequential invariant extended Kalman filtering method for navigation and positioning tasks described in Embodiment 1. In step S7, using the common state covariance matrix to map the left-invariant covariance matrix and the right-invariant covariance matrix includes:

[0078]

[0079] Among them, P State is the common state covariance matrix, is the left-invariant error Lie algebra, is the right-invariant error Lie algebra, is the Jacobian matrix of the error Lie algebra with respect to the left-invariant state equation, is the Jacobian matrix of the error Lie algebra with respect to the right-invariant state equation.

[0080] Embodiment Nine. A computer device described in this embodiment includes a memory and a processor. A computer program is stored in the memory. When the processor runs the computer program stored in the memory, the processor executes a sequential invariant extended Kalman filtering method for navigation and positioning tasks described in any one of Embodiments One to Eight.

[0081] Embodiment Ten. A computer-readable storage medium described in this embodiment has a computer program stored thereon. When the computer program is run by a processor, it executes the steps of a sequential invariant extended Kalman filtering method for navigation and positioning tasks described in any one of Embodiments One to Eight.

[0082] Embodiment Eleven. This embodiment provides a specific example for a sequential invariant extended Kalman filtering method for navigation and positioning tasks described in Embodiment One, and is also used to explain Embodiments Two to Eight. Specifically:

[0083] Step 1: According to the different coordinate systems where the navigation sensors are located, the observed quantities of the sensors are divided into two categories. One category is the navigation information provided in the vehicle body coordinate system, such as the zero-velocity assumption of a vehicle; the other category is the navigation information provided in the navigation coordinate system, such as the position provided by a satellite navigation system.

[0084] Step 2: Establish the system state to be estimated and write it in Lie group form:

[0085] The system state to be estimated is: Its Lie group form is:

[0086]

[0087] Where:

[0088]

[0089] In the above formulas respectively represent the attitude, velocity, and position estimation quantities of the vehicle body in the navigation coordinate system at time k, represents the estimated values of the gyro bias and accelerometer bias of the IMU at time k, represents the estimated values of the rotation and translation amounts of the IMU and the vehicle center rod arm at time k.

[0090] Step 3: Model the output of the inertial sensor as:

[0091]

[0092] ω k ,a k represent the true values of the IMU angular velocity and acceleration at time k; Denote the output value of the IMU at time k, which are the biases of angular velocity and acceleration at time k, and which are the random errors of angular velocity and acceleration at time k.

[0093] Step 4: Propagate the state, and the propagation equation is:

[0094]

[0095] The subscript k+1|k indicates estimating the data at time k+1 using the data at time k

[0096] Step 5: Propagate the covariance matrix, and the error covariance matrix P k+1|k is:

[0097]

[0098] where F k denotes the Jacobian matrix of the error Lie algebra with respect to the state equation, and G k denotes the noise driving matrix.

[0099] Step 6: Establish the observation equation based on the observed quantity. The left-invariant observation equation and the right-invariant observation equation can be written as:

[0100]

[0101]

[0102] In the above formulas, is the GNSS observation error, denotes the vehicle speed at time k+1, is the forward speed, is the lateral speed, is the vertical speed. denotes the lever arm rotation matrix between the IMU and the vehicle, denotes the displacement part of the lever arm.

[0103] According to the definition of the invariant extended Kalman filter, the left-invariant and right-invariant equation forms can be written as:

[0104]

[0105] Step 7: Perform the update process, and the specific equation is:

[0106]

[0107] where, Represent the Lie group estimator indicating the state,

[0108]

[0109] where K is the Kalman filter gain, is the exponential map, and its specific calculation process is as follows:

[0110]

[0111]

[0112] J := ρ(π / 2)

[0113] where ρ(θ) represents the 2×2 rotation matrix of angle θ. Expanded as:

[0114]

[0115] where:

[0116]

[0117] P k+1|k+1 =(I - KH k+1 )P k+1|k

[0118]

[0119] where, is the Jacobian matrix of the left-invariant error Lie algebra error with respect to the state equation, is the Jacobian matrix of the right-variant error Lie algebra error with respect to the state equation. is the driving matrix of the left-invariant error Lie algebra error with respect to the state noise, is the driving matrix of the right-variant error Lie algebra error with respect to the state noise. H k+1 is the constructed observation function matrix, and N k+1 is the observation error matrix.

[0120] The update process noise propagation law is modeled as:

[0121]

[0122] Using the common state covariance matrix P State , the left-invariant error Lie algebra and the right-invariant error Lie algebra are transformed, and finally the iteration process is completed:

[0123]

[0124] To verify the effectiveness of the present invention, in a 5-km complex urban road test, the average positioning accuracy of this method reached 1.1 m. Compared with single GNSS positioning (4.2 m) and the traditional EKF fusion method (2.5 m), the accuracy was improved by 73.8% and 56% respectively (as shown in Table 1). In dynamic scenarios (such as sharp turns, acceleration / braking), its attitude estimation error was reduced by more than 40%, effectively suppressing the drift problem caused by the accumulation of IMU zero bias.

[0125] Table 1

[0126] Method Average positioning accuracy (m) GNSS 4.2m GNSS and IMU integrated positioning based on EKF 2.5m The method proposed by the present invention 1.1m

[0127] By means of the sequential synchronous update of the left-invariant IEKF (L-IEKF) and the right-invariant IEKF (R-IEKF), the problem that the traditional IEKF cannot fuse observations of the navigation system (such as GNSS position) and the vehicle body system (such as zero-velocity assumption, wheel speed) is solved. For example, directly using the GNSS position to correct the global pose to avoid the non-linear interference of the vehicle movement on the observation model. Dynamically compensating for the offset between the IMU and the vehicle center through the lever arm parameter to improve the modeling accuracy of the lateral / vertical velocity constraint.

[0128] Introduce a common state covariance matrix to achieve the mutual mapping of the left and right invariant error covariances: unify the covariances of the left and right IEKFs to the common state space through the Jacobian matrix to avoid covariance distortion caused by coordinate system differences. Maintain the geometric invariance of error propagation during the update process, reduce the linearization approximation error, and improve the filtering stability by about 30%.

[0129] In urban canyon areas where GNSS signals are weak and multipath effects are severe, this method shortens the positioning failure duration to less than 20% of the traditional method by fusing high-frequency IMU data and sparse GNSS observations.

[0130] Those skilled in the art should understand that the embodiments of the present disclosure can be provided as a method, a system, or a computer program product. Therefore, the present disclosure can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present disclosure can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0131] This disclosure is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to embodiments of the disclosure. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, and the combination of flows and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing devices produce a means for implementing the functions specified in the flow Figure 1 one flow or multiple flows and / or blocks Figure 1 or means for implementing the functions specified in multiple blocks. These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, such that the instructions stored in the computer-readable memory produce a manufactured article including instruction means that implement the functions specified in the flow Figure 1 one flow or multiple flows and / or blocks Figure 1 or means for implementing the functions specified in multiple blocks.

[0132] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operation steps are executed on the computer or other programmable device to generate a computer-implemented process, so that the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in the flow Figure 1 one flow or multiple flows and / or blocks Figure 1 or means for implementing the functions specified in multiple blocks.

[0133] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present disclosure rather than to limit the scope of its protection. Although the present disclosure has been described in detail with reference to the above embodiments, those of ordinary skill in the art should understand that after reading the present disclosure, various changes, modifications, or equivalent replacements can still be made to the specific implementation manners of the invention. However, these changes, modifications, or equivalent replacements are all within the scope of the claims of the pending disclosure.

Claims

1. A sequential invariant extended Kalman filter method for navigation and positioning tasks, characterized in that: The method comprises: Step S1: According to the different coordinate systems of the navigation sensor, the sensor's observations are divided into two categories, one is the observations in the carrier coordinate system, and the other is the observations in the navigation system; Step S2: Construct the state vector in the form of Lie group: Step S3: Modeling the output of the inertial measurement unit; Step S4: performing state propagation according to the Lie group state equation; Step S5: Calculate the propagation of the covariance matrix; Step S6: Establishing a left invariant observation equation and a right invariant observation equation according to the two types of matrices divided in step S1; Step S7: synchronously update the state estimation through the left invariant extended Kalman filter and the right invariant extended Kalman filter, and use the common state covariance matrix to map the left invariant covariance matrix and the right invariant covariance matrix to complete sequential fusion.

2. A sequential invariant extended Kalman filter method for navigation and positioning tasks according to claim 1, characterized in that: The observations in the carrier coordinate system in step S1 include the vehicle zero speed assumption, and the observations in the navigation system include the position provided by the satellite navigation system.

3. A sequential invariant extended Kalman filter method for navigation and positioning tasks according to claim 1, characterized in that: The state vector in the Lie group form in step S2 is: in, represents the attitude estimate of the carrier in the navigation coordinate system at time k, represents the estimated velocity of the carrier in the navigation coordinate system at time k, represents the estimated position of the carrier in the navigation coordinate system at time k, Represents the estimated value of the IMU gyro bias at time k, represents the estimated value of the accelerometer zero bias at time k, represents the estimated value of the rotation of the IMU and the vehicle center arm at time k, Represents the estimated translation of the IMU and vehicle center lever arm at time k.

4. The sequential invariant extended Kalman filter method for navigation and positioning tasks according to claim 1, characterized in that: The step S3 of modeling the output of the inertial measurement unit includes: Among them, ω k represents the true value of the IMU angular velocity at time k, a k represents the true value of IMU acceleration at time k, represents the output value of IMU at time k, is the angular velocity zero bias at time k, is the acceleration bias at time k, is the random error of the angular velocity at time k, is the random error of acceleration at time k.

5. The sequential invariant extended Kalman filter method for navigation and positioning tasks according to claim 1, characterized in that: The step S4 performs state propagation according to the Lie group state equation, including: Among them, the subscript k+1k , which means using the k-time data to estimate the k+1-time data, N represents the observation variance matrix, g represents the gravitational acceleration, represents the random error of the angular velocity bias, represents the random error of the acceleration bias, represents the amount of arm rotation, Represents the random error in the translation of the lever arm.

6. A sequential invariant extended Kalman filter method for navigation and positioning tasks according to claim 5, characterized in that: The propagation of calculating the covariance matrix in step S5 includes: Among them, F k The Jacobian matrix of the error Lie algebra for the state equation, G k represents the noise driving matrix, P k|k represents the covariance matrix of the error Lie algebra, Q k represents the noise covariance matrix.

7. A sequential invariant extended Kalman filtering method for navigation and positioning tasks according to claim 6, characterized in that: The step S6 of establishing the left invariant observation equation and the right invariant observation equation includes: in, is the GNSS observation error, represents the speed of the car at time k+1, is the forward velocity, is the lateral velocity, is the vertical velocity, represents the arm rotation matrix of the IMU and the vehicle, Represents the displaced portion of the lever arm.

8. The sequential invariant extended Kalman filter method for navigation and positioning tasks according to claim 1, characterized in that: In step S7, the left invariant covariance matrix and the right invariant covariance matrix are mapped using the common state covariance matrix, including: Among them, P State is the public state covariance matrix, is the left invariant error Lie algebra, is the right invariant error Lie algebra, is the Jacobian matrix of the error Lie algebra for the left-invariant state equation, is the Jacobian matrix of the error Lie algebra for the right invariant state equation.

9. A computer device, characterized in that: It includes a memory and a processor, wherein a computer program is stored in the memory, and when the processor runs the computer program stored in the memory, the processor executes a sequential invariant extended Kalman filtering method for navigation and positioning tasks according to any one of claims 1-8.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, which, when executed by a processor, executes the steps of a sequential invariant extended Kalman filtering method for navigation and positioning tasks as described in any one of claims 1-8.

Images