Loose coupling integrated navigation method of Kalman-Buck filter

Abstract

The invention belongs to the technical field of integrated navigation, and particularly relates to a loose coupling integrated navigation method of a Kalman-Buck filter. The method comprises the following steps: firstly, performing SINS solution according to IMU output information, and then obtaining an observed quantity of a Kalman-Buck filter according to SINS solution data and GPS observation data; secondly, a Kalman-Buck filter is used for estimating the state of the system; and finally, correcting the SINS resolving value by using the state estimation value of the Kalman-Buck filter to finally obtain the position, speed and attitude information of the corrected carrier. Compared with the prior art, the method has the advantages that the Kalman-Buck filter is used for fusing the GPS measurement information and the SINS calculation information to perform loose coupling integrated navigation, and the Kalman-Buck filter is an important algorithm for processing state estimation in a continuous time linear system and can provide smoother state estimation, so that a system model does not need to be discretized; the solution can be directly carried out, and the navigation data obtained by the solution is smoother.

Description

Technical Field

[0001] The invention belongs to the technical field of integrated navigation, in particular to a loosely coupled integrated navigation method of a Kalman-Bucy filter. Background Art

[0002] With the development of micro-electromechanical system technology, the manufacturing process of various navigation sensors is also constantly improving, and inertial measurement units that integrate inertial sensors have been widely used in the field of navigation systems. SINS has good navigation accuracy in the short term, but as time accumulates, its errors will gradually diverge, making it difficult for the positioning results to meet the actual mission requirements. GPS can provide accurate position and speed information all day and night, but the signal is easily interfered with, and may even cause the satellite signal to lose lock. In highly dynamic and complex environments, relying solely on GPS positioning often leads to temporary navigation unavailability. The INS / GPS combined navigation system can significantly improve navigation accuracy and reliability by combining the advantages of the two navigation technologies. Compared with a single navigation system, combined navigation can effectively reduce errors and uncertainties in the face of environmental interference, signal obstruction or sensor failure, thereby achieving more stable positioning and heading estimation. Therefore, the combined navigation system has become the mainstream choice in modern navigation systems.

[0003] Traditional INS / GPS combined navigation methods usually use Kalman filtering as a data fusion method. In highly dynamic and complex environments, the state information estimated by Kalman filtering is not smooth and Kalman filtering is only applicable to discrete linear systems. If continuous-time problems are to be processed, discretization must be performed. Kalman-Bucy filter is an important algorithm for processing state estimation in continuous-time linear systems. It can filter the intrinsic signal of the system in the presence of Gaussian white noise in the system, thereby obtaining an estimated value close to the real signal, and Kalman-Bucy filter provides a smoother state estimate by updating the state estimate. Therefore, the present invention proposes a loosely coupled combined navigation method of Kalman-Bucy filter. Summary of the invention

[0004] The object of the present invention is to provide a loosely coupled integrated navigation method of a Kalman-Bucy filter to realize SINS / GPS loosely coupled integrated navigation.

[0005] The technical solution to achieve the purpose of the present invention is: a loosely coupled integrated navigation method of a Kalman-Bucy filter, the specific steps are as follows:

[0006] S1: define the coordinate system of the integrated navigation system;

[0007] S2: Establish IMU measurement error model;

[0008] S3: Establish the strapdown inertial navigation error model;

[0009] S4: Establish integrated navigation system model;

[0010] S5: Establish Kalman-Bucy filter model;

[0011] S6: Navigation parameter correction.

[0012] Compared with the prior art, the present invention has the following significant advantages:

[0013] The present invention realizes the loosely coupled combined navigation by fusing GPS measurement information with SINS solution information using Kalman-Bucy filter. Kalman-Bucy filter is an important algorithm for processing state estimation in continuous time linear system and can provide smoother state estimation, so there is no need to discretize the system model, and the solution can be performed directly, and the navigation data obtained by the solution is smoother. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 : Schematic diagram of the flow chart of the loosely coupled combined navigation method in the present invention. DETAILED DESCRIPTION

[0015] Combined with Figure 1 , further explain the technical solution of the present invention. If no specific technology or conditions are specified in the examples, the technology or conditions described in the literature in the field or the product instructions shall be followed.

