Multi-model state estimation method and system based on semantic constraint graph and hovering spectrum perception

Abstract

The invention provides a multi-model state estimation method and system based on a semantic constraint graph and hovering spectrum perception, belongs to the technical field of unmanned aerial vehicle navigation and target tracking, and aims to solve the problems that hovering jitter is difficult to model and the numerical stability is insufficient in existing unmanned aerial vehicle state estimation.The estimation method comprises the steps that S1, state estimation parameters are obtained; s2, establishing a semantic constraint graph; s3, calculating a transition probability; s4, operating the spectrum sensing hovering model, the path cruising model and the fast maneuvering model in parallel; s5, updating the mode probability; and S6, performing square root fusion on the state estimation of each model, and then outputting the state estimation. The system comprises a semantic constraint and transition probability calculation module, a square root filtering module and a probability updating and fusing module. According to the method, transfer is driven by a semantic constraint graph in an IMM framework, jitter is described by a spectrum sensing hovering model, and numerical stability is guaranteed by a square root unscented Kalman filter.

Description

TECHNICAL FIELD

[0001] The application relates to a multi-model state estimation method and system based on semantic constraint graphs and hover spectrum perception, and belongs to the technical field of unmanned aerial vehicle navigation and target tracking. BACKGROUND

[0002] The existing unmanned aerial vehicle state estimation mainly adopts an extended Kalman filter (EKF) or an unscented Kalman filter (UKF) of an interactive multiple model (IMM), estimates and fuses outputs in parallel through multiple motion models. However, the following problems still exist. (1) Difficulty in modeling hover jitter: the 8-30 Hz micro-jitter caused by the coupling of a propeller, a wind field and a control loop exists in the hover stage, and general models such as a constant speed model, a constant acceleration model and a coordinated turning model are difficult to depict, resulting in a mismatch of residual spectrum and a decrease in precision.

[0003] (2) Non-intelligent mode switching: the traditional IMM mainly uses a fixed or only likelihood-driven transition probability matrix, and cannot utilize task semantics such as flight control modes (such as a hover mode Loiter, an automatic task mode Auto and an automatic return home mode RTL), waypoint distance, path curvature, wind speed and the like, and is prone to false switching and delay.

[0004] (3) Insufficient numerical stability: when multiple models interact / fuse (especially containing different state dimensions), direct operation in the covariance domain is prone to non-positive definite covariance and numerical instability, affecting the reliability of the system. Therefore, an improved method is needed, which is simultaneously semantic-driven switching, hover spectrum perception, maneuvering self-adaption and square root numerical stability. SUMMARY

[0005] The application is proposed to solve the problems of difficulty in modeling hover jitter and insufficient numerical stability in the existing unmanned aerial vehicle state estimation, and further proposes a multi-model state estimation method and system based on semantic constraint graphs and hover spectrum perception.

[0006] The technical solution adopted by the application to solve the above problems is that the multi-model state estimation method based on semantic constraint graphs and hover spectrum perception proposed by the application comprises the following steps. Step 1: obtaining state estimation parameters, the state estimation parameters comprising sensor measurements , flight control modes, waypoint and path curvature, wind speed estimation and a sampling period; Step 2: reading the obtained state estimation parameters, obtaining a feature vector containing real-time observation information at the current time, and establishing a spectrum perception hover model, a path cruise model and a rapid maneuvering model; Step 3: Based on the double-layer heterogeneous model transition mechanism, establish hard constraints according to the obtained state estimation parameters, and calculate the model transition probability; Step 4: According to the calculated adaptive transition probability, run the spectrum sensing hovering model, path cruising model and rapid maneuvering model in parallel, and use the square root unscented Kalman filter to predict and update the state of each model to obtain the state estimation and innovation likelihood of each model; Step 5: Based on the innovation likelihood and transition probability of each model, update the mode probability, and in the square root domain, weight the state estimation of each model and interact and fuse to finally output the global state estimation value and its square root factor of covariance, and record the mode probability.

