Fixed lag online smoothing method and device based on Lie group Kalman filter

Abstract

Aiming at the problem of online smoothing of inertial / odometer or inertial / Doppler velocimeter combined navigation trajectory under random position correction conditions, the present invention proposes a fixed lag online smoothing method and device based on Lie group Kalman filtering, aiming to use a Lie group Kalman filter based on a world coordinate system to replace the extended Kalman filter, and use the randomly occurring absolute position correction information to smooth the inertial / odometer or inertial / Doppler velocimeter combined navigation state in the previous time period, so as to achieve more accurate navigation positioning results.

Description

Technical Field

[0001] The invention mainly relates to the technical field of integrated navigation and state estimation, in particular to a fixed lag online smoothing method and device based on Lie group Kalman filtering. Background Art

[0002] In the field of carrier velocity-assisted navigation, such as inertial / odometer or inertial / Doppler combined navigation, the navigation positioning accuracy is limited by the accuracy of the odometer and Doppler speed meter, and the position error will accumulate over time. Using randomly occurring position information for correction can effectively improve the positioning accuracy of carrier combined navigation, providing important navigation information support for land vehicles and underwater submersibles to perform tasks.

[0003] The current position correction technology can be achieved through direct correction or information fusion. Direct correction does not take into account prior information, and the trajectory changes greatly; while information fusion methods include Kalman filter and least square method, etc., which have the advantage that the use of fused position information can make the combined navigation positioning more accurate. Position correction technology needs to be combined with smoothing technology to make the trajectory smoother, and the current smoothing technology needs to store a large amount of data and perform post-processing calculations, while fixed interval smoothing technology can guarantee real-time performance to a certain extent, but the interval length depends on engineering experience. Summary of the invention

[0004] Aiming at the problem of online smoothing of an inertial / odometer or inertial / Doppler velocimeter combined navigation trajectory under the condition of random position correction, the present invention provides a fixed lag online smoothing method and device based on Lie group Kalman filtering, which realizes the online smoothing technology of the inertial / odometer or inertial / Doppler velocimeter combined navigation trajectory under the condition of random position correction.

[0005] The present invention aims to replace the extended Kalman filter with a Lie group Kalman filter based on a world coordinate system, and use the randomly occurring absolute position correction information to smooth the inertial / odometer or inertial / Doppler velocimeter combined navigation state in the previous time period to achieve a more accurate navigation positioning result.

[0006] In order to achieve the above object, the present invention is implemented by the following technical solutions:

[0007] In one aspect, the present invention provides a fixed lag online smoothing method based on Lie group Kalman filtering, comprising:

[0008] Receive navigation information from inertial navigation system, odometer or Doppler speedometer, and positioning system;

[0009] According to the initial navigation state and inertial navigation information, the Lie group Kalman filter system equation based on the world coordinate system is constructed;

[0010] The carrier speed measured by the odometer or Doppler speed meter is used as the observation quantity to construct the speed observation equation;

[0011] The position observation equation is constructed using the absolute position of the carrier measured by the positioning system as the observation quantity;

[0012] Obtaining the state and covariance of the Kalman filter observation update, including: when only the carrier velocity is observed, performing inertial / odometer combined navigation or inertial / Doppler combined navigation according to the Lie group Kalman filter system equation and the velocity observation equation based on the world coordinate system, and obtaining the state and covariance of the inertial / odometer combined navigation or inertial / Doppler combined navigation as the state and covariance of the Kalman filter observation update; when only the carrier absolute position is observed, performing inertial / satellite combined navigation or inertial / acoustic positioning combined navigation according to the Lie group Kalman filter system equation and the position observation equation based on the world coordinate system, and obtaining the state and covariance of the inertial / satellite combined navigation or inertial / acoustic positioning combined navigation as the state and covariance of the Kalman filter observation update; when both the carrier velocity observation and the carrier absolute position observation are present, sequentially updating the obtained state and covariance of the inertial / odometer combined navigation or inertial / Doppler combined navigation and the state and covariance of the inertial / satellite combined navigation or inertial / acoustic positioning combined navigation as the state and covariance of the Kalman filter observation update;

