Interactive multi-model underwater maneuvering target tracking method and system based on azimuth-pure second-order EKF

Abstract

The invention discloses an interactive multi-model underwater maneuvering target tracking method and system based on a pure azimuth second-order EKF. The method comprises the following steps: establishing a discrete state space model of a pure azimuth target tracking system; performing second-order linearization on the pure azimuth measurement model; calculating mixed input of a sub-filter corresponding to each model in the model set of the interactive multi-model algorithm; estimating a target state through parallel filtering of the azimuth-pure second-order EKF sub-filter corresponding to each model in the model set; updating the probability of each model in the model set; and combining the estimation results of all the sub-filters to obtain a final estimation result of the target state. According to the method, the target tracking precision can be improved, the problem of highly nonlinear measurement of the UUV on a target detection and tracking system through a passive sonar is effectively solved, good real-time performance is achieved, and real-time tracking of the UUV on the motion state of the underwater maneuvering target is achieved through adaptive matching of the real motion of the target through the interactive multi-model algorithm.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater target tracking, and in particular relates to an interactive multi-model underwater maneuvering target tracking method and system based on bearings-only second-order EKF. Background Art

[0002] In many scenarios, real-time state information of unknown maneuvering targets is required. Target tracking methods are crucial for obtaining real-time information about target position and motion. In the field of underwater target tracking, passive sonars equipped on unmanned underwater vehicles (UUVs) are often used to detect targets, thereby tracking their motion under high-cover conditions. Since passive sonars can only measure the target's pure bearings, and the mathematical relationship between the target position and the pure bearings angle is highly nonlinear, improved nonlinear estimation methods are needed to achieve high-precision target tracking. Furthermore, in practical engineering, to ensure real-time acquisition of target state, tracking methods must be computationally simple and have low time complexity. The extended Kalman filter (EKF) algorithm offers excellent real-time performance and low computational complexity, but it only utilizes a first-order approximation of the bearings-only measurement model, making it difficult to achieve ideal tracking accuracy. Alternative methods such as the unscented Kalman filter and the cubature Kalman filter offer higher theoretical accuracy, but are computationally complex and suffer from poor real-time performance. Furthermore, the performance of these single-model algorithms degrades significantly when the target's motion is maneuvering. Therefore, there is an urgent need for a bearing-only maneuvering target tracking method that takes into account both estimation accuracy and real-time performance.

[0003] The patent document with application number 202111659699.9 discloses "A KL interactive multi-model underwater target tracking method", which mainly solves the problem of high-precision tracking of targets in complex underwater environments. First of all, the patent adopts a KL interactive multi-model method, which is different from the interactive multi-model method based on pure bearing second-order EKF of the present invention. Secondly, the patent focuses on calculating the probability weighting coefficient of the model by introducing KL divergence, and combining the IMM algorithm to obtain a target model that better matches the real motion, thereby solving the problem of underwater target tracking when the physical properties of the seawater medium interfere with the measurement information. This is different from the present invention's focus on solving the underwater maneuvering target tracking method under highly nonlinear pure bearing measurement conditions, taking into account engineering real-time and tracking accuracy.

[0004] The patent document with application number 201910640770.5 discloses "A method for tracking underwater maneuvering targets assisted by support vector machines based on interactive multi-models", which mainly solves the problem of tracking underwater maneuvering targets with changeable motion forms. First, the patent adopts a support vector machine-assisted method based on interactive multi-models, which is different from the interactive multi-model method based on pure bearing second-order EKF of the present invention. Secondly, the patent focuses on using a support vector machine to judge the current target motion mode based on the target's historical trajectory, thereby making real-time corrections to the probabilities of each model in the IMM model set to solve the problem of tracking underwater maneuvering targets in scenarios with non-fixed motion forms. This is different from the present invention's focus on solving underwater maneuvering target tracking methods under highly nonlinear pure bearing measurement conditions, taking into account both engineering real-time performance and tracking accuracy. Summary of the Invention

[0005] The purpose of the present invention is to provide a method and system for tracking underwater maneuvering targets based on an interactive multi-model bearings-only second-order EKF for the real-time tracking of highly nonlinear bearings-only maneuvering targets.

[0006] The purpose of the present invention is achieved through the following technical solutions:

[0007] An interactive multi-model underwater maneuvering target tracking method based on bearings-only second-order EKF is proposed. The specific steps are as follows:

[0008] Step 1: Establish a discrete state space model of the bearings-only target tracking system, including the time propagation model of the target state X(k) and the measurement model of the bearings-only angle Z(k), and obtain the noise-free bearings-only measurement model M(X(k)).

[0009] Step 2: Perform a second-order Taylor series expansion on the noise-free pure bearings measurement model M(X(k)) at the predicted target state X(k|k-1), and linearize the quadratic term through trace operation to obtain a second-order linear pure bearings measurement model

[0010] Step 3: Estimate the target state X(k) through the interactive multiple model filtering algorithm to achieve real-time tracking of underwater maneuvering targets.

[0011] Furthermore, the step 1 specifically includes:

[0012] The target state Among them, k represents the discrete time series, x t (k) and y t (k) represents the northeast position of the target, and They represent the northeast velocity of the target respectively;

[0013] The time-dependent propagation model is X(k)=Γ * (k)X(k-1)+W(k), where Γ * (k) represents the target state transfer matrix, W(k) is the process noise, and its variance matrix is ​​Q(k);

[0014] The bearing-only measurement model is Z(k)=M(X(k))+V(k), where Z(k) represents the measured azimuth of the UUV to the target, and V(k) represents the measurement noise of the UUV passive sonar, whose variance is R(k);

[0015] The bearing-only measurement model without noise is: Among them, x u (k) and y u (k) represents the northeast position of the UUV.

[0016] Furthermore, the state transfer matrix Γ * (k) includes the uniform straight line Γ1(k), the turning model Γ2(k), and the Γ after the parameter ω in Γ2(k) is changed. i (k), i=3,4,…,r, the form is as follows:

[0017]

[0018] Where T represents the sampling time, ω represents the turning angular velocity, and Γ can be obtained by taking different values ​​of ω. i (k), i=3,4,…,r form;

[0019] The process noise variance matrix Q(k) is a 4-dimensional diagonal matrix related to the position and velocity noise in the northeast direction.

[0020] Furthermore, the second-order Taylor series expansion in step 2 is:

[0021]

[0022] Where X(k|k-1) represents the state around which the Taylor series expansion is carried out, which is generally the predicted target state.

[0023] The linearization process is as follows:

[0024]

[0025] Among them, E[·] represents the mathematical expectation, P(k|k-1) represents the covariance matrix of the state, is the Hessian matrix; tr{*} represents the trace of the matrix, and the final linearized measurement model is:

[0026]

[0027] Among them, δ(k) represents a constant term that is independent of the state vector, is the linearized measurement matrix.

[0028] Furthermore, the Hessian matrix is:

[0029]

[0030] Among them, x u (k) and y u (k) represents the current position of the UUV;

[0031] The linearized measurement matrix for:

[0032]

[0033] Furthermore, the interactive multiple model filtering algorithm in step 3 includes:

[0034] Step 3.1: Set the model set Γ i (k),i=1,2,…,r for each model Γ * (k) The output of the corresponding sub-filter in the previous cycle is based on the probability μ li (k-1) is mixed to obtain the mixed state of the i-th sub-filter and covariance P i o (k-1);

[0035]

[0036]

[0037]

[0038] Among them, r represents the number of preset models in the model set, X l (k-1) and P l (k-1) represents the estimated state and covariance corresponding to the lth model, φ li represents the transition probability from the lth model to the i-th model, μ l (k-1) represents the probability of the lth model at time k-1;

[0039] Step 3.2: For each model Γ in the model set i The corresponding sub-filters of (k) are parallel filtered using a pure bearing second-order EKF to estimate the target state X(k), with a total of r sub-filters;

[0040] Step 3.3: Using the likelihood function Λi (k) The probability μ for each model in the model set i (k) perform updates;

[0041] Updated model probability μ i (k) is:

[0042]

[0043] Among them, ε i (k) represents the measurement residual calculated by the i-th sub-filter, S i (k) is the corresponding covariance;

[0044] Step 3.4: Estimation result X for each bearing-only second-order EKF sub-filter l (k) and P l (k) According to the model probability μ l (k) Combine and obtain the state estimation result of the UUV to the target;

[0045] The state is estimated to be

[0046] The estimated covariance is

