A dual-transponder-assisted SINS / USBL integrated navigation method

Abstract

A dual-transponder-assisted SINS / USBL combined navigation method, 1) establishing a SINS / USBL combined navigation state equation, 2) establishing a SINS / USBL slant range difference measurement equation, 3) establishing a measurement time sequence arrival inconsistency measurement model, 4) solving noise parameters according to the inverse Gamma distribution, and calculating the measurement noise estimation result, 5) calculating the gain matrix according to the maximum entropy principle and the measurement noise estimation result estimated in step 4), 6) performing Kalman filtering fusion on the model of step 5, performing feedback correction on SINS, and outputting the navigation result. Repeat the above steps until the navigation is completed. The tight combination model based on slant range difference proposed in the present invention can suppress the unmodelable error in the USBL positioning process, and the robust filtering method based on maximum entropy and variational Bayes performs optimal estimation on the unknown or time-varying noise of SINS / USBL, thereby improving the positioning accuracy in complex underwater environments.

Description

Technical Field

[0001] The invention belongs to the SINS / USBL combined navigation technology of underwater vehicles, and in particular relates to a dual-transponder-assisted SINS / USBL combined navigation method. Background Art

[0002] Generally speaking, in addition to random noise, ultra-short baseline measurement noise also includes delay errors caused by signal processing, sound velocity bending errors caused by changes in physical properties such as water temperature and salinity, system clock errors, multipath effects, and other errors that cannot be modeled. Traditional noise models only consider random noise or unify all noise as random noise of the sensor, which is inaccurate for USBL time-varying systems. To address the above issues, the current solutions are as follows:

[0003] Application date: 2021 / 10 / 29, application number: CN202111268907.2, patent name: A USBL slant range correction method based on maximum entropy adaptive robust estimation. The specific scheme of this patent is as follows: 1) Record the SINS attitude transfer matrix, velocity and position information at the time when USBL sends the request message, 2) Record the azimuth, SINS attitude, velocity and position information at the time when USBL receives the transponder message, 3) Correct the USBL slant range according to the SINS solution results at the sending and receiving times, 4) Establish a SINS / USBL tight combination system model based on the USBL slant range corrected by 3) and the azimuth, altitude and SINS solution information collected by 2), 5) Calculate the dynamic model error adaptive factor and calculate the gain matrix according to the maximum entropy principle, 6) Perform Kalman filter fusion on the model of 5), perform feedback correction on SINS, and output the navigation result. Repeat the above steps until the navigation is completed, and use the attitude and position measured by the strapdown inertial navigation system to establish the change model between the USBL transmission and reception time, and derive the slant range correction value at the reception time. Finally, in order to suppress the influence of abnormal noise on positioning accuracy.

[0004] It adopts the traditional positioning model based on a single transponder, and adopts the maximum entropy-based anti-error filtering algorithm to eliminate abnormal wild value information in complex environments. Based on the traditional single transponder model, this paper proposes a SINS / USBL combined positioning model based on dual transponders, and adopts a combined navigation method based on slant range difference to eliminate unmodelable errors such as sound velocity bending, multipath effect, and signal processing delay in underwater environments, and derives the delay consistency model under the relative motion of dual transponders. And on the basis of maximum entropy anti-error estimation, it is proposed to combine maximum entropy anti-error filtering with the combined navigation noise adaptive model, and solve the problem of abnormal wild value in the system noise model unknown or time-varying and complex environment.

[0005] Application date: 2018 / 9 / 18, application number: CN201811087892.8, patent name: USBL / SINS tightly integrated navigation and positioning method based on hybrid derivative-free extended Kalman filter. The specific scheme of the patent is as follows, which is particularly suitable for the positioning of underwater equipment. The present invention is composed of an ultra-short baseline hydroacoustic positioning system USBL and a strapdown inertial navigation system SINS, and adopts a hybrid derivative-free extended Kalman filter HDEKF for combined navigation. The ultra-short baseline system calculates the time of one-way propagation of ultrasonic signals between the transponder and the hydrophone to obtain the slant range measurement between the two, and obtains the observation equation through the coordinate conversion formula. Then, the error state equation is established according to the error transfer formula of the strapdown inertial navigation system. Finally, a hybrid derivative-free Kalman filter is performed, and a standard linear Kalman filter is used for time update, and a derivative-free extended filter is used for measurement update. The present invention can effectively improve the navigation accuracy and stability of the USBL / INS combined navigation system and reduce the amount of real-time calculation.