[0013] The fixed window length and the number of window movement steps are set, and the state and covariance of the completed Kalman filter observation update within the fixed window are smoothed by RTS to obtain the smoothed state and covariance.

[0014] In the present invention, the inertial navigation information includes: navigation timestamp, roll angle, pitch angle, heading angle, longitude, latitude, altitude and three-dimensional speed information under the local navigation system.

[0015] The odometer or Doppler speedometer navigation information includes: navigation timestamp and carrier speed.

[0016] The positioning system navigation information is navigation information from a satellite positioning system or an acoustic positioning system, including a navigation timestamp and absolute three-dimensional positioning information.

[0017] On the other hand, a fixed lag online smoothing device based on Lie group Kalman filtering comprises:

[0018] The first module is used to receive navigation information from an inertial navigation system, navigation information from an odometer or Doppler speedometer, and navigation information from a positioning system;

[0019] The second module is used to construct a Lie group Kalman filter system equation based on the world coordinate system according to the initial navigation state and inertial navigation information;

[0020] The third module is used to construct a velocity observation equation using the carrier velocity measured by an odometer or Doppler velocimeter as an observation quantity;

[0021] The fourth module is used to construct a position observation equation using the absolute position of the carrier measured by the positioning system as an observation quantity;

[0022] The fifth module is used to obtain the state and covariance of the Kalman filter observation update, which includes: when only the carrier speed is observed, the inertial / odometer combined navigation or inertial / Doppler combined navigation is performed according to the Lie group Kalman filter system equation based on the world coordinate system and the speed observation equation, and the state and covariance of the inertial / odometer combined navigation or inertial / Doppler combined navigation are obtained as the state and covariance of the Kalman filter observation update; when only the carrier absolute position is observed, the inertial / satellite combined navigation or inertial / acoustic positioning combined navigation is performed according to the Lie group Kalman filter system equation based on the world coordinate system and the position observation equation, and the state and covariance of the inertial / satellite combined navigation or inertial / acoustic positioning combined navigation is obtained as the state and covariance of the Kalman filter observation update; when there are both carrier speed observation and carrier absolute position observation, the state and covariance of the inertial / odometer combined navigation or inertial / Doppler combined navigation and the state and covariance of the inertial / satellite combined navigation or inertial / acoustic positioning combined navigation are sequentially updated as the state and covariance of the Kalman filter observation update;

[0023] The sixth module is used to set the fixed window length and the number of window movement steps, and perform RTS smoothing on the state and covariance of the completed Kalman filter observation update within the fixed window to obtain the smoothed state and covariance.

[0024] On the other hand, the present invention provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and when the processor executes the computer program, the steps of the above-mentioned fixed lag online smoothing method based on Lie group Kalman filtering are implemented.

[0025] On the other hand, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the above-mentioned fixed lag online smoothing method based on Lie group Kalman filtering.

[0026] On the other hand, the present invention provides a computer program product, which is stored on a computer-readable storage medium and includes computer instructions, which, when executed by a processor, enable a computer device to implement the steps of the above-mentioned fixed lag online smoothing method based on Lie group Kalman filtering.

[0027] Compared with the prior art, the present invention has the following technical effects:

[0028] The present invention utilizes a Lie group Kalman filter based on a world coordinate system, making the ability to process nonlinear state estimation stronger, and uses RTS smoothing technology to achieve online smoothing of an inertial / odometer or inertial / Doppler velocimeter combined navigation trajectory under the condition of random absolute position observation, while making the combined navigation positioning accuracy higher. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the structures shown in these drawings without paying creative work.

[0030] Figure 1 A flowchart of a fixed lag online smoothing method based on Lie group Kalman filtering provided by an embodiment;

[0031] Figure 2 Schematic diagram of a fixed lag online smoothing method based on Lie group Kalman filtering provided in one embodiment;