[0047] Furthermore, the bearing-only second-order EKF in step 3.2 specifically includes:

[0048] Step 3.2.1: State prediction:

[0049] P i (k|k-1)=Γ * (k)P i o (k-1)Γ * (k) T +Q(k-1)

[0050] Among them, X i (k|k-1) represents the one-step prediction of the state at time k by the sub-filter corresponding to the i-th model at time k-1, P i (k|k-1) represents the corresponding covariance prediction;

[0051] Step 3.2.2: Measure the azimuth prediction:

[0052]

[0053] Step 3.2.3: Calculate residuals and covariance;

[0054]

[0055]

[0056] Step 3.2.4: Filter gain calculation:

[0057]

[0058] Step 3.2.5: State and covariance update:

[0059] X i (k) = X i (k|k-1)+G(k)ε i (k), P i (k) = P i (k|k-1)-G(k)S i (k)G(k) T .

[0060] A computer device / equipment / system comprises a memory, a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of an interactive multi-model underwater maneuvering target tracking method based on a bearings-only second-order EKF.

[0061] The beneficial effects of the present invention are:

[0062] To address the above issues, the present invention enhances the linear approximation of the system by performing a second-order Taylor series expansion on the bearings-only measurement model, thereby improving target tracking accuracy. This effectively addresses the highly nonlinear measurement problem of the UUV's target detection and tracking system using passive sonar, and exhibits good real-time performance. By adaptively matching the target's true motion through an interactive multi-model algorithm, the UUV achieves real-time tracking of the motion state of underwater maneuvering targets. This invention effectively solves the problem of underwater maneuvering target tracking under highly nonlinear bearings-only measurement conditions, balancing engineering real-time performance and tracking accuracy. The method is simple to implement, exhibits good real-time performance, and offers high tracking accuracy, making it suitable for practical engineering applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 is a flow chart of the method of the present invention;

[0064] Figure 2 Schematic diagram of UUV performing bearing-only measurement of a target using passive sonar;

[0065] Figure 3 This is the flow chart of the interactive multi-model filtering based on the bearing-only second-order EKF;

[0066] Figure 4 Schematic diagram of the root mean square error of the interactive multi-model target tracking position based on the second-order EKF, EKF and UKF respectively;

[0067] Figure 5 for Figure 4 Schematic diagram of the average running time of the three algorithms. DETAILED DESCRIPTION

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

[0069] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0070] according to Figure 1 The present invention provides an interactive multi-model underwater maneuvering target tracking method based on bearing-only second-order EKF, which specifically includes the following steps:

[0071] Step 1: Establish a discrete state space model of the bearings-only target tracking system, including the time propagation model of the target state X(k) and the measurement model of the bearings-only angle Z(k);

[0072] like Figure 1 As shown, the discrete state space model of the target tracking system includes the motion state of the maneuvering target The time propagation model and the measurement model of the pure azimuth angle Z(k) of the target by the UUV passive sonar;

[0073] Specifically, such as Figure 2 As shown, the target navigation path is a maneuvering path. Assume that the target state is The four components represent the north position, north velocity, east position and east velocity respectively, so the motion state of the maneuvering target is The propagation model over time is:

[0074] X(k)=Γ * (k)X(k-1)+W(k)

[0075] Where k represents the discrete time series, W(k) represents the process noise with variance Q(k), which is related to the position and velocity noise in the northeast direction. Q(k) is of the form:

[0076]

[0077] where σ x 、 σ y and Represents the noise variance of each component of X(k);

[0078] Target state transfer matrix Γ *(k) may change at any time, usually including two cases: uniform straight line Γ1(k) and turning Γ2(k). The models of the two cases are as follows:

[0079]

[0080] Where T represents the sampling time, ω represents the turning angular velocity, and different turning models Γ can be obtained by taking different values ​​of ω i (k),i=3,4,…,r;

[0081] according to Figure 2 , UUV listens to the navigation noise of the maneuvering target through the passive sonar array carried by it, thereby obtaining the target's azimuth information Z(k). From the perspective of the pure azimuth measurement mathematical model M(X(k)) without considering the noise, the azimuth angle Z(k) and the position of the target and the UUV are a highly nonlinear inverse trigonometric function relationship. The UUV position is known, and the target state to be estimated is The mathematical relationship between the measurement angle Z(k) and the UUV sonar is:

[0082] Z(k)=M(X(k))+V(k)

[0083]

[0084] Among them, Z(k) represents the measurement azimuth of the UUV to the target, x t (k) and y t (k) represents the northeast position of the target, x u (k) and y u (k) represents the northeast position of the UUV, V(k) represents the measurement noise of the UUV passive sonar, and its variance R(k) is determined by the accuracy of the passive sonar and is usually (2°π / 180°) 2 , M(X(k)) represents the bearings-only measurement model without noise.

[0085] Step 2: Perform second-order linearization on the bearings-only measurement model. The specific steps are as follows:

[0086] Step 2.1: Expand M(X(k)) into a Taylor series and truncate it to the quadratic term to obtain:

[0087]

[0088] X(k|k-1) represents the state around which the Taylor series expansion revolves, which is generally the predicted target state;

[0089] Step 2.2: Linearize the second-order Taylor series form of the pure bearing measurement model in step 2.1. In the second-order Taylor series expansion form of M(X(k)), there is a nonlinear component due to retaining the quadratic term, which needs to be linearized. Specifically:

[0090]

[0091] Among them, E[·] means finding the mathematical expectation, tr{*} means finding the trace of the matrix, P(k|k-1) means the covariance matrix of the state, is the Hessian matrix:

[0092]

[0093] Among them, x u (k) and y u (k) represents the current position of the UUV;

[0094] Then, the second-order linear bearings-only measurement model for:

[0095]

[0096] Among them, δ(k) represents a constant term independent of the state vector, and the linearized measurement matrix for:

[0097]

[0098] Step 3: Estimate the target state X(k) through the interactive multiple model filtering algorithm, which specifically includes the following steps:

[0099] Step 3.1: Input mix; specifically, Figure 3 As shown, the model set Γ i (k),i=1,2,…,r for each model Γ * (k) The output of the corresponding sub-filter in the previous cycle is based on the probability μ li (k-1) are mixed, with a total of r models, to obtain the mixed state of the i-th sub-filter and covariance P i o (k-1) input;

[0100]

[0101]

[0102] Among them, r represents the number of preset models in the model set, which is generally greater than or equal to 2, and X l (k-1) and Pl (k-1) represents the estimated state and covariance corresponding to the lth model, and the input interaction probability μ li (k-1) is calculated as follows:

[0103]

[0104] Among them, φ li represents the transition probability from model l to model i, μ l (k-1) represents the probability of model l at time k-1;

[0105] Step 3.2: Sub-filters are filtered in parallel; Figure 3 As shown, each model Γ in the model set i (k),i=1,2,…,r corresponds to a sub-filter; since Figure 2 The way the UUV passive sonar measures the target results in a highly nonlinear bearings-only measurement model in the target tracking state space model. Therefore, all sub-filters are filtered using a bearings-only second-order EKF to estimate the target state. There are r bearing-only second-order EKF sub-filters in total;

[0106] Step 3.2.1: State prediction; target motion state X i (k|k-1) and covariance P i (k|k-1) One-step prediction is as follows:

[0107]

[0108] P i (k|k-1)=Γ * (k)P i o (k-1)Γ * (k) T +Q(k-1)

[0109] Among them, X i (k|k-1) represents the one-step prediction of the state at time k by the sub-filter corresponding to the i-th model at time k-1, P i (k|k-1) represents the corresponding covariance prediction;

[0110] Step 3.2.2: Measure the prediction; transform the predicted state X i Substitute (k|k-1) into the second-order linear bearings-only measurement model The predicted value of the measured azimuth is obtained from:

[0111]

[0112] Step 3.2.3: Calculate the measurement residual εi (k) and its covariance S i (k), specifically:

[0113] Using the latest target azimuth Z(k) measured by UUV and the predicted azimuth Calculate the measurement residuals:

[0114]

[0115] Compute the measurement residual covariance:

[0116]

[0117] Step 3.2.4: Calculate the filter gain G(k); Kalman filtering solves the Kalman gain by minimizing the trace of the posterior estimate covariance, thus providing the optimal solution in terms of minimum mean square error. Based on this principle, Kalman filtering is performed on the second-order linear bearings-only target tracking model to obtain the filter gain G(k) of the bearings-only second-order EKF.