[0007] Further, step 2 specifically includes: Read the state estimation parameters to obtain the feature vector containing real-time observation information at the current time, wherein the feature vector includes sensor measurement data, flight control mode, nearest waypoint distance, path curvature, wind speed module length and flight speed, through the spectrum sensing hovering model, add a slight damping / resonance term in the feature vector, define the difference between the sensor measurement data and the predicted data as innovation, and use the band-pass energy of the innovation as an adaptive index to calculate the hovering frequency band energy ; Through the path cruising model, describe the UAV path tracking process in the Frenet coordinate, wherein the tangent s direction moves at a constant speed, and the lateral e direction is set to have a regression characteristic to naturally cover the turning section of the UAV; Through the rapid maneuvering model, obtain the estimated acceleration vector based on the feature vector at the current time, and based on the estimated acceleration vector , establish a maneuverability index , use coordinated turning + longitudinal acceleration dynamics to adjust the process noise based on the maneuverability index ; The calculation formula of the hovering frequency band energy is: (1); In formula (1), is the innovation power spectrum estimate at frequency , which can be approximately obtained by short-time FFT or band-pass filter energy, The larger the , the stronger the jitter, and the jitter intensity or the inherent frequency is increased, and vice versa; The expression of the maneuverability index is: (2); In formula (2), is the current estimated acceleration vector, with the unit of m / s 2 , is the first-order difference thereof, is a dimensionless weight, The greater the value, the more intense the maneuver, and the process noise is scaled up to improve the response speed.

[0008] Further, the hard constraint in step 3 includes the following: determine the current mode from the feature vector at the current time switching edge set allowed to transfer to the target , and set the remaining edges to zero, and the allowed flight control model switching path is taken as a hard constraint.

[0009] Further, the model transition probability in step 3 is calculated, including the following: On the allowed edges, according to the feature vector at the current time containing real-time observation information , wherein is the distance to the nearest waypoint, is the flight speed, is the path curvature, is the flight control mode, is the wind speed module, is the absolute value of the lateral path deviation in the current Frenet coordinate system, and the adaptive transition probability is calculated by the softmax function , and the calculated adaptive transition probability is taken as a soft calculation; The calculation formula of the adaptive transition probability is as follows: (3); In formula (3), is the bias, and are weight vectors and have the same dimension, which can be trained offline or fine-tuned online.

[0010] Further, in step 5, the state estimates of each model are weighted, interacted and fused in the square root domain, including the following: Step 5.1: When the spectrum-aware hovering model, the path cruising model and the fast maneuvering model are interacted and finally fused, they are not directly operated on the covariance. The covariance factor S of each model at the previous time is interacted and operated by QR decomposition in the square root domain to obtain the initial input of each SR-UKF sub-filter, which includes the mean and the covariance factor S, ; Step 5.2: Perform prediction and update operations based on the output input to output the state estimates and innovation likelihood of each model; Step 5.3: When the dimensions of each model are consistent, the state estimation results of each model are weighted, interacted and fused in the square root domain, and the global state estimation and covariance factor of the unmanned aerial vehicle are output. Step 5.4: When the dimensions of each model are inconsistent, the innovation likelihood and transition probability of each model are projected into a common superset or filled with zero variance before square root operation, and the consistency of each model is unified, and the weighted interaction and fusion operation in step 5.3 is repeated, and the global state estimation and covariance factor of the unmanned aerial vehicle are output.

[0011] Further, the weighted interaction and fusion operation in step 5.3 includes: The state estimation results of each model are stacked, QR decomposed and Cholesky incremented / decremented in the square root domain, and the weighted interaction and fusion are completed.

[0012] The multi-model state estimation system based on semantic constraint graph and hovering spectrum perception includes: The semantic constraint and transition probability calculation module is used to realize the double-layer model transition mechanism. The square root filtering module is used to perform square root unscented Kalman filtering of the spectrum perception hovering model, the path cruise model and the fast maneuvering model in parallel. The probability updating and fusion module is used to update the mode probability based on the likelihood of each model, and to weightedly fuse the state estimation of each model in the square root domain, and finally to output the global state estimation.

[0013] The beneficial effects of the present application are: 1. The present application has higher tracking accuracy. The present application accurately captures the hovering micro-dithering through the spectrum perception hovering model, so that the positioning and speed estimation of the hovering and near-hoovering states are more accurate and stable.

[0014] 2. The present application can realize faster and smarter switching. The present application proposes a double-layer transition mechanism, which combines the task / environment semantics and observation data in real time, avoids invalid switching and shortens the switching delay, and improves the response speed and reliability in complex tasks.