[0006] It adopts a slant range difference model based on multiple hydrophones, and the application background adopts a combined navigation model of a single transponder unit. Since the distance between the hydrophones is close, the slant range difference is small, which easily causes the singularity of matrix operation. The slant range difference combined navigation model of dual transponders proposed in this paper is based on multiple transponders. The slant range difference between the transponders is obvious, and the matrix operation is stable.

[0007] Application date: 2020 / 6 / 8, application number: CN202010513750.4, patent name: A method and system for AUV integrated navigation based on M estimation, which proposes a filtering algorithm based on generalized maximum likelihood estimation (M estimation). When the system measurement noise is non-Gaussian distribution, especially mixed Gaussian distribution (symmetric interference near Gaussian), the M-estimation algorithm is applied to weight the measurement residual and state prediction residual through influence function and weight function to reduce the impact of abnormal measurement peak on the navigation system. And the algorithm is applied to the SINS / USBL integrated navigation system. Under the conditions of non-Gaussian measurement noise and measurement anomaly caused by multipath effect of underwater acoustic sensor, the standard Kalman filter algorithm and the M estimation filter algorithm are compared. The position and velocity error accuracy of the latter are significantly improved compared with the former. And the more serious the pollution of the mixed Gaussian model, the more obvious the filtering effect of the SINS / USBL integrated navigation system based on M estimation filtering is, and the robustness and anti-interference performance are relatively better.

[0008] It uses a robust filtering algorithm based on Huber M estimation to eliminate the problem of decreased positioning accuracy caused by underwater abnormal wild values. This paper proposes to use a maximum entropy robust filtering algorithm to replace the Huber M estimation algorithm. In addition, in order to eliminate the decrease in positioning accuracy caused by the uncertainty and time-varying system noise model, a variational Bayesian method based on Gamma distribution is used to estimate the system noise model.

[0009] Application date: 2015 / 9 / 1, application number: CN201510551249.6, patent name: A SINSUSBL tightly coupled algorithm. The present invention belongs to the technical field of tightly coupled navigation algorithms, and specifically relates to a multi-responder SINS / USBL combined navigation algorithm. The technical solution of the present invention designs a new multi-responder SINS / USBL combined navigation scheme; and performs error modeling and simulation analysis on the multi-responder SINS / USBL tightly coupled navigation algorithm. The present invention can avoid the influence of USBL angle measurement error and USBL to SINS installation angle error on positioning accuracy, and there is no need to calibrate the installation angle of USBL to SINS, which is more convenient to use. Simulation results show that the present invention can obtain higher positioning accuracy than the existing single-responder SINS / USBL combined navigation algorithm.

[0010] It uses the relative distance between SINS and multiple transponders to establish the measurement equation of integrated navigation, which requires receiving multiple transponder information at the same time, making the system difficult to implement. This paper uses the relative distance difference between SINS and multiple transponders to establish an integrated navigation model, and the proposed delay consistency model under the relative motion of dual transponders can solve the integrated navigation algorithm when the transponders arrive at different times.

[0011] In order to weaken and eliminate the common errors in underwater acoustic signal propagation, this application proposes a high-precision tight coupling method based on slant distance difference under the condition of dual underwater transponders to improve the positioning accuracy of the combined navigation system. In addition, considering the timing arrival inconsistency caused by the relative position change between the carrier and the transponder, a measurement arrival inconsistency model is proposed. At the same time, in order to suppress the influence of underwater noise on positioning accuracy, a robust Kalman filter based on maximum correlation entropy is introduced. In view of the influence of unknown or time-varying system noise characteristics on the robust filtering estimation results, a variational Bayesian method based on Gaussian and inverse Gamma mixed distribution is derived to realize the adaptive estimation of the unknown noise variance matrix. Summary of the invention

