Electromagnetic log assisted ship integrated navigation method

Abstract

The application discloses a kind of electromagnetic log auxiliary ship integrated navigation method, first according to the influence of ocean current and measurement noise time-varying to electromagnetic log speed measurement, establishes a fixed model set, includes two sub-models, and is adaptively generated a model set by the method of expected model expansion;Secondly, the coupling term of state vector and measurement noise covariance matrix is decoupled using variational bayes based on mean field theory, and a new adaptive filtering formula is derived;Again the estimation fusion of the interactive multiple model (IMM) adaptive filtering result of fixed model set and expected model set is carried out respectively;Finally, the fused estimation result is used to correct the attitude, velocity and position calculated by the strapdown inertial navigation system.The application can eliminate the influence of ocean current and measurement noise time-varying, effectively improve the positioning accuracy of ship integrated navigation.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of navigation positioning, and relates to a ship combined navigation method assisted by an electromagnetic log. BACKGROUND

[0002] A strapdown inertial navigation system (SINS) is one of the main navigation methods for a ship and has advantages such as autonomy and concealment. However, the positioning error of the SINS will gradually diverge over time, and therefore, other external sensors are often used to assist the SINS. Most ships are equipped with an electromagnetic log (EM Log), and the speed measured by the EM Log is usually used to dampen the working mode of the SINS. However, the speed measured by the EM Log is relative to the speed of an ocean current, and the EM Log is used in a complex environment, and the measurement noise is time-varying, which will all cause a large error in the speed measured by the EM Log, and further affect the positioning accuracy of the SINS in the damping working mode. In order to improve the positioning accuracy of the SINS, it is very important to eliminate the influence of the above two aspects. Therefore, a SINS / EM Log combined navigation mode can be used to construct corresponding state equations and measurement equations, and an adaptive filtering method can be used to eliminate the influence of the ocean current and the time-varying measurement noise on the SINS / EM Log combined navigation, so as to improve the positioning accuracy of the combined navigation of the ship.

[0003] In published papers, such as Huang Fengrong's article "SINS / EM Log integrated navigation method based on Gaussian mixture model" in the 27th issue of the Journal of China Inertial Technology, a Gaussian mixture model unscented Kalman filter algorithm for SINS / EM Log integrated navigation is proposed, which approximates the time-varying measurement noise by a Gaussian mixture model and models the current velocity as a constant error to achieve the purpose of real-time and accurate estimation and compensation of inertial navigation system errors. However, in actual situations, the size of the current velocity will change with time, climate, temperature, salinity and other external factors, so a single velocity model often cannot accurately describe the current. In Cho S.Y.'s article "IMM-based INS / EM-Log integrated underwater navigation with sea current estimation function" in the 7th issue of the Journal of Positioning, Navigation, and Timing, an inertial navigation system / EM Log integrated navigation filter is proposed, which has the function of estimating the current by using an interactive multiple model (IMM) filter. The method eliminates the influence of the current by using a multi-model method, but the measurement noise is modeled as a constant, which cannot accurately reflect the influence of the time-varying measurement noise in complex underwater environments. At the same time, in the IMM filter, on the one hand, the model set needs to have enough models to cover the actual models of the system; on the other hand, in order to ensure the calculation speed, it is necessary to use as few models as possible. Obviously, these two requirements are contradictory, and the IMM filter cannot be realized.

[0004] In summary, solving the problems existing in SINS / EM Log integrated navigation and improving the positioning accuracy and robustness of integrated navigation have strong theoretical significance and engineering application value. SUMMARY

[0005] In view of the above prior art, the technical problem to be solved by the present application is to provide an electromagnetic log assisted ship integrated navigation method to improve the positioning accuracy and robustness of integrated navigation and eliminate the influence of current and time-varying measurement noise on integrated navigation.

[0006] To solve the above technical problems, the electromagnetic log assisted ship integrated navigation method of the present application comprises:

[0007] Step 1: Establish a fixed model set M f , M f contains two sub-models m1 and m2, and the state equation and measurement equation of SINS / EM Log integrated navigation of m1 and m2 are established and discretized;

[0008] Step 2: Discretize Mf Run an interactive multiple model adaptive filtering IMM [M f , k-1 ] for the model set at k-1 time to obtain the mode probability of M k-1 at k time f The mode probability of the m j th model in M k State estimation and the covariance matrix of the estimation error Where j=1,2;

[0009] Step 3: Use the expected model expansion method to generate the expected model set E k ;

