Cooperative positioning method and system based on GPS / INS and RS / MO multi-source data fusion

Abstract

The invention discloses a cooperative positioning method and system based on GPS / INS and RS / MO multi-source data fusion. The method comprises the steps that S1, GPS data and INS data in the vehicle navigation process are acquired; s2, fusing the GPS data and the INS data by using an adaptive Kalman filtering algorithm to obtain navigation collaborative data; s3, acquiring RS data and MO data in a vehicle navigation process, and fusing the RS data and the MO data to obtain optimized collaborative data; s4, adjusting the multi-target collaborative navigation based on the optimized collaborative data to obtain adjusted data; s5, performing state monitoring on the GPS, and performing positioning compensation by using the GPS / INS loose coupling integrated system when the GPS is interrupted; and step S6, realizing a multi-target collaborative navigation positioning result based on the positioning compensation and the adjusted data.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-source data fusion and robot positioning, and in particular to a collaborative positioning method based on multi-source data fusion of GPS / INS and RS / MO. Background Art

[0002] GPS (Global Navigation System) and INS (Inertial Navigation System) are two major navigation and positioning technologies. In most cases, ordinary GPS systems can provide real-time positioning information. However, since the general GPS measurement rate is usually only 5hz, even in a positive environment, if differential processing is not performed, the error range will reach 2m or even larger. In addition, GPS signals are easily lost in practical applications. Therefore, it is not suitable for high-precision and high-stability navigation and positioning processes such as unmanned vehicles, drones, and underwater robot navigation. On the other hand, INS sensors have the problem of rapid decline in accuracy over time. Therefore, INS positioning needs to be combined with additional navigation and positioning technologies for regular accuracy correction. More importantly, when the navigation target performs navigation tasks under specific circumstances, the GPS signal may be subject to long-term interference or even completely fail. But at the same time, the INS signal cannot provide long-term high-precision positioning information for the target. Therefore, another reliable positioning technology is needed to replace GPS for real-time positioning correction. Because sometimes it is multi-target collaborative navigation, it means that different navigation targets can share their observation and navigation data in real time. Therefore, the establishment of MO (Mutual Navigation) and collaborative navigation networks can not only help correct the real-time positioning of INS, but also improve the mutual perception ability of each collaborative target and improve the collaborative efficiency of the target. At the same time, RS (Remote Sensing) technology has great advantages in measuring the relative positions of all navigation targets, which can be used to correct the relative distance error of GPS / INS positioning data. In addition, RS images can provide real-time navigation environment information for navigation targets, such as road accessibility and the situation of collaborative targets.

[0003] GPS can provide basic positioning services for navigation targets. Real-time corrected INS data can reduce the random errors of GPS data. RS data can control the overall error of cooperative navigation targets. MO data improves the accuracy of RS data and controls the relative position accuracy of cooperative navigation targets. Since the random errors of GPS data approximately follow Gaussian distribution, the sum of the vector errors of target observation coordinates in GPS / INS navigation is approximately zero. The sum of the vector errors of the mutual distances between targets in RS / MO navigation is also approximately zero.

[0004] Wire and cable detection technology is a key factor in ensuring its stability and safety. At present, this technology has attracted the attention of power-related personnel. As the use time of wires and cables continues to increase, the probability of problems increases. Adverse problems in the application of wires and cables may cause large-scale power outages, and even cause serious accidents such as fires and electric shock casualties. Therefore, it is necessary to implement scientific detection of wires and cables to maximize the safety of wire and cable operation. Summary of the invention

[0005] In order to solve the above technical problems, the present invention provides a collaborative positioning method based on GPS / INS and RS / MO multi-source data fusion, the method comprising:

[0006] Step S1, obtaining GPS data and INS data during vehicle navigation;

[0007] Step S2: using an adaptive Kalman filter algorithm to fuse the GPS data with the INS data to obtain navigation collaboration data;

[0008] Step S3, obtaining RS data and MO data during vehicle navigation and fusing them to obtain optimized collaborative data;

[0009] Step S4: adjusting the multi-target collaborative navigation based on the optimized collaborative data to obtain adjusted data;

[0010] Step S5: Based on the adjusted data, the GPS status is monitored, and when the GPS is interrupted, the GPS / INS loosely coupled integrated system is used for positioning compensation;

[0011] Step S6: Implement multi-target collaborative navigation positioning results based on the positioning compensation and the adjusted data.

[0012] Optionally, in step S2, the process of fusing the GPS data with the INS data using an adaptive Kalman filter algorithm to obtain navigation collaborative data specifically includes:

[0013] S21, fusing the pseudorange and pseudorange rate of the GPS data and the INS data, and constructing a state equation based on the fusion result;

[0014] S22, constructing an observation equation based on the pseudo-range difference and pseudo-range rate difference of the GPS data and the INS data;

[0015] S23. Based on the state equation and the observation equation, use an adaptive Kalman filter algorithm to update the measurement noise variance and the system noise variance, and use the updated data as navigation cooperation data.

[0016] Optionally, in S21, the process of fusing the pseudorange and pseudorange rate of the GPS data and the INS data, and constructing the state equation based on the fusion result specifically includes:

[0017] The GPS / INS state variable of the filter contains the error vector of INS data and GPS data, expressed as:

[0018] X ig (t) = [X i (t) X g (t)]

[0019] Among them, X i (t) represents the INS error state variables, including platform error angle, velocity error, position error, gyroscope random drift, and acceleration deviation; X g (t) is the state variable of GPS error, which consists of the distance error caused by GPS clock error and the velocity error caused by GPS clock frequency error;

[0020] The state equation of the GPS / INS integrated navigation receiver is:

[0021]

[0022] In the formula, F i (t) and F g (t) represents the state transfer matrix, G i (t) and W g (t) are the system noise matrices of INS and GPS respectively.

[0023] Optionally, in S22, the content of constructing the observation equation based on the pseudo-range difference and pseudo-range rate difference of the GPS data and the INS data specifically includes:

[0024] Calculating the pseudorange and pseudorange rate between the INS and the GPS satellite based on the ephemeris data in the GPS data and the target motion parameters in the INS data;

[0025] The GPS / INS pseudorange difference and pseudorange rate difference are input into the filter as observation values;

[0026] Input the initial parameters into the linearized and discretized observation equation to drive the filter and perform GPS / INS error correction through iterative calculation;

[0027] The pseudo distance between the target and the GPS satellite can be expressed as:

[0028]