[0032] Figure 3 It is a comparison chart of the position error results of the three schemes of online smoothing in the comparative embodiment. DETAILED DESCRIPTION

[0033] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0034] The present invention provides a fixed lag online smoothing method based on Lie group Kalman filtering, which aims to replace the extended Kalman filter with the Lie group Kalman filter based on the world coordinate system, and use the randomly occurring absolute position correction information to smooth the inertial / odometer or inertial / Doppler velocimeter combined navigation state in the previous time period, so as to achieve a more accurate navigation positioning result.

[0035] Reference Figure 1 In one embodiment, a fixed lag online smoothing method based on Lie group Kalman filtering is provided, comprising:

[0036] Receive navigation information from inertial navigation system, odometer or Doppler speedometer, and positioning system;

[0037] According to the initial navigation state and inertial navigation information, the Lie group Kalman filter system equation based on the world coordinate system is constructed;

[0038] The carrier speed measured by the odometer or Doppler speed meter is used as the observation quantity to construct the speed observation equation;

[0039] The position observation equation is constructed using the absolute position of the carrier measured by the positioning system as the observation quantity;

[0040] Get the state and covariance of the Kalman filter observation update;

[0041] Specifically, the method includes: when only the carrier speed is observed, inertial / odometer combined navigation or inertial / Doppler combined navigation is performed according to the Lie group Kalman filter system equation and the speed observation equation based on the world coordinate system, and the state and covariance of the inertial / odometer combined navigation or the inertial / Doppler combined navigation are obtained as the state and covariance of the Kalman filter observation update; when only the carrier absolute position is observed, inertial / satellite combined navigation or inertial / acoustic positioning combined navigation is performed according to the Lie group Kalman filter system equation and the position observation equation based on the world coordinate system, and the state and covariance of the inertial / satellite combined navigation or the inertial / acoustic positioning combined navigation are obtained as the state and covariance of the Kalman filter observation update; when both the carrier speed observation and the carrier absolute position observation are performed, the state and covariance of the inertial / odometer combined navigation or the inertial / Doppler combined navigation and the state and covariance of the inertial / satellite combined navigation or the inertial / acoustic positioning combined navigation are sequentially updated as the state and covariance of the Kalman filter observation update;

[0042] The fixed window length and the number of window movement steps are set, and the state and covariance of the completed Kalman filter observation update within the fixed window are smoothed by RTS to obtain the smoothed state and covariance.

[0043] The inertial navigation information in the present invention includes: navigation timestamp, roll angle, pitch angle, heading angle, longitude, latitude, altitude and three-dimensional speed information under the local navigation system;

[0044] The odometer or Doppler speedometer navigation information includes: navigation timestamp, carrier speed;

[0045] The positioning system navigation information is navigation information from a satellite positioning system or an acoustic positioning system, including a navigation timestamp and absolute three-dimensional positioning information.

[0046] Furthermore, a Lie group Kalman filter system equation based on the world coordinate system is constructed, including:

[0047] The coordinate system of the inertial navigation system is b The world coordinate system is w Tie;

[0048] Define the filter state vector:

[0049] ;

[0050] in represents the attitude error angle of the inertial navigation, is the Lie group nonlinear velocity error, is the Lie group nonlinear position error, is the gyro constant bias, is the addition table constant zero bias, is the scale factor error, are the pitch and yaw installation error angles, respectively, and are modeled as constant errors, that is, . Pay attention to the roll installation error angle It is not observable, so it is usually only estimated .

[0051] The Lie group Kalman filter system equation based on the world coordinate system is as follows:

[0052] ;

[0053] in:

[0054] ;

[0055] ;

[0056] ;

[0057] in is the projection of the earth's rotation vector in the earth system, For b Tie to w The direction cosine matrix of the system, for w The gravity vector of the system, for w The ground velocity vector in the system, for w The position vector relative to the initial point in the system, Represents the conversion of a vector into a corresponding antisymmetric matrix, where are the noise vectors of the addition table and the gyroscope respectively.

