Method, device and product for realizing loose combination of satellite compass data and IMU (Inertial Measurement Unit) data

Abstract

The invention provides a method, device and product for realizing loose combination of satellite compass data and IMU (Inertial Measurement Unit) data, and the method comprises the following steps: when a preset second time interval is reached, executing the step S1: S1, obtaining an updated IMU elevation angle and an IMU course angle by using the current updated IMU data, and obtaining a first IMU elevation angle and a second IMU course angle; the course angle zero offset and the elevation angle zero offset of the IMU are predicted based on the Kalman filter, the change of the IMU elevation angle and the change of the IMU course angle; when a preset first time interval is reached, the step S2 is executed, in the step S2, the updated GNSS elevation angle and the updated GNSS course angle are obtained through the current updated GNSS signal, and the updated GNSS elevation angle and the updated GNSS course angle are used for updating the state variables; and if the first time interval arrives and the second time interval also arrives when the GNSS signal passes the resolving quality detection, executing the step S2 and not executing the step S1. By means of the technical scheme, the GNSS signals are used for correcting IMU triaxial angular velocity zero offset estimation, and the influence of IMU zero offset on navigation precision and reliability is reduced.

Description

Technical Field

[0001] The present invention relates to the field of navigation and positioning, and in particular to a method, a device and a product for realizing the loose combination of satellite compass data and IMU data. Background Art

[0002] In a loose coupling system, the angular velocity bias of the inertial measurement unit (IMU) is an important consideration because the gyroscope in the IMU may show a certain angular velocity even when it is stationary. This bias will accumulate over time, resulting in attitude estimation errors. However, in a loose coupling system, GNSS provides position and velocity information, while the inertial navigation system (INS) is responsible for providing acceleration and attitude information. The data fusion between the two is relatively simple, that is, the loose coupling system mainly corrects the position and velocity of the INS through GNSS data, rather than directly correcting the sensor errors in the INS, such as the three-axis angular velocity bias. Therefore, although the loose coupling system can use GNSS data to improve the overall navigation accuracy, it usually does not directly involve the correction of the three-axis angular velocity bias in the INS. The three-axis angular velocity bias is more often implemented in a tightly coupled or deeply integrated system. In a loose coupling, it relies more on the calibration mechanism and algorithm of the INS itself to minimize the impact of the bias. Summary of the invention

[0003] The embodiments of the present invention provide a method, device and product for realizing loose combination of satellite compass data and IMU data, so as to solve the problem that the IMU three-axis angular velocity zero bias cannot be effectively corrected in the traditional loose combination system, and further improve the accuracy and reliability of the entire navigation system.

[0004] In order to achieve the above-mentioned object, on the one hand, a method for realizing loose combination of satellite compass data and IMU data is provided, wherein the satellite compass data includes at least two GNSS signals received by at least two independent GNSS receivers, and the at least two GNSS signals are used to solve the baseline vector between the GNSS receivers, and the IMU data includes quaternion data, wherein the baseline vector is used to obtain the GNSS altitude angle and the GNSS heading angle, and the IMU data is used to obtain the IMU altitude angle and the IMU heading angle, and the method comprises:

[0005] Whenever the second time interval is reached, step S1 is executed:

[0006] S1, using the currently updated IMU data to obtain an updated IMU altitude angle and an IMU heading angle, and using the updated IMU altitude angle and the IMU heading angle to obtain a change in the IMU altitude angle and a change in the IMU heading angle, and predicting the heading angle zero bias and the altitude angle zero bias of the IMU based on a pre-established Kalman filter and the change in the IMU altitude angle and the change in the IMU heading angle;

[0007] Among them, in the established Kalman filter, the state variable is defined

[0008] Among them, θ represents the IMU heading angle, b gθ Indicates the zero bias of the IMU heading angle, represents the IMU altitude angle, Indicates the zero bias of the IMU altitude angle;

[0009] Whenever the first time interval is reached and the GNSS signal passes the predetermined solution quality test, step S2 is executed:

[0010] S2, using the currently updated GNSS signal to obtain an updated GNSS altitude angle and a GNSS heading angle, and using the updated GNSS altitude angle and the GNSS heading angle to update the state variable x;

[0011] If the second time interval also arrives when the first time interval arrives, then step S2 is executed and step S1 is not executed;

[0012] The first time interval is pre-set according to the update time interval of the GNSS signal; the second time interval is pre-set according to the update time interval of the IMU data; and the first time interval is greater than the second time interval.

[0013] Preferably, the method further comprises: whenever the second time interval is reached, using the latest updated state vector X, obtaining and outputting a corrected IMU heading angle and a corrected IMU altitude angle.

[0014] Preferably, the method further comprises: initializing the state vector X to obtain an initial value x0 of the state vector, wherein:

[0015]

[0016] θ0 represents the initial value of the IMU heading angle, where the value of θ0 is the initial value of the GNSS heading angle; Indicates the initial value of the zero bias of the IMU heading angle, which is 0; Indicates the initial value of the IMU altitude angle, The value is the initial value of the GNSS altitude angle; Indicates the initial value of the zero bias of the IMU altitude angle, which is 0;

[0017] Set the initial value P0 of the estimated error covariance matrix P to the identity matrix, where:

[0018]

[0019] Set the process noise covariance matrix Q, where:

[0020]

[0021] q θ represents the variance of the process noise of the IMU heading angle, q bθ represents the variance of the process noise of the zero bias of the IMU heading angle, represents the variance of the process noise of the IMU altitude angle, The variance of the process noise of the zero bias of the IMU altitude angle, q θ ,q bθ , as well as The values ​​of are preset;

[0022] Define the observation noise covariance matrix R, where:

[0023]

[0024] r θ is the noise variance of the IMU heading angle, is the noise variance of the IMU altitude angle, r θ and The values ​​are preset.

[0025] Preferably, the method, wherein predicting the heading angle zero bias and altitude angle zero bias of the IMU comprises: updating the state vector x, wherein:

[0026] x k+1 =Fx k +Gu k

[0027]

[0028] x k+1 is the state variable at time k+1, x k is the state variable at the kth moment, F is the state transfer matrix, G is the system noise matrix, u k is the system noise vector, Δt is the second time interval;

[0029] Update the estimated error covariance matrix P, where:

[0030] P k+1 =FP k F T +Q

[0031] P k+1 represents the estimated error covariance matrix at the k+1th moment, P k represents the estimated error covariance matrix at the kth moment, and T represents the transpose.

[0032] Preferably, the method, wherein the IMU altitude angle and the IMU heading angle are obtained using the following conversion formula:

[0033]

[0034]

[0035] represents the IMU altitude angle, θ IMU It indicates that the IMU heading angle is the converted heading angle; wherein q0, q1, q2, q3 are the quaternion data.

[0036] Preferably, in the method, step S2 comprises:

[0037] Establish the observation equation:

[0038] z k =Hx k +v k

[0039]

[0040] z k represents the observation equation at the kth moment, v k represents the observation noise at the kth moment, H is the observation matrix; wherein the observation noise covariance matrix R is decomposed by the square root method to obtain the lower triangular matrix L, so that R = LL T ; where v k =Lw k ;w k represents a random vector from a standard normal distribution;

[0041] Calculate the Kalman gain K at the kth moment k ,in:

[0042] K k =P k H T (HP k H T +R) -1

[0043] Update the state vector:

[0044] x k+1 =x k +K k (z k -Hx k )

[0045] x k+1 is the state variable at time k+1, x k is the state variable at the kth moment;

[0046] Update the estimated error covariance matrix:

[0047] P k+1 =(IK k H)P k ;

[0048] P k+1 is the estimation error covariance matrix at the k+1th moment.

[0049] Preferably, in the method, the first time interval is an integer multiple of the second time interval.

[0050] Preferably, the method further includes: formatting the output corrected IMU heading angle and the corrected IMU altitude angle so that the processed data conforms to the NMEA0183 transmission protocol requirements, and transmitting the processed data to a predetermined external device.