[0015] 3. The present application has better consistency and robustness. The whole process of square root calculation keeps the covariance semi-positive definite and improves the numerical stability, adapts to variable dimension models, and has better system consistency. BRIEF DESCRIPTION OF DRAWINGS

[0016] Figure 1 The flowchart of the multi-model state estimation method based on semantic constraint graph and hovering spectrum perception is shown in the figure. Figure 2 The logic diagram of the double-layer heterogeneous model transition mechanism is shown in the figure. Figure 3A schematic diagram illustrating the mechanisms of the spectrum-aware hovering model, the path cruise model, and the rapid maneuvering model; Figure 4 Flowchart for the SR-IMM-SRUKF algorithm implementation; Figure 5 This is a framework diagram of a multi-model state estimation system based on semantic constraint graphs and hover spectrum awareness. Figure 6 The time history and RMSE plot for low-frequency velocity error; Figure 7 PSD diagram of the velocity modulus during hovering; Figure 8 This is a magnified view of the X–Y region during the hovering period. Detailed Implementation

[0017] Combination Figures 1-5 This embodiment describes the SR-IMM-SRUKF algorithm used in implementing multi-model state estimation. The steps are as follows: Figure 4 As shown, Figure 1 As shown, the steps of the multi-model state estimation method based on semantic constraint graphs and hovering spectrum awareness described in this embodiment include: S1: Obtain state estimation parameters; The state estimation parameters in this embodiment include sensor measurements. Flight control mode (Loiter / Auto / RTL), waypoint and path curvature, wind speed estimation and sampling period; initialization of SR-UKF initial state and covariance factor for the three modes, as well as initial mode probability.

[0018] S2: Establish a semantic constraint graph; The hovering frequency band energy is calculated from the innovative sequence in the previous window, and feature vectors such as flight control mode, speed, waypoint distance, path curvature, and wind speed are read. A spectrum-aware hovering model, a path cruise model, and a rapid maneuvering model are then established. The mechanisms of each model are as follows: Figure 3 As shown; In the spectrum-aware hovering model, a slightly damped / resonance term is introduced into the feature vector. The difference between the sensor measurement data and the predicted data is defined as innovation. The innovative bandpass energy is used as an adaptive index to calculate the hovering frequency band energy. : (1); In formula (1), It is innovation In frequency Power spectrum estimation, The energy can be approximated using short-time FFT or bandpass filtering. A larger value indicates stronger jitter; increasing the jitter intensity or raising the natural frequency online will increase the jitter intensity, and vice versa. In the Waypoint-Cruise on Path (WC-Path) model, path tracking is described in Frenet coordinates; the tangential s-direction has an approximately constant speed, and the lateral e-direction has regression characteristics, which can naturally cover turning segments.

[0019] In the Aggressive-Maneuver Coordinated-Turn 3D model (AM-CT3D), a coordinated turning and longitudinal acceleration dynamics is used to establish a maneuverability index, which is then used to adjust for process noise. (2); In formula (2), This is the currently estimated acceleration vector, in m / s². 2 , Its first difference, For dimensionless weights, The larger the value, the more intense the maneuver; the process noise is amplified proportionally to improve the response speed.

[0020] After establishing the three operational models and explaining the mechanisms and principles of each model, the current mode is determined based on the feature vector at the current moment. Set of switching edges that allow transition to the target Then set the remaining edges to zero, and treat the allowed flight control model switching paths as hard constraints.

[0021] S3: Calculate the transition probability; On the allowed edges, based on the feature vector containing real-time observation information at the current moment. ,in, The distance to the nearest waypoint. For flight speed, For path curvature, Flight control mode The wind speed model length. This represents the absolute value of the lateral path deviation in the current Frenet coordinate system, in meters (m). It can be calculated from the e-direction of the WC-Path model and characterizes the degree to which the UAV deviates from the preset path, used to enhance the adaptability of the transfer probability. The adaptive transfer probability is calculated using the softmax function. And the calculated adaptive transition probability is used as soft computation; Adaptive transition probability The calculation formula is: (3); In formula (3), For bias, and All are weight vectors with the same dimension, and can be trained offline or fine-tuned online.

[0022] The two-layer heterogeneous model transfer mechanism established in this embodiment is as follows: Figure 2 As shown, it includes: outer layer—semantic constraint graph (Hard constraints): Based on flight control mode AP_mode and nearest waypoint distance ,speed (m / s), path curvature (m) -1 ), wind speed modulus Semantic quantities, determining the pattern at this moment Allow transfer to the target set Set the remaining edges to zero.

