High-speed target fixed parameter optimization volume Kalman filtering tracking method

Abstract

The invention relates to a fixed parameter optimization cubature Kalman filtering tracking method for a high-speed high-maneuvering target, belongs to the technical field of signal processing and target tracking, and aims to solve the problem of insufficient tracking performance caused by difficulty in parameter tuning and isolation of an improved mechanism when an existing method is used for coexistence of model mismatch, noise time variation and outlier interference. According to the scheme, a framework integrating off-line multi-parameter collaborative optimization and on-line multi-mechanism adaptive filtering is constructed; in the off-line stage, an optimal combination of key parameters such as covariance adjustment factors is determined through a grid search system; in the online stage, the combination is loaded, and a complete filtering process including innovation feedback type dynamic covariance adjustment, sliding window type noise estimation and outlier suppression and trace-related adaptive regularization is executed, so that an enhanced tracking method with active pre-judgment and closed-loop learning capabilities is formed. The method is mainly used for carrying out high-precision and high-robustness real-time state estimation on the high-speed high-maneuvering target.

Description

Technical Field

[0001] This invention belongs to the field of signal processing and target tracking technology. More specifically, this invention relates to a high-speed target fixed-parameter optimized ductile Kalman filter tracking method. Background Technology

[0002] In the state estimation and tracking problem of high-speed moving vehicles (such as drones, autonomous vehicles, and high-speed rail transit equipment), the target's dynamic model exhibits strong nonlinearity, and the actual working environment is complex, mainly facing three major technical challenges: 1) The statistical characteristics of process noise and measurement noise are time-varying, such as abrupt changes in noise variance caused by environmental disturbances or internal system changes; 2) Sensor measurements are susceptible to interference, resulting in outliers, such as abnormal observation data caused by electromagnetic interference or occlusion; 3) The model mismatch problem is prominent, that is, there is a significant deviation between the simplified mathematical model used for filtering and the complex motion of the actual target. These challenges make it difficult for traditional nonlinear filtering algorithms, such as extended Kalman filter (EKF), unscented Kalman filter (UKF), and capacitive Kalman filter (CKF), to achieve a good balance between estimation accuracy, convergence speed, and algorithm robustness.

[0003] Existing improved CKF algorithms, such as adaptive CKF and robust CKF, mostly focus on local optimization for a single problem, exhibiting significant limitations. For example, some adaptive CKFs only adjust the noise covariance through innovation, failing to address the optimistic estimation problem of covariance caused by model mismatch; some robust CKFs focus only on outlier suppression, lacking time-varying noise adaptation capabilities; others employ complex variational methods, which, while improving accuracy, incur heavy computational burdens and struggle to meet real-time requirements. The common limitation of these schemes lies in their optimization logic being a single mechanism for local patching (e.g., adjusting only noise or suppressing only outliers), lacking a systematic fusion framework. Regarding parameter adjustment, they typically rely on empirical settings or single-objective optimization (e.g., pursuing only accuracy), failing to achieve a synergistic trade-off between multiple objectives such as accuracy, stability, and real-time performance. For numerical stability design, existing techniques often lack dedicated mechanisms or employ fixed regularization coefficients, making it difficult to flexibly guarantee the positive definiteness of the covariance matrix during long-term operation. Regarding adaptability to complex scenarios, these methods typically only handle single disturbances (such as assumed fixed noise or only applicable to slight maneuvers), and lack the ability to adapt to complex scenarios involving strong maneuvers, time-varying noise, and continuous outliers. Furthermore, their response mechanisms are mostly passive adaptations, meaning adjustments are made only after errors occur, resulting in a lag in response. These methods suffer from the following deeper problems: 1) Parameter tuning lacks theoretical guidance and relies heavily on trial and error; 2) Improvement mechanisms are isolated, lacking a collaborative optimization framework; 3) Parameter value ranges are set arbitrarily, lacking scientific basis; 4) Algorithm behavior is uncertain, heavily influenced by initial parameters and adjustment strategies; 5) High computational complexity makes it difficult to meet real-time requirements.

[0004] More importantly, existing technologies lack theoretical support and scenario adaptation analysis for setting the value range of key parameters of filtering algorithms (such as fading factor, innovation threshold, regularization factor, and sliding window length). They often use empirical values ​​or simple extensions, which leads to the algorithm performance not reaching the optimal level and poor adaptability in different scenarios.

[0005] The aforementioned limitations restrict the overall performance of existing improvement solutions, making them ineffective in handling complex scenarios with multiple challenges, and even more difficult to meet the engineering application requirements of high-speed, highly maneuverable target tracking. Therefore, there is an urgent need for an integrated filtering solution that can scientifically determine the parameter value range, integrate multiple improvement mechanisms, and synergistically improve accuracy, robustness, real-time performance, and numerical stability through systematic parameter optimization. Summary of the Invention

[0006] The purpose of this invention is to provide a high-speed target tracking method using fixed-parameter optimized capacitive Kalman filtering. By introducing an offline multi-parameter collaborative optimization strategy, the optimal combination of key parameters such as the covariance adjustment factor and outlier suppression threshold is systematically determined. Furthermore, through online multi-mechanism fusion, innovation feedback dynamic covariance adjustment, sliding window noise estimation and outlier suppression are integrated, along with trace correlation adaptive regularization, are organically incorporated into the standard capacitive Kalman filtering framework. This method achieves collaborative response and proactive adaptation to strong maneuvers, time-varying noise, and outlier interference, significantly improving the estimation accuracy, algorithm robustness, numerical stability, and overall engineering practicality of target tracking in extremely complex scenarios.

[0007] To address the aforementioned problems and achieve the objectives and other advantages of this invention, a high-speed target fixed-parameter optimized voluminous Kalman filter tracking method is provided, comprising the following steps: Phase 1: Offline multi-parameter collaborative optimization: S1. Constructing a multi-objective optimization function J (Θ)= f 1 ·MAE+φ 2 ·MaxE+φ 3 ·T , where Θ=( l, t, ，L () represents the parameter vector to be optimized; MAE The mean absolute error, MaxE For the maximum error, T This refers to the time consumed in a single-step calculation. f 1 , f 2 , f 3The weight coefficients, determined by the analytic hierarchy process (AHP), are used to construct the multi-objective optimization function and satisfy the following conditions: f 1 +φ 2 +φ 3 =1; S2. Define a discrete four-dimensional parameter search space: covariance adjustment factor. l ∈{0.950, 0.980, 0.990, 0.995}, outlier suppression threshold t ∈{50, 100, 200, 300}, regularization coefficient ∈{10 -2 10 -3 10 -4 10 -5}, sliding window length L ∈{3, 5, 8, 10}; S3. Traverse the parameter search space using a grid search to evaluate the performance of all 256 parameter combinations and find the multi-objective optimization function. J The globally optimal parameter combination with the minimum (Θ) value l opt , t opt , , L opt ; In the second stage, an online adaptive capacitive Kalman filter is used, loading and applying the optimal parameter combination obtained in the first stage. l opt , t opt , , L opt : S1. System Initialization: Before the filtering iteration begins, complete the following initialization settings: ① Set the initial state estimate Initial error covariance matrix P 0∣0 Initial values ​​of process noise covariance Q 0 and initial values ​​of measurement noise covariance R 0; ② Initialize the new average norm to 0. ③ Create a length of L opt ④ Clear the sliding window cache; ⑤ Preset the first dynamic covariance adjustment factor. l 1=1.0; ⑤ Set the initial noise covariance estimate. = Q 0, ; S2, Volume Point Generation: For each discrete time step k , k ≥1, based on the previous time step k -1 optimal state estimation And error covariance matrix P k-1∣k-1 Generate a set of volume points { X i,k-1∣k-1},in i For volume point index, i =1,...,2 n , n For the state dimension; the formula for generating volume points is: ,in S k-1∣k-1 yes P k-1∣k-1 The Cholesky factorization of the lower triangular factors satisfies ; x i For the preset first i One standard volume point; S3, Time Update: S3A, State Prediction: For each volume point in the volume point set... X i,k-1∣k-1 By describing the nonlinear state transition function of the target motion law Propagate to obtain the volume point after propagation. This constitutes the volume point set after propagation. ; Calculate the predicted state value , oh i For the first i Preset weights corresponding to each volume point; S3B, Innovation Feedback-based Dynamic Covariance Adjustment: 1) Calculate the predicted measurement value: for each point in the propagated volume point set Measurement functions that describe the laws of sensor observation Mapping is performed to obtain the measurement prediction volume points. And calculate the predicted measurement value. ; 2) Based on current measurements z k Compared with the predicted value Calculate new information And calculate the current new information norm. ,in The value is the estimated value of the measurement noise covariance matrix at the previous time step; when k When =1, ;when k When ≥2, The mean norm of the updated information is estimated in step S5B at the previous time step. ;in A smoothing factor with values ​​ranging from 0.05 to 0.15; when k When =1, ;when k When ≥2, The mean norm of the new information obtained from the previous time step; 3) Judge and adjust: Let For adaptive threshold, if Then let the dynamic covariance adjustment factor l k =λ opt Otherwise l k =λ 1, k The adaptive coefficient has a value range of 2 to 3. s z This refers to the scalar value of the standard deviation of the measurement noise or the statistical value of each component of the vector, determined through prior calibration data of the sensor. 4) Calculate the adjusted prediction covariance matrix. ,in The value of the process noise covariance matrix at the previous time step; when k When =1, Q 0, when k When ≥2, It was estimated in step S5B at the previous time step; S4, Measurement Update: Calculate the new information covariance matrix Calculate the state-measurement cross-covariance matrix ; S5. Online estimation and outlier suppression of time-varying noise using a sliding window: S5A, Wild Value Suppression: Determine the current innovation norm ,like Then, a reliable new information is obtained by normalizing and limiting the current new information. Otherwise ; S5B, Noise Estimation: The length is L opt Within the sliding window, data from time [time] is cached. kL opt +1 to the current time k Continuous L opt Each data packet contains reliable information at the corresponding time. New information Measurement and prediction volume point set { Z i，j∣j-1}, Measurement and prediction values State prediction volumetric point set { }, State prediction value New covariance matrix P zz，j∣j-1 Kalman gain K j Posterior covariance matrix P j∣j Predicting the covariance matrix P j∣j-1 Online estimation of the measurement noise covariance matrix at the current time. process noise covariance matrix At that time, based on the calculations already completed in the cache L opt Historical data points, retrieve historical time index j from k−L opt + 1 to k Calculations are performed using the corresponding cached data: The formula for estimating the measurement noise covariance is: , R min =diag([ r 1, r 2,..., r m ]) is the preset lower bound matrix for measurement noise covariance. m For measurement dimensions, r m It is a positive scalar preset based on the sensor's accuracy; The formula for estimating the process noise covariance is: , in Q min =diag([ q 1 ,q 2 ,...,q n ]) is the preset lower bound matrix for process noise covariance. n For the state dimension, q n A positive scalar pre-defined based on the target's minimum maneuverability characteristics; , Historical cache from step S3A; P j∣j Historical cache from step S6; S6. Calculate the Kalman gain, state, and covariance updates for the current time step: Calculate the Kalman gain at the current time step. ; utilizing the Kalman gain K k and the reliable information mentioned above Update state estimation ; utilizing the Kalman gain K k and the aforementioned new information covariance matrix Update the posterior covariance matrix ; S7. Trace Correlation Adaptive Regularization: For the updated error covariance matrix P k∣k Using the optimal regularization coefficient Regularization is performed to obtain a numerically stable posterior covariance matrix. ,in, I To and P k∣k Identity matrices of the same dimension; S8, Iterative Loop: Set the current time... k Optimal state estimation and the regularized posterior covariance matrix P k∣k,reg As the next moment k+ 1. Filter the input and then use the current time. k Reliable news New information Measurement and prediction volume point set { Z i，j∣j-1}, Measurement and prediction values State prediction volumetric point set { }, State prediction value New covariance matrix P zz，j∣j-1 Kalman gain K j Posterior covariance matrix P j∣j Predicting the covariance matrix P j∣j-1 The data is stored in the cache of the sliding window; if the cache is full, the oldest data is overwritten according to the first-in-first-out principle; time index. k Increase by 1 when new measurement data z is received. k+1 If the time is not right, return to step S2 and continue execution.