[0029] Among them, v pkis the measurement noise caused by multipath effect, tropospheric error and ionospheric error; ρ k is the distance k between the target and the GPS satellite; ζ n is the error introduced in pseudo-range measurement, is the x-axis coordinate of GPS satellite k in the earth reference system, is the y-axis coordinate of GPS satellite k in the earth reference system, is the z-axis coordinate of GPS satellite k in the earth reference system;

[0030] The pseudo-range between target i and GPS satellite k obtained from INS data can be expressed as:

[0031]

[0032] Among them, (x i ,y i ,z i ) represents the coordinates of the target in the Earth reference system, calculated from INS data; (ζ x ,ζ y ,ζ z ) is the position error of the target, then:

[0033] x i =x+ζ x x i =x+ζ x x i =x+ζ x

[0034] The Taylor expansion at (x, y, z) is:

[0035]

[0036] Among them, ρ is the pseudo-range value when the coordinates of target i are (x, y, z), is the cosine value of the x, y, and z coordinate axes to the satellite observation direction, which can be set to

[0037] The measured value of the pseudorange difference can be expressed as:

[0038]

[0039] From the above formula, we can get the pseudo-range measurement equation of GPS / INS navigation system:

[0040]

[0041] When the GPS receiver is able to receive Doppler observations, the Doppler observations can be used for pseudorange rate difference measurement; the pseudorange rate between the target and satellite k measured by GPS can be expressed as:

[0042]

[0043] The pseudorange rate of the INS module can be expressed as:

[0044]

[0045] The measured value of the pseudorange rate difference can be expressed as:

[0046]

[0047] From the above formula, the pseudo-range rate difference measurement equation of GPS / INS navigation system can be obtained:

[0048]

[0049] in, is the state matrix at time t, X(t) is the INS error state variable at time t, is the measurement noise at time t.

[0050] Optionally, in step S5, the construction process of the GPS / INS loosely coupled integrated system specifically includes:

[0051] The state Kalman filter algorithm is used to integrate GPS / INS. The process model and observation model are:

[0052]

[0053] Among them, X is the state vector, F is the coefficient matrix, G is the system noise matrix, W is the process noise vector, Z is the observation vector, H is the observation matrix, V is the observation noise vector, and the observation vector Z represents the difference between the INS position and the GPS position;

[0054] The state vector X is set to:

[0055]

[0056] Among them, E、N、U To calculate whether the platform's misalignment angle in the local geographic frame is g, is the velocity error of the three axes of the g frame, and represents the position error, and ε x,y,z denote the accelerometer bias and gyroscope bias on the three axes of the body frame b, respectively;

[0057] The system matrix F is constructed according to the INS error equation:

[0058]

[0059] Among them, and are the Earth velocity vector and its error, and is the angular rate vector from the g frame to the inertial frame and its error;

[0060] Expressing the equation in discrete-time form:

[0061] X k =φ k,k-1 X k-1 +G K W K

[0062] Z K =H K X K +V K

[0063] The prediction and update process of Kalman filter is as follows:

[0064] Predicted State Estimates:

[0065]

[0066] Prediction covariance:

[0067]

[0068] Kalman gain:

[0069]

[0070] Update the state estimate:

[0071]

[0072] Update the covariance:

[0073] P K =[IK K H K ]P K,K-1

[0074] Among them, Φ K,K-1 is the system state transfer matrix from time k-1 to k, is the predicted state estimate, P K,K-1 is the estimated covariance of the forecast, K K is the Kalman gain matrix, is the updated state estimate, PK is the updated estimated covariance; Q K-1 and R K are the variance-covariance matrices of the state and observation, respectively, through and calculate.

[0075] The present invention also discloses a collaborative positioning system based on GPS / INS and RS / MO multi-source data fusion, the system comprising: a data acquisition module, a GPS / INS fusion module, an RS / MO fusion module, a navigation collaboration module, a GPS compensation module and a navigation positioning module;

[0076] The data acquisition module is used to acquire GPS data and INS data during vehicle navigation;

[0077] The GPS / INS fusion module is used to fuse the GPS data and the INS data using an adaptive Kalman filter algorithm to obtain navigation collaborative data;

[0078] The RS / MO fusion module is used to obtain RS data and MO data during vehicle navigation and then fuse them to obtain optimized collaborative data;

[0079] The navigation cooperation module adjusts the multi-target cooperative navigation based on the optimized cooperation data to obtain adjusted data;

[0080] The GPS compensation module is used to monitor the status of GPS and use the GPS / INS loosely coupled integrated system to perform positioning compensation when GPS is interrupted;

[0081] The navigation and positioning module is used to achieve collaborative navigation and positioning results for multiple targets based on the positioning compensation and the adjusted data.

[0082] Optionally, the GPS / INS fusion module further includes: a state equation construction submodule, an observation equation construction module and a collaborative data submodule;

[0083] The state equation construction submodule is used to fuse the pseudorange and pseudorange rate of the GPS data and the INS data, and construct a state equation based on the fusion result;

[0084] The observation equation construction submodule is used to construct an observation equation based on the pseudorange difference and pseudorange rate difference of the GPS data and the INS data;

[0085] The collaborative data submodule is used to update the measurement noise variance and the system noise variance based on the state equation and the observation equation using an adaptive Kalman filter algorithm, and use the updated data as navigation collaborative data.

[0086] Optionally, the workflow of the state equation construction submodule specifically includes:

[0087] The GPS / INS state variable of the filter contains the error vector of INS data and GPS data, expressed as:

[0088] X ig (t) = [X i (t)X g (t)]

[0089] Among them, X i (t) represents the INS error state variables, including platform error angle, velocity error, position error, gyroscope random drift, and acceleration deviation; X g (t) is the state variable of GPS error, which consists of the distance error caused by GPS clock error and the velocity error caused by GPS clock frequency error;

[0090] The state equation of the GPS / INS integrated navigation receiver is:

[0091]

[0092] In the formula, F i (t) and F g (t) represents the state transfer matrix, G i (t) and W g (t) are the system noise matrices of INS and GPS respectively.

[0093] Optionally, the workflow of the observation equation construction submodule specifically includes:

[0094] Calculating the pseudorange and pseudorange rate between the INS and the GPS satellite based on the ephemeris data in the GPS data and the target motion parameters in the INS data;

[0095] The GPS / INS pseudorange difference and pseudorange rate difference are input into the filter as observation values;

[0096] Input the initial parameters into the linearized and discretized observation equation to drive the filter and perform GPS / INS error correction through iterative calculation;