[0016] The invention provides a loosely coupled integrated navigation method of a Kalman-Bucy filter. The method comprises: establishing a complete system framework, realizing a system architecture for loosely coupled integrated navigation by fusing GPS measurement information with SINS (strapdown inertial navigation system) solution information using a Kalman-Bucy filter; the method first performs SINS solution according to initial navigation information and specific force and angular velocity information output by an IMU (inertial measurement unit), and then obtains the observed quantity of the Kalman-Bucy filter according to SINS solution data and GPS observation data; secondly, the system state is estimated by using a Kalman-Bucy filter according to the initial state and initial covariance; finally, the SINS solution value is corrected by using the state estimation value of the Kalman-Bucy filter, and finally the position, speed and attitude information of the corrected carrier are obtained.

[0017] Figure 1 The flowchart of the loosely coupled combined navigation method of the present invention is shown in FIG. The navigation method comprises the following steps.

[0018] S1: Define the coordinate system of the integrated navigation system:

[0019] S1.1: Define the carrier coordinate system: the origin of the coordinate system is the center of mass of the aircraft, x b The axis is along the carrier's horizontal axis to the right, y b The axis moves forward along the longitudinal axis of the carrier, z b The axis is upward along the vertical axis of the carrier, which is the "upper right front" system, recorded as system B.

[0020] S1.2: Define the navigation coordinate system: the origin of the coordinate system is at the center of mass of the carrier, x n The axis points eastward along the reference circle, y n The axis points north along the meridian of the reference ellipsoid, z n The axis points in the opposite direction of the center of the earth along the direction of the outer normal of the reference ellipsoid. This is the "northeastern sky" system, recorded as n system.

[0021] S1.3: Define the geocentric inertial coordinate system: This coordinate system does not rotate with the Earth, its origin is at the center of the Earth, and x i The axis points to the vernal equinox within the equator, z i The axis is along the Earth's rotation axis, y i Axis and x i Axis, z i The axes form a right-hand system, denoted as the i system;

[0022] S1.4: Define the Earth coordinate system: This coordinate system is fixed to the Earth, with its origin at the center of the Earth, and x e Axis, y e The axis lies in the equatorial plane, x e The axis points to the prime meridian, z e The axis is the earth's rotation axis, denoted as e-system;

[0023] S2: Establish IMU (Inertial Measurement Unit) measurement error model:

[0024] IMU measurement errors mainly include installation error, scale error and random error, etc. These three errors are usually considered as colored noise. This paper considers these errors as random errors, simplifies the measurement error model of gyroscopes and accelerometers, only considers constant drift and white noise, and considers that the error models of the three axes of gyroscopes and accelerometers are the same. The measurement error model of gyroscopes and accelerometers is as follows:

[0025] S2.1: Build a gyroscope measurement error model

[0026]

[0027] In the formula, is the angular velocity output by the gyroscope after adding the error, is the angular velocity output by the error-free gyroscope, εb is the gyroscope measurement error, ε b is the gyroscope measurement constant drift, ω g Output white noise for the gyroscope.

[0028] S2.2: Build an accelerometer measurement error model

[0029]

[0030] In the formula, is the specific force output by the accelerometer after adding the error, f b is the specific force output by the error-free accelerometer, is the accelerometer measurement error, is the accelerometer measurement constant bias, ω a Output white noise for the accelerometer.

[0031] S3: Establishing the Strapdown Inertial Navigation Error Model

[0032] The strapdown inertial navigation error equation is the basis for studying the propagation law of inertial navigation errors and performing integrated navigation. The following are the attitude, velocity and position error equations.

[0033] S3.1: Attitude error equation:

[0034]

[0035] Formula (18) is the attitude error equation. In the formula, represents the first-order derivative of the misalignment angle φ with respect to time t; δv n is the velocity error in the navigation coordinate system; δp is the position error; represents the rotation matrix from b to n; M aa is the coefficient matrix of φ in the attitude error equation; M av is δv in the attitude error equation n The coefficient matrix of ap is the coefficient matrix of δp in the attitude error equation;

[0036]

[0037] In the formula, represents the projection of the rotational angular velocity of system n relative to system i on system n, and × represents the cross product;

[0038]

[0039] In the formula, L represents latitude; h represents altitude; R M , R N They are respectively the principal curvature radius of the earth's meridian circle at the carrier and the principal curvature radius of the earth's meridional circle at the carrier.

[0040]

[0041] In the formula, ω ie is the angular velocity of the Earth's rotation; v n 、v e are the northward and eastward velocities of the carrier, respectively;

[0042] S3.2: Speed ​​error equation

[0043]