[0118] First, the covariance P of the posterior estimation error i (k) as follows:

[0119] P i (k) = cov(X(k)-X i (k))

[0120] Among them, X i (k) represents the model Γ i (k) The posterior estimate of the corresponding sub-filter, cov(·) represents the calculated covariance;

[0121] Second, in the Kalman filter framework, the posterior estimate X i (k) can be expressed as follows:

[0122]

[0123] Where G(k) represents the filter gain;

[0124] Then, X i Substitute the expression of (k) into P i (k) and simplifying it, we can get:

[0125]

[0126] Finally, take the above expression P i (k), and taking the partial derivative of the filter gain G(k) we can get:

[0127]

[0128] Let the above expression be 0, and we can get the filter gain of the bearing-only second-order EKF as:

[0129]

[0130] Step 3.2.5: For state X i (k) and covariance P i (k) perform updates;

[0131] X i (k) = X i (k|k-1)+G(k)ε i (k)

[0132] P i (k) = P i (k|k-1)-G(k)S i (k)G(k) T

[0133] X i (k) and P i (k) represent the updated target state estimate and covariance corresponding to the i-th model respectively.

[0134] Step 3.3: Model set probability update; specifically, Figure 3 As shown, using the likelihood function Λ i (k) The probability μ for each model in the model set i (k) is updated; the likelihood function Λ corresponding to the i-th model i (k) is:

[0135]

[0136] According to the likelihood function Λ i (k) The probability update for the i-th model is:

[0137]

[0138] Among them, ε i (k) represents the measurement residual calculated by the i-th sub-filter, S i (k) is the corresponding covariance;

[0139] Step 3.4: Estimate the joint; specifically, Figure 3 As shown, the estimation result X for each pure bearing second-order EKF sub-filter l (k) and P l (k) According to the model probability μ l (k) Combine and obtain the state estimation result of the UUV to the target;

[0140] The state is estimated to be:

[0141]

[0142] The estimated covariance is:

[0143]

[0144] exist Figure 4 and Figure 5 A Monte Carlo simulation example of applying the tracking method of the present invention is shown in FIG. Figure 4 Schematic diagram of the root mean square error of the interactive multi-model target tracking position based on the second-order EKF, EKF and UKF respectively; Figure 5 for Figure 4 Schematic diagram of the average running time of the three algorithms;

[0145] Figure 4 The specific form of the position root mean square error evaluation function at each moment shown in is:

[0146]

[0147] Among them, x t (k) and y t (k) represents the true position of the target at time k, and represents the estimated position of the target by the tracking algorithm at time k;

[0148] like Figure 4 As shown, the tracking method in the present invention has a tracking error smaller than that of the tracking methods based on EKF and UKF, and has higher tracking accuracy;

[0149] like Figure 5 As shown in the figure, in the Monte Carlo simulation experiment, by comparing the average time of running the three algorithms once, it can be seen that the interactive multi-model tracking method based on UKF takes the longest time, which is about twice as long as the other two methods, and the interactive multi-model tracking method based on EKF takes the shortest time. The interactive multi-model tracking method based on the second-order EKF in the present invention takes slightly longer than the interactive multi-model tracking method based on EKF, with only a few microseconds of time increase, and has good real-time performance.

[0150] The foregoing description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention are intended to be within the scope of protection of the present invention.

Claims

1. An interacting multi-model underwater maneuvering target tracking method based on bearings-only second-order EKF, characterized by: The specific steps are as follows: Step 1: Establish a discrete state space model of the bearings-only target tracking system, including the time propagation model of the target state X(k) and the measurement model of the bearings-only angle Z(k), and obtain the noise-free bearings-only measurement model M(X(k)). Step 2: Perform a second-order Taylor series expansion on the noise-free pure bearings measurement model M(X(k)) at the predicted target state X(k|k-1), and linearize the quadratic term through trace operation to obtain a second-order linear pure bearings measurement model Step 3: Estimate the target state X(k) through the interactive multiple model filtering algorithm to achieve real-time tracking of underwater maneuvering targets.