[0012] In order to solve the problems existing in the prior art, the present invention proposes a new dual underwater transponder assisted strapdown inertial navigation / ultra-short baseline combined navigation method. Firstly, based on the traditional tightly coupled model, a new tightly coupled model based on slant range difference is proposed. In order to solve the measurement timing difference caused by the relative change of the position between the carrier and the dual transponders, a measurement timing inconsistency model is introduced. Secondly, an improved robust maximum entropy Kalman filter is proposed. A mixed distribution of Gaussian and inverse Gamma is proposed to simulate non-stationary thick-tail underwater noise, which can be used for the optimal estimation of unknown or time-varying wild value features.

[0013] The present invention provides a dual-responder assisted SINS / USBL combined navigation method, comprising:

[0014] Step 1: Establish the SINS / USBL combined navigation state equation:

[0015] The system state model is:

[0016] X k =F k,k-1 X k-1 +W k

[0017] Among them, F k,k-1 is the state transfer matrix, calculated according to the SINS error equation, and W(k) is the system noise. The state vector X k for:

[0018]

[0019] in,

[0020] X USBL =[θ x θ y θ z δα δβ δD] T ;

[0021] Where, δθ=[θ x θ y θ z ] T represents the installation angle error, [δα δβ] T represents the angle measurement error, δD represents the distance measurement error; φ n =[φ x φ y φ z ] T Indicates the pitch angle, roll angle, and heading angle errors of the carrier; δV = [δV E δV N δV N ]T Indicates the carrier's eastward, northward, and celestial velocity errors; δP = [δL δλ δh] T Indicates the carrier's geographic longitude, latitude, and altitude errors; Indicates the acceleration zero bias of SINS; [ε x ε y ε z ] T Indicates the gyro bias of SINS;

[0022] Step 2: Establish the SINS / USBL slope range difference measurement equation:

[0023] At time k, the measurement information of transponder A is received, and the system measurement model is:

[0024] Z(A)=H(A)X k +V

[0025] Where Z represents the system measurement value, H represents the measurement equation transfer matrix, and V represents the measurement noise;

[0026] The measurement information received by USBL from transponder A is expressed as:

[0027]

[0028] The relative position vector of transponder A in the a system is obtained by SINS position calculation: Then the relative position vector of transponder A is The relationship between azimuth and slant range is expressed as:

[0029]

[0030] Taking partial differentials of the above formula, we get:

[0031]

[0032] Among them, vector For the convenience of expression, Then the matrix The expression is:

[0033]

[0034] The relative position vector of transponder A calculated according to the position of SINS is:

[0035]

[0036] in, represents the transfer matrix corresponding to the installation error angle between SINS and USBL, represents the SINS attitude transfer matrix, The coordinate transformation matrix from the earth coordinate system to the navigation system, is the relative position vector in the earth coordinate system, expressed as:

[0037]

[0038] The slant range azimuth information calculated using the SINS position and the transponder A position is expressed as:

[0039]

[0040] Where [α β R] T is the true value of azimuth and slant range, and the matrices are

[0041] in,

[0042]

[0043] The system measurement is:

[0044]

[0045] Among them, H u =[0 0 -1], δU=[δα δβ δD] T ;

[0046] The expression of the observation matrix H(A) is:

[0047] H(A)=[H a 0 3×3 H p 0 3×6 H I H u ]

[0048] Accordingly, at time k+i, the measurement model is established based on the position of transponder B:

[0049] Z(B)=H(B)X(k+i)+V

[0050] Step 3: Establish a measurement timing inconsistency model. The steps are as follows:

[0051] The system receives the measurement information of transponder A at time k and the measurement information of transponder B at time k+i. The slant range difference model is expressed as:

[0052]

[0053] The relationship between system states at different times is expressed as:

[0054]

[0055] Among them, Γ k+i,k =F k+i,k+i-1 F k+i-1,k+i-2 …F k+1,k

[0056] According to the above formula, the state matrix X k It is expressed as:

[0057]

[0058] The slope range difference model is expressed as:

[0059]

[0060] Among them, H k+i (3) represents the third row of the slant range observation matrix at time k+i, Represents the slant range error at time k+i:

[0061]

[0062] Assumptions The noise distribution of the slant range difference at time k+i is:

[0063]

[0064] In summary, the measurement model based on dual transponders at time k+i is:

[0065]

[0066] Step 4: Solve the noise parameters according to the inverse Gamma distribution and calculate the measurement noise estimation result;

[0067] Step 5: Estimate the result based on the maximum entropy principle and measurement noise;

[0068] Step 6: Perform Kalman filter fusion. The steps are as follows:

[0069] State one-step prediction:

[0070] x k+i|k+i-1 =F k+i-1 x k+i-1

[0071] State one-step prediction mean square error:

[0072]

[0073] Filter gain:

[0074]

[0075] State Estimation:

[0076] X k+i =X k+|k+i-1 +K k+i (Z k+i -H k+i X k+i|k+i-1 )

[0077] State estimation mean square error matrix:

[0078] P k+i =P k+i|k+i-1 -K k H k P k+i|k+i-1 .

[0079] As a further improvement of the present invention, the measurement noise estimation result calculated in step 4 is specifically as follows:

[0080] 4.1 At time k+i, the joint probability density is expressed as Gaussian and inverse Gamma distribution:

[0081]

[0082] Among them, p(X k+i , R k+i |Z 1：k+i-1 ) represents the joint probability density function, N(X k+i |X k+i|k+i-1 , P k+i|k+i-1 ) represents Gaussian distribution, P k+i|k+i-1 represents the state transfer matrix, represents the inverse Gamma distribution, α k，i+i represents the shape parameter, β k+i，i represents the scale parameter, and the iterative update is as follows:

[0083] α k，i =0.5+α k+i|(k+i-1)，i

[0084]

[0085] 4.2 Calculate the chi-square test value:

[0086]

[0087] Among them, r k+i =Z k+i -H k+i X k + i|k+i-1 ,

[0088] 4.3 Update the measurement noise distribution value:

[0089]

[0090] Among them, T λ is the threshold for chi-square fault detection.

[0091] As a further improvement of the present invention, step 5 calculates the gain matrix as follows:

[0092] 5.1 Establish loss function:

[0093]

[0094] Among them, σ represents the kernel width of the Gaussian kernel function, X k+i|k+i-1 Indicates the state of the prediction, e k+i，p Indicates e k+i The pth component of n represents the measurement dimension.

[0095] 5.2 Loss Function J MCKF In X k+i Take the derivative and set the solution to 0, then

[0096]

[0097] Among them, ψ k =diag[G σ (e ki )].

[0098] X k+i It is expressed in the form of Kalman estimation:

[0099] X k+i =X k+i|k+i-1 +K k+i (Z k+i -H k+i X k+i|k+i-1 )

[0100] in,

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

[0102] The present invention proposes a dual-transponder-assisted SINS / USBL combined navigation method, 1) establishing a SINS / USBL combined navigation state equation, 2) establishing a SINS / USBL slant range difference measurement equation, 3) establishing a measurement time sequence arrival inconsistency measurement model, 4) solving noise parameters according to the inverse Gamma distribution, and calculating the measurement noise estimation result, 5) calculating the gain matrix according to the maximum entropy principle and the measurement noise estimation result estimated in step 4), 6) performing Kalman filtering fusion on the model of step 5, performing feedback correction on SINS, and outputting the navigation result. Repeat the above steps until the navigation is completed. The proposed tight combination model based on slant range difference can suppress the unmodelable error in the USBL positioning process, and the robust filtering method based on maximum entropy and variational Bayes performs optimal estimation on the unknown or time-varying noise of SINS / USBL, thereby improving the positioning accuracy in complex underwater environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0103] Figure 1 It is a schematic diagram of a close combination of dual transponders with slant range difference;