[0023] Inner layer—softmax scoring (adaptive): Transition probabilities are calculated only on allowed edges based on the feature vector (the meaning and units of each component are described above).

[0024] S4: Parallel operation of spectrum-aware hovering model, path cruise model and rapid maneuvering model; S5: Update mode probability; S501: During the interactive mixing and final fusion of the spectral sensing hovering model, path cruise model, and rapid maneuvering model, direct computation on the covariance is not performed. Instead, QR decomposition is used in the square root domain to interactively compute the covariance factor S of each model at the previous time step, resulting in the initial inputs of each SR-UKF sub-filter. The inputs include the mean and the covariance factor S. ; S502: Performs prediction and update operations based on input and output, and outputs the state estimates and innovation likelihoods of each model; S503: Update the mode probability based on the innovation likelihood and transition probability of each model. When the dimensions of each model are consistent, perform weighted interaction and fusion of the state estimation results of each model in the square root domain, output the global state estimate and covariance factor of the UAV, and record the mode probability. S504: When the dimensions of each model are inconsistent, the innovation likelihood and transition probability of each model are updated to the mode probability by first projecting them to a common superset or filling them with zero variance before performing square root operation. After unifying the consistency of each model, the weighted interaction and fusion operation in step 5.3 is repeated to output the global state estimate and covariance factor of the UAV and record the mode probability.

[0025] S6: Output the state estimates of each model after square root fusion.

[0026] Within the square root domain, the state estimation results of each model are subjected to matrix stacking, QR decomposition, and Cholesky incremental / reduced-order updates to complete weighted interaction and fusion.

[0027] Furthermore, this implementation also provides a multi-model state estimation system based on semantic constraint graphs and hover spectrum awareness, such as... Figure 5 As shown, it includes: The semantic constraint and transition probability calculation module is used to implement the two-layer model transition mechanism; The square root filtering module is used to perform square root unscented Kalman filtering on the spectrum-aware hovering model, the path cruise model, and the rapid maneuvering model in parallel. The probability update and fusion module is used to update the mode probability based on the likelihood of each model, and to perform weighted fusion of the state estimates of each model in the square root domain, and finally output the global state estimate.

[0028] To verify the vibration suppression and steady-state tracking capabilities of the multi-model state estimation method proposed in this invention under the "hovering micro-jitter" condition, the following simulation comparison was designed. The control algorithm is the conventional IMM-UKF (CV+CA two models).

[0029] Scene and parameters: Total duration T=10s, sampling period dt=0.005s; observation is three-axis position, measurement noise standard deviation 0.05m; Trajectory: 0–2s uniformly ascends to hovering point (5, 3, 2)m; 2–8s superimposed on this point a three-axis phase-shifted micro-jitter with an amplitude of 0.02m and a frequency of 15Hz; 8–10s moves away. The proposed method is to unify the three models into a 9-dimensional state parallel SR-UKF, add "sharpening" semantic constraints to the model transition matrix during the hovering period, fuse the JH-Spec branch at five sub-frequency points in the range of 14.75–15.25 Hz, and perform weak velocity prior and SG derivative alignment within the hovering stability window (Savitzky-Golay, order 3, window 23). Evaluation window: Comparison is only performed within the stable hovering window [2.3s, 7.7s] (avoiding the take-off and landing edges); Indicator definition: 1) Position RMSE, velocity RMSE: respectively for Take the root mean square; 2) Low-frequency velocity RMSE (≤5Hz): Apply a 4th-order Butterworth low-pass filter (zero-phase filter) to the velocity error and take the RMSE within the stable window; 3) High-frequency velocity suppression ratio (10–30Hz): Perform Welch PSD on the velocity modulus within the stable window, integrate to obtain the band-limited variance, and take the square root to obtain the band-limited RMS. Suppression ratio = RMS baseline / RMS proposed (>1 is better); 4) NIS monitoring: Average expected value of hovering segment ≈ 3 of measurement dimension.

[0030] The comparison results of the main performance indicators are shown in Table 1 and Table 2: Table 1

[0031] Table 2

