CN115307643A - Double-responder assisted SINS/USBL combined navigation method - Google Patents

Abstract

A double-responder assisted SINS/USBL combined navigation method comprises the steps of 1) establishing an SINS/USBL combined navigation state equation, 2) establishing an SINS/USBL slant range difference measurement equation, 3) establishing a measurement model with inconsistent arrival of measurement time sequences, 4) solving noise parameters according to inverse Gamma distribution, calculating a measurement noise estimation result, 5) calculating a gain matrix according to a maximum entropy principle and the measurement noise estimation result estimated in the step 4), and 6) performing Kalman filtering fusion on the model in the step 5, performing feedback correction on SINS, and outputting a navigation result. And repeating the steps until the navigation is finished. The tight combination model based on the slope distance difference can inhibit the non-modeling error in the USBL positioning process, and the robust filtering method based on the maximum entropy and the variational Bayes can perform optimal estimation on the unknown or time-varying noise of the SINS/USBL, thereby improving the positioning accuracy under the underwater complex environment.

Description

Double-responder assisted SINS/USBL combined navigation method

Technical Field

The invention belongs to an SINS/USBL combined navigation technology of an underwater vehicle, and particularly relates to a SINS/USBL combined navigation method assisted by a double responder.

Background

In general, besides random noise, the ultra-short baseline measurement noise includes delay error caused by signal processing, sound velocity bending error caused by physical property changes such as water temperature and salinity, system clock error, multipath effect and other errors which cannot be modeled. The traditional noise model only considers random noise or unifies all the noise into random noise of the sensor, which is inaccurate for USBL time varying systems, and for the above problems, the current existing solutions are as follows:

application No. 2021/10/29, application No.: CN202111268907.2, patent name: a maximum entropy adaptive robust estimation-based USBL slant range correction method comprises the following specific scheme that 1) an SINS attitude transfer matrix, speed and position information at the time when a USBL sends a request message are recorded, 2) the azimuth angle at the time when the USBL receives a responder message, SINS attitude, speed and position information are recorded, 3) the slant range of the USBL is corrected according to the calculation result of the sending time and the receiving time SINS, 4) the SINS/USBL tight combination system model is established according to the corrected USBL slant range and the collected azimuth angle, elevation angle and SINS calculation information, 5) a dynamic model error adaptive factor is calculated, a gain matrix is calculated according to the maximum entropy principle, and 6) Kalman filtering fusion is carried out on the model of 5), the SINS is subjected to feedback correction, and a navigation result is output. And repeating the steps until the navigation is finished, establishing a change model between the USBL transmitting time and the USBL receiving time by utilizing the attitude and the position measured by the strapdown inertial navigation system, and deducing the slope distance correction value of the receiving time. Finally, in order to suppress the influence of abnormal noise on the positioning accuracy.

The method adopts a traditional positioning model based on a single transponder and adopts an anti-difference filtering algorithm based on maximum entropy to eliminate abnormal outlier information in a complex environment. On the basis of a traditional single-transponder model, a SINS/USBL combined positioning model based on double transponders is provided, and a combined navigation mode based on slope distance difference is adopted to eliminate unmoldable errors such as sound velocity bending, multipath effect, signal processing time delay and the like in an underwater environment and deduce a time delay consistent model under relative motion of the double transponders. On the basis of maximum entropy robust estimation, maximum entropy robust filtering and a combined navigation noise self-adaptive model are combined, and meanwhile, the problem of abnormal outliers in unknown or time-varying and complex environments of a system noise model is solved.

Application date 2018/9/18, application No.: CN201811087892.8, patent name: the patent specifically discloses a USBL/SINS tight combination navigation positioning method based on hybrid derivative-free extended Kalman filtering, which is particularly suitable for positioning underwater equipment. The invention consists of an ultra-short baseline underwater sound positioning system USBL and a strapdown inertial navigation system SINS, and adopts a hybrid derivative-free extended Kalman filter HDEKF to carry out combined navigation. The ultra-short baseline system obtains a slant-distance measurement value between the transponder and the hydrophone by calculating the one-way propagation time of the ultrasonic signal between the transponder and the hydrophone, and obtains an observation equation by a coordinate conversion formula. And establishing an error state equation according to an error transfer formula of the strapdown inertial navigation system. And finally, performing hybrid derivative-free Kalman filtering, performing time updating by using standard linear Kalman filtering, and performing measurement updating by using derivative-free extended filtering. The method can effectively improve the navigation precision and stability of the USBL/INS integrated navigation system and reduce the real-time calculation amount.