[0008] Preferably, the high-speed target fixed-parameter optimized voluminous Kalman filter tracking method is characterized in that the globally optimal parameter combination determined in the first stage is: l opt =0.950, t opt =300, =10 -4 , L opt =3.

[0009] Preferably, the high-speed target fixed-parameter optimized voluminous Kalman filter tracking method is characterized in that the state transition function... The measurement function is a function established based on the target aerodynamic model. This is a nonlinear function describing the sensor observation model.

[0010] Preferably, the high-speed target fixed-parameter optimized voluminous Kalman filter tracking method is characterized in that the state transition function... Using the Singh maneuver model ,in, F Here is the state transition matrix of the Singer maneuver model. G For the input matrix, u k-1 For process noise; the measurement function This is a nonlinear radar observation function used to determine the target's Cartesian coordinate state. x,y,z ] T Convert to polar coordinate observation vectors containing slant range, azimuth, and elevation angles. .

[0011] Preferably, the high-speed target fixed parameter optimized volumetric Kalman filter tracking method is characterized in that the sliding window in step S5 is implemented using a first-in-first-out circular queue buffer mechanism.

[0012] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the high-speed target fixed-parameter optimized volumetric Kalman filter tracking method.

[0013] The present invention has at least the following beneficial effects: This invention can effectively improve estimation accuracy. Through innovation feedback-based dynamic covariance adjustment and multi-parameter collaborative optimization, in a complex five-stage maneuvering scenario involving high-speed, high-maneuverability targets, the average position error of this method (…) MAE The accuracy is 4.24m, which is 22.94% higher than the standard volumetric Kalman filter (CKF, 5.51m) and 47.95% higher than the standard unscented Kalman filter (UKF, 8.15m), effectively solving the technical problem of error accumulation in the high-speed and high-maneuver stage.

[0014] This invention significantly enhances the robustness of the algorithm. The innovative sliding window integrated mechanism can simultaneously estimate time-varying noise online and suppress measurement outliers. The standard deviation of this method is only 5.57m, a 67.02% reduction compared to CKF (16.89m) and an 81.56% reduction compared to UKF (30.20m), demonstrating excellent anti-interference capabilities.

[0015] This invention fundamentally guarantees numerical stability. The proposed trace correlation adaptive regularization mechanism, through system optimization of 256 parameter combinations, determines the optimal regularization coefficient to be 10. -4 This coefficient achieves the best balance between accuracy and stability while ensuring 100% numerical stability of the covariance matrix. It exhibits no resetting phenomenon in 500 simulation steps, completely avoiding the filtering divergence problem caused by matrix nondefiniteness.

[0016] This invention achieves a systematic breakthrough in parameter optimization. For the first time, it utilizes a grid search method to optimize four-dimensional parameter space (…). l , t , , L System optimization was performed, and 256 parameter combinations were tested. The findings revealed: ① Innovation threshold t This is the most critical parameter affecting performance; increasing it from 50 to 300 improves performance by 99.5%. ② The optimal parameter combination is... l =0.950、 t =300、 =10 -4 , L =3; ③ The parameter sensitivity ranking is as follows: t >λ> > L The above findings provide a scientific basis for parameter configuration in high-speed, highly maneuverable target tracking.

[0017] This invention has good engineering applicability and real-time performance. The algorithm design fully considers the requirements of embedded deployment. Through methods such as circular queue caching and matrix operation optimization, the single-step calculation time on a typical embedded processor is less than 0.5 milliseconds, which can meet the requirements of high-frequency real-time state updates above 100Hz. It has the potential to be directly applied to mobile platforms with strict real-time requirements (such as drones and robots).

[0018] This invention offers high application flexibility and scalability. The provided offline multi-parameter collaborative optimization framework offers a systematic method for parameter tuning in different application scenarios. Users can quickly determine the optimal parameter set based on specific trade-offs between accuracy, stability, and real-time performance, enabling the algorithm to flexibly adapt to various application scenarios, from industrial monitoring to intelligent navigation.

[0019] Other advantages, objectives and features of the present invention will become apparent in part from the following description, and in part from those skilled in the art through study and practice of the invention. Attached Figure Description

[0020] Figure 1 It is the mean absolute error between FPOCKF and the comparison algorithm ( MAE Compare the bar charts; Figure 2 It is a time-series curve of the tracking error of a high-speed, high-maneuverability target in five stages; Figure 3 It is a three-dimensional surface plot of parameter sensitivity; Figure 4 It is a parameter-sensitive grid subgraph matrix; Figure 5 It is the FPOCKF fixed-parameter optimized volumetric Kalman filter process; Figure 6 This is a comprehensive performance comparison chart between the FPOCKF filtering algorithm and the comparison algorithm. Detailed Implementation

[0021] The present invention will now be described in further detail so that those skilled in the art can implement it based on the description.

[0022] The proposed method for fixed-parameter optimized cubic Kalman filtering (FPOCKF) of high-speed targets, applicable to scenarios involving high speed, high maneuverability, high nonlinearity, time-varying noise, and measurement anomalies, is innovative in its core innovation of constructing a unified framework of "offline multi-parameter collaborative optimization + multi-mechanism fusion." The specific technical solution is as follows: 1. System Model Definition This invention can be widely applied to various engineering systems that require high-precision and robust state estimation, such as autonomous vehicle and drone navigation, robot positioning and motion control, industrial Internet of Things (IIoT) device monitoring, and moving target tracking in intelligent transportation systems.

[0023] The core innovation of the method proposed in this invention lies in constructing a unified framework of "offline multi-parameter collaborative optimization + multi-mechanism fusion". Specifically, this framework includes the following three layers: First layer: Innovation in basic algorithm mechanisms To address the three core issues in high-speed, highly maneuverable target tracking, the following solutions were designed: (1) Innovative feedback dynamic covariance adjustment mechanism (to solve the problems of model mismatch and strong maneuvering); (2) Integrated mechanism for online estimation of time-varying noise and suppression of outliers using sliding window (solving the problems of time-varying noise and outliers); (3) Trace correlation adaptive regularization mechanism (to solve the problem of numerical stability).

[0024] Second layer: System parameter co-optimization By constructing a multi-objective optimization function and employing methods such as offline grid search, the four key parameters ( l , t , , L The globally optimal configuration.

[0025] The third level: Enhancing the efficiency of engineering.

[0026] Techniques such as circular queue caching and matrix operation optimization are employed to ensure that the algorithm runs in real time on resource-constrained embedded platforms.

[0027] The following provides a detailed explanation of each layer.

[0028] Consider a discrete nonlinear high-speed, high-maneuverability target tracking system, with the state equation and measurement equation as follows: (1) Equation of state: (1) in ,Right now The target state vector at any given time includes position, velocity, and acceleration. f (·) represents the nonlinear state transition function, derived based on the aerodynamic model of a high-speed, highly maneuverable target, and its specific form is: (2) F Here is the state transition matrix of the Singer maneuver model. G For the input matrix, u k-1 This is process noise.

[0029] (2) Measurement equation: (3) in for Time measurement vector, For measuring noise, the measurement function h (·) represents the nonlinear radar observation function, used to represent the target's Cartesian coordinates [x, y, z]. T Convert to a polar coordinate observation vector containing slant range, azimuth, and elevation angle.

[0030] 2. Core Improvement Mechanism (1) Innovative feedback-based dynamic covariance adjustment mechanism To address the filter divergence problem caused by model mismatch and high-speed, high-maneuverability issues, this invention designs a dynamic covariance adjustment mechanism based on innovation mean norm feedback to achieve rapid response to dynamic changes in the system. First, define the new information sequence. (4) in The measured predicted value is obtained through volumetric point propagation.

[0031] Calculate the current innovation norm (Mahanobis distance): (5) in The value of the measurement noise covariance matrix estimated at the previous time step is given by... k When =1, = R 0; The new average norm is then updated using a smoothing factor weighting. ; (6) in This is a smoothing factor, with a value range of 0.05 to 0.15. For example, it can be set to... =0.1; when k When =1, The mean norm of the new information is k The innovation norm is set to 1; then an adaptive threshold is set. (7) in To measure the noise standard deviation, it is estimated and determined by fusing sensor prior calibration data with real-time measurement statistics. k The adaptive coefficient (ranging from 2 to 3) ensures that the threshold is adapted to the accuracy of different sensors.

[0032] Then switch the covariance adjustment factor. (If determined to be model mismatch / high maneuverability), then l k =λ opt (Value range 0.95–0.995, optimal value 0.95), adjust the prediction covariance to enhance the innovation weighting, otherwise l k =1, maintaining conventional filtering; finally, the optimized prediction covariance is: (8) in For the volume point after propagation, oh i For volume point weights,n For the state dimension.

[0033] This mechanism breaks through the traditional "passive adaptation" covariance adjustment logic and achieves "active prediction" of model mismatch and strong maneuverability through dynamic feedback of the innovation mean norm. Compared with static noise adjustment, the response speed is improved by more than 30%, and the filtering divergence problem is solved from the root.

[0034] (2) Integrated mechanism of online estimation of time-varying noise and suppression of outliers using sliding window To address the issues of time-varying noise and measurement outliers, this invention designs an integrated mechanism of sliding window covariance matching and threshold suppression to achieve adaptive noise estimation and outlier tolerance. Sliding window design: Employs a first-in, first-out (FIFO) buffering mechanism, implemented through a circular buffer, with a buffer capacity of [missing information]. L opt (Optimal window length, ranging from 3 to 10). When k < L opt When, use the initial value to fill; when k ≥ L opt When updating the cache, discard the earliest data according to the first-in-first-out principle to ensure that the time complexity of the cache update is O(1).

[0035] Wild value suppression logic: Determine the current innovation norm d k ,like d k >t opt ( t To optimize the threshold (ranging from 50 to 300, with an optimal value of 300), normalization and amplitude limiting are performed to obtain reliable information. (9) Otherwise (10) The formula for estimating the measurement noise covariance is: (11) R min =diag([ r 1, r 2,..., r m ]) is the preset lower bound matrix for measurement noise covariance. m For measurement dimensions, r m It is a positive scalar preset based on the sensor's accuracy.

[0036] The formula for estimating the process noise covariance is: (12) in Q min =diag([ q 1, q 2,..., q n ]) is the preset lower bound matrix for process noise covariance. n For the state dimension, q n A positive scalar pre-defined based on the target's minimum maneuverability characteristics; K i For Kalman gain, P j∣j For posterior covariance; Cached Data: A sliding window caches the following data for noise estimation: reliable information. And new information ; Measurement and prediction of volume point set { } and measurement prediction values State prediction volumetric point set { } and state prediction value ; New information covariance matrix P zz，j∣j-1 Kalman gain K j ; Posterior covariance matrix P j∣j and predict covariance matrix P j∣j-1 Output: Reliable information after processing Online estimation of noise covariance and ; This mechanism organically combines time-varying noise estimation with outlier suppression, avoiding the performance fragmentation caused by the "modular processing" of existing technologies. The sliding window FIFO design takes into account both estimation accuracy and real-time performance. Compared with a single outlier suppression scheme, the ability to resist continuous outlier interference is improved by more than 60%.

[0037] (3) Trace correlation adaptive regularization mechanism To address the numerical collapse problem caused by the non-positive definiteness of the covariance matrix, this invention proposes a trace correlation adaptive regularization mechanism to balance numerical stability and estimation accuracy. The specific regularization formula is as follows: (13) in Regularization coefficient (value range 10) -6 ~10 -4 The optimal value was determined to be 10 through parameter optimization. -4 ), Covariance matrix P traces, I It is an identity matrix.

[0038] Validation of regularization coefficient optimization: Through testing with 256 parameter combinations, the regularization coefficient was found to be... In 10 -5 -10 -2 The impact on performance is relatively small within this range, with an optimal value of 10. -4 The specific test data is as follows: =10 -5 At that time, the average error was 468.806m; =10 -4 At that time, the average error was 466.119m (optimal). =10 -3 At that time, the average error was 469.738m; =10 -2 At that time, the average error was 454.742m.

[0039] This mechanism overcomes the limitations of traditional fixed-coefficient regularization by determining the optimal regularization coefficients through parameter optimization. While ensuring the covariance matrix remains 100% positive definite, it achieves a higher level of regularization efficiency compared to existing fixed-coefficient regularization methods. The proposed solution improves estimation accuracy by 15% to 20%.

[0040] 3. Multi-parameter collaborative optimization strategy To achieve globally optimal parameter configuration, this invention constructs a multi-objective optimization function, which traverses the parameter space through a grid search: the multi-objective optimization function is as follows. J(Θ)= f 1·MAE+ f 2·MaxE+ f 3·T (14) Where Θ=( l , t , , L () represents the parameter vector to be optimized; MAE The mean absolute error, MaxE The maximum error is T, and the calculation time for a single step is T. f 1, f 2, f 3 represents the weight coefficients determined by the analytic hierarchy process (AHP) for constructing the multi-objective optimization function, and satisfying the following conditions: f 1+ f 2+ f3=1; Five experts in the field of target tracking were invited to rate the importance of "accuracy, stability, and real-time performance," and the weight vector was calculated and passed through a consistency test (CR < 0.1). The weight coefficients were determined based on the Analytic Hierarchy Process (AHP). f 1 = 0.5 f 2 = 0.3, f 3 = 0.2.

[0041] Parameter optimization implementation process: (1) Determining the parameter space range Based on the four-in-one parameter optimization framework, the scientific parameter space is determined as follows: ① Gradual decay factor l Based on the theory of bidirectional covariance adjustment, for high-speed scenarios dominated by measurement noise, the following is determined: l The value range of <1 is {0.950, 0.980, 0.990, 0.995}.

[0042] ②Innovation suppression threshold t Based on the statistical characteristics of the new information, and considering the increased amplitude of the new information during the strong maneuver phase, the following is determined: t =10σ z ~60σ z That is, {50,100,200,300}.

[0043] ③ Regularization factor Based on numerical stability constraints, {10} are determined for the three-dimensional system. -2 10 -3 10 -4 10 -5}

[0044] ④ Sliding window length L Based on the balance between real-time performance and smoothness, and considering the characteristics of noise mutation and computational constraints, {3,5,8,10} is determined.

[0045] Based on the above considerations, a four-dimensional parameter space is set up for grid search: Covariance Adjustment Factor l ∈{0.950,0.980,0.990,0.995}; Innovation threshold t ∈{50,100,200,300} Regularization coefficient ∈{10 -2 10 -3 10 -4 10 -5}; Sliding window length L ∈{3,5,8,10}; There are a total of 4×4×4×4=256 parameter combinations.

[0046] (2) Parameter optimization execution based on grid search Offline parameter optimization was performed using grid search for 256 parameter combinations. ① Construct a comprehensive index function J (Θ)=0.5· MAE +0.3· MaxE +0.2· T ; ② Perform a grid search within the scientific parameter space, traversing 256 parameter combinations; ③Performance evaluation in a five-stage high-speed target simulation scenario; ④ Determine the globally optimal parameter combination: l opt =0.950, t opt =300, =10 -4 , L opt =3.

[0047] Each of the 256 parameter combinations was tested, and the mean position error (MAE) for each combination was recorded. The test results showed that the optimal parameter combination was group 57. l =0.950、 t =300、 =10 -4 , L =3, corresponding to an average position error of 4.24m.

[0048] (3) Parameter sensitivity analysis By fixing other parameters and varying a single parameter, we can analyze the impact of each parameter on performance. New interest threshold t Most impactful: When t When the value was increased from 50 to 300, the average error decreased from 989.424m to 4.51m, representing a performance improvement of 99.5%. The covariance adjustment factor λ had the next greatest impact. l When the regularization coefficient varies within the range of 0.950-0.995, the average error ranges from 462.88 to 463.77 m. ϵ The impact is relatively small. In 10 -5 -10 -2 When varying within the specified range, the average error ranges from 454.74 to 469.74 meters. (Sliding window length) L Minimal impact: L When the value varies within the range of 3-10, the average error ranges from 463.09 to 463.13 m.

[0049] (4) Optimal parameter verification based on online fixed parameter filtering The main steps are: ①System initialization: Load the optimal parameter combination Θ opt =(0.950,300, 10 -4 ,3); ②For each moment k : a) Volume point generation and time update; b) Dynamic covariance adjustment based on feedback: If the mean norm of the innovation... >2.5 s z ,but ; c) Measurement updates and outlier suppression: If the new information If the value is greater than 300, then amplitude limiting will be applied; d) Noise estimation: based on L Sliding window estimation of =3 and ; e) State Updates and Regularization: Using =10 -4 Perform regularization; ③ Output the state estimation sequence.