[0051] On the other hand, the present invention also provides a navigation device, which includes a memory and a processor, wherein the memory stores at least one program, and the at least one program is executed by the processor to implement any of the methods for loosely combining satellite compass data and IMU data as described above.

[0052] In yet another aspect, the present invention further provides a computer program product, comprising a computer program, wherein when the computer program is executed by a processor, the computer program implements any of the above methods for achieving loose combination of satellite compass data and IMU data.

[0053] The above technical solution has the following technical effects:

[0054] The technical solution of the embodiment of the present invention aims at the different updating frequencies of IMU data and GNSS positioning data, and uses the updated GNSS positioning data to correct the zero bias of the IMU heading angle and the zero bias of the IMU altitude angle; and predicts the zero bias of the IMU heading angle and the zero bias of the IMU altitude angle at the updating time of the IMU data and updates the zero bias of the IMU heading angle and the zero bias of the IMU altitude angle at the updating time of the GNSS data, and stops using the IMU data to predict the zero bias of the IMU heading angle and the zero bias of the IMU altitude angle when the GNSS data is updated, thereby realizing the effective fusion of the GNSS positioning data and the IMU data, and realizing the estimation of the zero bias of the IMU three-axis angular velocity, reducing the influence of the zero bias of the IMU three-axis angular velocity on the measurement of the heading angle and the altitude angle, and improving the stability and accuracy of the navigation system. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 A schematic flow chart of a method for realizing loose combination of satellite compass data and IMU data according to an embodiment of the present invention;

[0056] Figure 2 FIG. 4 is a schematic structural diagram of a navigation device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0057] To further illustrate the various embodiments, the present invention provides drawings. These drawings are part of the disclosure of the present invention, which are mainly used to illustrate the embodiments and can be used in conjunction with the relevant descriptions in the specification to explain the operating principles of the embodiments. With reference to these contents, a person of ordinary skill in the art should be able to understand other possible implementations and advantages of the present invention. The components in the figures are not drawn to scale, and similar component symbols are generally used to represent similar components.

[0058] The present invention will now be further described with reference to the accompanying drawings and specific implementation methods.

[0059] Embodiment 1:

[0060] In order to estimate the IMU three-axis angular velocity zero bias, reduce the influence of the IMU three-axis angular velocity zero bias on the heading angle and altitude angle measurement, and further improve the stability and accuracy of the navigation system, this embodiment provides a method for realizing the loose combination of satellite compass data and IMU data.

[0061] Figure 1 FIG. 1 is a flow chart of a method for realizing a loose combination of satellite compass data and IMU data according to an embodiment of the present invention. Figure 1As shown, the satellite compass data includes at least two GNSS signals received by at least two independent GNSS receivers, the at least two GNSS signals are used to solve the baseline vector between the GNSS receivers, the IMU data includes quaternion data, wherein the baseline vector is used to obtain the GNSS altitude angle and the GNSS heading angle, and the IMU data is used to obtain the IMU altitude angle and the IMU heading angle, and the method includes:

[0062] Whenever the second time interval is reached, step S1 is executed:

[0063] S1, using the currently updated IMU data to obtain an updated IMU altitude angle and an IMU heading angle, and using the updated IMU altitude angle and the IMU heading angle to obtain a change in the IMU altitude angle and a change in the IMU heading angle, and predicting the IMU heading angle zero bias and the altitude angle zero bias based on a pre-established Kalman filter and the change in the IMU altitude angle and the change in the IMU heading angle;

[0064] Among them, in the established Kalman filter, the state variable is defined Among them, θ represents the IMU heading angle, b gθ Indicates the zero bias of the IMU heading angle, represents the IMU altitude angle, Indicates the zero bias of the IMU altitude angle;

[0065] Whenever the first time interval is reached and the GNSS signal passes the predetermined solution quality test, step S2 is executed:

[0066] S2, using the currently updated GNSS signal to obtain an updated GNSS altitude angle and GNSS heading angle, and using the updated GNSS altitude angle and GNSS heading angle to update the state variable x;