The method adopts an inclined distance difference model based on multiple hydrophones, adopts a combined navigation model of a single transponder unit in an application background, and is easy to cause singularity of matrix operation because the distances between the hydrophones are close and the inclined distance difference is small. The slope distance difference combined navigation model of the double transponders takes a plurality of transponders as an application background, the slope distance difference between the transponders is obvious, and the matrix operation is stable.

Application No. 2020/6/8, application No.: CN202010513750.4, patent name: an AUV (autonomous Underwater vehicle) integrated navigation method and system based on M estimation provide a filtering algorithm based on generalized maximum likelihood estimation (M estimation), and apply M-estimation algorithm under the condition that system measurement noise is non-Gaussian distribution, especially mixed Gaussian distribution (symmetric interference near Gaussian), and weight measurement residual errors and state prediction residual errors through an influence function and a weight function, so that the influence of measurement abnormal peak values on a navigation system is reduced. The algorithm is applied to the SINS/USBL combined navigation system, under the conditions that non-Gaussian measurement noise and measurement abnormality are generated by an underwater acoustic sensor due to multipath effects, compared with a standard Kalman filtering algorithm and an M estimation filtering algorithm, the position and speed error accuracy of the latter is obviously improved relative to the former and the mixed Gaussian model is more seriously polluted, the SINS/USBL combined navigation system based on the M estimation filtering has more obvious filtering effect, and the robustness and the anti-interference performance are relatively better.

A robust filtering algorithm based on Huber M estimation is adopted to solve the problem of positioning accuracy reduction caused by an underwater abnormal outlier, and the robust filtering algorithm adopting the maximum entropy is proposed to replace the Huber M estimation algorithm. And in order to eliminate the positioning accuracy reduction caused by uncertainty and time variation of the system noise model, a variational Bayesian method based on Gamma distribution is adopted to estimate the system noise model.

Application date 2015/9/1, application number: CN201510551249.6, patent name: the invention discloses a SINSUSBL tight coupling algorithm, belongs to the technical field of tight coupling navigation algorithms, and particularly relates to a multi-transponder SINS/USBL combined navigation algorithm. The technical scheme of the invention designs a novel multi-transponder SINS/USBL combined navigation scheme; and carrying out error modeling and simulation analysis on the multi-transponder SINS/USBL tight coupling navigation algorithm. The invention can avoid the influence of the error of the USBL angle measurement and the error of the mounting angle from the USBL to the SINS on the positioning precision, does not need to calibrate the mounting angle from the USBL to the SINS, and is more convenient to use. Simulation results show that the method can obtain higher positioning precision than the existing single-responder SINS/USBL combined navigation algorithm.

The measuring equation of the integrated navigation is established by adopting the relative distance between the SINS and the plurality of responders, the information of the plurality of responders is required to be received at the same time, and the system is difficult to realize. The combined navigation model is established by using the relative distance difference between the SINS and the plurality of transponders, and the proposed delay consistent model under the relative motion of the two transponders can solve the combined navigation algorithm under the condition that the transponders arrive at different times.

In order to weaken and eliminate common errors of underwater acoustic signal propagation, the application provides a high-precision tight coupling method based on inclination distance difference under the condition of double-water-level transponders so as to improve the positioning precision of the combined navigation system. In addition, a measurement arrival inconsistency model is proposed in consideration of timing arrival inconsistency caused by a relative positional change between a carrier and a repeater. Meanwhile, in order to inhibit the influence of underwater noise on positioning accuracy, a robust Kalman filter based on the maximum correlation entropy is introduced. Aiming at the influence of the noise characteristics of an unknown or time-varying system on the robust filtering estimation result, a variational Bayesian method based on Gaussian and inverse Gamma mixed distribution is deduced, and the self-adaptive estimation of the variance matrix of the unknown noise is realized.

Disclosure of Invention

In order to solve the problems in the prior art, the invention provides a novel combined navigation method of assisted strapdown inertial navigation/ultra-short baseline by using a double-underwater responder. Firstly, on the basis of the traditional tight coupling model, a new tight coupling model based on the slope distance difference is provided. To account for measurement timing differences due to relative changes in position between the carrier and the dual transponder, a measurement timing inconsistency model is introduced. Secondly, an improved robust maximum entropy kalman filter is proposed. The method provides the mixture distribution of Gaussian and inverse Gamma to simulate the non-stationary thick tail underwater noise, and can be used for the optimal estimation of unknown or time-varying outlier characteristics.