[0058] Inertial navigation attitude error angle The differential equation is:

[0059] ;

[0060] in is the projection of the earth's rotation vector in the earth system, For b Tie to w The direction cosine matrix of the system, is the gyro output error.

[0061] Lie group nonlinear velocity error The differential equation is:

[0062] ;

[0063] Lie group nonlinear position error The differential equation is:

[0064] ;

[0065] The gyro constant bias, the meter constant bias, the installation error angle between the inertial navigation system and the odometer, or the installation error angle between the inertial navigation system and the Doppler velocimeter are considered to be constants, and their differential equations are as follows:

[0066] ;

[0067] The speed of the vehicle measured by an odometer or Doppler speedometer As the observed quantity, assume that the measured value of the carrier velocity sensor is , superscript m Represents the carrier coordinate system, and the wavy line indicates that it contains the scale factor error , modeled as:

[0068] ;

[0069] The velocity observation equation is obtained as:

[0070] ;

[0071] in ,in is the attitude transformation matrix from the world coordinate system to the carrier coordinate system, , , The installation error matrix is ​​the inertial navigation system coordinate system b Coordinate system and carrier coordinate system m The direction cosine matrix between the systems, ,in , is the roll installation error angle;

[0072] ;

[0073] Speed ​​measurement noise for odometers or Doppler velocimeters.

[0074] The position observation equation is constructed using the absolute position of the carrier measured by the positioning system as the observation quantity:

[0075] ;

[0076] in Measure noise for satellite or acoustic positioning.

[0077] Reference Figure 2 , Figure 2 The schematic diagram of a fixed lag online smoothing method based on Lie group Kalman filtering provided in an embodiment is to set a fixed window length and a number of window movement steps, perform RTS smoothing on the state and covariance of the completed Kalman filter observation update within the fixed window, and obtain the smoothed state and covariance, including:

[0078] Set fixed window length L and window movement steps M , assuming that the current window time is ;

[0079] Record the forward filtering results within the current window length, including the current state and covariance And the state transfer matrix for the next prediction and status and covariance ,in ;

[0080] Perform inverse RTS smoothing on the state and covariance within the window, including:

[0081] First get the smoothed gain matrix , and then calculate the correction amount to get the smoothed state and covariance ,in:

[0082] ;

[0083] The number of steps for the window movement is M， That is, after the smoothing in the current window is completed, M The state at the moment, and then the next fixed window length is LIn the reverse RTS smoothing, when the next fixed window smoothing is performed, the smoothing basis of the state overlapping with the previous window is the forward filtering state, and the smoothed state is not used for repeated smoothing, that is, the smoothing result in the current window covers the smoothing result repeated in the previous fixed window, to prevent the smoother from repeating the historical state and causing errors.

[0084] Through the above-mentioned fixed lag online smoothing method based on Lie group Kalman filtering, the smoothing processing of the state within the sliding window is solved, and the current state is filtered to improve the accuracy of the state estimation at the next moment, thereby improving the positioning accuracy of the overall inertial / Doppler / random acoustic positioning combined navigation or inertial / odometer / random satellite positioning combined navigation.

[0085] In order to verify the effectiveness of the fixed lag online smoothing method based on Lie group Kalman filter provided by the present invention, a sports car experiment was used for experimental verification. The raw data frequency of the inertial navigation system's gyro is 200Hz, the resolution of the odometer is 2000 lines, and the satellite single-point positioning accuracy is better than 1 meter. The fixed window length L is 600 seconds, and the skip length M is 60 seconds. The three schemes of LG-EKF, EKF and the fixed lag online smoothing method based on Lie group Kalman filter (LG-EKF-RTS) provided by the present invention were used for online smoothing respectively. Figure 3 The comparison chart of the position error results of the online smoothing of the three schemes of the comparative embodiment shows that, by using the fixed lag online smoothing method (LG-EKF-RTS) based on the Lie group Kalman filter provided by the present invention, it is possible to obtain higher positioning accuracy under the condition of position correction. The position error corresponding to the fixed lag online smoothing method (LG-EKF-RTS) based on the Lie group Kalman filter provided by the present invention is improved by 6% compared with the Lie group extended Kalman filter (LG-EKF), and is improved by 35% compared with the extended Kalman filter (EKF). Specifically, the statistical comparison table of the position error of the implementation effects of several schemes of this embodiment is as follows:

[0086]

[0087] In one embodiment, a fixed lag online smoothing device based on Lie group Kalman filtering is provided, comprising:

[0088] The first module is used to receive navigation information from an inertial navigation system, navigation information from an odometer or Doppler speedometer, and navigation information from a positioning system;

[0089] The second module is used to construct a Lie group Kalman filter system equation based on the world coordinate system according to the initial navigation state and inertial navigation information;

[0090] The third module is used to construct a velocity observation equation using the carrier velocity measured by an odometer or Doppler velocimeter as an observation quantity;

[0091] The fourth module is used to construct a position observation equation using the absolute position of the carrier measured by the positioning system as an observation quantity;

[0092] The fifth module is used to obtain the state and covariance of the Kalman filter observation update, which includes: when only the carrier speed is observed, the inertial / odometer combined navigation or inertial / Doppler combined navigation is performed according to the Lie group Kalman filter system equation based on the world coordinate system and the speed observation equation, and the state and covariance of the inertial / odometer combined navigation or inertial / Doppler combined navigation are obtained as the state and covariance of the Kalman filter observation update; when only the carrier absolute position is observed, the inertial / satellite combined navigation or inertial / acoustic positioning combined navigation is performed according to the Lie group Kalman filter system equation based on the world coordinate system and the position observation equation, and the state and covariance of the inertial / satellite combined navigation or inertial / acoustic positioning combined navigation is obtained as the state and covariance of the Kalman filter observation update; when there are both carrier speed observation and carrier absolute position observation, the state and covariance of the inertial / odometer combined navigation or inertial / Doppler combined navigation and the state and covariance of the inertial / satellite combined navigation or inertial / acoustic positioning combined navigation are sequentially updated as the state and covariance of the Kalman filter observation update;

[0093] The sixth module is used to set the fixed window length and the number of window movement steps, and perform RTS smoothing on the state and covariance of the completed Kalman filter observation update within the fixed window to obtain the smoothed state and covariance.

[0094] The implementation methods of the above modules and the construction of the model can all adopt the methods described in any of the above embodiments, which will not be repeated here.

[0095] On the other hand, the present invention provides a computer device, including a memory and a processor, the memory storing a computer program, and the processor implementing the steps of the fixed lag online smoothing method based on Lie group Kalman filtering provided in any of the above embodiments when executing the computer program. The computer device may be a server. The computer device includes a processor, a memory, a network interface and a database connected via a system bus. The processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store sample data. The network interface of the computer device is used to communicate with an external terminal via a network connection.

[0096] On the other hand, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the fixed lag online smoothing method based on Lie group Kalman filtering provided in any of the above embodiments.

[0097] Those of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be implemented by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application may include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0098] Matters not covered by the present invention are known technologies.

[0099] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0100] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the attached claims.