[0044] Formula (22) is the velocity error equation. In the formula, Indicates δv n The first derivative with respect to time t; M va is the coefficient matrix of φ in the velocity error equation; M vv is the velocity error equation δv n The coefficient matrix of vp is the coefficient matrix of δp in the velocity error equation;

[0045] M va =(f n ×) (23)

[0046] In the formula, f n It represents the specific force under n system;

[0047]

[0048] In the formula, v n Represents the velocity of the carrier in the navigation coordinate system, that is, v n =[v e ,v n ,v u ] T , where v u Indicates the celestial velocity of the carrier; is the projection of the Earth's rotation angular velocity in the n system; It represents the projection of the rotation angular velocity of the n system relative to the e system on the n system;

[0049]

[0050] S3.3: Position error equation

[0051]

[0052] Formula (26) is the position error equation. In the formula, is the first-order derivative of δp with respect to time t, M pv is the position error equation δv n The coefficient matrix of ppis the coefficient matrix of δp in the position error equation;

[0053]

[0054] S4: Establish integrated navigation system model:

[0055] Based on the strapdown inertial navigation error model in Section S3, a loosely coupled integrated navigation system model can be obtained with velocity and position as measurement information, and misalignment angle, velocity error, position error, gyroscope measurement constant drift, and accelerometer measurement constant zero bias as state quantities. The integrated navigation system model is mainly composed of state equations and measurement equations, and the specific formula is as follows:

[0056] The state vector X selected by the present invention is:

[0057]

[0058] Where φ e ,φ n ,φ u are the east misalignment angle, north misalignment angle and celestial misalignment angle respectively, that is, φ=[φ e ,φ n ,φ u ] T ; δv e ,δv n ,δv u They are respectively the eastward velocity error, the northward velocity error and the celestial velocity error, that is, δv n =[δv e ,δv n ,δv u ] T ; δL, δλ, δh are latitude error, longitude error and height error respectively, that is, δp = [δL, δλ, δh] T ; ε x ,ε y ,ε z is the constant drift of the gyroscope measurement in the x, y, and z axis directions in the carrier coordinate system, that is, ε b =[ε x ,ε y ,ε z ] T ; is the constant zero bias of the accelerometer in the x, y, and z axis directions in the carrier coordinate system, that is,

[0059] S4.1: Establish the state equation of the integrated navigation system;

[0060] The state equation of the integrated navigation system is obtained based on the strapdown inertial navigation error model in Section S3.

[0061] The state equation of the integrated navigation system is:

[0062]

[0063] In the formula, represents the first-order derivative of the state vector X with respect to time t; F is the state transfer matrix, G is the noise driving matrix, both of which are deterministic time-varying matrices about the time parameter t; W is the process noise;

[0064] The state transfer matrix F is:

[0065]

[0066] Where, 0 3×3 represents a zero matrix of 3 rows and 3 columns;

[0067] The noise driving matrix G is:

[0068]

[0069] Where, 0 9×3 represents a zero matrix of 9 rows and 3 columns;

[0070] The process noise W is:

[0071] W=[ω gx ω gy ω gz ω ax ω ay ω az ] T (32)

[0072] In the formula, ω gx ,ω gy ,ω gz They represent the gyroscope output white noise in the x direction, the gyroscope output white noise in the y direction, and the gyroscope output white noise in the z direction in the carrier coordinate system, that is, ω g =[ω gx ,ω gy ,ω gz ] T ;ω ax ,ω ay ,ω az They represent the accelerometer output white noise in the x direction, the y direction, and the z direction in the carrier coordinate system, that is, ω a =[ω ax ,ω ay ,ω az ] T .

[0073] S4.2 Establishing the measurement equation

[0074] The measurement equation is obtained by subtracting the position and velocity information obtained by SINS and the position and velocity information obtained by GPS.

[0075] The measurement equation is:

[0076] Z=HX+V (33)

[0077] Where Z is the measurement vector; H is the measurement matrix; V is the measurement noise.

[0078] The calculation formula of the measurement vector is:

[0079]

[0080] Where p represents the position obtained by inertial navigation, that is, p = [L λ h] T , where λ represents latitude; v GPS Indicates the speed calculated by GPS; p GPS Indicates the location obtained by GPS.

[0081] The measurement matrix H is:

[0082]

[0083] In the formula, I 3×3 represents the third-order unit matrix, 0 3×6 Represents a zero-zero matrix with 3 rows and 6 columns.

[0084] The measurement noise V is:

[0085]

[0086] Where V v and V p They are the GPS receiver velocity measurement white noise and position measurement white noise respectively.