[0097] The pseudo distance between the target and the GPS satellite can be expressed as:

[0098]

[0099] Among them, v pk is the measurement noise caused by multipath effect, tropospheric error and ionospheric error; ρ k is the distance k between the target and the GPS satellite; ζn is the error introduced in pseudo-range measurement, is the x-axis coordinate of GPS satellite k in the earth reference system, is the y-axis coordinate of GPS satellite k in the earth reference system, is the z-axis coordinate of GPS satellite k in the earth reference system;

[0100] The pseudo-range between target i and GPS satellite k obtained from INS data can be expressed as:

[0101]

[0102] Among them, (x i ,y i ,z i ) represents the coordinates of the target in the Earth reference system, calculated from INS data; (ζ x ,ζ y ,ζ z ) is the position error of the target, then:

[0103] x i =x+ζ x x i =x+ζ x x i =x+ζ x

[0104] The Taylor expansion at (x, y, z) is:

[0105]

[0106] Among them, ρ is the pseudo-range value when the coordinates of target i are (x, y, z), is the cosine value of the x, y, and z coordinate axes to the satellite observation direction, which can be set to

[0107] The measured value of the pseudorange difference can be expressed as:

[0108]

[0109] From the above formula, we can get the pseudo-range measurement equation of GPS / INS navigation system:

[0110]

[0111] When the GPS receiver is able to receive Doppler observations, the Doppler observations can be used for pseudorange rate difference measurement; the pseudorange rate between the target and satellite k measured by GPS can be expressed as:

[0112]

[0113] The pseudorange rate of the INS module can be expressed as:

[0114]

[0115] The measured value of the pseudorange rate difference can be expressed as:

[0116]

[0117] From the above formula, the pseudo-range rate difference measurement equation of GPS / INS navigation system can be obtained:

[0118]

[0119] in, is the state matrix at time t, X(t) is the INS error state variable at time t, is the measurement noise at time t.

[0120] Optionally, in the GPS compensation module, the construction process of the GPS / INS loosely coupled integrated system specifically includes:

[0121] The state Kalman filter algorithm is used to integrate GPS / INS. The process model and observation model are:

[0122]

[0123] Among them, X is the state vector, F is the coefficient matrix, G is the system noise matrix, W is the process noise vector, Z is the observation vector, H is the observation matrix, V is the observation noise vector, and the observation vector Z represents the difference between the INS position and the GPS position;

[0124] The state vector X is set to:

[0125]

[0126] Among them, E、N、U To calculate whether the platform's misalignment angle in the local geographic frame is g, is the velocity error of the three axes of the g frame, and represents the position error, and ε x,y,z denote the accelerometer bias and gyroscope bias on the three axes of the body frame b, respectively;

[0127] The system matrix F is constructed according to the INS error equation:

[0128]

[0129] Among them, and are the Earth velocity vector and its error, and is the angular rate vector from the g frame to the inertial frame and its error;

[0130] Expressing the equation in discrete-time form:

[0131] X k =φ k,k-1 X k-1 +G K W K

[0132] Z K =H K X K +V K

[0133] The prediction and update process of Kalman filter is as follows:

[0134] Predicted State Estimates:

[0135]

[0136] Prediction covariance:

[0137]

[0138] Kalman gain:

[0139]

[0140] Update the state estimate:

[0141]

[0142] Update the covariance:

[0143] P K =[IK K H K ]P K,K-1

[0144] Among them, Φ K,K-1 is the system state transfer matrix from time k-1 to k, is the predicted state estimate, P K,K-1 is the estimated covariance of the forecast, K K is the Kalman gain matrix, is the updated state estimate, P K is the updated estimated covariance; Q K-1 and R K are the variance-covariance matrices of the state and observation, respectively, through and calculate.

[0145] Compared with the prior art, the present invention has the following beneficial effects:

[0146] The present invention improves the stability and accuracy of the unmanned vehicle navigation service through adaptive Kalman filter data fusion, observation target range table matching data fusion, and data joint adjustment. BRIEF DESCRIPTION OF THE DRAWINGS

[0147] In order to more clearly illustrate the technical solution of the present invention, the following briefly introduces the drawings required for use in the embodiments. 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 these drawings without paying creative labor.

[0148] Figure 1 A method step diagram of a collaborative positioning method for multi-source data fusion according to an embodiment of the present invention;

[0149] Figure 2 The present invention is a flowchart of a method for obtaining navigation cooperation data according to an embodiment of the present invention. DETAILED DESCRIPTION

[0150] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0151] Embodiment 1

[0152] A collaborative positioning method based on GPS / INS and RS / MO multi-source data fusion, such as Figure 1 As shown, the method includes:

[0153] Step S1, obtaining GPS data and INS data during vehicle navigation.

[0154] Step S2: Use an adaptive Kalman filter algorithm to fuse the GPS data and the INS data to obtain navigation collaboration data.

[0155] Step S2: Use an adaptive Kalman filter algorithm to fuse the GPS data with the INS data to obtain navigation cooperation data, such as Figure 2 shown.

[0156] Step 2.1: Establish the equation of state

[0157] Adaptive Kalman filter algorithm is used to fuse the pseudorange and pseudorange rate of INS module and GPS module. The GPS / INS state variable of the filter contains all the error vectors of INS module and GPS module, which can be expressed as:

[0158] X ig(t) = [X i (t) X g (t)]

[0159] In the formula, X i (t) represents the INS error state variables, including platform error angle, velocity error, position error, gyroscope random drift, and acceleration deviation; X g (t) is the state variable of GPS error, which consists of the distance error caused by GPS clock error and the velocity error caused by GPS clock frequency error.

[0160] The state equation of the GPS / INS integrated navigation receiver is:

[0161]

[0162] In the formula, F i (t) and F g (t) represents the state transfer matrix, G i (t) and W g (t) are the system noise matrices of INS and GPS respectively.

[0163] Step 2.2: Establish the observation equation

[0164] The observation equation uses pseudorange as the observation value. First, the pseudorange and pseudorange rate between the INS module and the GPS satellite are calculated based on the ephemeris data provided by the GPS module and the target motion parameters given by the INS module; then the GPS / INS pseudorange difference and pseudorange rate difference are input into the filter as observation values.

[0165] Finally, the initial parameters are input into the linearized and discretized observation equation to drive the filter, and the GPS / INS error is corrected through iterative calculation.

[0166] The pseudo distance between the target and the GPS satellite can be expressed as:

[0167]