The invention provides a double-responder assisted SINS/USBL combined navigation method, which comprises the following steps:

step 1: establishing an SINS/USBL combined navigation state equation:

the system state model is:

X _k ＝F _k，k-1 X _k-1 +W _k

wherein, F _k，k-1 And W (k) is system noise calculated according to an SINS error equation as a state transition matrix. State vector X _k Comprises the following steps:

wherein the content of the first and second substances,

X _USBL ＝[θ _x θ _y θ _z δα δβ δD] ^T ；

wherein δ θ = [ θ = _x θ _y θ _z ] ^T Represents the mounting angle error, [ delta [ alpha ] delta [ beta ]] ^T Representing and measuring angle errors, and delta D represents a distance measurement error; phi is a ⁿ ＝[φ _x φ _y φ _z ] ^T Representing errors of a carrier pitch angle, a roll angle and a course angle; δ V = [ δ V = _E δV _N δV _N ] ^T Representing east, north and sky speed errors of the carrier; δ P = [ δ L δ λ δ h] ^T Representing the geographic longitude, latitude and altitude error of the carrier;

representing acceleration zero offset of the SINS; [ epsilon ] _x ε _y ε _z ] ^T Representing a gyro zero bias of the SINS;

step 2: establishing an SINS/USBL slope distance difference measurement equation:

receiving the measurement information of the transponder A at the moment k, wherein a system measurement model is as follows:

Z(A)＝H(A)X _k +V

wherein Z represents a system measurement value, H represents a measurement equation transfer matrix, and V represents measurement noise;

the measurement information that USBL receives transponder a is represented as:

obtaining a relative position vector of the responder A under the system a by using SINS position calculation

The relative position vector of transponder a

And the relation expression between the azimuth angle and the slope distance is as follows:

partial differentiation of the above equation yields:

wherein, the vector

For convenience of expression, remember

Then matrix

The expression of (a) is:

the relative position vector of transponder a calculated from the position of the SINS is:

wherein the content of the first and second substances,

a transfer matrix corresponding to the installation error angle between the SINS and the USBL,

representing the SINS attitude transition matrix,

a coordinate transformation matrix from the terrestrial coordinate system to the navigation system,

is a relative position vector under the terrestrial coordinate system and is expressed as:

calculating and obtaining the information of the slant range azimuth angle by using the SINS position and the position of the transponder A, and expressing the information as follows:

wherein, [ alpha beta ] R] ^T The matrices are true values of azimuth and slope, respectively

Wherein the content of the first and second substances,

the system measurement is:

wherein 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, the establishment of the metrology model according to the position of transponder B is received at time k + i:

Z(B)＝H(B)X(k+i)+V

and 3, step 3: establishing a measurement time sequence inconsistency model, which comprises the following steps:

the system receives the measurement information of the responder A at the moment k and receives the measurement information of the responder B at the moment k + i, and the slope distance difference model is expressed as follows:

the system state relationship at different times is expressed as:

wherein, gamma is _k+i，k ＝F _k+i，k+i-1 F _k+i-1k+i-2 ...F _k+1，k

According to the above formula, the state matrix X _k Expressed as:

the slope difference model is expressed as:

wherein H _k+i (3) The third row of the slant-viewing matrix representing the time k + i,

represents the slope error at time k + i:

suppose that

The noise distribution of the slope difference at time k + i is:

to sum up, the measurement model based on the dual transponders at time k + i is:

and 4, step 4: solving noise parameters according to the inverse Gamma distribution, and calculating a measurement noise estimation result;

and 5: estimating a result according to a maximum entropy principle and measurement noise;

step 6: performing Kalman filtering fusion, comprising the following steps:

and (3) 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:

and (3) filtering gain:

and (3) 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 。

as a further improvement of the present invention, the result of calculating the measurement noise estimation in step 4 is specifically as follows:

4.1 at time k + i, the joint probability density is expressed as a Gaussian and inverse Gamma distribution:

wherein, p (X) _k+i ，R _k+i |Z _1：k+i-1 ) Representing a joint probability density function, N (X) _k+i |X _k+i|k+i-1 ，P _k+i|k+i-1 ) Denotes a Gaussian distribution, P _k+i|k+i-1 A state transition matrix is represented that represents the state transition,