[0010] Step 4: Run an interactive multiple model adaptive filtering IMM [E k , M k-1 ] for the model set at k-1 time to obtain the mode probability of E k at k time j The mode probability of the m k th model in E k State estimation and the covariance matrix of the estimation error

[0011] Step 5: Estimate the results obtained in steps 2 and 4, and use the fused estimation results to correct the attitude, velocity and position calculated by SINS to obtain the corrected attitude, velocity and position, and determine whether the navigation is completed, if yes, end, otherwise let the time k=k+1, jump to step 2.

[0012] Further, the discrete state equation and the measurement equation are as follows:

[0013]

[0014] In the formula, the superscript j is the m j th submodel; the subscripts k-1 and k are the k-1 and k times; x k and z k are the discrete state vector and the measurement vector respectively; w k-1 and υ k are the process noise vector and the measurement noise vector respectively; w k-1 obeys the Gaussian distribution with zero mean and the covariance matrix Q k-1 , Q k-1 is a diagonal matrix; υ k obeys the Gaussian distribution with unknown variance, and the variance obeys the inverse gamma distribution, the covariance matrix is R k , R k is a diagonal matrix; Φ k|k-1 is the discrete state transition matrix, Φ k|k-1 ≈I+F(tk-1 )T s , T s is the sampling time; Γ k-1 is the discrete process noise coupling matrix, Γ k－1 ≈ G(t k-1 ); H k is the discrete observation matrix.

[0015] Further, step 2 specifically comprises:

[0016] Step 2.1: Let the transition probability of the model be π ij , which represents the probability of transitioning from model m i to model m j , and the probability density transition matrix is:

[0017]

[0018] is the state estimation of the sub-model m j at time k-1; is the covariance matrix of the estimation error of the sub-model m j at time k-1; is the mode probability of the sub-model m j at time k-1, then after the interactive action, the mixed estimation input of the filter of m j at time k-1 is:

[0019]

[0020] wherein the mixed probability at time k-1 is:

[0021]

[0022] In the formula, is the mode probability of the sub-model m i at time k-1; is the prediction probability of the sub-model m j at time k-1,

[0023] The covariance matrix of the mixed estimation error of the filter of m j at time k-1 is calculated as:

[0024]

[0025] Step 2.2: Run the filter of m j at time k-1 input and based on the variational derivation filter formula based on the mean field theory to obtain the output of the filter: the sub-model mj State estimation and the sub-model m at time k j The covariance matrix of the estimated error Specifically:

[0026] Submodel m j One-step prediction of the state and sub-model m j The predicted covariance matrix of the estimated error for:

[0027]

[0028]

[0029] Submodel m j The state estimate of the tth iteration and sub-model m j The covariance matrix of the estimated error at the tth iteration is for:

[0030]

[0031]

[0032] In the formula, the superscript (t) is the number of iterations, and the sub-model m j The predicted mean square error matrix of the measurement for:

[0033]

[0034] Where, sub-model m j The covariance matrix of the measurement noise for:

[0035]

[0036] Where, sub-model m j Shape parameters and sub-model m j The scale parameter As shown in the following formula:

[0037]

[0038]

[0039] Where, For sub-model m j The scale parameter of

[0040] When the set number of iterations is reached, or the When , the iteration stops and e is the given value;

[0041] Finally, the result of the iteration is output to and

[0042] Step 2.3: Under the Gaussian assumption, the sub-model m at time k j The likelihood value is:

[0043]

[0044] Where, is the sub-model m at time k j The residual of is the sub-model m at time k j The covariance matrix of the residuals;

[0045] Calculate the normalization constant:

[0046]

[0047] Get the sub-model m at time k j The pattern probability of:

[0048]

[0049] Step 2.4: The covariance matrices of the overall state estimate at time k and the overall estimation error at time k are respectively:

[0050]

[0051]

[0052] When k=k+1, the and and in step 2.3 As the input value corresponding to steps 2.1 and 2.2 at time k+1.

[0053] Furthermore, the expected model set E in step 3 k for:

[0054]

[0055] Furthermore, in step 5, the estimation fusion of the results obtained in step 2 and step 4 includes:

[0056] Model set M at time k k Satisfied: M k =M f ∪E k ;M k Submodel m in ipattern probability M k overall state estimation M k covariance matrix of overall estimation error is:

[0057]

[0058]

[0059]