[0168] In the formula, v pk is the measurement noise caused by multipath effect, tropospheric error and ionospheric error. k is the distance k between the target and the GPS satellite; ζ n is the error introduced in the pseudo-range measurement.

[0169] The pseudo distance between target i and GPS satellite k obtained from INS data can be expressed as:

[0170]

[0171] In the formula, (x i,y i ,z i ) represents the coordinates of the target in the Earth reference system, calculated from INS data.

[0172] (ζ x ,ζ y ,ζ z ) is the position error of the target, then:

[0173] x i =x+ζ x x i =x+ζ x x i =x+ζ x

[0174] The Taylor expansion at (x, y, z) is:

[0175]

[0176] Where ρ is the pseudo-range value when the coordinates of target i are (x, y, z), is the cosine value of the x, y, and z coordinate axes to the satellite observation direction, which can be set to

[0177] Therefore, the measured value of the pseudorange difference can be expressed as:

[0178]

[0179] From the above formula, we can get the pseudo-range measurement equation of GPS / INS navigation system:

[0180]

[0181] At the same time, when the GPS receiver can receive Doppler observations, the Doppler observations can be used for pseudo-range rate difference measurement. The pseudo-range rate between the target and satellite k measured by GPS can be expressed as:

[0182]

[0183] The pseudorange rate of the INS module can be expressed as:

[0184]

[0185] So the measured value of the pseudorange rate difference can be expressed as:

[0186]

[0187] From the above formula, the pseudo-range rate difference measurement equation of GPS / INS navigation system can be obtained:

[0188]

[0189] in, is the state matrix at time t, X(t) is the INS error state variable at time t, is the measurement noise at time t.

[0190] Step 2.3: Design an adaptive Kalman filter algorithm

[0191] In the GPS / INS data fusion algorithm, the system noise covariance matrix and the measurement noise covariance matrix will change dynamically over time. The piecewise static characteristics of the measurement information sequence are used to estimate the system noise covariance matrix and the measurement noise covariance matrix.

[0192]

[0193] P ig (k,k-1)=F(k-1)P ig (k-1)F T (k-1)+Q(k-1)

[0194] The Kalman gain matrix K(k) under the optimal estimation condition is calculated as:

[0195]

[0196] P ig (k)=[IK(k)H p (k)P ig (k,k-1)]

[0197] Define the residual matrix B(k) as:

[0198]

[0199] The residual value is used to measure the divergence of the filter. The covariance matrix C(k) is:

[0200]

[0201] The statistical characteristics of the residual matrix are estimated using the ergodic characteristics of the static random sequence, and the estimated C(k) value C(k) satisfies:

[0202]

[0203] Where M is the number of sampling points.

[0204] In order to improve the efficiency of the filtering algorithm, the calculation of C(k) can be simplified as:

[0205]

[0206] The update equation for the measurement noise variance is:

[0207]

[0208] The update equation of the system noise variance is:

[0209]

[0210] Step S3: acquiring RS data and MO data during vehicle navigation and fusing them to obtain optimized collaborative data.

[0211] Each target in the system has a unique position. When the number of targets is large enough, each target has its own unique relative position relative to other targets. Therefore, the set of distances between any target and other cooperative targets can be regarded as a unique array. Based on this, this paper designs the following MO and RS data fusion steps: (1) Use the YOLO-V3 (You Only Look Once) target monitoring model to identify all targets, calculate their positions and number them. (2) Based on the conversion between the RS image coordinate system and the ground coordinate system, calculate the distance between each target and record it in the ranging table 1.

[0212] Table 1

[0213]

[0214] (3) The navigation target uses MO to measure the distance to the surrounding cooperative targets and records it in distance measurement table 2. Target 6 in the table was not identified in the first step.

[0215] Table 2

[0216]

[0217] (4) Perform similarity matching on the target distance row of table B and each distance row, and select the row with the highest repetition rate as the matching row. The method of accurate matching is: when the distance difference is less than the threshold D (depending on the maximum error value of MO and RS distance measurement being larger), the match is considered successful, and the matching parameter C is increased by 1. In the next match, the corresponding matching value is skipped to avoid repeated matching. Define NA1 as the number of ranging values ​​of one target to other targets in RS observation, and NA2 as the number of ranging values ​​of one target to other targets in MO. The matching degree F satisfies:

[0218]

[0219] (5) Select the row with the highest F value as the matching row, and take the corresponding target in the RS image as the positioning target. Then output the calculated target coordinates.

[0220] Step S4: adjusting the multi-target collaborative navigation based on the optimized collaborative data to obtain adjusted data.

[0221] When the GPS / INS signal fails, the positioning system obtains data from the RS / MO system, where the RS platform can be an aerial target or camera at high altitude, and the MO platform consists of land, sea and air collaborative targets and laser radar, camera, communication equipment, etc. In order to achieve collaborative navigation of land, sea and air targets in RS / MO mode, the RS platform should usually have a certain position. If not, when some collaborative targets or detected landmarks have their own precise coordinates, the coordinates can be calculated through position conduction.

[0222] It is assumed that there are UAV targets or other RS ​​platform data collection in the RS collaborative navigation area. At the same time, all navigation targets can communicate with other targets and conduct mutual ranging observations. When GPS / INS works well, the RS / MO target recognition algorithm is continuously trained using accurate GPS / INS data. At the same time, a feedback mechanism of high-quality RS / MO data to GPS / INS data is designed for the detection and correction of GPS / INS errors. GPS / INS data is fused with RS / MO data in real time, and the final coordinates are output after translation matching correction and rotation matching correction.

[0223] (1) Local rotation (deformation) calculation process

[0224] First, the relative spatial relationship of GPS / INS observation data is locally rotated to eliminate errors; the main purpose is to rotate the GPS / INS points to different degrees so that the relative spatial relationship between different target points in GPS / INS observations and RS / MO observation points is as close as possible.