[0067] If the second time interval also arrives when the first time interval arrives, then step S2 is executed and step S1 is not executed;

[0068] Among them, the first time interval is pre-set according to the update time interval of the GNSS signal; the second time interval is pre-set according to the update time interval of the IMU data; and the first time interval is greater than the second time interval.

[0069] Preferably, the first time interval is the update time interval of the GNSS signal or is less than the update time interval of the GNSS signal but greater than the second time interval; the second time interval is the update time interval of the IMU data or is greater than the update time interval of the IMU data but less than the first time interval.

[0070] In a specific implementation, the update time interval of the GNSS signal is several seconds, and the first time interval can be set to 1s or 2s; the update time interval of the IMU data is a few tenths of a second, such as 0.1 seconds, and the second time interval can be set to 0.2s-0.5s, etc.

[0071] Preferably, the first time interval is an integer multiple of the second time interval.

[0072] Embodiment 2:

[0073] Satellite compass can output high-precision heading angle and altitude angle, but once signal blocking or signal interference occurs, the quality of the solution result will not meet the standard. At this time, IMU is needed to assist in continuing to output heading angle and altitude angle. The goal of the standard loose combination is to correct the positioning and velocity information of GNSS and reduce the influence of the three-axis acceleration zero bias of IMU. The technical solution of this embodiment modifies the loose combination, which uses GNSS data to realize the zero bias estimation of the IMU three-axis angular velocity, so as to reduce the influence of the IMU three-axis angular velocity zero bias on the heading angle and altitude angle measurement.

[0074] The method for realizing loose combination of satellite compass data and IMU data in an embodiment of the present invention comprises the following steps:

[0075] Step 1: Get real-time data

[0076] 1. Acquire data from a satellite compass at every first predetermined time interval, such as every 1 second, wherein:

[0077] The satellite compass uses at least two independent GNSS receivers to receive satellite signals and calculates the baseline vector between the two GNSS receiver antennas through phase difference and time difference, where:

[0078] The baseline vector B is the relative position of the antennas of the two GNSS receivers in space xyz and is expressed as:

[0079] B=(B x ,B y ,B z )

[0080] Convert the baseline vector to GNSS altitude and GNSS heading, where:

[0081] GNSS altitude angle Represents the projection of the baseline vector in the vertical direction, and the calculation formula is:

[0082]

[0083] GNSS heading angle θ GNSS Represents the projection of the baseline vector in the horizontal direction, and the calculation formula is:

[0084]

[0085] Pre-set the detection conditions for solution quality, and record the solution quality of GNSS data as a flag GNSS , flag when the solution quality test passes GNSS =1; otherwise, flag GNSS =0 or other predetermined value.

[0086] 2. Obtain IMU data from the IMU module every second predetermined time interval, such as every 200 ms, wherein:

[0087] The IMU module uses quaternions to represent the rotational attitude of the sensor, and converts the quaternions into altitude and heading angles in Euler angles through conversion formulas.

[0088] Quaternion (q0, q1, q2, q3) represents rotation, quaternion is converted to Euler angle The formula is:

[0089]

[0090] Represents the IMU altitude angle, θ IMU represents the IMU heading angle, ψ IMU Indicates the IMU roll angle.

[0091] Step 2: Initialization

[0092] 1. In the pre-built Kalman filter, define the state variables Among them, θ represents the IMU heading angle, b gθ Indicates the zero bias of the IMU heading angle, represents the IMU altitude angle, Indicates the zero bias of the IMU altitude angle.

[0093] Initialize the state vector x0, where:

[0094]

[0095] θ0 represents the initial value of the IMU heading angle. In this embodiment, the initial value of the GNSS heading angle is used to initialize the IMU heading angle. In a specific implementation, the GNSS heading angle of the first second is used as the value of θ0. Indicates the initial value of the zero bias of the IMU heading angle, which is 0; Indicates the initial value of the IMU altitude angle. In this embodiment, the initial value of the GNSS altitude angle is used to initialize the IMU altitude angle. In a specific implementation, the GNSS altitude angle of the first second is used as The value of Indicates the initial value of the zero bias of the IMU altitude angle, which is 0;