[0050] (5) Optimal parameter verification based on Monte Carlo simulation Use optimal parameter configuration ( l =0.950、 t =300、 =10 -4 , L =3) Conduct 1000 Monte Carlo simulations to verify the results, which show that the parameter combination can maintain optimal performance in a variety of scenarios.

[0051] This strategy abandons the subjectivity of existing technologies' "experience-based parameter tuning" and achieves globally optimal parameter configuration through systematic grid search. Compared with traditional single-objective optimization methods, it improves real-time performance by 67% while maintaining an accuracy loss of ≤5%, achieving a balance between "accuracy, stability, and real-time performance".

[0052] (6) Parameter extension in engineering applications For different engineering scenarios, the parameter determination principles proposed in this invention are extended as follows: ① Gradual decay factor l : The scenario dominated by process noise is expanded to {0.9, 0.95, 1.0, 1.05, 1.1}. Measurement noise-dominant scenario: maintain {0.950, 0.980, 0.990, 0.995}.

[0053] ②Innovation suppression threshold t : Known s z Scenario: Set to {10σ} z ,20σ z ,40σ z ,60σ z}; For time-varying noise scenarios: an adaptive thresholding method is used.

[0054] ③ Regularization factor : High-dimensional systems ( n >10): Expand to {10 -6 10 -5 10 -4 10 -3}; Non-positive definite scenarios: Increase to {10} -3 10 -2}

[0055] ④ Sliding window length L: Steady noise scenario: Expanded to {5,10,15,20}; For scenarios with frequent mutations: maintain {3,5,8,10}.

[0056] (7) Optimization of embedded platform To meet the real-time requirements of embedded platforms, the following optimization measures are adopted: ① Matrix operation optimization: Block matrix operations are adopted to decompose large matrices into smaller matrices for processing; ② Memory management optimization: Use pre-allocated memory pools to avoid dynamic memory allocation; ③ Sliding window optimization: Implemented using a circular buffer, with enqueue and dequeue operations having O(1) complexity; ④ Numerical calculation optimization: Single-precision floating-point arithmetic is adopted to improve speed while ensuring accuracy; ⑤ Parallel computing optimization: Perform parallel computing of volume point propagation on processors that support SIMD.

[0057] (8) Multi-sensor fusion extension Extending FPOCKF to multi-sensor fusion scenarios: ① Sensor data synchronization: Achieve data alignment among multiple sensors based on timestamps; ② Fusion Strategy Design: A sequential fusion strategy is adopted to process data from each sensor in turn; ③ Noise covariance adjustment: Dynamically adjust the noise covariance weights based on sensor accuracy; ④ Consistency test: The covariance consistency test of the new information is used to eliminate abnormal sensor data.

[0058] Complete technical solution for high-speed target fixed parameter optimized volumetric Kalman filter tracking system This invention aims to overcome the combined challenges of model mismatch, time-varying noise, outlier interference, and numerical instability faced by existing nonlinear filtering algorithms in high-speed, high-maneuverability target tracking scenarios. To address these challenges, this invention proposes a systematic solution. Its core innovation lies in constructing a unified framework that integrates "offline multi-parameter collaborative optimization" and "online multi-mechanism adaptive filtering." The offline phase determines the algorithm's optimal operating point through scientific optimization, while the online phase is a recursive closed-loop process integrating three innovative mechanisms. This achieves a comprehensive improvement in estimation accuracy, robustness, numerical stability, and computational efficiency in complex scenarios.