representing an inverse Gamma distribution, alpha _k，i+i Denotes the shape parameter, beta _k+i，i Scale parameters are represented, and the iteration is updated as follows:

α _k，i ＝0.5+α _{k+i|(k+i-1)，i}

4.2 calculating chi-square detection value:

wherein the content of the first and second substances,

4.3 updating the measurement noise distribution value:

wherein, T _λ Is the threshold for chi-square fault detection.

As a further improvement of the present invention, the step 5 of calculating the gain matrix specifically includes the following steps:

5.1 establish the loss function:

where α represents the kernel width of the Gaussian kernel function, X _k+i|k+i-1 Indicating a prediction of the state, e _k+i，p Denotes e _k+i The p-th component of (a) is,

n represents the measurement dimension.

5.2 pairs loss function J _MCKF At X _k+i The derivative is obtained and the solution is 0, then

Wherein psi _k ＝diag[G _σ (e _ki )].

Mixing X _k+i 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 )

wherein the content of the first and second substances,

compared with the prior art, the invention has the beneficial effects that:

the invention provides a double-responder assisted SINS/USBL combined navigation method, which comprises the steps of 1) establishing an SINS/USBL combined navigation state equation, 2) establishing an SINS/USBL skew difference measurement equation, 3) establishing a measurement model with inconsistent arrival of measurement time sequences, 4) solving noise parameters according to inverse Gamma distribution, calculating a measurement noise estimation result, 5) calculating a gain matrix according to a maximum entropy principle and the measurement noise estimation result estimated in the step 4), and 6) performing Kalman filtering fusion on the model in the step 5, performing feedback correction on SINS, and outputting a navigation result. And repeating the steps until the navigation is finished. The tight combination model based on the slope distance difference can inhibit the unmoldable error in the USBL positioning process, and the robust filtering method based on the maximum entropy and the variational Bayes can perform optimal estimation on the unknown or time-varying noise of the SINS/USBL, so that the positioning accuracy under the underwater complex environment is improved.

Drawings

FIG. 1 is a schematic diagram of a dual transponder pitch differential assembly;

FIG. 2 is a diagram of a dual transponder assisted integrated navigation concept as described herein.

Detailed Description

The invention is described in further detail below with reference to the following figures and embodiments:

the invention provides a double-responder assisted SINS/USBL combined navigation method, wherein a schematic diagram of a double-responder slant-range differential tight combination is shown in figure 1, and a schematic diagram of a double-responder assisted combined navigation is shown in figure 2:

step 1: establishing an SINS/USBL combined navigation state equation:

the system state model is:

X _k ＝F _k，k-1 X _k-1 +W _k

wherein, F _k，k-1 And W (k) is system noise calculated according to an SINS error equation as a state transition matrix. State vector X _k Comprises the following steps:

wherein the content of the first and second substances,

X _USBL ＝[θ _x θ _y θ _z δα δβ δD] ^T 。

wherein δ θ = [ θ = _x θ _y θ _z ] ^T Represents the mounting angle error, [ delta [ alpha ] delta [ beta ]] ^T And indicating and measuring angle errors, and delta D indicating distance measurement errors.

φ ⁿ ＝[φ _x φ _y φ _z ] ^T Presentation carrierPitch angle, roll angle, course angle error; δ V = [ δ V = _E δV _N δV _N ] ^T Representing east, north and sky speed errors of the carrier; δ P = [ δ L δ λ δ h] ^T Representing the geographic longitude, latitude and altitude error of the carrier;

representing acceleration zero offset of the SINS; [ epsilon ] _x ε _y ε _z ] ^T Representing the gyro zero bias of SINS.

1. Step 2: establishing an SINS/USBL slope distance difference measurement equation:

receiving the measurement information of the transponder A at the moment k, wherein a system measurement model is as follows:

Z(A)＝H(A)X _k +V

wherein Z represents a system measurement value, H represents a measurement equation transfer matrix, and V represents measurement noise.

The measurement information that USBL receives from transponder a may be expressed as:

the relative position vector of the responder A under the system a is obtained by SINS position calculation

The relative position vector of transponder a

And the relation expression between the azimuth angle and the slope distance is as follows:

partial differentiation of the above equation gives:

wherein, the vector

For convenience of expression, remember