[0225] (x i ′+Δx i ′-x o ′)(x p ′-x o ′)+(y i ′+Δy i ′-y o ′)(y p ′-y o ′)=(x i -x o )(x p -x o )+(y i -y o )(y p -y o

[0226] At the same time, the relative spatial relationship between the adjusted GPS / INS coordinate point and the center of the point set should be as close as possible to the relative spatial relationship between the RS / MO coordinate point and the center of the point set. Therefore, the coordinate adjustment is performed through local rotation, which is reflected in the local rotation formula:

[0227] (x i ′-Δx i ′-x o ′) 2 +(y i ′-Δy i ′-y o ′) 2 =(x i ′-x o ′) 2 +(y i ′-y o ′) 2

[0228] Combined with the above formula, the coordinate correction is solved to obtain a GPS / INS coordinate point set with a more accurate relative spatial relationship for the next calculation.

[0229] (2) Calculation process of overall translation

[0230] Secondly, the RS / MO coordinate points need to be translated as a whole to eliminate the overall offset error caused by the offset of the remote sensing platform. Then:

[0231]

[0232] The translation value dp when calculating d has the minimum value and performs an overall translation on the coordinates in the RS / MO observations.

[0233] (3) Overall rotation calculation process

[0234] The RS / MO coordinate points are rotated as a whole to eliminate the error caused by the rotation of the RS platform in RS / MO observation. In order to eliminate the error of the rotated coordinates, the sum of the Euclidean distances between the rotation centers of the target points in the RS / MO observation and the corresponding target points in the GPS / INS observation should be minimized.

[0235] The straight line connecting the target and the center of rotation can be expressed as:

[0236] (y i ′-y i )x-(x i ′-x i )y+(x i ′-x i )y i +(y i ′-y i )xi =0

[0237] (4) The rotation error measurement coefficient Dr satisfies:

[0238]

[0239] can be simplified to

[0240]

[0241] (5) Overall rotation process of multi-target positioning data: As can be seen from the formula, the rotation error Dr has a minimum value. When the Dr value is minimum, the rotation value θ is calculated to rotate the coordinates in the RS / MO observation as a whole. Repeat the above three steps until the difference observed by GPS / INS and RS / MO is less than the threshold 1. Finally, the RS / MO coordinates (x i ,y i ) is used as the navigation target position after joint adjustment.

[0242] Step S5: monitor the GPS status, and use the GPS / INS loosely coupled integrated system to perform positioning compensation when the GPS is interrupted. Step S6: implement the multi-target collaborative navigation positioning results based on the positioning compensation and the adjusted data. The state KF is used for GPS / INS integration, and the process model and observation model are:

[0243]

[0244] Among them, X is the state vector, F is the coefficient matrix, G is the system noise matrix, W is the process noise vector, Z is the observation vector, H is the observation matrix, V is the observation noise vector, and the observation vector Z represents the difference between the INS position and the GPS position. The state vector X is set as:

[0245]

[0246] Among them, E、N、U To calculate whether the platform's misalignment angle in the local geographic frame is g, is the velocity error of the three axes of the g frame, and represents the position error, and ε x,y,z denote the accelerometer bias and gyroscope bias on the three axes of the body frame b, respectively.

[0247] The system matrix F is constructed according to the INS error equation:

[0248]

[0249] Among them, and are the Earth velocity vector and its error, and is the angular rate vector from the g frame to the inertial frame and its error.

[0250] Expressing the equation in discrete-time form:

[0251] X k =φ k,k-1 X k-1 +G K W K

[0252] Z K =H K X K +V K

[0253] The prediction and update process of Kalman filter is as follows:

[0254] Predicted State Estimates:

[0255]

[0256] Prediction covariance:

[0257]

[0258] Kalman gain:

[0259]

[0260] Update the state estimate:

[0261]

[0262] Update the covariance:

[0263] P K =[IK K H K ]P K,K-1

[0264] Among them, Φ K,K-1 is the system state transfer matrix from time k-1 to k, is the predicted state estimate, P K,K-1 is the estimated covariance of the forecast, K K is the Kalman gain matrix, is the updated state estimate, P K is the updated estimated covariance. Q K-1 and R K are the variance-covariance matrices of the state and observation, respectively, which can be obtained by and calculate.

[0265] Analysis of the results obtained: The purpose of the experiment is to verify the improvement of collaborative positioning accuracy compared with ordinary GPS / INS positioning. The experiment uses three ground remote-controlled aircraft and one collaborative UAV to perform navigation tasks simultaneously. The purpose of the navigation task is to let three ground remote-controlled vehicles numbered A, B, and C start from point A at the same time. Then vehicles A, B, and C move to points B, C, and D in an area 1 km wide and 2 km long in the city of Bonn respectively. The vehicle stores and outputs positioning-related GPS, INS, RS (collected from the drone) and MO data during the completion of the navigation task. At the same time, the vehicle collects 3 sets of DGPS reference group data (measurement accuracy is about 15-20cm) according to the actual trajectory for comparative analysis of positioning accuracy. During the navigation process, the UAV performs navigation collaboration along the moving direction of the ground vehicle according to the pre-set "s"-shaped navigation trajectory and a specific navigation altitude. At the same time, the UAV collects RS image data straight down at a frequency of 1HZ and transmits the collected data to the ground vehicle in real time.

[0266] The experiment lasted for 300 seconds. In order to simulate the failure of GPS signal, the GPS signal switch was turned off at 90-120 seconds and 180-210 seconds during the simulation. Each GPS positioning failure lasted for 30 seconds. In the GPS / INS / RS / MO group, mutual observation of targets was performed every 15 seconds to measure the mutual distance between the collaborative target and the external navigation environment. The image output frequency of RS observation was 1 Hz. Finally, 300 experimental images were collected. In general, the experiment was divided into four test groups: GPS / INS data navigation, RS / MO data navigation, GPS / INS / RS / MO data navigation, and differential GPS. Finally, the experimental data was collected, and the fusion accuracy and efficiency of the four navigation test groups were calculated. Then, the experiment generated trajectory and error comparison charts for visual analysis.

[0267] The following conclusions can be drawn from the analysis of the test results: Under the control of the differential GPS test group, the positioning error of the GPS / INS test group obeys the normal distribution. It can be used to control the overall offset error of the RS / MO control group. The relative position error of the RS / MO positioning group is small, which can eliminate the random positioning error of the navigation target in the GPS / INS test group. The GPS / INS / RS / MO test group ensures the positioning accuracy and stability of all targets, and the navigation efficiency is high.

[0268] At the same time, according to the positioning data of the navigation target, three sets of relative position data between navigation targets A, B, and C during the navigation process can be calculated. Then, 10 sets of positioning data with a navigation time of t = 0s to t = 300s and an interval of 30s are selected to calculate the relative positions of target A to target B, target B to target C, and target A to target C.

[0269] The experimental results show that the positioning data after the fusion of GPS / INS / RS / MO data has higher accuracy and stability. The GPS / INS combined navigation system using adaptive Kalman filtering can make up for the deficiencies of GPS and INS navigation systems to the greatest extent, and realize navigation and positioning in complex environments with high-precision navigation. It can also be used to control the overall offset error of the RS / MO control group. At the same time, the relative position error of the RS / MO positioning group is small, which can eliminate the random positioning error of the navigation target in the GPS / INS test group. RS / MO positioning after range table matching can improve the stability of GPS / INS multi-target collaborative navigation positioning. In addition, it can also improve the navigation target's perception of the navigation environment and ensure the collaborative relationship between targets in completing navigation tasks. Fusion of GPS / INS / RS / MO multi-source data into collaborative navigation can enable the collaborative navigation system to perform better in navigation scenarios with high requirements for positioning accuracy, such as disaster relief navigation and polar ship navigation.

[0270] Embodiment 2

[0271] A collaborative positioning system based on GPS / INS and RS / MO multi-source data fusion, the system comprising: a data acquisition module, a GPS / INS fusion module, an RS / MO fusion module, a navigation collaboration module, a GPS compensation module and a navigation positioning module;

[0272] The data acquisition module is used to acquire GPS data and INS data during vehicle navigation;

[0273] The GPS / INS fusion module is used to fuse the GPS data and the INS data using an adaptive Kalman filter algorithm to obtain navigation collaborative data;

[0274] The GPS / INS fusion module also includes: a state equation construction submodule, an observation equation construction module and a collaborative data submodule;

[0275] The state equation construction submodule is used to fuse the pseudorange and pseudorange rate of the GPS data and the INS data, and construct a state equation based on the fusion result;

[0276] The observation equation construction submodule is used to construct an observation equation based on the pseudorange difference and pseudorange rate difference of the GPS data and the INS data;

[0277] The navigation cooperation module adjusts the multi-target cooperative navigation based on the optimized cooperative data to obtain the adjusted data. The RS / MO fusion module is used to obtain RS data and MO data in the vehicle navigation process and then fuse them to obtain the optimized cooperative data.

[0278] Optionally, the workflow of the state equation construction submodule specifically includes:

[0279] The GPS / INS state variable of the filter contains the error vector of INS data and GPS data, expressed as:

[0280] X ig (t) = [X i (t)X g (t)]

[0281] Among them, X i (t) represents the INS error state variables, including platform error angle, velocity error, position error, gyroscope random drift, and acceleration deviation; X g (t) is the state variable of GPS error, which consists of the distance error caused by GPS clock error and the velocity error caused by GPS clock frequency error;

[0282] The state equation of the GPS / INS integrated navigation receiver is:

[0283]

[0284] In the formula, F i (t) and F g (t) represents the state transfer matrix, G i (t) and W g (t) are the system noise matrices of INS and GPS respectively.

[0285] Optionally, the workflow of the observation equation construction submodule specifically includes:

[0286] Calculating the pseudorange and pseudorange rate between the INS and the GPS satellite based on the ephemeris data in the GPS data and the target motion parameters in the INS data;

[0287] The GPS / INS pseudorange difference and pseudorange rate difference are input into the filter as observation values;

[0288] Input the initial parameters into the linearized and discretized observation equation to drive the filter and perform GPS / INS error correction through iterative calculation;

[0289] The pseudo distance between the target and the GPS satellite can be expressed as:

[0290]

[0291] Among them, v pk is the measurement noise caused by multipath effect, tropospheric error and ionospheric error; ρ k is the distance k between the target and the GPS satellite; ζ nis the error introduced in pseudo-range measurement, is the x-axis coordinate of GPS satellite k in the earth reference system, is the y-axis coordinate of GPS satellite k in the earth reference system, is the z-axis coordinate of GPS satellite k in the earth reference system;

[0292] The pseudo-range between target i and GPS satellite k obtained from INS data can be expressed as:

[0293]

[0294] Among them, (x i ,y i ,z i ) represents the coordinates of the target in the Earth reference system, calculated from INS data; (ζ x ,ζ y ,ζ z ) is the position error of the target, then:

[0295] x i =x+ζ x x i =x+ζ x x i =x+ζ x

[0296] The Taylor expansion at (x, y, z) is:

[0297]

[0298] Among them, ρ is the pseudo-range value when the coordinates of target i are (x, y, z), is the cosine value of the x, y, and z coordinate axes to the satellite observation direction, which can be set to

[0299] The measured value of the pseudorange difference can be expressed as:

[0300]

[0301] From the above formula, we can get the pseudo-range measurement equation of GPS / INS navigation system:

[0302]

[0303] When the GPS receiver is able to receive Doppler observations, the Doppler observations can be used to measure the pseudorange rate difference; the pseudorange rate between the target and satellite k measured by GPS can be expressed as:

[0304]

[0305] The pseudorange rate of the INS module can be expressed as:

[0306]

[0307] The measured value of the pseudorange rate difference can be expressed as:

[0308]

[0309] From the above formula, the pseudo-range rate difference measurement equation of GPS / INS navigation system can be obtained:

[0310]

[0311] The GPS compensation module is used to monitor the status of GPS and use the GPS / INS loosely coupled integrated system to perform positioning compensation when GPS is interrupted;

[0312] The navigation and positioning module is used to implement the multi-target collaborative navigation and positioning results based on the positioning compensation and the adjusted data

[0313] Step 5: Build a GPS / INS Loosely Coupled Integrated System

[0314] The 15-state KF is used for GPS / INS integration, and the process model and observation model are:

[0315]

[0316] Among them, X is the state vector, F is the coefficient matrix, G is the system noise matrix, W is the process noise vector, Z is the observation vector, H is the observation matrix, V is the observation noise vector, and the observation vector Z represents the difference between the INS position and the GPS position. The state vector X is set to

[0317]

[0318] Among them, E、N、U To calculate whether the platform's misalignment angle in the local geographic frame is g, is the velocity error of the three axes of the g frame, and represents the position error, and ε x,y,z denote the accelerometer bias and gyroscope bias on the three axes of the body frame b, respectively.

[0319] The system matrix F is constructed according to the INS error equation:

[0320]

[0321] Among them, and are the Earth velocity vector and its error, and is the angular rate vector from the g frame to the inertial frame and its error.

[0322] Expressing the equation in discrete-time form:

[0323] X k =φ k,k-1 X k-1 +G K W K

[0324] Z K =H K X K +V K

[0325] The prediction and update process of Kalman filter is as follows:

[0326] Predicted State Estimates:

[0327]

[0328] Prediction covariance:

[0329]

[0330] Kalman gain:

[0331]

[0332] Update the state estimate:

[0333]

[0334] Update the covariance:

[0335] P K =[IK K H K ]P K,K-1

[0336] Among them, Φ K,K -1 is the system state transfer matrix from time k-1 to k, is the predicted state estimate, P K,K -1 is the estimated covariance of the forecast, K K is the Kalman gain matrix, is the updated state estimate, P K is the updated estimated covariance. Q K-1 and R K are the variance-covariance matrices of the state and observation, respectively, which can be obtained by and calculate.

[0337] The embodiments described above are only descriptions of the preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Without departing from the design spirit of the present invention, various modifications and improvements made to the technical solutions of the present invention by ordinary technicians in this field should all fall within the protection scope determined by the claims of the present invention.

Claims

1. A collaborative positioning method based on GPS / INS and RS / MO multi-source data fusion, characterized in that: The method comprises: Step S1, obtaining GPS data and INS data during vehicle navigation; Step S2: using an adaptive Kalman filter algorithm to fuse the GPS data with the INS data to obtain navigation collaboration data; Step S3, obtaining RS data and MO data during vehicle navigation and fusing them to obtain optimized collaborative data; Step S4: adjusting the multi-target collaborative navigation based on the optimized collaborative data to obtain adjusted data; Step S5: Based on the adjusted data, the GPS status is monitored, and when the GPS is interrupted, the GPS / INS loosely coupled integrated system is used for positioning compensation; Step S6: Implement multi-target collaborative navigation positioning results based on the positioning compensation and the adjusted data.

2. The collaborative positioning method based on GPS / INS and RS / MO multi-source data fusion according to claim 1 is characterized in that: In step S2, the process of fusing the GPS data and the INS data using an adaptive Kalman filter algorithm to obtain navigation collaborative data specifically includes: S21, fusing the pseudorange and pseudorange rate of the GPS data and the INS data, and constructing a state equation based on the fusion result; S22, constructing an observation equation based on the pseudo-range difference and pseudo-range rate difference of the GPS data and the INS data; S23. Based on the state equation and the observation equation, use an adaptive Kalman filter algorithm to update the measurement noise variance and the system noise variance, and use the updated data as navigation cooperation data.

3. The collaborative positioning method based on GPS / INS and RS / MO multi-source data fusion according to claim 2 is characterized in that: In S21, the process of fusing the pseudorange and pseudorange rate of the GPS data and the INS data and constructing the state equation based on the fusion result specifically includes: The GPS / INS state variable of the filter contains the error vector of INS data and GPS data, expressed as: X ig (t)=[X i (t) X g (t)] Among them, X i (t) represents the INS error state variables, including platform error angle, velocity error, position error, gyroscope random drift, and acceleration deviation; X g (t) is the state variable of GPS error, which consists of the distance error caused by GPS clock error and the velocity error caused by GPS clock frequency error; The state equation of the GPS / INS integrated navigation receiver is: In the formula, F i (t) and F g (t) represents the state transfer matrix, G i (t) and W g (t) are the system noise matrices of INS and GPS respectively.

4. The collaborative positioning method based on GPS / INS and RS / MO multi-source data fusion according to claim 2 is characterized in that: In S22, the content of constructing the observation equation based on the pseudo-range difference and pseudo-range rate difference of the GPS data and the INS data specifically includes: Calculating the pseudorange and pseudorange rate between the INS and the GPS satellite based on the ephemeris data in the GPS data and the target motion parameters in the INS data; The GPS / INS pseudorange difference and pseudorange rate difference are input into the filter as observation values; Input the initial parameters into the linearized and discretized observation equation to drive the filter and perform GPS / INS error correction through iterative calculation; The pseudo distance between the target and the GPS satellite can be expressed as: Among them, v pk is the measurement noise caused by multipath effect, tropospheric error and ionospheric error; ρ k is the distance k between the target and the GPS satellite; ζ n is the error introduced in pseudo-range measurement, is the x-axis coordinate of GPS satellite k in the earth reference system, is the y-axis coordinate of GPS satellite k in the earth reference system, is the z-axis coordinate of GPS satellite k in the earth reference system; The pseudo-range between target i and GPS satellite k obtained from INS data can be expressed as: Among them, (x i ,y i ,z i ) represents the coordinates of the target in the Earth reference system, calculated from INS data; (ζ x ,ζ y ,ζ z ) is the position error of the target, then: x i =x+ζ x x i =x+ζ x x i =x+ζ x The Taylor expansion at (x, y, z) is: Among them, ρ is the pseudo-range value when the coordinates of target i are (x, y, z), is the cosine value of the x, y, and z coordinate axes to the satellite observation direction, which can be set to The measured value of the pseudorange difference can be expressed as: From the above formula, we can get the pseudo-range measurement equation of GPS / INS navigation system: When the GPS receiver is able to receive Doppler observations, the Doppler observations can be used for pseudorange rate difference measurement; the pseudorange rate between the target and satellite k measured by GPS can be expressed as: The pseudorange rate of the INS module can be expressed as: The measured value of the pseudorange rate difference can be expressed as: From the above formula, the pseudo-range rate difference measurement equation of GPS / INS navigation system can be obtained: in, is the state matrix at time t, X(t) is the INS error state variable at time t, is the measurement noise at time t.

5. The collaborative positioning method based on GPS / INS and RS / MO multi-source data fusion according to claim 1 is characterized in that: In step S5, the construction process of the GPS / INS loosely coupled integrated system specifically includes: The state Kalman filter algorithm is used to integrate GPS / INS. The process model and observation model are: Among them, X is the state vector, F is the coefficient matrix, G is the system noise matrix, W is the process noise vector, Z is the observation vector, H is the observation matrix, V is the observation noise vector, and the observation vector Z represents the difference between the INS position and the GPS position; The state vector X is set to: Among them, E、N、U To calculate whether the platform's misalignment angle in the local geographic frame is g, is the velocity error of the three axes of the g frame, and represents the position error, and ε x,y,z denote the accelerometer bias and gyroscope bias on the three axes of the body frame b, respectively; The system matrix F is constructed according to the INS error equation: δh=δV U Among them, and are the Earth velocity vector and its error, and is the angular rate vector from the g frame to the inertial frame and its error; Expressing the equation in discrete-time form: X k =φ k,k-1 X k-1 +G K W K Z K =H K X K +V K The prediction and update process of Kalman filter is as follows: Predicted State Estimates: Prediction covariance: Kalman gain: Update the state estimate: Update the covariance: P K =[I-K K H K ]P K,K-1 Among them, Φ K,K-1 is the system state transfer matrix from time k-1 to k, is the predicted state estimate, P K,K-1 is the estimated covariance of the forecast, K K is the Kalman gain matrix, is the updated state estimate, P K is the updated estimated covariance; Q K-1 and R K are the variance-covariance matrices of the state and observation, respectively, through and calculate.

6. A collaborative positioning system based on multi-source data fusion of GPS / INS and RS / MO, used to implement the collaborative positioning method according to any one of claims 1 to 5, characterized in that: The system includes: a data acquisition module, a GPS / INS fusion module, a RS / MO fusion module, a navigation coordination module, a GPS compensation module and a navigation positioning module; The data acquisition module is used to acquire GPS data and INS data during vehicle navigation; The GPS / INS fusion module is used to fuse the GPS data and the INS data using an adaptive Kalman filter algorithm to obtain navigation collaborative data; The RS / MO fusion module is used to obtain RS data and MO data during vehicle navigation and then fuse them to obtain optimized collaborative data; The navigation cooperation module adjusts the multi-target cooperative navigation based on the optimized cooperation data to obtain adjusted data; The GPS compensation module is used to monitor the status of GPS and use the GPS / INS loosely coupled integrated system to perform positioning compensation when GPS is interrupted; The navigation and positioning module is used to achieve collaborative navigation and positioning results for multiple targets based on the positioning compensation and the adjusted data.

7. The collaborative positioning system based on GPS / INS and RS / MO multi-source data fusion according to claim 6 is characterized in that: The GPS / INS fusion module also includes: a state equation construction submodule, an observation equation construction module and a collaborative data submodule; The state equation construction submodule is used to fuse the pseudorange and pseudorange rate of the GPS data and the INS data, and construct a state equation based on the fusion result; The observation equation construction submodule is used to construct an observation equation based on the pseudorange difference and pseudorange rate difference of the GPS data and the INS data; The collaborative data submodule is used to update the measurement noise variance and the system noise variance based on the state equation and the observation equation using an adaptive Kalman filter algorithm, and use the updated data as navigation collaborative data.

8. The collaborative positioning system based on GPS / INS and RS / MO multi-source data fusion according to claim 7 is characterized in that: The workflow of the state equation construction submodule specifically includes: The GPS / INS state variable of the filter contains the error vector of INS data and GPS data, expressed as: X ig (t)=[X i (t)X g (t)] Among them, X i (t) represents the INS error state variables, including platform error angle, velocity error, position error, gyroscope random drift, and acceleration deviation; X g (t) is the state variable of GPS error, which consists of the distance error caused by GPS clock error and the velocity error caused by GPS clock frequency error; The state equation of the GPS / INS integrated navigation receiver is: In the formula, F i (t) and F g (t) represents the state transfer matrix, G i (t) and W g (t) are the system noise matrices of INS and GPS respectively.

9. The collaborative positioning system based on GPS / INS and RS / MO multi-source data fusion according to claim 7, characterized in that: The workflow of the observation equation construction submodule specifically includes: Calculating the pseudorange and pseudorange rate between the INS and the GPS satellite based on the ephemeris data in the GPS data and the target motion parameters in the INS data; The GPS / INS pseudorange difference and pseudorange rate difference are input into the filter as observation values; Input the initial parameters into the linearized and discretized observation equation to drive the filter and perform GPS / INS error correction through iterative calculation; The pseudo distance between the target and the GPS satellite can be expressed as: Among them, v pk is the measurement noise caused by multipath effect, tropospheric error and ionospheric error; ρ k is the distance k between the target and the GPS satellite; ζ n is the error introduced in pseudo-range measurement, is the x-axis coordinate of GPS satellite k in the earth reference system, is the y-axis coordinate of GPS satellite k in the earth reference system, is the z-axis coordinate of GPS satellite k in the earth reference system; The pseudo-range between target i and GPS satellite k obtained from INS data can be expressed as: Among them, (x i ,y i ,z i ) represents the coordinates of the target in the Earth reference system, calculated from INS data; (ζ x ,ζ y ,ζ z ) is the position error of the target, then: x i =x+ζ x x i =x+ζ x x i =x+ζ x The Taylor expansion at (x, y, z) is: Among them, ρ is the pseudo-range value when the coordinates of target i are (x, y, z), is the cosine value of the x, y, and z coordinate axes to the satellite observation direction, which can be set to The measured value of the pseudorange difference can be expressed as: From the above formula, we can get the pseudo-range measurement equation of GPS / INS navigation system: When the GPS receiver is able to receive Doppler observations, the Doppler observations can be used for pseudorange rate difference measurement; the pseudorange rate between the target and satellite k measured by GPS can be expressed as: The pseudorange rate of the INS module can be expressed as: The measured value of the pseudorange rate difference can be expressed as: From the above formula, the pseudo-range rate difference measurement equation of GPS / INS navigation system can be obtained: in, is the state matrix at time t, X(t) is the INS error state variable at time t, is the measurement noise at time t.

10. The collaborative positioning system based on GPS / INS and RS / MO multi-source data fusion according to claim 6, characterized in that: In the GPS compensation module, the construction process of the GPS / INS loosely coupled integrated system specifically includes: The state Kalman filter algorithm is used to integrate GPS / INS. The process model and observation model are: Among them, X is the state vector, F is the coefficient matrix, G is the system noise matrix, W is the process noise vector, Z is the observation vector, H is the observation matrix, V is the observation noise vector, and the observation vector Z represents the difference between the INS position and the GPS position; The state vector X is set to: Among them, E、N、U To calculate whether the platform's misalignment angle in the local geographic frame is g, is the velocity error of the three axes of the g frame, and represents the position error, and ε x,y,z denote the accelerometer bias and gyroscope bias on the three axes of the body frame b, respectively; The system matrix F is constructed according to the INS error equation: δh=δV U Among them, and are the Earth velocity vector and its error, and is the angular rate vector from the g frame to the inertial frame and its error; Expressing the equation in discrete-time form: X k =φ k,k-1 X k-1 +G K W K Z K =H K X K +V K The prediction and update process of Kalman filter is as follows: Predicted State Estimates: Prediction covariance: Kalman gain: Update the state estimate: Update the covariance: P K =[I-K K H K ]P K,K-1 Among them, Φ K,K-1 is the system state transfer matrix from time k-1 to k, is the predicted state estimate, P K,K-1 is the estimated covariance of the forecast, K K is the Kalman gain matrix, is the updated state estimate, P K is the updated estimated covariance; Q K-1 and R K are the variance-covariance matrices of the state and observation, respectively, through and calculate.

Images