[0087] S5: Establish Kalman-Bucy filter model:

[0088] The Kalman-Bucy filter consists of two differential equations:

[0089]

[0090] Formula (37) is used for state estimation, represents the estimated value of the state vector X at time t, express The first-order derivative with respect to time; F(t) represents the state transfer matrix at time t; K(t) represents the Kalman filter gain at time t; Z(t) represents the measurement vector at time t; H(t) represents the measurement matrix at time t; Formula (38) is used for covariance calculation, and P(t) represents the covariance at time t; represents the first-order derivative of P(t) with respect to time; G(t) represents the noise driving matrix at time t; Q(t) is the variance matrix of process noise at time t; R(t) is the variance matrix of measurement noise at time t.

[0091] The Kalman filter gain at time t can be expressed as:

[0092] K(t)=P(t)H T (t)R -1 (t) (39)

[0093] Equations (37) to (39) together constitute the Kalman-Bucy filter.

[0094] The Kalman-Bucy filter is an important algorithm for state estimation in continuous-time linear systems. Because the object of the processing is a continuous-time system, it can provide smoother state estimation than the Kalman filter and does not need to discretize the integrated navigation system model. According to the Kalman-Bucy filter, as long as the estimated value of the initial state vector at time t0 is given in advance And covariance P(t0), and according to the measurement vector Z(t) at time t, the estimated value of the state vector at time t can be obtained recursively according to the above equation

[0095] S6: Navigation parameter correction:

[0096] The attitude, velocity and position calculated by SINS will accumulate over time, resulting in gradually increasing errors. The Kalman-Bucy filter is used to fuse the SINS information and GPS information to output the estimated value of the state vector. The estimated value is used to correct the attitude, velocity and position calculated by SINS. The corrected attitude, velocity and position have higher accuracy. The attitude, velocity and position correction formulas are as follows:

[0097] S6.1: The posture correction formula is:

[0098]

[0099] In the formula, is the rotation matrix from b system to n system after correction; I represents the unit matrix.

[0100] S6.2: The speed correction formula is:

[0101]

[0102] In the formula, are the corrected eastward, northward and celestial velocities of the carrier.

[0103] S6.3: Position correction is:

[0104]

[0105] In the formula, are the corrected latitude, longitude and altitude.

[0106] Although the embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are exemplary and are not to be construed as limitations on the present invention. A person skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention without departing from the principles and intent of the present invention.

Claims

1. A loosely coupled integrated navigation method of a Kalman-Bucy filter, characterized in that: The specific steps are as follows: S1: define the coordinate system of the integrated navigation system; S2: Establish IMU measurement error model; S3: Establish the strapdown inertial navigation error model; S4: Establish integrated navigation system model; S5: Establish Kalman-Bucy filter model; S6: Navigation parameter correction.

2. The loosely coupled integrated navigation method according to claim 1, characterized in that: The coordinate system in S1 is defined as follows: S1.1: Define the carrier coordinate system: the origin of the coordinate system is the center of mass of the aircraft, x b The axis is along the carrier's horizontal axis to the right, y b The axis moves forward along the longitudinal axis of the carrier, z b The axis is upward along the vertical axis of the carrier, which is the "right front upper" system, recorded as system b; S1.2: Define the navigation coordinate system: the origin of the coordinate system is at the center of mass of the carrier, x n The axis points eastward along the reference circle, y n The axis points north along the meridian of the reference ellipsoid, z n The axis points in the opposite direction of the center of the earth along the outer normal of the reference ellipsoid. This is the "northeastern" system, recorded as n system; S1.3: Define the geocentric inertial coordinate system: This coordinate system does not rotate with the Earth, its origin is at the center of the Earth, and x i The axis points to the vernal equinox within the equator, z i The axis is along the Earth's rotation axis, y i Axis and x i Axis, z i The axes form a right-hand system, denoted as the i system; S1.4: Define the Earth coordinate system: This coordinate system is fixed to the Earth, with its origin at the center of the Earth, and x e Axis, y e The axis lies in the equatorial plane, x e The axis points to the prime meridian, z e The axis is the earth's rotation axis, denoted as the e system.