[0060] wherein the superscript i is a sub-model m k within M i ; is a likelihood value of the sub-model m i at time k; is a prediction probability of the sub-model m i at time k; is a state estimation of the sub-model m i at time k; is a covariance matrix of estimation error of the sub-model m i at time k.

[0061] The beneficial effects of the present application are: firstly, according to the influence of ocean current and time-varying measurement noise on the electromagnetic log speed measurement, a fixed model set is established, containing two sub-models, and a model set is adaptively generated through the expected model expansion method; secondly, the coupling term of the state vector and the covariance matrix of the measurement noise is decoupled by using the variational Bayes based on the mean field theory, and a new adaptive filtering formula is derived; thirdly, the results of the interactive multiple model (IMM) adaptive filtering of the fixed model set and the expected model set are respectively estimated and fused; and finally, the fused estimation results are used to correct the attitude, speed and position calculated by the strapdown inertial navigation system. The present application takes the misalignment angle of SINS, the speed error of SINS, the position error of SINS, the gyro drift, the accelerometer bias and the ocean current velocity as the state vector; takes the speed error of SINS and EM Log as the measurement vector; establishes the basic model of the system, and uses the adaptive filtering of the variable structure multiple model based on the expected model expansion. The expected model expansion method adaptively generates a model, which can improve the timeliness of the algorithm; the variational Bayes based on the mean field theory decouples the coupling term of the state vector and the covariance matrix of the measurement noise. The present application greatly improves the positioning accuracy and robustness of SINS / EM Log integrated navigation by using the new derived multiple model adaptive filtering integrated navigation method at the cost of a small amount of calculation, effectively reduces the influence of ocean current and time-varying measurement noise on SINS / EM Log integrated navigation, thereby improving the positioning accuracy of ship integrated navigation, and has certain engineering application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 is a fixed model set;

[0063] Figure 2 is the model set at time k;

[0064] Figure 3 This is a flow chart of the multi-model adaptive filtering algorithm implemented in the present invention. DETAILED DESCRIPTION

[0065] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0066] Because EM Log is affected by ocean currents and complex ocean environments, adaptive filtering using a variable-structure multi-model augmented by an expected model can significantly improve the positioning accuracy of SINS / EM Log integrated navigation. This method establishes a fixed model set, adaptively generates a model set using the expected model augmentation, and uses a newly derived filtering formula for adaptive filtering. This improves the positioning accuracy of SINS / EM Log integrated navigation and minimizes the impact of ocean currents and time-varying measurement noise on ship integrated navigation. The specific steps are as follows:

[0067] Step 1: After the SINS and EM Log installed on the ship are fully preheated, put them into working condition.

[0068] Step 2: Create a fixed model set M f , M f It contains two sub-models: m1 and m2. The state equations and measurement equations of the SINS / EM Log combined navigation of m1 and m2 are established and discretized.

[0069] The northeastern sky geographic coordinate system (subscripts E, N, U respectively) is selected as the navigation coordinate system (n system), and the upper right front is the carrier coordinate system (b system). The system state vector is:

[0070]

[0071] Where, superscript n and superscript b are projections in the n-frame and b-frame, respectively; subscripts E, N, and U are the directions of the three coordinate axes of the n-frame; x, y, and z are the directions of the three coordinate axes of the b-frame; φ is the misalignment angle of the SINS; δV is the velocity error of the SINS; δL and δλ are the latitude error and longitude error of the SINS, respectively; ε is the gyro drift; is the accelerometer zero bias; V C is the ocean current speed.

[0072] The error of the inertial device is approximately a combination of random constant bias and white noise, that is, ε = ε b +εw , where ε b is the random constant drift of the gyroscope, ε w is the zero-mean Gaussian white noise of the gyroscope; is the random constant bias of the accelerometer, is the zero-mean Gaussian white noise of the accelerometer.

[0073] Generally, the change of the ocean current velocity can be described by a first-order Markov model, and the equation of the ocean current velocity is where, τ is the correlation time of the ocean current velocity, τ = [τ E τ N ]T; is the zero-mean Gaussian white noise.

[0074] The state equation of the system sub-model is established:

[0075]

[0076] where the state transition matrix F(t), the process noise coupling matrix G(t), and the process noise vector ω(t) are respectively:

[0077]

[0078]

[0079]

[0080] where, is the direction cosine matrix of the b system relative to the n system; is a J-row K-column matrix; 0 J×K is a J-row K-column zero matrix; I 2×2 is a 2-row 2-column unit matrix; ω φ (t), ω δV (t) are all zero-mean Gaussian white noises. F1(t) ~ F8(t) are as follows:

[0081]

[0082]

[0083]

[0084]

[0085]

[0086] Where, ω ie is the angular velocity of the Earth's rotation; L is the local latitude; R M is the meridian radius; R P is the radius of the Maoyou circle; are the east and north speeds respectively; They are the comparison of east, north and sky directions respectively.

[0087] Establish the measurement equation of the system sub-model:

[0088] z(t)=H(t)x(t)+υ(t)

[0089] Where z(t) is the measurement vector; the difference between the velocity calculated by SINS in frame b and the velocity measured by EM Log is used as the measurement, and the observation matrix H(t) is:

[0090]

[0091] Where C JK for The Jth row and Kth column of is the element of t; υ(t) is the measurement noise vector, which follows a Gaussian distribution with zero mean and unknown variance, and its variance follows an inverse gamma distribution (IG). The model parameter settings of sub-models m1 and m2 are shown in Table 1.

[0092] Table 1 Model parameter settings

[0093]

[0094] The discretization results of the above state equation and measurement equation are:

[0095]

[0096] Where, superscript j is the sub-model m j ; Subscripts k-1 and k are k-1 and k moments respectively; x k and z k are discrete state vector and measurement vector respectively; w k-1 and υ k are process noise vector and measurement noise vector respectively; w k-1 Obey zero mean, covariance matrix is ​​Q k-1 Gaussian distribution, Q k-1 is a diagonal matrix; k It obeys a Gaussian distribution with zero mean and unknown variance, whose variance obeys the inverse gamma distribution, and the covariance matrix is ​​R k , R k is a diagonal matrix; Φ kk-1 is the discrete state transfer matrix, Φ kk-1 ≈I+F(t k-1 )T s, T s is the sampling time; Γ k-1 is the discrete process noise coupling matrix, Γ k-1 ≈G(t k-1 );H k is the discrete measurement matrix.

[0097] Step 3: M f Run an interactive multi-model (IMM) adaptive filter (IMM[M f ,M k-1 ],M k-1 is the model set at time k-1), we get M f Neutron Model m j The pattern probability State Estimation and the covariance matrix of the estimation error

[0098] The IMM adaptive filtering described in step 3 specifically includes the following sub-steps:

[0099] Step 3.1: Input Interaction

[0100] Let the transition probability of the model be π ij , indicating that from the model m i Transfer to model m j The probability of . Then the probability density transfer matrix can be obtained as shown below:

[0101]

[0102] is the sub-model m at time k-1 j state estimation; is the sub-model m at time k-1 j The covariance matrix of the estimation error; is the sub-model m at time k-1 j The pattern probability of m can be obtained after the interaction. j The mixed estimation input of the filter at time k-1 is as follows:

[0103]

[0104] The mixing probability at time k-1 is as follows:

[0105]

[0106] Where, is the sub-model m at time k-1 i The probability of the pattern; is the sub-model m at time k-1j the predicted probability of

[0107] the input of the filter of m j at time k-1 is given by

[0108]

[0109] Step 3.2: Filtering calculation

[0110] the input of the filter of m j at time k-1 is given by and Running the filtering formula derived from the variational mean-field theory, the output of the filter, the state estimate of the sub-model m j at time k and the covariance matrix of the estimation error of the sub-model m j at time k can be obtained as and The filtering update is given by

[0111] the one-step prediction of the state of the sub-model m j and the predicted covariance matrix of the estimation error of the sub-model m j are given by

[0112]

[0113]

[0114] the state estimate of the t-th iteration of the sub-model m j and the covariance matrix of the estimation error of the sub-model m j at the t-th iteration are given by

[0115]

[0116]

[0117] where the superscript (t) denotes the iteration number, the predicted mean square error matrix of the measurement of the sub-model m j is given by

[0118]

[0119] where the covariance matrix of the measurement noise of the sub-model m j at the t-th iteration at time k is given by ​​​​​​

[0120]

[0121] where the shape parameter of the sub-model m j and the scale parameter of the sub-model m j are given by

[0122]

[0123]

[0124] where is the scale parameter of the sub-model m j at time k-1; z k is the measurement vector at time k; is the observation matrix of the sub-model m j at time k; is the state estimate of the sub-model m j at time k in the tth iteration; is the covariance matrix of the estimation error of the sub-model m j at time k in the tth iteration.