[0059] Phase 1: Offline multi-parameter collaborative optimization (establishing the optimal operating point of the algorithm) Objective: Before actual deployment of the algorithm, to scientifically determine the optimal combination of four key parameters through a systematic grid search, so as to improve the tracking accuracy of the online filtering algorithm. MAE ), robustness ( MaxE ) and real-time ( T It achieves a globally optimal balance across three competitive metrics, thereby overcoming the subjectivity and limitations of traditional "experience-based parameter tuning" and providing an optimal initial working point for online algorithms that has been globally verified.

[0060] Specific implementation process: 1. Define the multi-objective optimization function and system model: Optimization Function: Constructing a Multi-Objective Optimization Function J(Θ)= f 1 MAE+ f 2 MaxE+ f 3 · T (Formula 1) As a combination of evaluation parameters Θ= (l,t, ,L Performance standards, including: Θ=( l , t , , L ): The parameter vector to be optimized; l Covariance adjustment factor; t Outlier suppression threshold; Regularization coefficient; L : Length of the sliding window; MAE Mean absolute error, a measure of average tracking accuracy; MaxE Maximum error measures tracking robustness in the worst-case scenario; T : The time taken for a single-step calculation, which measures the real-time performance of the algorithm; f 1 , f 2 , f 3 The weighting coefficients are determined by experts through the Analytic Hierarchy Process (AHP) after scoring "accuracy, stability, and real-time performance" and undergo a consistency test (CR < 0.1) to ensure that the evaluation criteria are objective and reasonable. f 1+ f 2+ f 3 = 1. f 1 = 0.5 f 2 = 0.3, f 3 = 0.2.

[0061] System Model: The optimized performance evaluation is based on simulation of the target motion model. This model also provides the mathematical foundation for subsequent online filtering. Equations of state: (Formula 2) Purpose and Reason: To describe the intrinsic dynamic laws governing the evolution of a target state (such as position, velocity, and acceleration) over time, providing the theoretical basis for time prediction (extrapolation) by filters. It offers a mathematical model for inferring future states from past states.

[0062] in: ,Right now k The target's true state vector at any given time contains information such as position, velocity, and acceleration. It is a nonlinear state transition function, derived based on aerodynamic models of high-speed, high-maneuverability targets (such as the Singer model). : That is, process noise, which has a mean of zero and a covariance matrix of Q k−1 A random vector is used to simulate unmodeled dynamic or random disturbances.

[0063] Specification of the state transition function: (Formula 3) Purpose and Reason: To provide a more accurate dynamic description of high-speed, highly maneuverable targets. The Singer maneuver model is an aerodynamic model that can better simulate the statistical characteristics of target acceleration changes (such as time correlation and finite variance), and is the model in formula (2). This is a specific and preferred implementation, especially suitable for high-mobility tracking scenarios.

[0064] in: F This is the state transition matrix of the Singer maneuver model, which reflects the linear transition relationship between states. G The input matrix is ​​denoted as . u k−1 Input for motor acceleration.

[0065] Measurement equation: (Formula 4) Purpose and Reason: To establish a mathematical relationship between the target's true state and sensor observations. This is the theoretical basis for filters to use external observations (such as radar and GPS data) to correct and update state estimates.

[0066] in: z k : k The actual sensor observation (measurement) vector at any given time. The measurement function. This is a nonlinear function describing the sensor observation model, used to represent the target's Cartesian coordinate state [x, y, z]. T Convert to polar coordinate observation vectors containing slant range, azimuth, and elevation angles. . v k Measurement noise, v k ~ N (0, R k That is, it follows a function with a mean of zero and a covariance matrix of... R k The normal distribution is used to simulate sensor errors and observation environment interference.

[0067] 2. Perform grid search and determine optimal parameters: Operation: Perform an exhaustive grid search within a predefined four-dimensional discrete parameter space. The parameter search space is: Covariance Adjustment Factor l ∈{0.950, 0.980, 0.990, 0.995}; Outlier suppression threshold t ∈{50, 100, 200, 300}; regularization coefficient ∈{10 -2 10 -3 10 -4 10 -5}; Sliding window length L ∈{3, 5, 8, 10}; a total of 256 parameter combinations.

[0068] Evaluation: Conduct thorough Monte Carlo simulation tests on all 256 parameter combinations Θ, record and evaluate the performance metrics for each combination. MAE , MaxE and T And calculate its comprehensive index. J (Θ).

[0069] Determine the optimal parameters: By iterating and comparing, determine the parameters that make the optimal parameters... J (Θ) The smallest globally optimal parameter combination. The optimal combination verified in the document's examples is: l opt =0.950, t opt =300, =10 -4 , L opt =3, corresponding to an average position error of 4.24m.

[0070] Results: This process abandons subjective "experience-based parameter tuning" and, through systematic searching and scientific function evaluation, finds a parameter set that achieves the optimal balance between accuracy, robustness, and real-time performance. Therefore, the sliding window length... L =3 is the optimal value obtained through optimization, rather than being set arbitrarily. This ensures that the algorithm can start with the best configuration in subsequent online tracking.

[0071] Connection to subsequent online processes: This set of optimal parameters will be directly loaded and used in the entire online filtering process of the second stage. Specifically, l opt Used for online step 3b (dynamic covariance adjustment). t opt and L opt Used for online step 4b (noise estimation and outlier suppression). Used for online step fiveb (regularization).

[0072] Phase 2: Online multi-mechanism adaptive capacitive Kalman filtering (performing robust tracking) When the online process begins, the optimal parameter set obtained in the first stage is loaded first. l opt =0.950, t opt =300, =10 -4 , L opt =3). Then, follow these steps for each time point. k The measurement data is processed iteratively.

[0073] Step 1: System Initialization Objective: To set the starting point for recursive calculations in a recursive filtering algorithm, and to provide the initial state estimate and uncertainty measure required for the algorithm to start running.

[0074] Input: Initial state estimate Initial error covariance P 0∣0 Initial value of process noise covariance Q 0. Initial value of measurement noise covariance R 0, and four key hyperparameters: covariance adjustment factor l opt (Optimized value is 0.95), outlier suppression threshold t opt (Optimization value is 300), Regularization coefficient (Optimized value is 300), sliding window length L opt (Optimization value is 3).

[0075] Initial state estimation : Source: Usually determined based on prior information. For example, it can be obtained by converting data from the first effective measurement by sensors (such as radar or GPS); or it can be set based on prior reports of the target's initial position, system-preset initial points, etc.

[0076] Properties: This is a vector containing state components such as the target's initial position and velocity.

[0077] Initial error covariance P 0∣0 : Meaning: It represents the initial estimate Quantification of uncertainty (confidence level).

[0078] Setting principle: It is usually set as a diagonal matrix. The elements on the diagonal (variance) are set according to the degree of confidence in the initial estimate of the corresponding state component: if the confidence is high (the estimate is accurate), a smaller value is set; if the confidence is low or the uncertainty is high, a larger value is set. For example, if the initial position is relatively certain but the initial velocity is uncertain, the position variance is set to a small value, and the velocity variance is set to a large value. This is a common practice in engineering.

[0079] Initial values ​​of process noise covariance Q 0 and initial values ​​of measurement noise covariance R 0: Physical meaning: Q 0 characterizes the intensity of dynamics or disturbances not modeled in the system's state equations (motion model); R 0 represents the intensity of the sensor's measurement error.

[0080] Basis for determination: Based on sensor and system characteristics: R 0 is usually set directly based on the factory accuracy specifications (such as error variance) of the selected sensor. Q The value of 0 is set based on the analysis of the target's motion characteristics (such as maneuverability) and the assessment of modeling errors.

[0081] Based on experience or measured data: preliminary calibration is performed through statistical analysis of historical or test data.

[0082] Starting with simple values: When precise prior information is lacking, those skilled in the art often set it as a diagonal matrix and assign a reasonable magnitude as the starting point for iteration based on engineering experience, and then rely on the algorithm's own adaptive capabilities for online adjustment.

[0083] Operation: In k At time 0, set the initial values ​​of the filter: 1) Set the initial estimate of the target state. 2) Set the initial error covariance matrix P 0∣0 =P 0; 3) Initialize the new interest mean norm 4) Initialize the new information sliding window cache (length is...) L ) is empty. 5) Set adaptive parameters: l 1 = 1.0 (fade-off is not enabled at the initial moment).

[0084] in: The algorithm's estimate of the target's initial state ("^" indicates estimation) is set based on prior information (such as the target's initial position report). P 0∣0 This is a measure of the uncertainty of the initial estimate (covariance matrix), with the diagonal elements representing the variance of the estimates for each state component. A larger value indicates greater uncertainty in the initial estimate. R 0 The initial value of the measurement noise covariance is the initial preset value of the measurement noise covariance, which represents the sensor measurement error. Q 0 The initial value of the process noise covariance is the initial preset value of the process noise covariance, which represents the uncertainty of the system model. : The initial average norm of the new interest rate, representing the historical value of the new interest rate statistics ( Indicates new information, (The mean norm of the new information). l 1 is the covariance adjustment coefficient. This is the initial value of the statistic used for state determination in subsequent step 3b.

[0085] Connection to subsequent steps: In the first filtering loop ( k =1), step one andP 0∣0 It is directly used as the input for step two and assigned to the "previous best estimate". And error covariance matrix P k−1∣k−1 (at this time k -1=0), thus starting the entire filtering loop; Q 0 The process noise component that provides the prediction covariance affects the noise estimation start point and convergence speed. ；R 0 Its function is to standardize information and provide measurement uncertainty, which can affect Kalman gain and trust allocation; Its function is to initiate a smoothing process, avoiding historical biases that could subsequently affect the sensitivity of strong maneuver detection; initially l 1=1.0 indicates that the covariance adjustment coefficient remains neutral in the initial stage and is not adjusted initially to accumulate statistical information. Subsequent steps will gradually establish a baseline through gridded optimization, which will affect the learning of the adjustment strategy.

[0086] Start-up time ( k =0) For the complete configuration table, see Table 1.

[0087] Table 1 FPOCKF Algorithm Initialization Parameter Configuration Table Notes: In the above configuration, the initial state Satisfy physical constraints (i.e.) V MIN ≤ v 0≤ V MAX and A MIN ≤ a 0≤ A MAX This ensures that the algorithm starts within a valid initial domain.

[0088] P Setting it to 0 reflects a low confidence level (large variance) for the initial velocity information, which is beneficial for the filter to quickly correct the estimated value in the early stages of iteration.

[0089] With the above settings, the filter has a clear initial state, reasonable metrics, and hyperparameters tuned for specific scenarios, and can now proceed with formal testing. k The iterative loop at time =1.

[0090] Step 2: Volume point generation (at time...) k ) Objective: To estimate the optimal state based on the previous time step. and its uncertaintyP k-1∣k-1 This generates a set of sampling points (called volume points). These points are used to obtain the posterior probability distribution of the high-precision approximation state and are the core foundation for achieving high-precision nonlinear estimation using volume Kalman filtering (CKF) transcending linearization methods (such as EKF).

[0091] Input: From step one (when) k =1) or step six (when k >1) "The optimal estimate of the previous time step", i.e., the state Covariance P k-1∣k-1 .