3. The loosely coupled integrated navigation method according to claim 1, characterized in that: The specific steps in S2 are: S2.1: Establish gyroscope measurement error model: In the formula, is the angular velocity output by the gyroscope after adding the error, is the angular velocity output by the error-free gyroscope, ε b is the gyroscope measurement error, ε b is the gyroscope measurement constant drift, ω g Output white noise for the gyroscope; S2.2: Establish accelerometer measurement error model: In the formula, is the specific force output by the accelerometer after adding the error, f b is the specific force output by the error-free accelerometer, is the accelerometer measurement error, is the accelerometer measurement constant bias, ω a Output white noise for the accelerometer.

4. The loosely coupled integrated navigation method according to claim 1, characterized in that: The specific steps in S3 are: S3.1: Attitude error equation: In the formula, represents the first-order derivative of the misalignment angle φ with respect to time t; δv n is the velocity error in the navigation coordinate system; δp is the position error; represents the rotation matrix from b to n; M aa is the coefficient matrix of φ in the attitude error equation; M av is δv in the attitude error equation n The coefficient matrix of ap is the coefficient matrix of δp in the attitude error equation; S3.2: Speed ​​error equation: In the formula, Indicates δv n The first derivative with respect to time t; M va is the coefficient matrix of φ in the velocity error equation; M vv is the velocity error equation δv n The coefficient matrix of vp is the coefficient matrix of δp in the velocity error equation; S3.3: Position error equation: In the formula, is the first-order derivative of δp with respect to time t, M pv is the position error equation δv n The coefficient matrix of pp is the coefficient matrix of δp in the position error equation.

5. The loosely coupled integrated navigation method according to claim 4, characterized in that: The specific steps in S4 are: S4.1: Establish the state equation of the integrated navigation system: Where X is the state vector, represents the first-order derivative of the state vector X with respect to time t; F is the state transfer matrix, G is the noise driving matrix, both are deterministic time-varying matrices about the time parameter t; W is the process noise; S4.2: Establish the measurement equation: The measurement vector equation is: Z=HX+V (7) Where Z is the measurement vector; H is the measurement matrix; V is the measurement noise.

6. The loosely coupled integrated navigation method according to claim 5, characterized in that: The calculation formula of the measurement vector Z is: In the formula, v n represents the velocity of the carrier in the navigation coordinate system, and p represents the position obtained by inertial navigation, that is, p = [Lλh] T , where L represents latitude; h represents height, λ represents latitude; v GPS Indicates the speed calculated by GPS; p GPS Indicates the location obtained by GPS.

7. The loosely coupled integrated navigation method according to claim 1, characterized in that: In S5, the Kalman-Bucy filter consists of two differential equations: In the formula, represents the estimated value of the state vector X at time t, express The first-order derivative with respect to time; F(t) represents the state transfer matrix at time t; K(t) represents the Kalman filter gain at time t; Z(t) represents the measurement vector at time t; H(t) represents the measurement matrix at time t; Formula (35) is used for covariance calculation, and P(t) represents the covariance at time t; represents the first-order derivative of P(t) with respect to time; G(t) represents the noise driving matrix at time t; Q(t) is the variance matrix of process noise at time t; R(t) is the variance matrix of measurement noise at time t; The Kalman filter gain at time t can be expressed as: K(t)=P(t)H T (t)R -1 (t) (11)。 8. The loosely coupled integrated navigation method according to claim 6, characterized in that: In S5, according to the Kalman-Bucy filter, as long as the estimated value of the state vector at the initial time t0 is given in advance And covariance P(t0), and according to the measurement vector Z(t) at time t, the estimated value of the state vector at time t can be obtained recursively according to the above equation 9. The loosely coupled integrated navigation method according to claim 1, characterized in that: In S6, The posture correction formula is: In the formula, is the rotation matrix from b system to n system after correction; I represents the unit matrix; Represents the rotation matrix from b system to n system; The speed correction formula is: In the formula, v e 、v n 、v u are the eastward speed, northward speed and celestial speed of the carrier; are the corrected eastward velocity, northward velocity, and celestial velocity; δv e ,δv n ,δv u They are eastward velocity error, northward velocity error and celestial velocity error respectively; The position correction formula is: Where L, λ, and h are latitude, longitude, and altitude. are the corrected latitude, longitude and altitude, δL, δλ, δh are the latitude error, longitude error and altitude error respectively.

10. The loosely coupled integrated navigation method according to claim 5, characterized in that: In S4.1, The state transfer matrix F is: Where M aa , M av , M ap , M va , M vv , M vp , M pv , M pp , 0 3×3 represents the third-order zero matrix; C b n Represents the rotation matrix from b system to n system, 0 3×3 Represents a zero matrix with 3 rows and 3 columns.

Images