2. The method for tracking underwater maneuvering targets based on an interacting multi-model bearings-only second-order EKF according to claim 1, characterized in that: The step 1 specifically includes: The target state Among them, k represents the discrete time series, x t (k) and y t (k) represents the northeast position of the target, and They represent the northeast velocity of the target respectively; The time-dependent propagation model is X(k)=Γ * (k)X(k-1)+W(k), where Γ * (k) represents the target state transfer matrix, W(k) is the process noise, and its variance matrix is ​​Q(k); The bearing-only measurement model is Z(k)=M(X(k))+V(k), where Z(k) represents the measured azimuth of the UUV to the target, and V(k) represents the measurement noise of the UUV passive sonar, whose variance is R(k); The bearing-only measurement model without noise is: Among them, x u (k) and y u (k) represents the northeast position of the UUV.

3. The method for tracking underwater maneuvering targets based on an interacting multi-model bearings-only second-order EKF according to claim 2, characterized in that: The state transfer matrix Γ * (k) includes the uniform straight line Γ1(k), the turning model Γ2(k), and the Γ after the parameter ω in Γ2(k) is changed. i (k), i=3,4,…,r, the form is as follows: Where T represents the sampling time, ω represents the turning angular velocity, and Γ can be obtained by taking different values ​​of ω. i (k), i=3,4,…,r form; The process noise variance matrix Q(k) is a 4-dimensional diagonal matrix related to the position and velocity noise in the northeast direction.

4. The method for tracking underwater maneuvering targets based on an interacting multi-model bearings-only second-order EKF according to claim 1, characterized in that: The second-order Taylor series expansion in step 2 is: Where X(k|k-1) represents the state around which the Taylor series expansion is carried out, which is generally the predicted target state. The linearization process is as follows: Among them, E[·] represents the mathematical expectation, P(k|k-1) represents the covariance matrix of the state, is the Hessian matrix; tr{*} represents the trace of the matrix, and the final linearized measurement model is: Among them, δ(k) represents a constant term that is independent of the state vector, is the linearized measurement matrix.

5. The method for tracking underwater maneuvering targets based on an interacting multi-model bearings-only second-order EKF according to claim 4, characterized in that: The Hessian matrix is: Among them, x u (k) and y u (k) represents the current position of the UUV; The linearized measurement matrix for:

6. The method for tracking underwater maneuvering targets based on an interacting multi-model bearings-only second-order EKF according to claim 1, characterized in that: The interactive multi-model filtering algorithm in step 3 includes: Step 3.1: Set the model set Γ i (k),i=1,2,…,r for each model Γ * (k) The output of the corresponding sub-filter in the previous cycle is based on the probability μ li (k-1) is mixed to obtain the mixed state of the i-th sub-filter and covariance P i o (k-1); Among them, r represents the number of preset models in the model set, X l (k-1) and P l (k-1) represents the estimated state and covariance corresponding to the lth model, φ li represents the transition probability from the lth model to the i-th model, μ l (k-1) represents the probability of the lth model at time k-1; Step 3.2: For each model Γ in the model set i The corresponding sub-filters of (k) are parallel filtered using a pure bearing second-order EKF to estimate the target state X(k), with a total of r sub-filters; Step 3.3: Using the likelihood function Λ i (k) The probability μ for each model in the model set i (k) perform updates; Updated model probability μ i (k) is: Among them, ε i (k) represents the measurement residual calculated by the i-th sub-filter, S i (k) is the corresponding covariance; Step 3.4: Estimation result X for each bearing-only second-order EKF sub-filter l (k) and P l (k) According to the model probability μ l (k) Combine and obtain the state estimation result of the UUV to the target; The state is estimated to be The estimated covariance is 7. The method for tracking underwater maneuvering targets based on an interacting multi-model bearings-only second-order EKF according to claim 6, characterized in that: The bearing-only second-order EKF in step 3.2 specifically includes: Step 3.2.1: State prediction: Among them, X i (k|k-1) represents the one-step prediction of the state at time k by the sub-filter corresponding to the i-th model at time k-1, P i (k|k-1) represents the corresponding covariance prediction; Step 3.2.2: Measure the azimuth prediction: Step 3.2.3: Calculate residuals and covariance; Step 3.2.4: Filter gain calculation: Step 3.2.5: State and covariance update: X i (k)=X i (k|k-1)+G(k)ε i (k),P i (k)=P i (k|k-1)-G(k)S i (k)G(k) T 。 8. A computer device / apparatus / system comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 7.

Images