[0101] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A fixed lag online smoothing method based on Lie group Kalman filtering, characterized in that: include: Receive navigation information from inertial navigation system, odometer or Doppler speedometer, and positioning system; According to the initial navigation state and inertial navigation information, the Lie group Kalman filter system equation based on the world coordinate system is constructed; The carrier speed measured by the odometer or Doppler speed meter is used as the observation quantity to construct the speed observation equation; The position observation equation is constructed using the absolute position of the carrier measured by the positioning system as the observation quantity; Obtaining the state and covariance of the Kalman filter observation update, including: when only the carrier velocity is observed, performing inertial / odometer combined navigation or inertial / Doppler combined navigation according to the Lie group Kalman filter system equation and the velocity observation equation based on the world coordinate system, and obtaining the state and covariance of the inertial / odometer combined navigation or inertial / Doppler combined navigation as the state and covariance of the Kalman filter observation update; when only the carrier absolute position is observed, performing inertial / satellite combined navigation or inertial / acoustic positioning combined navigation according to the Lie group Kalman filter system equation and the position observation equation based on the world coordinate system, and obtaining the state and covariance of the inertial / satellite combined navigation or inertial / acoustic positioning combined navigation as the state and covariance of the Kalman filter observation update; when both the carrier velocity observation and the carrier absolute position observation are present, sequentially updating the obtained state and covariance of the inertial / odometer combined navigation or inertial / Doppler combined navigation and the state and covariance of the inertial / satellite combined navigation or inertial / acoustic positioning combined navigation as the state and covariance of the Kalman filter observation update; Set the fixed window length and the number of window movement steps, perform RTS smoothing on the state and covariance of the completed Kalman filter observation update within the fixed window, and obtain the smoothed state and covariance, including: Set fixed window length L and window movement steps M , assuming that the current window time is ; Record the forward filtering results within the current window length, including the current state and covariance And the state transfer matrix for the next prediction and status and covariance ,in ; Perform inverse RTS smoothing on the state and covariance within the window, including: First get the smoothed gain matrix , and then calculate the correction amount to get the smoothed state and covariance ,in: ； The number of steps for the window movement is M , that is, after the smoothing in the current window is completed, M The state at the moment, and then the next fixed window length is L In the reverse RTS smoothing, when the next fixed window smoothing is performed, the smoothing basis of the state overlapping with the previous window is the forward filtering state, and the smoothed state is not used for repeated smoothing, that is, the smoothing result in the current window covers the smoothing result repeated in the previous fixed window, to prevent the smoother from repeating the historical state and causing errors.

2. The fixed lag online smoothing method based on Lie group Kalman filtering according to claim 1 is characterized in that: Inertial navigation information includes: navigation timestamp, roll angle, pitch angle, heading angle, longitude, latitude, altitude and three-dimensional speed information under the local navigation system; The odometer or Doppler speedometer navigation information includes: navigation timestamp, carrier speed; The positioning system navigation information is navigation information from a satellite positioning system or an acoustic positioning system, including a navigation timestamp and absolute three-dimensional positioning information.

3. The fixed lag online smoothing method based on Lie group Kalman filtering according to claim 2 is characterized in that: Construct the Lie group Kalman filter system equation based on the world coordinate system, including: The coordinate system of the inertial navigation system is b The world coordinate system is w Tie; Define the filter state vector: ； in represents the attitude error angle of the inertial navigation, is the Lie group nonlinear velocity error, is the Lie group nonlinear position error, is the gyro constant bias, is the addition table constant zero bias, is the scale factor error, are the pitch and yaw installation error angles, respectively, and are modeled as constant errors, that is, ; The Lie group Kalman filter system equation based on the world coordinate system is as follows: ； in: ； ； ； in is the projection of the earth's rotation vector in the earth system, For b Tie to w The direction cosine matrix of the system, for w The gravity vector of the system, for w The ground velocity vector in the system, for w The position vector relative to the initial point in the system, Represents the conversion of a vector into a corresponding antisymmetric matrix, where are the noise vectors of the addition table and the gyroscope respectively.

4. The fixed lag online smoothing method based on Lie group Kalman filtering according to claim 3 is characterized in that: Inertial navigation attitude error angle The differential equation is: ； in is the projection of the earth's rotation vector in the earth system, For b Tie to w The direction cosine matrix of the system, is the gyro output error.