[0096] 2. Set the initial value P0 of the estimated error covariance matrix P to the identity matrix, where:

[0097]

[0098] 3. Set the process noise covariance matrix Q, where:

[0099]

[0100] q θ The variance of the process noise of the IMU heading angle, q bθ The variance of the process noise representing the zero bias of the IMU heading angle, represents the variance of the process noise of the IMU altitude angle, The variance of the process noise representing the zero bias of the IMU altitude angle, q θ ,q bθ , as well as The values ​​are preset.

[0101] 4. Define the observation noise covariance matrix R, where:

[0102]

[0103] Among them, r θ is the noise variance of the IMU heading angle, is the noise variance of the IMU altitude angle, r θ and The values ​​are preset.

[0104] Step 3: Data prediction

[0105] Every predetermined second time interval, such as every 200 milliseconds, the state vector and covariance matrix are predicted using the IMU data:

[0106] 1. Calculate the relative angle change of IMU heading angle and IMU altitude angle to obtain the IMU heading angle change Δθ IMU And IMU altitude angle changes In this example, the changes in the IMU heading angle and IMU altitude angle at the kth moment and the k-1th moment are calculated, where:

[0107] Δθ IMU =θ IMU (k)-θ IMU (k-1)

[0108]

[0109] 2. Update the state vector x, where:

[0110] x k+1 =Fx k +Gu k

[0111]

[0112] x k+1 is the state variable at time k+1, x k is the state variable at the kth moment, F is the state transfer matrix, G is the system noise matrix, u k is the system noise vector, Δt is the second time interval

[0113] 3. Update the estimated error covariance matrix P, where:

[0114] P k+1 =FP k F T +Q

[0115] P k+1 represents the estimated error covariance matrix at the k+1th moment, P k represents the estimated error covariance matrix at the kth moment, and T represents the transpose.

[0116] Step 4: Data update

[0117] Based on step 3, every second when the flag GNSS =1, data is updated, and step 3 is stopped at this moment, and only step 4 is executed, that is, when the GNSS update data whose solution quality has been detected is used to update the state variables, the updated IMU data is stopped from being used to predict the state variables. In this way, based on the different update frequencies of IMU data and GNSS data, when the GNSS data is not updated, the IMU data is used to predict and update the state variables, and when the GNSS data is updated, the GNSS data is used to update the state variables, thereby effectively integrating the IMU data and the GNSS data to realize the zero bias correction of the IMU heading angle and the IMU altitude angle, wherein:

[0118] Establish the observation equation:

[0119] z k =Hx k +v k

[0120]

[0121] z k represents the observation equation at the kth moment, v krepresents the observation noise at the kth moment, obtained from the covariance matrix R, and H is the observation matrix; wherein the observation noise covariance matrix R is decomposed by the square root method to obtain the lower triangular matrix L, so that R = LL T ; where v k =Lw k ;w k represents a random vector from a standard normal distribution;

[0122] Calculate the Kalman gain K at the kth moment k ,in:

[0123] K k =P k H T (HP k H T +R) -1

[0124] Update the state vector:

[0125] x k+1 =x k +K k (z k -Hx k )

[0126] x k+1 is the state variable at time k+1, x k is the state variable at the kth moment;

[0127] Update the estimated error covariance matrix:

[0128] P k+1 =(IK k H)P k ;

[0129] P k+1 is the estimation error covariance matrix at the k+1th moment.

[0130] Step 5: Output smooth IMU heading angle and IMU altitude angle

[0131] 1. Calculation result output:

[0132] The heading angle and altitude angle are output at a predetermined second time interval, such as a 200ms time interval; wherein, according to the updated state vector x k , get the corrected IMU heading angle θ IMU and IMU altitude angle ψ IMU , and output the updated IMU heading angle zero bias and the zero bias of the IMU altitude angle

[0133] 2. Data transmission:

[0134] The MCU formats the corrected IMU heading angle and IMU altitude angle output by the Kalman filter to ensure that the data complies with the NMEA0183 transmission protocol requirements, and transmits them to external devices, such as through an RS-422 interface.

[0135] 3.Device display:

[0136] The external device receives the data transmitted through the RS-422 interface, parses the received IMU heading angle and IMU altitude angle data, and displays the current heading and altitude information on the display.

[0137] Embodiment three:

[0138] The present invention also provides a navigation device, such as Figure 2 As shown, the device of this embodiment includes a processor 201, a memory 202, a bus 203, and a computer program stored in the memory 202 and executable on the processor 201. The processor 201 includes one or more processing cores. The memory 202 is connected to the processor 201 via the bus 203. The memory 202 is used to store program instructions. When the processor executes the computer program, the steps in the above method embodiment of the first embodiment of the present invention are implemented.

[0139] Further, as an executable solution, the navigation device may be a computer unit, which may be a computing device such as a desktop computer, a notebook, a PDA, and a cloud server. The computer unit may include, but is not limited to, a processor and a memory. Those skilled in the art may understand that the composition structure of the above-mentioned computer unit is only an example of a computer unit and does not constitute a limitation on the computer unit, and may include more or less components than the above-mentioned, or a combination of certain components, or different components. For example, the computer unit may also include input and output devices, network access devices, buses, etc., which are not limited in the embodiments of the present invention.

[0140] Further, as an executable solution, the processor may be a central processing unit (CPU), other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc. The processor is the control center of the computer unit, and various interfaces and lines are used to connect the various parts of the entire computer unit.

[0141] The memory can be used to store the computer program and / or module, and the processor realizes various functions of the computer unit by running or executing the computer program and / or module stored in the memory, and calling the data stored in the memory. The memory can mainly include a program storage area and a data storage area, wherein the program storage area can store an operating system and at least one application required for a function; the data storage area can store data created according to the use of the mobile phone, etc. In addition, the memory can include a high-speed random access memory, and can also include a non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (SecureDigital, SD) card, a flash card (Flash Card), at least one disk storage device, a flash memory device, or other volatile solid-state storage devices.

[0142] Embodiment 4:

[0143] The present invention also provides a computer program product, comprising a computer program, wherein when the computer program is executed by a processor, the steps of the method described above are implemented.

[0144] Although the present invention has been specifically shown and described in conjunction with the preferred embodiments, it should be understood by those skilled in the art that various changes may be made to the present invention in form and details without departing from the spirit and scope of the present invention as defined by the appended claims, all of which are within the scope of protection of the present invention.

Claims

1. A method for realizing a loose combination of satellite compass data and IMU data, wherein the satellite compass data comprises at least two GNSS signals received by at least two independent GNSS receivers, wherein the at least two GNSS signals are used to resolve a baseline vector between the GNSS receivers, and the IMU data comprises quaternion data. It is characterized in that The baseline vector is used to obtain a GNSS altitude angle and a GNSS heading angle, and the IMU data is used to obtain an IMU altitude angle and an IMU heading angle. The method comprises: Whenever the second time interval is reached, step S1 is executed: S1, using the currently updated IMU data to obtain an updated IMU altitude angle and an IMU heading angle, and using the updated IMU altitude angle and the IMU heading angle to obtain a change in the IMU altitude angle and a change in the IMU heading angle, and predicting the heading angle zero bias and the altitude angle zero bias of the IMU based on a pre-established Kalman filter and the change in the IMU altitude angle and the change in the IMU heading angle; Among them, in the established Kalman filter, the state variable is defined Among them, θ represents the IMU heading angle, b gθ Indicates the zero bias of the IMU heading angle, represents the IMU altitude angle, Indicates the zero bias of the IMU altitude angle; Whenever the first time interval is reached and the GNSS signal passes the predetermined solution quality test, step S2 is executed: S2, using the currently updated GNSS signal to obtain an updated GNSS altitude angle and a GNSS heading angle, and using the updated GNSS altitude angle and the GNSS heading angle to update the state variable x; If the second time interval also arrives when the first time interval arrives, then step S2 is executed and step S1 is not executed; The first time interval is pre-set according to the update time interval of the GNSS signal; the second time interval is pre-set according to the update time interval of the IMU data; and the first time interval is greater than the second time interval.