[0104] Figure 2 This is a schematic diagram of the dual-transponder assisted combined navigation principle described in the present invention. DETAILED DESCRIPTION

[0105] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0106] The present invention proposes a dual-transponder assisted SINS / USBL combined navigation method, wherein the dual-transponder slant range difference close combination schematic diagram is as follows: Figure 1 As shown in the figure, the principle diagram of combined navigation assisted by dual transponders is as follows Figure 2 As shown:

[0107] Step 1: Establish the SINS / USBL combined navigation state equation:

[0108] The system state model is:

[0109] X k =F k,k-1 X k-1 +W k

[0110] Among them, F k,k-1 is the state transfer matrix, calculated according to the SINS error equation, and W(k) is the system noise. The state vector X k for:

[0111]

[0112] in,

[0113] XUSBL =[θ x θ y θ z δα δβ δD] T .

[0114] Where, δθ=[θ x θ y θ z ] T represents the installation angle error, [δα δβ] T φ represents the angle measurement error, and δD represents the distance measurement error. n =[φ x φ y φ z ] T Indicates the pitch angle, roll angle, and heading angle errors of the carrier; δV = [δV E δV N δV N ] T Indicates the carrier's eastward, northward, and celestial velocity errors; δP = [δL δλ δh] T Indicates the carrier's geographic longitude, latitude, and altitude errors; Indicates the acceleration zero bias of SINS; [ε x ε y ε z ] T Indicates the gyro bias of SINS.

[0115] 1. Step 2: Establish the SINS / USBL slope range difference measurement equation:

[0116] At time k, the measurement information of transponder A is received, and the system measurement model is:

[0117] Z(A)=H(A)X k +V

[0118] Among them, Z represents the system measurement value, H represents the measurement equation transfer matrix, and V represents the measurement noise.

[0119] The measurement information received by USBL from transponder A can be expressed as:

[0120]

[0121] The relative position vector of transponder A in the a system is obtained by SINS position calculation: Then the relative position vector of transponder A is The relationship between azimuth and slant range is expressed as:

[0122]

[0123] Taking partial differential of the above formula we can get:

[0124]

[0125] Among them, vector For the convenience of expression, Then the matrix The expression is:

[0126]

[0127] The relative position vector of transponder A calculated according to the position of SINS is:

[0128]

[0129] in, represents the transfer matrix corresponding to the installation error angle between SINS and USBL, represents the SINS attitude transfer matrix, The coordinate transformation matrix from the earth coordinate system to the navigation system, is the relative position vector in the earth coordinate system, expressed as:

[0130]

[0131] The slant range azimuth information calculated using the SINS position and the transponder A position is expressed as:

[0132]

[0133] Where [α β R] T is the true value of azimuth and slant range, and the matrices are

[0134] in,

[0135]

[0136] The system measurement is:

[0137]

[0138] Among them, H u =[0 0 -1], δU=[δα δβ δD] T .

[0139] The expression of the observation matrix H(A) is:

[0140] H(A)=[H a 0 3×3 H p 03×6 H I H u ]

[0141] Accordingly, at time k+i, the measurement model is established based on the position of transponder B:

[0142] Z(B)=H(B)X(k+i)+V

[0143] Step 3: Establish a measurement timing inconsistency model. The steps are as follows:

[0144] The system receives the measurement information of transponder A at time k and the measurement information of transponder B at time k+i. The slant range difference model can be expressed as:

[0145]

[0146] The relationship between system states at different times can be expressed as:

[0147]

[0148] Among them, Γ k+i,k =F k+i,k+i-1 F k+i-1,k+i-2 ...F k+1,k

[0149] According to the above formula, the state matrix X k It can be expressed as:

[0150]