[0032] Quantification results (stable hovering window) Location RMSE: 0.0306 → 0.0256m (decreased by 16.27%); Speed ​​RMSE: 2.3415 → 2.3124 m / s (a decrease of 1.24%) (The overall speed RMSE is significantly affected by the proportion of low-frequency drift, resulting in limited improvement). Low-frequency speed RMSE (≤5Hz) Figure 6 As shown: 0.0897→0.0159m / s (a decrease of 82.24%); High-frequency speed band-limited RMS (10–30Hz): 0.0554→0.0064m / s (a decrease of 88.47%), corresponding to a high-frequency speed suppression ratio of ≈8.68 times; Triaxial low-frequency RMSE (m / s): X: 0.0564→0.0091 (decreased by 83.80%), Y: 0.0441→0.0071 (decreased by 83.94%), Z: 0.0541→0.0110 (decreased by 79.73%). The comparison of triaxial low-frequency velocity RMSE is shown in Table 3: Table 3

[0033] NIS mean: 3.06, within a reasonable range; hovering segment model probability: JH-Spec-0.94, WC-Path-0.03, AM-CT3D-0.03 (JH-Spec dominates, as expected).

[0034] 3D trajectory + projection + height map and position / velocity error time sequence comparison diagram as follows Figure 7 and Figure 8 As shown.

[0035] Model and module descriptions: Semantic constraint module: Maps Loiter / Auto / RTL, speed threshold, waypoint radius, curvature threshold, and wind speed threshold to allowed / prohibited edges; blocks unreasonable switching such as "cruise → hover" when the UAV is moving away from the waypoint at high speed.

[0036] Transition probability module: Perform softmax scoring of equation (1) on allowed edges to obtain JH-Spec: Includes a mildly damped / resonant term in the state; increases jitter intensity or raises the natural frequency when rising; decreases it when falling.

[0037] WC-Path: Decoupling in both s and e directions: Approximately constant velocity along the path, with a tendency to return to the center of the path laterally; constraints are injected by curvature at turns.

[0038] AM-CT3D: Characterizes 3D maneuverability by coordinating turning and longitudinal acceleration; uses noise adjustment to enable the filter to quickly enter / exit "high maneuverability" state.

[0039] SR-IMM interaction / fusion: Instead of adding or subtracting in the covariance domain, QR and Cholesky (incremental / reduced order) are used uniformly in the square root domain to ensure PSD and stability; dimensionality changes are handled by filling with common supersets or zero variance before interaction.

[0040] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the present invention. Any simple modifications, equivalent substitutions, and improvements made to the above embodiments without departing from the scope of the present invention, based on the technical essence of the present invention and within the spirit and principles of the present invention, shall still fall within the protection scope of the present invention.

Claims

1. A multi-model state estimation method based on semantic constraint graphs and hovering spectrum perception, characterized in that, include: Step 1: Obtain state estimation parameters, which include sensor measurements. Flight control mode, waypoint and path curvature, wind speed estimation and sampling period; Step 2: Read the acquired state estimation parameters, obtain the feature vector containing real-time observation information at the current moment, and establish the spectrum-aware hovering model, the path cruise model, and the rapid maneuver model; Step 3: Based on the two-layer heterogeneous model transfer mechanism, establish hard constraints according to the obtained state estimation parameters and calculate the model transfer probability; Step 4: Based on the calculated adaptive transition probabilities, run the spectrum-aware hovering model, the path cruise model, and the rapid maneuver model in parallel, and use the square root unscented Kalman filter to predict and update the state of each model, and obtain the state estimate and innovation likelihood of each model. Step 5: Update the mode probability based on the innovation likelihood and transition probability of each model, and perform weighted interaction and fusion of the state estimates of each model in the square root domain. Finally, output the global state estimate and the square root factor of its covariance, and record the mode probability.