Then matrix

The expression of (a) is:

the relative position vector of transponder a calculated from the position of the SINS is:

wherein the content of the first and second substances,

a transfer matrix corresponding to an installation error angle between the SINS and the USBL,

representing the SINS attitude transition matrix,

a coordinate transformation matrix from the terrestrial coordinate system to the navigation system,

is a relative position vector under the terrestrial coordinate system and is expressed as:

calculating and obtaining the information of the slant range azimuth angle by using the SINS position and the position of the transponder A, and expressing the information as follows:

wherein, [ alpha β R [ ]] ^T The matrices are true values of azimuth and slope, respectively

Wherein, the first and the second end of the pipe are connected with each other,

the system measurement is:

wherein 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, the establishment of the metrology model according to the position of transponder B is received at time k + i:

Z(B)＝H(B)X(k+i)+V

and step 3: establishing a model for measuring time sequence inconsistency, which comprises the following steps:

the system receives the measurement information of the transponder a at the time k, receives the measurement information of the transponder B at the time k + i, and the slope difference model can be expressed as:

the system state relationships at different times can be expressed as:

wherein, gamma is _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 Can be expressed as:

the slope difference model can be expressed as:

wherein H _k+i (3) The third row of the skew observation matrix representing the time k + i,

represents the pitch error at time k + i:

suppose that

The noise distribution of the slope difference at time k + i is:

in summary, the measurement model for the time k + i based on the dual transponder is:

and 4, step 4: solving noise parameters according to the inverse Gamma distribution, and calculating a measurement noise estimation result, wherein the steps are as follows:

4.1 at time k + i, the joint probability density is expressed as a Gaussian and inverse Gamma distribution:

wherein, p (X) _k+i ，R _k+i |Z _1：k+i-1 ) Representing a joint probability density function, N (X) _k+i |X _k+i|k+i-1 ，P _k+i|k+i-1 ) Denotes a Gaussian distribution, P _k+i|k+i-1 A state transition matrix is represented that represents the state transition,

representing an inverse Gamma distribution, alpha _k，i+i Denotes the shape parameter, beta _k+i，i Scale parameters are represented, and the iteration is updated as follows:

α _k，i ＝0.5+α _{k+i|(k+i-1)，i}

4.2 calculating chi-square detection value:

wherein the content of the first and second substances,

4.3 updating the measurement noise distribution value:

wherein, T _λ Is the threshold for chi-square fault detection.

And 5: calculating a gain matrix according to a maximum entropy principle and a measurement noise estimation result:

5.1 establish the loss function:

where σ denotes the kernel width, X, of the four Gaussian kernel function _k+i|k+i-1 Indicating a prediction of the state, e _k+i，p Denotes e _k+i The p-th component of (a) is,

n represents the measurement dimension.

5.2 pairs loss function J _MCKF At X _k+i The derivative is obtained and the solution is 0, then

Wherein psi _k ＝diag[G _σ (e _ki )].

Mixing X _k+i 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 )

wherein, the first and the second end of the pipe are connected with each other,

step 6: performing Kalman filtering fusion, comprising the following steps:

and (3) 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:

filtering gain:

and (3) state estimation:

X _k+i ＝X _k+i|k+i-1 +K _k+i (Z _{k machine} -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

the above description is only a preferred embodiment of the present invention, and is not intended to limit the present invention in any way, and any modifications or equivalent variations made in accordance with the technical spirit of the present invention may fall within the scope of the present invention as claimed.

Claims (3)

1. A double-responder assisted SINS/USBL combined navigation method is characterized in that: the method comprises the following steps:

step 1: establishing an SINS/USBL combined navigation state equation:

the system state model is:

X _k ＝F _k，k-1 X _k-1 +W _k

wherein, F _k，k-1 Is a state transition matrix calculated according to SINS error equation, W (k) is system noise, and state vector X _k Comprises the following steps:

wherein, the first and the second end of the pipe are connected with each other,

X _USBL ＝[θ _x θ _y θ _z δα δβ δD] ^T ；

wherein δ θ = [ θ = _x θ _y θ _z ] ^T Represents the mounting angle error, [ delta [ alpha ] delta [ beta ]] ^T Representing and measuring angle errors, and delta D represents a distance measurement error;

φ ⁿ ＝[φ _x φ _y φ _z ] ^T representing errors of a carrier pitch angle, a roll angle and a course angle; δ V = [ δ V = _E δV _N δV _N ] ^T Representing east, north and sky speed errors of the carrier; δ P = [ δ L δ λ δ h] ^T Representing the geographic longitude, latitude and altitude error of the carrier;

representing acceleration zero offset of the SINS; [ epsilon ] _x ε _y ε _z ] ^T Representing a gyro zero bias of the SINS;