[0151] The slope range difference model can be expressed as:

[0152]

[0153] Among them, H k+i (3) represents the third row of the slant range observation matrix at time k+i, Represents the slant range error at time k+i:

[0154]

[0155] Assumptions The noise distribution of the slant range difference at time k+i is:

[0156]

[0157] In summary, the measurement model based on dual transponders at time k+i is:

[0158]

[0159] Step 4: Solve the noise parameters according to the inverse Gamma distribution and calculate the measurement noise estimation result. The steps are as follows:

[0160] 4.1 At time k+i, the joint probability density is expressed as Gaussian and inverse Gamma distribution:

[0161]

[0162] Among them, p(X k+i , R k+i |Z 1：k+i-1 ) represents the joint probability density function, N(X k+i |X k+i|k+i-1 ,P k+i|k+i-1 ) represents Gaussian distribution, P k+i|k+i-1 represents the state transfer matrix, represents the inverse Gamma distribution, α k，i+i represents the shape parameter, β k+i，i represents the scale parameter, and the iterative update is as follows:

[0163] α k,i =0.5+α k+i|(k+i-1)，i

[0164]

[0165] 4.2 Calculate the chi-square test value:

[0166]

[0167] Among them, r k+i =Z k+i -H k+i X k+i|k+i-1 ,

[0168] 4.3 Update the measurement noise distribution value:

[0169]

[0170] Among them, T λ is the threshold for chi-square fault detection.

[0171] Step 5: Calculate the gain matrix based on the maximum entropy principle and the measurement noise estimation result:

[0172] 5.1 Establish loss function:

[0173]

[0174] Among them, σ represents the kernel width of the Gaussian kernel function, X k+i|k+i-1 Indicates the state of the prediction, e k+i，p Indicates e k+iThe pth component of n represents the measurement dimension.

[0175] 5.2 Loss Function J MCKF In X k+i Take the derivative and set the solution to 0, then

[0176]

[0177] Among them, ψ k =diag[G σ (e ki )].

[0178] X k+i It is expressed in the form of Kalman estimation:

[0179] X k+i =X k+i|k+i-1 +K k+i (Z k+i -H k+i X k+i|k+i-1 )

[0180] in, Step 6: Perform Kalman filter fusion. The steps are as follows:

[0181] State one-step prediction:

[0182] x k+i|k+i-1 =F k+i-1 x k+i-1

[0183] State one-step prediction mean square error:

[0184]

[0185] Filter gain:

[0186]

[0187] State Estimation:

[0188] X k+i =X k+i|k+i-1 +K k+i (Z k+i -H k+i X k+i|k+i-1 )

[0189] State estimation mean square error matrix:

[0190] P k+i =P k+i|k+i-1 -K k H k P k+i|k+i-1