2. The multi-model state estimation method based on semantic constraint graph and hovering spectrum perception according to claim 1, characterized in that, Step 2 specifically includes: The state estimation parameters are read to obtain a feature vector containing real-time observation information at the current moment. This feature vector includes sensor measurement data, flight control mode, nearest waypoint distance, path curvature, wind speed modulus, and flight speed. Using a spectral sensing hovering model, a slightly damped / resonance term is input into the feature vector. The difference between the sensor measurement data and the predicted data is defined as innovation. The innovative bandpass energy is used as an adaptive index to calculate the hovering frequency band energy. ; The path-following process of the UAV is described in Frenet coordinates using a path cruise model, where the tangential direction... s The direction is approximately constant velocity, and the lateral direction is... e The orientation is set to return to its original characteristics to naturally cover the turning segments of the drone; The estimated acceleration vector is obtained based on the feature vector at the current moment using a fast maneuver model. And based on the estimated acceleration vector Establish mobility index It adopts coordinated turning + longitudinal acceleration dynamics, based on the maneuverability index. Noise during adjustment; Hovering band energy The calculation formula is: （1）； In formula (1), It is innovation In frequency Power spectrum estimation, The energy can be approximated using short-time FFT or bandpass filtering. A larger value indicates stronger jitter; increasing the jitter intensity or raising the natural frequency online will increase the jitter intensity, and vice versa. Mobility index The expression is: （2）； In formula (2), This is the currently estimated acceleration vector, in m / s². 2 , Its first difference, For dimensionless weights, The larger the value, the more intense the maneuver; the process noise is amplified proportionally to improve the response speed.

3. The multi-model state estimation method based on semantic constraint graph and hovering spectrum perception according to claim 1, characterized in that, Step 3 involves establishing hard constraints, specifically including: Determine the current mode based on the feature vector at the current time. Set of switching edges that allow transition to the target Then set the remaining edges to zero, and treat the allowed flight control model switching paths as hard constraints.

4. The multi-model state estimation method based on semantic constraint graph and hovering spectrum perception according to claim 1, characterized in that, Step 3 involves calculating the model transition probabilities, specifically including: On the allowed edges, based on the feature vector containing real-time observation information at the current moment. ,in, The distance to the nearest waypoint. For flight speed, For path curvature, Flight control mode For wind speed model length, The absolute value of the lateral path deviation in the current Frenet coordinate system is used to calculate the adaptive transition probability using the softmax function. And the calculated adaptive transition probability is used as soft computation; Adaptive transition probability The calculation formula is: （3）； In formula (3), For bias, and All are weight vectors with the same dimension, and can be trained offline or fine-tuned online.

5. The multi-model state estimation method based on semantic constraint graph and hovering spectrum perception according to claim 1, characterized in that, Step 5 involves weighted interaction and fusion of the state estimates of each model within the square root domain, specifically including: Step 5.1: During the interactive mixing and final fusion of the spectral sensing hovering model, path cruise model, and rapid maneuvering model, no direct computation is performed on the covariance. Instead, QR decomposition is used in the square root domain to interactively compute the covariance factor S of each model at the previous time step, obtaining the initial inputs of each SR-UKF sub-filter. The inputs include the mean and the covariance factor S. ; Step 5.2: Perform prediction and update operations based on the input and output, and output the state estimate and innovation likelihood of each model; Step 5.3: Update the mode probability based on the innovation likelihood and transition probability of each model. When the dimensions of each model are consistent, perform weighted interaction and fusion of the state estimation results of each model in the square root domain to output the global state estimate and covariance factor of the UAV. Step 5.4: When the dimensions of each model are inconsistent, the innovation likelihood and transition probability of each model are first projected onto the common superset or filled with zero variance before the square root operation is performed. After unifying the consistency of each model, the weighted interaction and fusion operation in step 5.3 is repeated to output the global state estimate and covariance factor of the UAV.

6. The multi-model state estimation method based on semantic constraint graph and hovering spectrum perception according to claim 5, characterized in that, Step 5.3, the weighted interaction and fusion operations, include: Within the square root domain, the state estimation results of each model are subjected to matrix stacking, QR decomposition, and Cholesky incremental / reduced-order updates to complete weighted interaction and fusion.

7. A multi-model state estimation system based on semantic constraint graphs and hover spectrum awareness, applied to the multi-model state estimation method based on semantic constraint graphs and hover spectrum awareness as described in any one of claims 1-6, characterized in that, include: The semantic constraint and transition probability calculation module is used to implement the two-layer model transition mechanism; The square root filtering module is used to perform square root unscented Kalman filtering on the spectrum-aware hovering model, the path cruise model, and the rapid maneuvering model in parallel. The probability update and fusion module is used to update the mode probability based on the likelihood of each model, and to perform weighted fusion of the state estimates of each model in the square root domain, and finally output the global state estimate.

Images