[0092] Operation: Generate volume points: based on the posterior estimate from the previous time step. Covariance P k-1|k-1 Cholesky decomposition factor S k-1|k-1 Calculate 2 n Volume points ( n (State dimension) ① Perform Cholesky decomposition to obtain the lower triangular factor matrix. S k-1∣k-1 (satisfy ).

[0093] ② Calculate the volume point set: (Formula 5) Output: Volume point set { X i,k-1∣k-1}, i =1,...,2 n .

[0094] in: : k The optimal estimate of the state at time -1. S k-1∣k-1 The square root covariance factor, whose column vector defines the main direction of state uncertainty. x i : Preset first i A standard volume point is used for sampling on a unit hypersphere. X i,k-1∣k-1 The generated volume points are distributed in the following areas: On the ellipsoid centered at the center, the distribution of the state estimate is represented. n : The dimension of the state vector.

[0095] Connection to subsequent steps: The output volume point set is directly passed to step 3a for state prediction using a nonlinear state transition function.

[0096] Step 3: Time Update (Prediction) 3a. Propagation volume point and calculated state prediction mean: Objective: To develop a nonlinear system dynamics model The state distribution generated in step two is extrapolated (predicted) to the current time. k To obtain a predicted value of the current state. This value serves as a baseline "anchor" for subsequent calibration using measurement data.

[0097] Input: The set of volume points output in step two X i,k-1∣k-1 .

[0098] operate: ① Transform each volume point through a state transition function spread: (Formula 6) ② Calculate the weighted average of the volume points after propagation, as a one-step prediction of the state: (Formula 7) Output: State prediction (“anchor point”) And the propagated set of volume points { }

[0099] in: :based on k -1 time information pair k Prediction and estimation of state at any given time.

[0100] The volume point after propagation represents the distribution of the predicted state.

[0101] oh i Each volume point x i The corresponding preset weights.

[0102] Connection with subsequent steps: Output This will be used in step 3b to calculate the predicted covariance, in step 4a to calculate the measurement prediction, and as the starting point for the state update in step 5a. The output... This will be specifically used for the prediction covariance calculation in step 3b and the measurement prediction calculation in step 4a.

[0103] 3b. Feedback-based dynamic covariance adjustment: Objective: To address the risk of filter divergence caused by model mismatch (mathematical model does not match actual motion) or strong target maneuvering, a proactive, feedback-based covariance adjustment mechanism is designed.

[0104] Input: Output of step 3a and The process noise covariance at the previous time step Q k-1 The mean norm of the new information obtained from the previous time step. ; and we need to wait for the measurement prediction value calculated in step foura. and the actual measurement at the current moment z k .

[0105] operate: ① Calculate the predicted measurement value: Partially perform step foura to obtain Each point in the propagated volumetric point set Mapping is performed using the measurement function h(·): (Formula 8) Calculate the predicted measurement value: (Formula 9) ② Calculate the new information: based on the current measurement z k Compared with the predicted value Calculate new information (Formula 10) ③ Calculate the current innovation norm: Calculate the current innovation norm (Mahanobis distance): (Formula 11) in The value is the estimated value of the measurement noise covariance matrix at the previous time step; when k When =1, = R 0, when k When ≥2, It was estimated in step 4b at the previous time step; ④ Update the new average norm: using a smoothing factor c (Values ​​range from 0.05 to 0.15, e.g., 0.1) Update the average norm of the innovation to smooth out noise and reflect trends: (Formula 12) in The mean norm of the new information obtained from the previous time step; when k When =1, The mean norm of the new interest rate is the new interest rate norm when k is 1; ⑤ Judgment and Adjustment: Calculate the adaptive threshold: (Formula 13) in k This is the adaptive coefficient (value range 2 to 3). s z The noise standard deviation is determined using prior calibration data of the sensor.

[0106] Judgment: If If the condition is determined to be a strong maneuver or model mismatch, the optimization factor is activated. l k = l opt (Value range: 0.95–0.995, with an optimal value of 0.95); otherwise, let l k = l 1 = 1.

[0107] ⑥ Calculate the adjusted prediction covariance: (Formula 14) in The value of the process noise covariance matrix at the previous time step; when k When =1, = Q 0, when k When ≥2, It was estimated in step 4b at the previous time step; Output: Adjusted prediction covariance matrix P k∣k-1 Current news The updated mean norm of the new information .

[0108] in: The new information, namely the difference between the actual observation and the predicted observation, is a key indicator reflecting the consistency of the filtering and is used in step fourb. d k The current innovation norm is used for the outlier suppression judgment in step 4a; The mean norm of the innovation, the smoothed innovation magnitude, serves as a core statistic for determining whether the system is in a state of strong maneuvering or model mismatch. Its current value is used for judgment in this step, and its updated value will be passed to the next time step. k +1) Step 3b is used; Γ is an adaptive threshold, based on sensor accuracy ( s z Dynamically set judgment thresholds; l k : Dynamic covariance adjustment factor, the value is determined based on the judgment result ( l opt Or 1), used to adjust the magnitude of the prediction covariance (when In fact, this shrinks the prediction covariance, which is contrary to the traditional approach of "amplifying" covariance to increase gain. This is a counterintuitive result discovered in this application for optimization in high-speed scenarios. P k∣k-1 The adjusted forecast covariance reflects the uncertainty in state prediction.

[0109] Connection with subsequent steps: Output P k∣k-1 It is directly used in step foura to calculate the innovation covariance, and in step fivea to calculate the Kalman gain and the updated posterior covariance. The output... Used for step 4b. Output Save it for the next moment ( k The input "news mean norm of the previous time step" in step 3b (+1 time step) is used to determine the strong maneuver / model mismatch state at this time step.

[0110] Results: This mechanism fundamentally suppresses filter divergence caused by model mismatch or strong maneuvers. Compared with the traditional method of "passively adjusting only after errors occur", it achieves "active prediction", improving response speed by more than 30% and significantly improving tracking stability during strong maneuvers.

[0111] Step 4: Measurement Update (Processing) 4a. Calculate measurement predictions and related covariance: Objective: To predict the distribution of the state space (from...) and P k∣k-1 (Characteristics) through measurement functions h (⋅) Mapped to the measurement space, the predicted measurement value is calculated. New information covariance P zz，k∣k-1 State-Measurement Cross-Covariance P xz，k∣k-1 These statistics are crucial for calculating the optimal Kalman gain and performing state correction.

[0112] Input: The propagation volume point output from step 3a Step 3b outputs the predicted covariance. P k∣k-1 Measurement noise covariance at the current moment .

[0113] Source: In the first filtering cycle ( k =1), Initial value R 0; from the second cycle ( k ≥2) start, Using the online estimation from step 4b of the previous time step This enables adaptive updating of the noise model.

[0114] operate: ① Calculate and measure the predicted volume points and mean values: (Formula 15) (Formula 16) ② Calculate the covariance matrix of the innovation (measurement prediction): (Formula 17) 3. Calculate the cross-covariance matrix of state and measurement: (Formula 18) Output: Predicted measurement values New information covariance Mutual covariance .

[0115] in: These are measurement prediction values ​​obtained based on state prediction; P zz，k∣k-1 The information covariance reflects the uncertainty of measurement prediction (including state prediction error and measurement noise). P xz，k∣k-1 The state-measurement cross-covariance reflects the correlation between state prediction error and measurement prediction error.

[0116] Connection with subsequent steps: Output It will return to step 3b to calculate the new information. Output P zz，k∣k-1 and P xz，k∣k-1 It is directly passed to step five a for calculating the Kalman gain.

[0117] 4b. Online estimation and outlier suppression of time-varying noise using a sliding window method: Objective: To simultaneously address the problems of time-varying noise estimation and measurement outlier suppression within a unified sliding window framework.

[0118] Input: The current information output in step 3b and the current new information norm; length is L opt Historical data cached within the sliding window of value 3; output of step four a. P zz，k∣k-1 Step 5a involves calculating the Kalman gain. K k (Used for process noise estimation); Sensor noise lower limit R min Lower limit of process noise Q min .

[0119] operate: ① Outlier suppression: Determine the current innovation norm d k : like d k >t opt ( t opt The optimized threshold value ranges from 50 to 300. t opt If the value is 300, then normalizing and limiting the current information yields a reliable information: (Formula 19) Otherwise, reliable information retains its original value, i.e.: (Formula 20) ② Noise estimation: using the nearest noise level within the sliding window L opt Calculations are performed using historical data of the steps: Formula for estimating measurement noise covariance: (Formula 21) R min =diag([ r 1 , r 2 , ..., r m ]) is the preset lower bound matrix for measurement noise covariance. m For measurement dimensions, r t ( t=1,2,...,m () is a positive scalar preset based on sensor accuracy; max ( · , R min ) represents the result matrix and R min Take the largest value for each element to ensure that the estimated value is not lower than the theoretical lower limit of the sensor and to prevent the matrix from being non-positive definite.

[0120] Formula for estimating process noise covariance: (Formula 22) in Q min =diag([ q 1 ,q 2 ,...,q n ]) is the preset lower bound matrix for process noise covariance. n For the state dimension, q s ( s =1,2,..., n ) is a positive scalar preset based on the target's minimum maneuverability characteristics; max (·, Q min ) represents the result matrix and Q min By taking the largest value for each element, we can ensure that the estimated value is not lower than the theoretical lower limit of process noise covariance and prevent the matrix from being non-positive definite.

[0121] Sliding window caches data: The sliding window caches data from time... kL opt +1 to the current time k Continuous L opt 1 data packet, each data packet containing: Reliable News And new information ; Measurement and prediction of volume points { Z i，j∣j-1} and measurement prediction values ; State prediction volume point { } and measurement prediction values ; New covariance matrix P zz，j∣j-1 Kalman gain K j ; Posterior covariance matrix P j∣j and predict covariance matrix P j∣j-1 ; Output: Reliable information after processing Online estimation of noise covariance and .

[0122] in: The processed reliable information, after outliers are suppressed, is used for the state update in step 5a; and The online estimated measurement and process noise covariance reflects the statistical characteristics of the noise at the current moment. This will be used as the basis for the next moment's... k+1 The noise covariance estimates in steps 3b and 4a enable the filter to adapt to time-varying noise environments. R min and Q min The physical lower limit of sensor noise ensures that the estimated value is reasonable.

[0123] Connection with subsequent steps: Output It is directly passed to step five a for state update. The output is... and Will serve as the next moment ( k The prior value of the noise covariance at time +1, i.e., let = , = This allows for feedback updates to the noise covariance input in steps 3b and 4a of the next round, achieving closed-loop adaptation.

[0124] Results: This mechanism avoids the performance fragmentation caused by the "modular processing" of existing technologies, organically integrates noise adaptation and outlier suppression, and the sliding window FIFO design takes into account both estimation accuracy and real-time performance. Compared with a single outlier suppression scheme, the ability to resist continuous outlier interference is improved by more than 60%, which greatly improves the robustness and adaptability of the algorithm under complex interference.

[0125] Step 5: State Update (Correction) and Regularization 5a. Calculate the Kalman gain and update the state: Objective: To predict uncertainty ( P k∣k-1 ) and measurement uncertainty ( P zz,k∣k-1 ), calculate the optimal Kalman gain K k Then, using K k Anchor points for state prediction With processed reliable measurement information Perform optimal fusion (weighted averaging) to obtain the optimal state estimate for the current time step. and its error covariance P k∣k This is the core idea of ​​Kalman filtering.

[0126] Input: Output of step 3a Step 3b output P k∣k-1 Step 4a output P xz，k∣k-1 and P zz，k∣k-1 Reliable information output in step fourb .

[0127] operate: ① Calculate the Kalman gain: (Formula 23) ② Update state estimation: (Formula 24) in, To use reliable information that has undergone threshold processing.

[0128] ③ Update error covariance: (Formula 25) Output: Optimal posterior state estimate Posterior error covariance P k∣k Kalman gain K k .

[0129] in: K k Kalman gain, the optimal weight matrix, determines the extent to which information (measurement) should be trusted over prediction; the gain is small when prediction is more reliable and large when measurement is more reliable. The calculated Kalman gain... K k It will immediately provide feedback to step 4b, which is used for process noise covariance estimation. Calculation (Formula 22); : Optimal state estimation that integrates prediction and measurement information; P k∣k The posterior error covariance reflects the optimal estimate. The uncertainty is passed to step five b for regularization.

[0130] Connection with subsequent steps: Output and P k∣k This is the final result of this filtering process, and it is immediately passed to step five (b). Simultaneously, the calculated... Kk This feedback will be sent to step 4b for process noise covariance estimation. Calculation (Formula 22).

[0131] 5b. Trace Correlation Adaptive Regularization: Objective: To address the numerical stability problem in recursive calculations where the covariance matrix may lose its positive definiteness due to finite-precision rounding errors (leading to filter collapse), an adaptive regularization method is proposed.

[0132] Input: The posterior error covariance output from step 5a P k∣k Optimal regularization coefficient =10 -4 .

[0133] Operation: Apply the updated error covariance matrix P k∣k Perform regularization: (Formula 26) Output: Regularized, numerically stable posterior covariance matrix P k∣k,reg .

[0134] Among them: I To and P k∣k An identity matrix of the same dimension. P k∣k,reg The regularized covariance matrix, while preserving the information of the original matrix, is forced to be positive definite by adding a small perturbation proportional to the total uncertainty. This perturbation is then passed to step seven as the initial covariance for filtering at the next time step.

[0135] Connection with subsequent steps: Output P k∣k,reg This will be passed to step seven as the initial covariance for filtering at the next time step.

[0136] Effect: This mechanism overcomes the limitations of traditional fixed-coefficient regularization by determining the optimal regularization coefficient through parameter optimization. =10 -4 While ensuring that the covariance matrix is ​​100% positive definite, the estimation accuracy is improved by 15% to 20% compared with the fixed scheme, which fundamentally guarantees the reliability of the algorithm in long-term operation in environments such as embedded platforms.

[0137] Step Six: Stability Theory Guarantee The superiority of the FPOCKF algorithm is not only reflected in simulation experiments, but also in the rigorous stability guarantee obtained from the perspective of control theory.

[0138] In the theoretical analysis, we define the estimation error as: (Formula 27) in: x k The goal is at all times k The actual state (used as a mathematical variable in theoretical analysis for derivation); This is the optimal state estimate output in step 5a (state update) of the FPOCKF algorithm's online process. This error... In actual online filtering, it is not directly calculated (because the real state...). x k (Unknown), but in offline theoretical analysis and performance evaluation (such as simulation), it can be obtained by comparing the known real trajectory with the estimated trajectory, and is the core indicator for measuring the accuracy of the algorithm.

[0139] By constructing based on estimation error and subsequent covariance P k∣k Lyapunov function (from step 5a) (Formula 28) Furthermore, by incorporating the complete update formulas (Formulas 24 and 25) and the three core mechanisms (dynamic covariance adjustment, online noise estimation, and adaptive regularization) into the derivation, it can be proven that its difference satisfies the following key inequalities: (Formula 29) in: α >0 is a parameter related to the algorithm (e.g., ... l opt 、 The existence of the positive definiteness of the covariance matrix and the boundedness of the gain matrix (etc.) is guaranteed by the positive definiteness of the covariance matrix and the boundedness of the gain matrix in the algorithm. d k For a system process noise oh k Measurement noise v k And the bounded terms determined by the nonlinear linearization residuals, i.e., the existence of constants. d max Make d k ≤ d max For all k Established; △ V k A trend of ≤0 indicates "error energy". V k An overall decline or limited growth is a key characteristic of system stability.

[0140] This inequality has a clear physical and mathematical meaning: ① Convergence driving term It is a strictly negative definite term, ensuring that when the estimation error... When the value is large, the Lyapunov function V k This will significantly reduce the error, thus forcing it to shrink towards zero, demonstrating the algorithm's proactive convergence capability.

[0141] ② Bounded perturbation term d k It represents the influence of external noise and model uncertainty that cannot be completely eliminated, but its boundedness ensures that these disturbances will not accumulate indefinitely and will not cause filter divergence.

[0142] Overall stability conclusion: This inequality mathematically proves rigorously that the estimation error of the FPOCKF algorithm is uniformly and eventually bounded. That is, there exists a compact set independent of the initial state. Oh This leads to estimation error Enter within a limited time and remain thereafter Oh This fundamentally eliminates filter divergence caused by model mismatch, time-varying noise, or outlier interference, laying a solid theoretical foundation for the algorithm's robustness in extremely complex scenarios. This theoretical guarantee stems from the systematic integration of three mechanisms—dynamic covariance adjustment, time-varying noise estimation and outlier suppression, and adaptive regularization—resulting in overall performance properties that are not possessed by existing CKF algorithms improved using a single mechanism. It not only provides theoretical guidance for algorithm design but also serves as a crucial source of confidence in engineering applications, ensuring the reliability and stability of FPOCKF during long-term operation.

[0143] Theoretical parameters are determined by constants in the inequalities. α With bounded terms d k This is achieved by substituting the specific update equations of the FPOCKF algorithm into the Lyapunov function. V k It was later determined through systematic mathematical derivation. α The existence of >0 depends on the optimal parameters. l opt 、 The positive definiteness of the covariance matrix and the boundedness of the gain are guaranteed. d k The upper realm d max Then, the statistical upper bound of system noise, the smoothness constant of the nonlinear function, and the adaptive mechanism in the algorithm (such as...) are all taken into account. l k The range of values, Rmin The algorithm is defined by the settings of ( ). This derivation process is completed in offline theoretical analysis, and its conclusion—that UUB stability holds—is the theoretical cornerstone of the algorithm design. When performing online filtering, the algorithm does not require explicit computation. α and d k Its inherent stability has been achieved through the fusion of the aforementioned multi-mechanism and parameter optimization, and has been verified in simulations through the boundedness of the error trajectory.

[0144] Step 7: Iteration Loop Objective: To complete the current task. k The entire filtering calculation is performed, and the final optimal state estimate is obtained. and stable covariance estimation P k∣k,reg Passed to the next moment k +1, as the next moment ( k The initial value is incremented by 1, and the process returns to step two, thus driving the recursive loop for continuous tracking. This is the key to achieving real-time, continuous tracking.

[0145] Input: The optimal state estimate output from step 5a Step 5b outputs the regularized covariance. P k∣k,reg .

[0146] operate: Cache data to the sliding window: Set the current time... k The following data is stored in the sliding window cache: reliable information And new information ; Measurement and prediction of volume points { Z i，j∣j-1} and measurement prediction values State prediction volume point { } and measurement prediction values ; New information covariance matrix P zz，j∣j-1 Kalman gain K j ; Posterior covariance matrix P j∣j and predict covariance matrix P j∣j-1 If the cache is full (i.e.) k ≥ L opt If the data is first-in, first-out (FIFO), then the earliest data will be covered.

[0147] ② Update the state and covariance to the initial values ​​for the next time step (i.e., the current posterior estimate becomes the "previous best estimate" for the next time step): (Formula 30) (Formula 31) ③Time index update: k = k +1.

[0148] Output and Loop Triggering: After this step is completed, the algorithm enters a waiting state. When a new measurement... z k+1 Upon arrival (marking the start of a new discrete sampling time), the algorithm will immediately use the data updated in this step. and P k∣k,reg (They now correspond to) and P k-1∣k-1 As input, automatically start a new round from step two. k (+1 time) Filtering calculation. Measurement z k+1 This refers to the external input that drives this new round of iterations. This is the core mechanism for achieving real-time, continuous tracking.

[0149] The core logic of the process is summarized as follows: FPOCKF's innovative process lies in its creation of an enhanced closed loop that sequentially performs prediction, adaptive correction and stabilization, anti-outlier correction, online noise learning, and updates. The logic is as follows: ① Immediately assess the model's reliability after prediction (using the covariance adjustment factor). l k And ensure that the values ​​are healthy (regularization).

[0150] ② Before using the measurement, perform a "security check" (new information threshold). t opt ), to prevent bad data pollution.

[0151] ③ Utilize historical information to dynamically learn the uncertainty of the system (covariance matching) so that the filter model continuously approximates the real environment.

[0152] This process transforms FPOCKF from a state estimator into an intelligent filtering system with self-diagnosis, self-protection, and self-learning capabilities, thereby demonstrating excellent accuracy, robustness, and adaptability in extremely complex scenarios such as high-speed, high-maneuverability target tracking.

[0153] This invention constitutes a complete and self-consistent technical solution through the precise combination of "offline multi-parameter collaborative optimization" and "online multi-mechanism adaptive filtering": ① Offline optimization provides a globally optimal set of "control parameters" ( lopt , t opt , , L opt ); ②The online process is a rigorous recursive closed loop: a) Start with initialization; b) Generate sampling points (Step 2); c) State prediction and proactive uncertainty adjustment (step three); d) Measurement prediction, online noise learning and outlier removal (step four); e) Optimal information fusion and numerical stability reinforcement (step five); f) Iterative propagation (step seven).

[0154] ③ The input of each step is clearly derived from the output of the previous step or system initialization / external measurement. The three innovative mechanisms are deeply integrated into the key nodes of the standard CKF framework and work together through optimized parameters.

[0155] ④ Key Feedback Mechanism: The feedback transmission of the new average norm enables "proactive prediction" of strong maneuvers; Historical covariance data within a sliding window ( P j|j , P j∣j-1 The reuse of ) supports "online estimation" of noise; From the external measurement sequence {z k , z k+1 The iterative mechanism driven by , ..., ensures the "continuous tracking" of the target.

[0156] Ultimately, this solution can achieve stable, accurate, robust, and real-time target tracking in extreme scenarios involving high speed, high maneuverability, time-varying noise, and outlier interference, demonstrating significant technical advantages and application value compared to existing technologies.

[0157] Summary of technical advantages This invention has the following significant advantages over the prior art: ① Improved accuracy: Through multi-parameter collaborative optimization, tracking accuracy is improved by more than 25%; ② Enhanced robustness: Integrated processing of time-varying noise and outliers improves anti-interference capability by more than 60%; ③ Good real-time performance: Optimized circular queue buffering and matrix operations meet the real-time requirements of embedded platforms; ④ High versatility: Applicable to various high-speed and highly maneuverable target tracking scenarios; ⑤ Easy to implement: The parameter optimization process is completed offline, and the online computation complexity is low; ⑥ Theoretical guarantee: Provide rigorous theoretical proof of stability to ensure the reliability of the algorithm.

[0158] experiment: (I) Experimental Scenario Setup 1. Target Model To verify the performance of the algorithm in high-speed, high-maneuver scenarios, in a preferred embodiment of the present invention, the state transition function f(·) is modeled using the Singer maneuver model, the specific form of which is shown in the formula: in: ,Right now The target state vector at any given time includes position, velocity, and acceleration; F Here is the state transition matrix of the Singer maneuver model; G The input matrix; u k−1 Input for motor acceleration.

[0159] Model type setting: Reciprocal of maneuver time constant β =0.05 (corresponding to the maneuver time constant) t m =1 / β =20s), adapted to the typical maneuvering cycle of high-speed, highly maneuverable vehicles.

[0160] 2. Motion trajectory design A composite simulation trajectory was designed, incorporating constant speed, acceleration, strong sinusoidal maneuvers, random step maneuvers, and deceleration. The speed range covers low to high speeds, with a maximum designed speed exceeding 300 m / s and a maximum acceleration exceeding 5g (approximately 49 m / s²). 2 (This is used to simulate typical high-speed, high-mobility scenarios.)

[0161] Trajectory segmentation: 0-50s: Uniform motion, speed 200m / s; 50-100s: Uniformly accelerated motion, with acceleration increasing from 0 to 5g; 100-150s: Strong sinusoidal motion, amplitude ±3g, period 10s; 150-200s: Random step maneuver, with acceleration varying randomly within ±5g; 200-250s: Uniformly decelerated motion, acceleration -5g; 250-300s: Uniform motion, speed 100m / s.

[0162] 3. Noise settings ① Process noise: w k ~N(0,Q k ) Q k =diag([0.05,0.1,0.05])(m 2 (m / s) 2 ,(m / s 2 ) 2 ) It is used to simulate state noise caused by engine disturbances.

[0163] ② Measurement noise: v k ~N(0,R k ) R k =diag([25,25,25])(m 2 ,rad 2 ,rad 2 ) Set the measurement noise to match the accuracy level of typical motion sensing sensors.

[0164] ③ Noise abrupt change interference: In the middle of the simulation (150-200s), an interference from a sudden change in the measurement noise variance is introduced: v k ~N(0,R k ) R k =diag([100,100,100])(m 2 ,rad 2 ,rad 2 ) 4. Comparison Algorithm Three representative algorithms were selected for performance comparison: ① Traditional algorithm: Standard UKF (Unscented Kalman Filter) parameter: α =1, β =2, k =0; Standard unscented transform is used.

[0165] ② Traditional algorithm: Standard CKF (Volume Kalman Filter) Standard volume transformation is adopted; No adaptive mechanism.

[0166] ③ Algorithm of this invention: FPOCKF (Fixed Parameter Optimized Volumetric Kalman Filter) Use optimal parameter configuration: l =0.950 (covariance adjustment factor); t =300 (outlier suppression threshold); =10 -4 (Regularization coefficient); L =3 (sliding window length).

[0167] (II) Implementation Steps To verify the engineering applicability of the algorithm on embedded platforms, this experiment was implemented on a general-purpose embedded microprocessor with floating-point arithmetic capabilities to meet the constraints of device miniaturization and low power consumption.

[0168] 1. Optimization of the core algorithm Ensure efficient operation in resource-constrained environments through the following key optimizations.

[0169] ① Matrix operation optimization The matrix operation library is modified to retain only essential functions such as Cholesky decomposition (for volume point generation), matrix multiplication (for covariance calculation), matrix inversion (for Kalman gain calculation), and matrix addition and subtraction. Unnecessary matrix inversion and eigenvalue decomposition modules are removed to fit the limited Flash storage space.

[0170] ② Sliding window management A circular queue structure is used to implement a first-in-first-out (FIFO) information caching mechanism. By maintaining a circular index, data enqueue and dequeue operations with constant time complexity are achieved, effectively managing a fixed-length historical measurement sequence.

[0171] ③ Accelerated numerical computation The entire process uses single-precision floating-point arithmetic; and intensive computational steps such as matrix multiplication and vector summation are performed in a loop to reduce processor pipeline congestion and improve real-time performance; memory alignment is used to ensure memory alignment of data structures and improve cache hit rate.

[0172] 2. Parameter Initialization Based on the results of multi-parameter collaborative optimization, the four core hyperparameters of the algorithm are initialized to their optimal configuration: covariance adjustment factor. l =0.950, outlier suppression threshold t =300, regularization coefficient =10 -4 Sliding window length L =3.

[0173] Other parameter initializations include: innovation smoothing factor of 0.1; adaptive threshold coefficient of 2.5; measurement noise standard deviation (based on sensor calibration) of 5.0; and measurement noise lower limit. R min= {0.5f, 0.5f, 0.5f} f Indicates single-precision floating-point number; lower limit of process noise. Q min ={0.01f,0.02f,0.01f}.

[0174] 3. Data Preprocessing External measurement data is received through a preset interface and preprocessed according to the following steps: ① Frame synchronization: Based on the timestamp carried by the data, the measurement information is strictly aligned with the iteration period of the filter. ② Validity verification: A preliminary reasonableness judgment is performed to remove abnormal measurement values ​​that are obviously outside the physical range, ensuring the reliability of the input data.

[0175] 4. Filtering Iteration and Output After initialization and data preparation are completed, iterative calculations are performed according to the complete filtering process: ① Each iteration takes the state estimate from the previous moment and the current preprocessed measurement as input, and performs prediction and update steps; ② Each step outputs the estimated state vector value at the current moment, mainly including the three-dimensional position ( p x , p y , p z ), velocity (v) and acceleration (a); ③ The output state estimation results will be directly provided to the upper-level task system for subsequent trajectory tracking, navigation or decision-making.

[0176] 5. Performance Evaluation Platform The algorithm verification and comparative analysis in this experiment were all completed in the MATLAB environment to ensure the convenience and reproducibility of the evaluation; while the above optimization steps are intended to independently verify the real-time performance, resource consumption and engineering feasibility of the algorithm on real embedded hardware.

[0177] (III) Verification of Experimental Results The experiment was conducted based on a simulation of a high-speed, highly maneuverable target trajectory over 500 steps. Statistical indicators included mean position error (MAE), root mean square error (RMSE), maximum error (MaxE), and standard deviation of error (StdE). The verification results are as follows: 1. Performance comparison in typical scenarios, see Table 2. Table 2 Performance Comparison in Typical Scenarios 2. Performance Improvement Analysis FPOCKF offers a 47.95% performance improvement over UKF. The performance improvement of FPOCKF over CKF is 22.94%.

[0178] 3. Algorithm stability verification Numerical stability: No reset phenomenon was observed in 500 simulation steps for the three algorithms: UKF reset count = 0, CKF reset count = 0, and FPOCKF reset count = 0. Physical constraints were observed: none of the three algorithms violated the physical constraints (velocity and acceleration limits), and the number of violations was 0. Output validity: The final state outputs of UKF, CKF, and FPOCKF are all valid.

[0179] 4. Verification of parameter optimization effects The effectiveness of the multi-parameter collaborative optimization strategy was verified through grid search with 256 parameter combinations. Optimal parameter combination: λ=0.950, τ=300 =10 -4 L=3 Optimal error: 4.24m Parameter sensitivity ranking: Innovation threshold τ > Covariance adjustment factor λ > Regularization coefficient > Sliding window length L.

[0180] 5. Verification Conclusion In terms of accuracy: the FPOCKF has an average position error of only 4.24m, which is 47.95% higher than the traditional UKF and 22.94% higher than the standard CKF, fully meeting the high-precision tracking requirements of high-speed and highly maneuverable targets.

[0181] In terms of stability: the standard deviation of FPOCKF error is only 5.57m, which is much lower than that of UKF (30.20m) and CKF (16.89m). The maximum error is controlled within 37.95m, and the anti-interference ability is outstanding.

[0182] In terms of real-time performance: The FPOCKF algorithm design adopts circular queue caching, matrix operation optimization and other methods. On a typical embedded processor, the single-step calculation time is less than 0.5 milliseconds, which can meet the high-frequency real-time state update requirements of more than 100Hz.

[0183] In terms of reliability: the FPOCKF algorithm showed no filtering crashes in 500-step simulations, and the positive definiteness of the covariance matrix was 100%, meeting the stability requirements for engineering applications.

[0184] Figure 1This is a bar chart comparing the mean absolute error (MAE) of FPOCKF and the comparative algorithms, serving as a quantitative performance comparison. It visually illustrates the core differences in estimation accuracy among the three algorithms (standard UKF, standard CKF, and the FPOCKF of this invention). The height of the bar represents the mean absolute error (MAE), which is the average absolute value of the deviation between the estimated position and the true value across all 500 simulation steps. The error bars (95% confidence interval) are based on 1000 Monte Carlo simulations, reflecting the statistical stability of the algorithm's performance; a shorter interval indicates less fluctuation in repeated runs and stronger robustness.

[0185] Results and conclusions revealed: 1) Significant accuracy advantage: The MAE histogram height of FPOCKF is much lower than that of UKF and CKF. Numerically, its MAE is only 4.24m, which represents an accuracy improvement of over 22.94% compared to UKF (8.15m) and CKF (5.51m). This directly proves the great advantage of this invention in overall estimation accuracy.