2. The method according to claim 1, It is characterized in that Also includes: Whenever the second time interval is reached, the latest updated state vector X is used to obtain and output a corrected IMU heading angle and a corrected IMU altitude angle.

3. The method according to claim 1, It is characterized in that Also includes: Initialize the state vector X to obtain the initial value x of the state vector 0 ,in: θ 0 Represents the initial value of the IMU heading angle, where θ 0 The value is the initial value of the GNSS heading angle; Indicates the initial value of the zero bias of the IMU heading angle, which is 0; Indicates the initial value of the IMU altitude angle, The value is the initial value of the GNSS altitude angle; Indicates the initial value of the zero bias of the IMU altitude angle, which is 0; Set the initial value P of the estimated error covariance matrix P 0 is the identity matrix, where: Set the process noise covariance matrix Q, where: q θ represents the variance of the process noise of the IMU heading angle, q bθ represents the variance of the process noise of the zero bias of the IMU heading angle, represents the variance of the process noise of the IMU altitude angle, The variance of the process noise of the zero bias of the IMU altitude angle, q θ ,q bθ , as well as The values ​​of are preset; Define the observation noise covariance matrix R, where: r θ is the noise variance of the IMU heading angle, is the noise variance of the IMU altitude angle, r θ and The values ​​are preset.

4. The method according to claim 3, It is characterized in that Predicting the heading angle zero bias and altitude angle zero bias of the IMU includes: updating the state vector x, where: x k+1 =Fx k +Gu k x k+1 is the state variable at time k+1, x k is the state variable at the kth moment, F is the state transfer matrix, G is the system noise matrix, u k is the system noise vector, Δt is the second time interval; Update the estimated error covariance matrix P, where: P k+1 =FP k F T +Q P k+1 represents the estimated error covariance matrix at the k+1th moment, P k represents the estimated error covariance matrix at the kth moment, and T represents the transpose.

5. The method according to claim 1, It is characterized in that Use the following conversion formula to obtain the IMU altitude angle and the IMU heading angle: represents the IMU altitude angle, θ IMU Indicates that the IMU heading angle is the converted heading angle; where q 0 ,q 1 ,q 2 ,q 3 is the quaternion data.

6. The method according to claim 3, It is characterized in that The step S2 comprises: Establish the observation equation: z k =Hx k +v k z k represents the observation equation at the kth moment, v k represents the observation noise at the kth moment, H is the observation matrix; wherein the observation noise covariance matrix R is decomposed by the square root method to obtain the lower triangular matrix L, so that R = LL T ; where v k =Lw k ;w k represents a random vector from a standard normal distribution; Calculate the Kalman gain K at the kth moment k ,in: K k =P k H T (HP k H T +R) -1 Update the state vector: x k+1 =x k +K k (z k -Hx k ) x k+1 is the state variable at time k+1, x k is the state variable at the kth moment; Update the estimated error covariance matrix: P k+1 =(I-K k H)P k ; P k+1 is the estimation error covariance matrix at the k+1th moment.

7. The method according to claim 1, It is characterized in that The first time interval is an integer multiple of the second time interval.

8. The method according to claim 2, It is characterized in that Also includes: The output corrected IMU heading angle and the corrected IMU altitude angle are formatted so that the processed data conform to the NMEA0183 transmission protocol requirements, and the processed data are transmitted to a predetermined external device.

9. A navigation device, It is characterized in that The method comprises a memory and a processor, wherein the memory stores at least one program, and the at least one program is executed by the processor to implement the method for realizing the loose combination of satellite compass data and IMU data as claimed in any one of claims 1 to 8.

10. A computer program product comprising a computer program, It is characterized in that When the computer program is executed by a processor, the method for realizing loose combination of satellite compass data and IMU data as claimed in any one of claims 1 to 8 is implemented.

Images