5. The fixed lag online smoothing method based on Lie group Kalman filtering according to claim 4 is characterized in that: Lie group nonlinear velocity error The differential equation is: ； Lie group nonlinear position error The differential equation is: 。 6. The fixed lag online smoothing method based on Lie group Kalman filtering according to claim 4 is characterized in that: The gyro constant bias, the meter constant bias, the installation error angle between the inertial navigation system and the odometer, or the installation error angle between the inertial navigation system and the Doppler velocimeter are considered to be constants, and the scale factor error is also considered to be a constant. The differential equations are as follows: 。 7. The fixed lag online smoothing method based on Lie group Kalman filtering according to claim 3, 4 or 5, characterized in that: The speed of the vehicle measured by an odometer or Doppler speedometer As the observed quantity, assume that the measured value of the carrier velocity sensor is , superscript m Represents the carrier coordinate system, and the wavy line indicates that it contains the scale factor error , modeled as: ； The velocity observation equation is obtained as: ； in ,in is the attitude transformation matrix from the world coordinate system to the carrier coordinate system, , , The installation error matrix is ​​the inertial navigation system coordinate system b Coordinate system and carrier coordinate system m The direction cosine matrix between the systems, ,in , is the roll installation error angle; ； Speed ​​measurement noise for odometers or Doppler velocimeters.

8. The fixed lag online smoothing method based on Lie group Kalman filtering according to claim 6, characterized in that: The position observation equation is constructed using the absolute position of the carrier measured by the positioning system as the observation quantity: ； in , Measure noise for satellite or acoustic positioning.

9. A fixed lag online smoothing device based on Lie group Kalman filtering, characterized in that: include: The first module is used to receive navigation information from an inertial navigation system, navigation information from an odometer or Doppler speedometer, and navigation information from a positioning system; The second module is used to construct a Lie group Kalman filter system equation based on the world coordinate system according to the initial navigation state and inertial navigation information; The third module is used to construct a velocity observation equation using the carrier velocity measured by an odometer or Doppler velocimeter as an observation quantity; The fourth module is used to construct a position observation equation using the absolute position of the carrier measured by the positioning system as an observation quantity; The fifth module is used to obtain the state and covariance of the Kalman filter observation update, which includes: when only the carrier speed is observed, the inertial / odometer combined navigation or inertial / Doppler combined navigation is performed according to the Lie group Kalman filter system equation based on the world coordinate system and the speed observation equation, and the state and covariance of the inertial / odometer combined navigation or inertial / Doppler combined navigation are obtained as the state and covariance of the Kalman filter observation update; when only the carrier absolute position is observed, the inertial / satellite combined navigation or inertial / acoustic positioning combined navigation is performed according to the Lie group Kalman filter system equation based on the world coordinate system and the position observation equation, and the state and covariance of the inertial / satellite combined navigation or inertial / acoustic positioning combined navigation is obtained as the state and covariance of the Kalman filter observation update; when there are both carrier speed observation and carrier absolute position observation, the state and covariance of the inertial / odometer combined navigation or inertial / Doppler combined navigation and the state and covariance of the inertial / satellite combined navigation or inertial / acoustic positioning combined navigation are sequentially updated as the state and covariance of the Kalman filter observation update; The sixth module is used to set the fixed window length and the number of window movement steps, perform RTS smoothing on the state and covariance of the completed Kalman filter observation update within the fixed window, and obtain the smoothed state and covariance, including: Set fixed window length L and window movement steps M , assuming that the current window time is ; Record the forward filtering results within the current window length, including the current state and covariance And the state transfer matrix for the next prediction and status and covariance ,in ; Perform inverse RTS smoothing on the state and covariance within the window, including: First get the smoothed gain matrix , and then calculate the correction amount to get the smoothed state and covariance ,in: ； The number of steps for the window movement is M， That is, after the smoothing in the current window is completed, M The state at the moment, and then the next fixed window length is L In the reverse RTS smoothing, when the next fixed window smoothing is performed, the smoothing basis of the state overlapping with the previous window is the forward filtering state, and the smoothed state is not used for repeated smoothing, that is, the smoothing result in the current window covers the smoothing result repeated in the previous fixed window, to prevent the smoother from repeating the historical state and causing errors.

Images