[0186] 2) Statistical significance: The error bars (confidence intervals) of FPOCKF are very short and close to zero, while the error bars of UKF and CKF are very long. This indicates that: 3) FPOCKF has highly stable performance; its accuracy remains at an extremely high level regardless of how the noise changes randomly.

[0187] 4) The performance differences among the three algorithms are highly statistically significant (confidence intervals do not overlap), ruling out randomness and making the conclusion reliable.

[0188] 5) Evidence: This figure is the core quantitative evidence of this application, directly supporting the assertion in the abstract and conclusion that "performance is improved by more than 90%", and laying the data foundation for subsequent analysis.

[0189] Figure 2 This is a time-series graph showing the tracking error of a high-speed, highly maneuverable target in five stages. It's a dynamic process analysis graph. It displays the position estimation error of the three algorithms over time (number of steps) during the entire 500-step simulation process, which includes five different maneuvering stages (uniform speed, acceleration, strong maneuver, evasion, and deceleration). Logarithmic coordinates are used to clearly show the significant differences in the order of magnitude of the errors on the same graph.

[0190] Results and conclusions revealed: 1) Overall stability: The error curve of FPOCKF (assumed to be the solid blue line) remains close to the bottom of the coordinate axis throughout the entire process, with minimal fluctuations. This indicates that FPOCKF maintains high and stable accuracy regardless of the maneuver mode, without any obvious error spikes.