[0125] The iteration of the above filtering formula is performed for t≤10 times, and when is satisfied, is the state estimate of the sub-model m j at time k in the t-1th iteration, and the iteration is stopped.

[0126] Finally, the result of the completed iteration is output to and

[0127] Step 3.3: Model probability update

[0128] Under the Gaussian assumption, the likelihood value of the sub-model m j at time k is given by

[0129]

[0130] where is the residual of the sub-model m j at time k; is the covariance matrix of the residual of the sub-model m j at time k.

[0131] The normalization constant is calculated as follows:

[0132]

[0133] ​​The sub-model m at time k can be obtained j The mode probability of m at time k is shown as follows:

[0134]

[0135] Step 3.4: output interaction

[0136] The overall state estimation at time k and the covariance matrix of the overall estimation error at time k are shown as follows, respectively:

[0137]

[0138]

[0139] When k=k+1, the input values corresponding to steps 3.1 and 3.2 in step 3.2 and and the input value in step 3.3 will become the input values corresponding to steps 3.1 and 3.2 at time k+1.

[0140] Step 4: generate the expected model set E using the method of expected model expansion k .

[0141] According to the m obtained in step 3 A new model set E is generated using expected model expansion k , containing 1 sub-model shown as follows:

[0142]

[0143] Step 5: run an interactive multiple model (IMM) adaptive filter (IMM[E k , M k , M k-1 ]) on E k to obtain the mode probability state estimation and the covariance matrix of the estimation error

[0144] Step 6: estimate fusion is performed on the results obtained in steps 3 and 5, the fused estimation result is used to correct the SINS calculated attitude, velocity and position, and the corrected attitude, velocity and position and other navigation parameters are output, the time k is added by 1, and then the process jumps to step 3. The algorithm runs until the navigation ends and the loop stops.

[0145] Calculate the estimation fusion: the model set M at time k k , M k =M f ∪E k . M f and Mk As shown in Figure 1 and Figure 2 . The mode probability of the sub-model m k in M i is shown as follows: The overall state estimation of M k is shown as follows: The covariance matrix of the overall estimation error of M k is shown as follows:

[0146]

[0147]

[0148]

[0149] In the formula, the superscript i is the sub-model m k in M i ; is the likelihood value of the sub-model m i at the k th moment; is the prediction probability of the sub-model m i at the k th moment; is the state estimation of the sub-model m i at the k th moment; is the covariance matrix of the estimation error of the sub-model m i at the k th moment.

[0150] Thus, the ship integrated navigation method assisted by the electromagnetic log is completed.

[0151] In order to illustrate the effectiveness of the algorithm, the algorithm is simulated. The simulation conditions are set as follows: the gyro drift of each axis is 0.04° / h, the accelerometer zero offset of each axis is 10 ～4 g. The motion states are set as U-turn, straight sailing, turning, acceleration, deceleration and the like; the related time variation of the ocean current velocity is 2h-1h-2h-1h, and the measurement noise variation is 0.3kn-0.525kn-0.3kn-0.525kn. The simulation results of the root mean square error (RMSE) of the longitude and latitude combined position are shown in Table 2. Group A is the Kalman filter of the expected model expansion variable structure multi-model, group B is the Sage-Husa adaptive filter of the expected model expansion variable structure multi-model, and group C is the simulation result of the present application.

[0152] Table 2 Comparison of position errors of algorithms

[0153]

[0154] ​Although embodiments of the present application and the accompanying drawings disclose the present application for illustrative purposes, those skilled in the art can understand that various substitutions, changes and modifications are possible without departing from the spirit and scope of the present application and the appended claims, and therefore the scope of the present application is not limited to the disclosed embodiments and drawings.

Claims

1. A ship integrated navigation method assisted by an electromagnetic speed log, characterized in that: include: Step 1: Create a fixed model set M f , M f It includes two sub-models m1 and m2, establishes the state equations and measurement equations of the SINS / EM Log integrated navigation of m1 and m2, and discretizes them; Step 2: M f Run an interactive multi-model adaptive filter IMM[M f ,M k-1 ],M k-1 is the model set at time k-1, and the M at time k is obtained f Neutron Model m j The pattern probability State Estimation Covariance matrix of estimation errors and the covariance matrix of the measurement noise for: Where j = 1, 2, sub-model m j The shape parameter of the tth iteration and scale parameters They are: and The superscript (t) indicates the tth iteration, z k is a discrete measurement vector, For sub-model m j The discrete measurement matrix of Step 3: Generate the expected model set E using the expected model expansion method k ; Step 4: E k Run an interactive multi-model adaptive filter IMM[E k ,M k-1 ], get E at time k k The pattern probability State Estimation and the covariance matrix of the estimation error Step 5: Estimate and fuse the results obtained in step 2 and step 4, and use the fused estimation results to correct the attitude, velocity and position solved by SINS to obtain the corrected attitude, velocity and position. Determine whether the navigation is completed. If so, it is completed. Otherwise, set time k = k + 1 and jump to step 2.