[0191] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. A dual-transponder assisted SINS / USBL integrated navigation method, characterized in that: include: Step 1: Establish the SINS / USBL combined navigation state equation: The system state model is: X k =F k,k-1 X k-1 +W k Among them, F k,k-1 is the state transfer matrix, calculated according to the SINS error equation, W(k) is the system noise, and the state vector X k for: in, X USBL =[θ x i y i z d α d β d D ] T ; Where, δθ=[θ x θ y θ z ] T represents the installation angle error, [δα δβ] T represents the angle measurement error, δD represents the distance measurement error; φ n =[φ x φ y φ z ] T Indicates the pitch angle, roll angle, and heading angle errors of the carrier; δV = [δV E δV N δV N ] T Indicates the carrier's eastward, northward, and celestial velocity errors; δP = [δL δλ δh] T Indicates the carrier's geographic longitude, latitude, and altitude errors; Indicates the acceleration zero bias of SINS; [ε x ε y ε z ] T Indicates the gyro bias of SINS; Step 2: Establish the SINS / USBL slope range difference measurement equation: At time k, the measurement information of transponder A is received, and the system measurement model is: Z(A)=H(A)X k +V Where Z represents the system measurement value, H represents the measurement equation transfer matrix, and V represents the measurement noise; The measurement information received by USBL from transponder A is: The relative position vector of transponder A in the a system is obtained by SINS position calculation: Then the relative position vector of transponder A is The relationship between azimuth and slant range is expressed as: but: The relative position vector of transponder A calculated according to the position of SINS is: in, represents the transfer matrix corresponding to the installation error angle between SINS and USBL, represents the SINS attitude transfer matrix, The coordinate transformation matrix from the earth coordinate system to the navigation system, is the relative position vector in the earth coordinate system, expressed as: The slant range azimuth information calculated using the SINS position and the transponder A position is expressed as: Where [α β R] T is the true value of azimuth and slant range, and the matrices are in, The system measurement is: Among them, H u =[0 0 -1],δU=[δα δβ δD] T ; The expression of the observation matrix H(A) is: H(A)=[H a 0 3×3 H p 0 3×6 H I H u ] Accordingly, at time k+i, the measurement model is established based on the position of transponder B: Z(B)=H(B)X(k+i)+V Step 3: Establish a measurement timing inconsistency model. The steps are as follows: The system receives the measurement information of transponder A at time k and the measurement information of transponder B at time k+i. The slant range difference model is: The relationship between the system states at different times is: X k+i =F k+i,k+i-1 X k+i-1 +W k+i Among them, Γ k+i,k =F k+i，k+i-1 F k+i-1,k+i-2 ...F k+1,k According to the above formula, the state matrix X k for: The slope difference model is Among them, H k+i (3) represents the third row of the slant range observation matrix at time k+i, Represents the slant range error at time k+i: Assumptions The noise distribution of the slant range difference at time k+i is: In summary, the measurement model based on dual transponders at time k+i is: Step 4: Solve the noise parameters according to the inverse Gamma distribution and calculate the measurement noise estimation result; Step 5: Estimate the result based on the maximum entropy principle and measurement noise; Step 6: Perform Kalman filter fusion. The steps are as follows: State one-step prediction: x k+i|k+i-1 =F k+i-1 x k+i-1 State one-step prediction mean square error: Filter gain: State Estimation: X k+i =X k+i|k+i-1 +K k+i (Z k+i -H k+i X k+i|k+i-1 ) State estimation mean square error matrix: P k+i =P k+i|k+i-1 -K k H k P k+i|k+i-1 。 2. A dual-responder assisted SINS / USBL integrated navigation method according to claim 1, characterized in that: Step 4 calculates the measurement noise estimation result as follows: 4.1 At time k+i, the joint probability density is expressed as Gaussian and inverse Gamma distribution: Among them, p(X k+i , R k+i |Z 1:k+i-1 ) represents the joint probability density function, N(X k+i |X k+i|k+i-1 ,P k+i|k+i-1 ) represents Gaussian distribution, P k+i|k+i-1 represents the state transfer matrix, represents the inverse Gamma distribution, α k，i+i represents the shape parameter, β k+i，i represents the scale parameter, and the iterative update is as follows: α k，i ＝0.5+α k+i|(k+i-1),i 4.2 Calculate the chi-square test value: Among them, r k+i =Z k+i -H k+i X k+i|k+i-1 , 4.3 Update the measurement noise distribution value: Among them, T λ is the threshold for chi-square fault detection.

3. A dual-responder assisted SINS / USBL integrated navigation method according to claim 2, characterized in that: Step 5 calculates the gain matrix as follows: 5.1 Establish loss function: Among them, σ represents the kernel width of the Gaussian kernel function, X k+i|k+i-1 Indicates the state of the prediction, e k+i，p Indicates e k+i The pth component of n represents the measurement dimension; 5.2 Loss Function J MCKF In X k+i Take the derivative and set the solution to 0, then Among them, ψ k =diag[G σ (e ki )] X k+i It is expressed in the form of Kalman estimation: X k+i =X k+i|k+i-1 +K k+i (Z k+i -H k+i X k+i|k+i-1 ) in,

Images