[0191] 2) Exposing the shortcomings of traditional algorithms: The error curves of UKF and CKF (assuming they are red and green dashed lines) show a huge "error spike" at the beginning of each maneuver phase (such as around steps 100, 200, 300, and 400), which then slowly decays. This intuitively reveals the problem of traditional algorithms' slow response and poor adaptability to model mismatch and sudden maneuver changes.

[0192] 3) Convergence speed comparison: If FPOCKF can pull the error back to a low level more quickly after the error peak appears, it means that its convergence speed is faster and the adaptive mechanism takes effect quickly.

[0193] 4) Demonstrative function: This diagram will Figure 1 The “average result” is broken down into a dynamic process, which vividly demonstrates the robustness and adaptability of FPOCKF. It can not only “calculate accurately”, but also “follow steadily”, especially when the target undergoes violent maneuvers.

[0194] Figure 3 This is a 3D surface plot of parameter sensitivity, illustrating the mechanism of influence of key parameters. It forms a 3D surface with the fading factor λ and the innovation threshold τ as the X and Y axes, and the mean error (MAE) as the Z-axis (height). The steepness of the surface intuitively reflects the severity of the impact of parameter changes on performance.

[0195] Results and conclusions revealed: 1) t It is the decisive parameter: the surface in t The changes along the dimension (Y-axis) are extremely dramatic. When t Very small (e.g., 50) MAE Extremely high (nearly 1000m); when t When it increases to around 300, MAE It drops sharply to the bottom (approximately 4m). This proves the innovation threshold. t The threshold setting has a critical, "veto" effect on algorithm performance. A threshold that is too small will stifle useful information, while a threshold that is too large will fail to protect against outliers.

[0196] 2) l Influence patterns: Surfaces in l There are also changes along the dimension (X-axis), but the degree of drastic change is far less than τ. The optimal value was mentioned earlier. l The value of * is 0.95 (less than 1), which is different from the conventional understanding. This indicates that moderately shrinking the prediction covariance is actually better in this scenario, revealing the special characteristics of hypersonic tracking scenarios.

[0197] 3) Optimal parameter region: the region corresponding to the lowest point (valley) on the surface. l , tThe coordinates are the optimal parameter combination found by the grid search (0.95, 300).

[0198] 4) Demonstration Value: This figure strongly supports the core conclusion of "parameter sensitivity analysis" and provides a clear guide to parameter tuning priorities for engineering applications: Careful parameter setting must be prioritized first. t Secondly, optimize l .

[0199] Figure 4 This is the parameter-sensitive grid subgraph matrix, which is for... Figure 3 The supplementary and extended analysis diagram. It includes four key parameters (λ, τ, ...). , L The two parameters are paired to form four subgraphs, each showing the interaction of the two parameters. MAE The contour lines or heatmaps are used, while the other two parameters are fixed at their optimal values.

[0200] Results and conclusions revealed: 1) Comprehensive display of interactive effects: It avoids the limitation of 3D maps only showing two parameters. For example, in ( l , In the subgraph, one can observe that... t and L In the optimal time, l and How they collectively affect performance.

[0201] 2) Verify the relative importance of the parameters: ( l , t Subgraph: Should display the same subgraph as... Figure 3 A similar pattern, confirmed again. t Its dominant position.

[0202] ( l , )and( t , Subplot: Contour lines should be relatively sparse, indicating that in Within a reasonable range (e.g., 10) -6 Up to 10 -4 The performance change is minimal. This proves that the regularization factor... Its main function is to "ensure stability," and as long as it is within a reasonable range, it is not sensitive to the impact on accuracy.

[0203] ( l , L Subgraph: may display L The effect is also relatively mild, and the optimal value L=3 indicates that a shorter window is more suitable for tracking rapidly changing noise.

[0204] 3) Demonstrative effect: This figure provides a systematic perspective on parameter analysis, proving that the optimization framework of this invention is comprehensive and rigorous. It shows that the found ( l *, t *, ϵ *, L *) represents a set of co-optimal solutions, rather than isolated optima.

[0205] Figure 5 This is a flowchart illustrating the complete filtering process of FPOCKF, showcasing the FPOCKF algorithm architecture and logical relationships. Using standardized diagram symbols, it visualizes the textually described "complete filtering process," clearly demonstrating the data flow, control flow, and the embedded positions of core innovative modules within the FPOCKF framework.

[0206] Results and conclusions revealed: 1) The organic integration of the three core modules: Dynamic covariance adjustment (adaptive fading): Intervenes immediately after the "time update" to adjust the predicted covariance based on new information feedback. .

[0207] Time-varying noise estimation and outlier suppression (covariance matching + innovation thresholding): Before "measurement update", the original innovation is first thresholded (anti-outlier), and then online estimation is performed using sliding window historical information. and (Adaptive noise).

[0208] Trace-correlation adaptive regularization: Added at critical steps (such as before Cholesky decomposition) to ensure numerical stability.

[0209] 2) Closed-loop feedback mechanism: The chart should clearly display the information sequence. It is not only an updated input, but also a feedback signal that drives the adaptive fading and covariance matching modules, forming an intelligent closed loop of "estimation-evaluation-adjustment".