2. The electromagnetic speed log-assisted integrated navigation method for ships according to claim 1, characterized in that: The discretized state equation and measurement equation are specifically: Where, superscript j is the sub-model m j ; Subscripts k-1 and k are k-1 and k moments respectively; x k and z k are discrete state vector and measurement vector respectively; w k-1 and υ k are process noise vector and measurement noise vector respectively; w k-1 Obey zero mean, covariance matrix is ​​Q k-1 Gaussian distribution, Q k-1 is a diagonal matrix; k It obeys a Gaussian distribution with zero mean and unknown variance, whose variance obeys the inverse gamma distribution, and the covariance matrix is ​​R k , R k is a diagonal matrix; Φ k|k-1 is the discrete state transfer matrix, Φ k|k-1 ≈I+F(t k-1 )T s , T s is the sampling time; Γ k-1 is the discrete process noise coupling matrix, Γ k-1 ≈G(t k-1 ), G is the process noise coupling matrix; H k is the discrete measurement matrix.

3. The electromagnetic speed log-assisted integrated navigation method for ships according to claim 1, characterized in that: Step 2 specifically includes: Step 2.1: Let the transition probability of the model be π ij , indicating that from the model m i Transfer to model m j The probability density transfer matrix is: is the sub-model m at time k-1 j state estimation; is the sub-model m at time k-1 j The covariance matrix of the estimation error; is the sub-model m at time k-1 j The mode probability, after the interaction, m j The mixed estimation input of the filter at time k-1 is: The mixing probability at time k-1 is: Where, is the sub-model m at time k-1 i The probability of the pattern; is the sub-model m at time k-1 j The predicted probability of Calculate m j The covariance matrix of the mixed estimation error of the filter at the k-1 moment is: Step 2.2: Set m j The input of the filter at time k-1 and Run the filter formula derived from the variational method based on mean field theory to obtain the output of the filter: the sub-model m at time k j State estimation and the sub-model m at time k j The covariance matrix of the estimation error Specifically: Submodel m j One-step prediction of the state and sub-model m j The predicted covariance matrix of the estimated error for: Submodel m j The state estimate of the tth iteration and sub-model m j The covariance matrix of the estimated error at the tth iteration is for: In the formula, the superscript (t) is the number of iterations, and the sub-model m j The predicted mean square error matrix of the measurement for: Where, sub-model m j The covariance matrix of the measurement noise for: Where, sub-model m j Shape parameters and sub-model m j The scale parameter As shown in the following formula: Where, For sub-model m j The scale parameter of When the set number of iterations is reached, or the When , the iteration stops, e is a given value; finally, the result of the iteration is output to and Step 2.3: Under the Gaussian assumption, the sub-model m at time k j The likelihood value of is: Where, is the sub-model m at time k j The residual of is the sub-model m at time k j The covariance matrix of the residuals of ; compute the normalization constant: Get the sub-model m at time k j The pattern probability of: Step 2.4: The covariance matrices of the overall state estimate at time k and the overall estimation error at time k are respectively: When k=k+1, the and and in step 2.3 As the input value corresponding to steps 2.1 and 2.2 at time k+1.

4. The electromagnetic speed log-assisted integrated navigation method for ships according to claim 1, characterized in that: The expected model set E described in step 3 k for:

5. The electromagnetic speed log-assisted integrated navigation method for ships according to claim 1, characterized in that: In step 5, the estimated fusion of the results obtained in step 2 and step 4 includes: Model set M at time k k Satisfied: M k =M f ∪E k ;M k Submodel m in i The pattern probability M k The overall state estimate M k The covariance matrix of the overall estimation error for: In the formula, superscript i is M k Sub-model m i ; is the sub-model m at time k i The likelihood value of ; is the sub-model m at time k i The predicted probability of is the sub-model m at time k i state estimation; is the sub-model m at time k i The covariance matrix of the estimation errors.

Images