step 2: establishing an SINS/USBL slope distance difference measurement equation:

receiving the measurement information of the transponder A at the moment k, wherein a system measurement model is as follows:

Z(A)＝H(A)X _k +V

wherein Z represents a system measurement value, H represents a measurement equation transfer matrix, and V represents measurement noise;

the measurement information of the responder a received by the USBL is:

the relative position vector of the responder A under the system a is obtained by SINS position calculation

The relative position vector of transponder a

And the relation expression between the azimuth angle and the slope distance is as follows:

then:

wherein the vector

For convenience of expression, remember

Then matrix

The expression of (a) is:

the relative position vector of transponder a calculated from the position of the SINS is:

wherein the content of the first and second substances,

a transfer matrix corresponding to the installation error angle between the SINS and the USBL,

representing the SINS attitude transition matrix,

a coordinate transformation matrix from the terrestrial coordinate system to the navigation system,

is a relative position vector under the terrestrial coordinate system and is expressed as:

calculating and obtaining the information of the slant range azimuth angle by using the SINS position and the position of the transponder A, and expressing the information as follows:

wherein, [ alpha β R [ ]] ^T The matrices are true values of azimuth and slope, respectively

Wherein the content of the first and second substances,

the system measurement is:

wherein 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, the establishment of the metrology model according to the position of transponder B is received at time k + i:

Z(B)＝H(B)X(k+i)+V

and 3, step 3: establishing a measurement time sequence inconsistency model, which comprises the following steps:

the system receives the measurement information of the responder A at the moment k and receives the measurement information of the responder B at the moment k + i, and the slope distance difference model is as follows:

the system state relationship at different time is as follows:

wherein, gamma is _k+i，k ＝F _k+i，k+i-1 F _{k+i-1，k+i-2} …K _k+i，k

According to the above formula, the state matrix X _k Comprises the following steps:

the skew difference model is

Wherein H _k+i (3) The third row of the skew observation matrix representing the time k + i,

represents the slope error at time k + i:

suppose that

The noise distribution of the slope difference at time k + i is:

in summary, the measurement model for the time k + i based on the dual transponder is:

and 4, step 4: solving noise parameters according to the inverse Gamma distribution, and calculating a measurement noise estimation result;

and 5: estimating a result according to a maximum entropy principle and a measurement noise;

step 6: performing Kalman filtering fusion, comprising the following steps:

and (3) 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:

and (3) filtering gain:

and (3) 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. the method as claimed in claim 1, wherein the calculation of the measurement noise estimation result in step 4 is as follows:

4.1 at time k + i, the joint probability density is expressed as a Gaussian and inverse Gamma distribution:

wherein, p (X) _k+i ，R _k+i |Z _1：k+i-1 ) Representing a joint probability density function, N (X) _k+i |X _k+i|k+i-1 ，P _k+i|k+i-1 ) Denotes a Gaussian distribution, P _k+i|k+i-1 A state transition matrix is represented that represents the state transition,

representing an inverse Gamma distribution, alpha _k，i+i Denotes the shape parameter, beta _k+i，i Scale parameters are represented, and the iteration is updated as follows:

α _k，i ＝0.5+α _{k+i|(k+i-1)，i}

4.2 calculating chi-square detection value:

wherein r is _k+i ＝Z _k+i -H _k+i X _k+i|k+i-1 ,

4.3 updating the measurement noise distribution value:

wherein, T _λ Is the threshold for chi-square fault detection.

3. The method of claim 1, wherein the step 5 of computing the gain matrix is specifically as follows:

5.1 establish the loss function:

where σ represents the kernel width of the Gaussian kernel function, X _k+i|k+i-1 Indicating a prediction of the state, e _k+i，p Denotes e _k+i The p-th component of (a) is,

n represents the measurement dimension.

5.2 pairs loss function J _MCKF At X _k+i The derivative is obtained and the solution is 0, then

Wherein psi _k ＝diag[G _σ (e _ki )].

Mixing X _k+i 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 )

wherein the content of the first and second substances,

Images