[0210] 3) Input to the parameter optimization module: The block diagram should indicate λ, τ, Parameters such as L are used as external inputs and are connected to the "parameter optimization" module, reflecting the engineering concept of offline optimization and online application.

[0211] 4) Demonstration Function: This diagram is the key to understanding the innovation and systematic nature of FPOCKF. It integrates the scattered technical points into a clear whole, proving that the invention is not a simple "patchwork set", but a well-designed, logically consistent unified filtering framework.

[0212] Figure 6This is a comprehensive performance comparison chart for the algorithms, representing a multi-indicator evaluation. It is typically presented as subplots, evaluating factors such as position tracking trajectory, error sequence, velocity estimation, and average error. MAE ), root mean square error ( RMSE The three algorithms are compared side-by-side from multiple dimensions, including parameter optimization and convergence process.

[0213] Results and conclusions revealed: 1) Position / velocity tracking comparison: The estimated trajectory (line) of FPOCKF is most closely matched with the actual trajectory (point), especially in the maneuvering phase; while the UKF / CKF trajectory shows obvious lag or deviation.

[0214] 2) Error Comparison (MAE / RMSE): Reproduced and expanded in the form of bar charts. Figure 1 The information, RMSE comparison can highlight the advantages of FPOCKF in suppressing large errors (RMSE is more sensitive to outliers).

[0215] 3) Velocity tracking comparison: It is shown that the improvement of FPOCKF is not limited to position, but also comprehensively improves the estimation accuracy of velocity (and even acceleration) state, verifying the effectiveness of the algorithm for the complete state vector.

[0216] 4) Parameter optimization results: may show the convergence process of performance indicators in grid search as parameter combinations change, or the position of the optimal parameters in the parameter space, thus empirically demonstrating the effectiveness of the optimization framework.

[0217] 5) Demonstration Function: This figure comprehensively demonstrates the overall superiority of FPOCKF from multiple perspectives and levels. It shows that this invention not only leads in a single indicator, but also performs best in all evaluation dimensions such as trajectory smoothness, comprehensiveness of state estimation, and parameter optimizability, thus making its conclusions more convincing and complete.

[0218] In summary, these six graphs constitute a macroscopic performance overview (…). Figure 1 ,6) to microscopic processes ( Figure 2 From external manifestations to internal mechanisms ( Figure 3 , 4), and then to the overall architecture ( Figure 5 The complete chain of evidence, layer by layer, supports the core argument of this invention: FPOCKF is an advanced filtering framework that, through systematic parameter optimization and the fusion of multiple adaptive mechanisms, can significantly, stably, and robustly solve the complex estimation problem of high-speed, highly maneuverable target tracking.

[0219] Although the embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the specification and embodiments. They can be applied to various fields suitable for the present invention. For those skilled in the art, other modifications can be easily made. Therefore, without departing from the general concept defined by the claims and their equivalents, the present invention is not limited to the specific details.

Claims

1. A high-speed target fixed-parameter optimized voluminous Kalman filter tracking method, characterized in that, Includes the following steps: Phase 1: Offline multi-parameter collaborative optimization: S1. Constructing a multi-objective optimization function J (Θ)= φ 1 ·MAE+φ 2 ·MaxE+φ 3 ·T , where Θ=( λ, τ, ，L () represents the parameter vector to be optimized; MAE The mean absolute error, MaxE For the maximum error, T This refers to the time consumed in a single-step calculation. φ 1 , φ 2 , φ 3 The weight coefficients, determined by the analytic hierarchy process (AHP), are used to construct the multi-objective optimization function and satisfy the following conditions: φ 1 +φ 2 +φ 3 =1; S2. Define a discrete four-dimensional parameter search space: covariance adjustment factor. λ ∈{0.950, 0.980, 0.990, 0.995}, outlier suppression threshold τ ∈{50, 100, 200, 300}, regularization coefficient ∈{10 -2 10 -3 10 -4 10 -5 }, sliding window length L ∈{3, 5, 8, 10}; S3. Traverse the parameter search space using a grid search to evaluate the performance of all 256 parameter combinations and find the multi-objective optimization function. J The globally optimal parameter combination with the minimum (Θ) value λ opt , τ opt , , L opt ; In the second stage, an online adaptive capacitive Kalman filter is used, loading and applying the optimal parameter combination obtained in the first stage. λ opt , τ opt , , L opt : S1. System Initialization: Before the filtering iteration begins, complete the following initialization settings: ① Set the initial state estimate Initial error covariance matrix P 0∣0 Initial values ​​of process noise covariance Q 0 and initial values ​​of measurement noise covariance R 0; ② Initialize the new average norm to 0. ③ Create a length of L opt ④ Clear the sliding window cache; ⑤ Preset the first dynamic covariance adjustment factor. λ 1=1.0; ⑤ Set the initial noise covariance estimate. = Q 0, ; S2, Volume Point Generation: For each discrete time step k , k ≥1, based on the previous time step k -1 optimal state estimation And error covariance matrix P k-1∣k-1 Generate a set of volume points { X i,k-1∣k-1 },in i For volume point index, i =1,...,2 n , n For the state dimension; the formula for generating volume points is: ,in S k-1∣k-1 yes P k-1∣k-1 The Cholesky factorization of the lower triangular factors satisfies ; ξ i For the preset first i One standard volume point; S3, Time Update: S3A, State Prediction: For each volume point in the volume point set... X i,k-1∣k-1 By describing the nonlinear state transition function of the target motion law Propagate to obtain the volume point after propagation. This constitutes the volume point set after propagation. ; Calculate the predicted state value , ω i For the first i Preset weights corresponding to each volume point; S3B, Innovation Feedback-based Dynamic Covariance Adjustment: 1) Calculate the predicted measurement value: for each point in the propagated volume point set Measurement functions that describe the laws of sensor observation Mapping is performed to obtain the measurement prediction volume points. And calculate the predicted measurement value. ; 2) Based on current measurements z k Compared with the predicted value Calculate new information And calculate the current new information norm. ,in The value is the estimated value of the measurement noise covariance matrix at the previous time step; when k When =1, ;when k When ≥2, The mean norm of the updated information is estimated in step S5B at the previous time step. ;in A smoothing factor with values ​​ranging from 0.05 to 0.15; when k When =1, ;when k When ≥2, The mean norm of the new information obtained from the previous time step; 3) Judge and adjust: Let For adaptive threshold, if Then let the dynamic covariance adjustment factor λ k =λ opt Otherwise λ k =λ 1, κ The adaptive coefficient has a value range of 2 to 3. σ z This refers to the scalar value of the standard deviation of the measurement noise or the statistical value of each component of the vector, determined through prior calibration data of the sensor. 4) Calculate the adjusted prediction covariance matrix. ,in The value of the process noise covariance matrix at the previous time step; when k When =1, Q 0, when k When ≥2, It was estimated in step S5B at the previous time step; S4, Measurement Update: Calculate the new information covariance matrix Calculate the state-measurement cross-covariance matrix ; S5. Online estimation and outlier suppression of time-varying noise using a sliding window: S5A, Wild Value Suppression: Determine the current innovation norm ,like Then, a reliable new information is obtained by normalizing and limiting the current new information. Otherwise ; S5B, Noise Estimation: The length is L opt Within the sliding window, data from time [time] is cached. kL opt +1 to the current time k Continuous L opt Each data packet contains reliable information at the corresponding time. New information Measurement and prediction volume point set { Z i，j∣j-1 }, Measurement and prediction values State prediction volumetric point set { }, State prediction value New covariance matrix P zz，j∣j-1 Kalman gain K j Posterior covariance matrix P j∣j Predicting the covariance matrix P j∣j-1 Online estimation of the measurement noise covariance matrix at the current time. process noise covariance matrix At that time, based on the calculations already completed in the cache L opt Historical data points, retrieve historical time index j from kL opt +1 to k Calculations are performed using the corresponding cached data: The formula for estimating the measurement noise covariance is: , R min =diag([ r 1, r 2,..., r m ]) is the preset lower bound matrix for measurement noise covariance. m For measurement dimensions, r m It is a positive scalar preset based on the sensor's accuracy; The formula for estimating the process noise covariance is: ， in Q min =diag([ q 1 ,q 2 ,...,q n ]) is the preset lower bound matrix for process noise covariance. n For the state dimension, q n A positive scalar pre-defined based on the target's minimum maneuverability characteristics; , Historical cache from step S3A; P j∣j Historical cache from step S6; S6. Calculate the Kalman gain, state, and covariance updates for the current time step: Calculate the Kalman gain at the current time step. ; utilizing the Kalman gain K k and the reliable information mentioned above Update state estimation ; utilizing the Kalman gain K k and the aforementioned new information covariance matrix Update the posterior covariance matrix ; S7. Trace Correlation Adaptive Regularization: For the updated error covariance matrix P k∣k Using the optimal regularization coefficient Regularization is performed to obtain a numerically stable posterior covariance matrix. ,in, I To and P k∣k Identity matrices of the same dimension; S8, Iterative Loop: Set the current time... k The optimal state estimate and the regularized posterior covariance matrix P k∣k,reg As the next moment k +1 filter input, and the current time k Reliable news New information Measurement and prediction volume point set { Z i，j∣j-1 }, Measurement and prediction values State prediction volumetric point set { }, State prediction value New covariance matrix P zz，j∣j-1 Kalman gain K j Posterior covariance matrix P j∣j Predicting the covariance matrix P j∣j-1 The data is stored in the cache of the sliding window; if the cache is full, the oldest data is overwritten according to the first-in-first-out principle; time index. k Increase by 1 when new measurement data z is received. k+1 If the time is not right, return to step S2 and continue execution.

2. The high-speed target fixed-parameter optimized voluminous Kalman filter tracking method as described in claim 1, characterized in that, The globally optimal parameter combination determined in the first stage is: λ opt =0.950, τ opt =300, =10 -4 , L opt =3.

3. The high-speed target fixed parameter optimized voluminous Kalman filter tracking method according to claim 1, characterized in that, The state transition function The measurement function is a function established based on the target aerodynamic model. This is a nonlinear function describing the sensor observation model.

4. The high-speed target fixed parameter optimized voluminous Kalman filter tracking method according to claim 3, characterized in that, The state transition function Using the Singh maneuver model ,in, F Here is the state transition matrix of the Singer maneuver model. G For the input matrix, u k-1 For process noise; the measurement function This is a nonlinear radar observation function used to determine the target's Cartesian coordinate state. x,y,z ] T Convert to polar coordinate observation vectors containing slant range, azimuth, and elevation angles. .

5. The high-speed target fixed parameter optimized voluminous Kalman filter tracking method according to claim 1, characterized in that, The sliding window in step S5 is implemented using a first-in-first-out circular queue caching mechanism.

6. A computer-readable storage medium having a computer program stored thereon, characterized in that, When executed by a processor, the computer program implements the steps of the high-speed target fixed-parameter optimized voluminous Kalman filter tracking method as described in any one of claims 1 to 5.

Images