Outlier rejection error-only bearing tracking method based on total least squares criterion

Abstract

Provided is an outlier-resistant error-only bearing tracking method based on the total least squares criterion, comprising the following steps: initialization; entering filter iteration; complete time update; partial measurement update; wild value detection and weight calculation; extreme wild value judgment; rejected measurement update; normal measurement update; filter iteration loop; output and end. The present application combines M estimation with the total least squares criterion, does not require linearization or regression-based reconstruction, endows the SCKF framework with robustness, while retaining its derivative-free structure and the numerical stability advantage of square root propagation; adopts a non-iterative three-section equivalent weight design, avoids the need for parameter adjustment depending on specific scenarios, and effectively improves the universality and engineering usability of the algorithm; by introducing an explicit rejection region, the filter instability problem caused by extreme wild values can be effectively suppressed, ensuring that the algorithm can still achieve stable and reliable measurement update in the presence of severe wild values.

Description

Technical Field

[0001] This invention relates to the field of target tracking and navigation positioning, specifically to a pure orientation tracking method based on the overall least squares criterion that is resistant to anomalies. Background Technology

[0002] Nonlinear filtering has long been an important topic in aerospace guidance, navigation, and target tracking. Several methods have been proposed, including the Extended Kalman Filter (EKF), the Deterministic Sampling Filter (DSF), and the Particle Filter (PF). EKF uses a nonlinear system model based on the Jacobian matrix for linearization, but under strongly nonlinear conditions, it may produce inaccurate estimates and lead to numerical instability. DSF methods, such as the Unscented Kalman Filter (UKF) and the Cubature Kalman Filter (CKF), approximate the posterior density by propagating a set of deterministically selected sigma points or cubic points, and are generally more accurate and stable than EKF. The Square-root Cubature Kalman Filter (SCKF) further enhances numerical robustness by propagating the covariance through the Cholesky factor and typically employs a factor update method based on QR decomposition, which helps maintain the positive definiteness of the covariance throughout the filtering process.

[0003] Despite these advances, standard DSFs are typically derived under the assumption of Gaussian noise. In practice, measurements are often contaminated by outliers caused by sensor malfunctions, multipath effects, or intermittent interference. This non-Gaussian contamination can generate unusually large innovations, leading to a severe degrade in estimation performance or even filter divergence. PF can handle non-Gaussian noise through Monte Carlo integration, but this is computationally expensive and may fail when contamination is severe and sporadic.

[0004] To mitigate the impact of outliers while maintaining computational feasibility, robust estimation offers an attractive alternative. Huber's M-estimation employs l2-norm characteristics for nominal measurements and transitions to l1-norm behavior for contaminated measurements, thus limiting the impact of large residuals. However, since M-estimation was originally developed for linear systems, extending it to nonlinear systems through EKF-type linearization can reduce accuracy under highly nonlinear conditions. To address this issue, some researchers have developed regression-based robust DSFs by combining M-estimation with UKF and differential filters, based on linear or nonlinear regression frameworks. Nonlinear regression variants like the Novel Robust Unscented Kalman Filter (NRUKF) can improve robustness but sacrifice the derivative-free advantage of DSFs and introduce an iterative process that increases computational burden.

[0005] Several alternative robust filtering paradigms have emerged in recent years. The Maximum Correntropy Criterion (MCC) utilizes a similarity measure induced by correlation entropy, naturally reducing the weight of large residuals, and can be combined with the SCKF to produce a robust, derivative-free filter. However, the performance of MCC-based filters depends on a kernel bandwidth parameter that is typically scene-dependent, as excessively large bandwidth weakens outlier suppression capabilities, while excessively small bandwidth may over-attenuate information-rich measurements. Heavy-tailed probabilistic models offer another direction. Filters based on the Student's t-distribution adaptively increase measurement uncertainty under large residuals, while the Gaussian-Student's t-mixture extension further broadens the flexibility of noise modeling. However, these methods typically require iterative variational Bayesian (VB) inference, and their estimation accuracy depends on the degrees of freedom hyperparameters. Variational Bayesian Robust CKF with outlier detection can jointly estimate state and unknown noise statistics, improving adaptability under model mismatch and contamination. However, the VB scheme significantly increases computational overhead and parameter tuning due to the introduction of internal iterations, especially when using an adaptive stopping strategy, where the average cost may rise further under severe contamination.

[0006] Pure azimuth tracking uses azimuth measurements to estimate the target's position and velocity, and is a typical nonlinear filtering problem. Pure azimuth tracking is particularly sensitive to measurement outliers because a few contaminated azimuth angles can dominate the update process and cause divergence. Summary of the Invention

[0007] To address the pure azimuth tracking problem, this invention proposes a pure azimuth tracking method based on the overall least squares criterion, which is robust to anomaly errors and includes the following steps:

[0008] Step 1: Initialization;

[0009] Initialize all parameters, including the initial state estimation vector. Square root factor matrix of the initial covariance matrix Process noise covariance matrix Measurement noise covariance matrix Type I significance level Second type significance level , actual measurement vector sequence;

[0010] Step 2: Enter the filtering iteration;

[0011] Enter the filtering iteration process, for each time step Perform steps three through nine below, where n is the number of iterations, i.e., the total length of the sequence;

[0012] Step 3: Full time update;

[0013] Volume points are generated and propagated, calculated based on standard SCKF time updates. Prior state estimation vector at time step and prior estimation of covariance matrix ;

[0014] Step 4: Partial Measurement Update;

[0015] Based on the estimation results, the volume points are regenerated and propagated, and the calculations are updated according to the standard SCKF measurements. Time-prior measurement estimated vector The square root factor matrix of the new covariance matrix State-observation cross-covariance matrix ;

[0016] Step 5: Outlier detection and weight calculation;

[0017] Within the standard SCKF framework, the noise covariance matrix will be measured. Replace with one Weight matrix at time step ;in , for Time of the first Weights of each measurement channel; , for Time of the first Measurement noise variance for each measurement channel; Used to generate diagonal lines ( A diagonal matrix; To measure the number of channels;

[0018] For each measurement channel, a standard Euclidean distance between the actual and estimated measurements is introduced to identify outliers. Time of the first Judgment variables for each measurement channel as follows:

[0019] (1)

[0020] in, for Real measurement vector at time The One component; for Prior measurement estimate vector at time step The One component; for The diagonal elements, This is the square root factor matrix of the new information covariance matrix;

[0021] When both process noise and measurement noise are zero-mean Gaussian distributed, the judgment variable It approximately follows a standard Gaussian distribution; let Indicates significance level The two-tailed quantiles of the corresponding standard normal distribution then represent the probability of false positives for outliers. Represented as:

[0022] (2)

[0023] like If the value is found to be contaminated by an outlier, then the corresponding measured value is determined to be affected; based on the significance level... Determine the threshold for outlier detection;

[0024] When wild value is After the demotion, the new Time of the first Judgment variables for each measurement channel It should not exceed the threshold ,Right now:

[0025] (3)

[0026] in, for The The diagonal element, i.e. the th element The theoretical equivalent standard deviation of information from each measurement channel. for The square root factor matrix of the time-equivalent innovation covariance matrix;

[0027] By Replace with We can deduce that:

[0028] (4)

[0029] The equivalent weight of RSCKF is calculated according to equation (4); excessive weights are not considered. This can lead to RSCKF performance distortion; therefore, when small outliers occur, the equality sign should be used in the calculation. At that time, Set as , for The corresponding two-sided quantiles of the standard normal distribution The significance level is the first type.

[0030] When large-amplitude outliers appear It will deviate significantly from the true measurement, and the filter is prone to divergence; therefore, when At that time, refuse to perform measurement update, Set to 0, for The corresponding two-sided quantiles of the standard normal distribution The significance level is the second type.

[0031] Three-segment equivalent weight function The calculation is expressed as:

[0032] (5)

[0033] Step 6: Determining extreme outliers;

[0034] Determine all measurement channels If all values ​​are 0, proceed to step seven; otherwise, proceed to step eight.

[0035] Step 7: Reject measurement updates;

[0036] Will Posterior state estimation vector at time step Set as The square root factor matrix of the posterior estimate of the covariance matrix Set as Jump to step nine;

[0037] Step 8: Normal measurement update;

[0038] The following are some formula substitutions in the standard SCKF:

[0039] (6)

[0040] in, The equivalent square root factor matrix satisfies ; for Time-centered measurement volume point matrix; This represents the operation of obtaining a lower triangular matrix through QR decomposition; T represents matrix transpose.

[0041] (7)

[0042] in, for The equivalent Kalman gain matrix at each time step. for Time-state-observation cross-covariance matrix; "Indicates solving the corresponding trigonometric equation system;

[0043] (8)

[0044] in, for The posterior state estimate vector at time t; for The prior state estimate vector at time t. for Real measurement vector at any given time. for The prior measurement estimate vector at time;

[0045] (9)

[0046] in, for The square root factor matrix of the posterior estimate covariance matrix at time t; for Time-centralized state volume matrix;

[0047] Calculated sequentially , , and ;

[0048] Step 9: Filtering Iteration Loop;

[0049] Complete the current moment The filtering calculation is performed, and the process returns to the second step to process the next time step, until the measurement sequence of all time steps has been processed.

[0050] Step 10: Output and end;

[0051] Output all time points and The algorithm ends.

[0052] Compared with existing filter methods, the present invention has the following advantages:

[0053] (1) Combining the M-estimation with the Total Least Squares (TLS) Errors-In-Variables (EIV) model with consistency correction can give the SCKF framework robustness without linearization or regression-based reconstruction, while retaining its advantages of derivative-free structure and numerical stability of square root propagation.

[0054] (2) The non-iterative three-segment equivalent weight design avoids the need for parameter adjustment based on specific scenarios, effectively improving the universality and ease of use of the algorithm.

[0055] (3) By introducing an explicit rejection region, the filter instability caused by extreme outliers can be effectively suppressed, ensuring that the algorithm can still achieve stable and reliable measurement updates in the case of severe pulse pollution. Attached Figure Description

[0056] Figure 1 When no outliers appear, the root mean square error of the RSCKF proposed in this invention is compared with that of other algorithms. Comparison chart, in which Figure 1 (a) shows the root mean square error of the location. Compare, Figure 1 (b) shows the root mean square error of velocity. Compare;

[0057] Figure 2 When moderate outliers occur, the root mean square error of the RSCKF proposed in this invention is compared with that of other algorithms. Comparison chart, in which Figure 2 (a) shows the root mean square error of the location. Compare, Figure 2 (b) shows the root mean square error of velocity. Compare;

[0058] Figure 3 When severe outliers occur, the root mean square error of the RSCKF algorithm proposed in this invention is compared with that of other algorithms. Comparison chart, in which Figure 3 (a) shows the root mean square error of the location. Compare, Figure 3 (b) shows the root mean square error of velocity. Compare;

[0059] Figure 4The mean square error of the RSCKF algorithm proposed in this invention is compared with that of other algorithms under different outlier values. Comparison chart. Detailed Implementation

[0060] The present invention will now be described in detail with reference to the accompanying drawings and examples.

[0061] To achieve the above objectives, taking a pure bearing-only target tracking task (Yang HR, Chen Y, Hu ZT, et al. Distributed bearing-only target tracking algorithm based on variational Bayesian inference under random measurement anomalies[J]. High Technology Letters, 2025, 31(1): 86-94.) as an example, the measurement noise is modeled as a Gaussian mixture distribution (Laurent B, Marteau C, Maugis-Rabusseau C. Multidimensional two-component Gaussian mixtures detection[J]. Annales de l'Institut Henri Poincaré - Probabilités et Statistiques, 2018, 54(2): 842-865.), where the measurement noise includes a nominal Gaussian distribution and a contaminated distribution caused by outliers. When the measurement noise is contaminated by outliers, the squared error loss function of the standard SCKF becomes suboptimal. To address this issue, this invention introduces the M-estimation technique and combines it with the TLS criterion to ensure robustness against measurement outliers and linearization errors in pure orientation tracking problems. This invention provides a robust square-root cumbersome Kalman filter (RSCKF) method for pure orientation tracking based on the overall least squares criterion, which is characterized by the following algorithm steps:

[0062] Step 1: Initialization;

[0063] Initialize all parameters, including the initial state estimation vector. Square root factor matrix of the initial covariance matrix Process noise covariance matrix Measurement noise covariance matrix Type I significance level Second type significance level , actual measurement vector sequence.

[0064] Step 2: Enter the filtering iteration;

[0065] Enter the filtering iteration process, for each time step Perform steps three through nine below, where n is the number of iterations, i.e., the total length of the sequence.

[0066] Step 3: Full time update;

[0067] Volume points are generated and propagated, calculated based on standard SCKF time updates. Prior state estimation vector at time step and prior estimation of covariance matrix (Arasaratnam I, Haykin S. Cubature Kalman Filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269.). (The subscript is missing.) The vertical line is a standard notation in the field of filtering. The first line represents the target time to be estimated, indicated by the vertical line. The following is the cutoff time for the observation data used for estimation, which means: using... All observation data up to and including the time specified, for The prior results are obtained by estimating the state at time t.

[0068] Step 4: Partial Measurement Update;

[0069] Based on the estimation results, the volume points are regenerated and propagated, and the calculations are updated according to the standard SCKF measurements. Time-prior measurement estimated vector The square root factor matrix of the new covariance matrix State-observation cross-covariance matrix (Arasaratnam I, Haykin S. Cubature Kalman Filters[J]. IEEETransactions on Automatic Control, 2009, 54(6): 1254-1269.).

[0070] Step 5: Outlier detection and weight calculation;

[0071] This invention introduces the M-estimation technique, which, through derivation under TLS, can be achieved simply by estimating the measurement noise covariance matrix within the standard SCKF framework. Replace with one Weight matrix at time step A robust solution can be obtained without changing the overall filtering structure. , for Time of the first Weights of each measurement channel; , for Time of the first Measurement noise variance for each measurement channel; Used to generate diagonal lines ( A diagonal matrix; To measure the number of channels, the subscript... Used to identify measurements.

[0072] A well-designed It can effectively reduce the weight of outliers, thereby suppressing their influence. The design comprises two core components: an outlier detection threshold and a weighting strategy for identified outliers.

[0073] The original Huber weight design (Tseng CH, Lin SF, Jwo D J. Robust Huber-Based Cubature Kalman Filter for GPS Navigation Processing[J]. The Journal of Navigation, 2016, 69(5): 1123-1142.) has the following shortcomings: First, the judgment threshold set based on the residual is usually selected based on experience due to the complex sources of error, and lacks theoretical basis; second, the weights have a linear decay characteristic, and large value outliers may still affect the stability of the filter; third, the calculation of the weights requires iteration, resulting in low computational efficiency.

[0074] To address the aforementioned problems, this invention introduces a standard Euclidean distance between the actual and estimated measured values ​​for each measurement channel to achieve outlier identification, defining... Time of the first Judgment variables for each measurement channel as follows:

[0075] (1)

[0076] in, for Real measurement vector at time The Each component, namely Time of the first The actual measurement values ​​of each measurement channel; for Prior measurement estimate vector at time step The Each component, namely Time of the first Prior measurement estimates for each measurement channel; for The The diagonal element, i.e. the th element The theoretical standard deviation of information from each measurement channel. The square root factor matrix of the new information covariance matrix, with subscripts... The measurement itself is the identifier.

[0077] When both process noise and measurement noise are zero-mean Gaussian distributed, the variables It approximately follows a standard Gaussian distribution. Let... Indicates significance level The two-tailed quantiles of the corresponding standard normal distribution then represent the probability of false positives for outliers. It can be represented as:

[0078] (2)

[0079] like If so, it can be determined that the corresponding measured value is contaminated by outliers. In other words, when outliers exist, It typically deviates from a standard Gaussian distribution. Therefore, it can be based on the significance level. Determine the threshold for outlier detection.

[0080] The standard Euclidean distance quantifies the discrepancy between the actual and estimated measurements at each time step. This is especially relevant when the noise follows a Gaussian distribution. It approximately follows a Gaussian distribution, and Usually less than the judgment threshold; conversely, when an outlier occurs, The value will increase abnormally, thus enabling outlier identification. Although metrics such as Mahalanobis distance can also detect outliers, they can only characterize the global difference between the predicted measurement value and the actual measurement vector at a certain moment, and cannot distinguish the degree of contamination of different measurement channels. If a uniform correction weight is applied to all channels, the estimation efficiency of uncontaminated channels will inevitably be reduced, resulting in a waste of effective measurement information.

[0081] To ensure the effectiveness of robust methods, when outliers are... After the demotion, the new Time of the first Judgment variables for each measurement channel It should not exceed the threshold ,Right now:

[0082] (3)

[0083] in, for The The diagonal element, i.e. the th element The theoretical equivalent standard deviation of information from each measurement channel. for Square root factor matrix of the equivalent innovation covariance matrix at time, subscript The measurement itself is the identifier.

[0084] By Replace with It can be deduced that:

[0085] (4)

[0086] Therefore, the equivalent weight of RSCKF in this invention can be calculated according to equation (4) without iteration. Furthermore, excessively large... This can lead to RSCKF performance distortion; therefore, when small outliers occur, this invention uses the equality calculation. That is, when... ( for The corresponding two-sided quantiles of the standard normal distribution When the significance level is the first type, Set as .

[0087] When large-amplitude outliers appear It will deviate significantly from the true measurement, and the filter is prone to divergence. Therefore, when ( for The corresponding two-sided quantiles of the standard normal distribution When the significance level is 2, refuse to perform the measurement update. Set to 0;

[0088] Final three-segment equivalent weight function The calculation can be expressed as:

[0089] (5)

[0090] Compared to Huber's original weight design, the proposed The outlier detection threshold is based on The confidence level is naturally determined, thus avoiding inappropriate empirical values. In addition, Equation (5) does not require iterative calculation, so its computational efficiency is better than that of the original Huber weights. Moreover, rejecting large outliers can directly reduce computational overhead and protect the state and covariance from adverse effects in subsequent processes.

[0091] Step 6: Determining extreme outliers;

[0092] Determine all measurement channels If all values ​​are 0, proceed to step seven; otherwise, proceed to step eight.

[0093] Step 7: Reject measurement updates;

[0094] Directly Posterior state estimation vector at time step Set as The square root factor matrix of the posterior estimate of the covariance matrix Set as Jump to step nine. (The subscript is missing from the original text.) The vertical line is a standard notation in the field of filtering. The preceding line represents the time to be estimated, indicated by the vertical line. The following is the cutoff time for the observation data used for estimation, which means: using... All observation data up to and including the time specified, for The posterior result obtained after the state at time step is filtered and updated.

[0095] Step 8: Normal measurement update;

[0096] The following are some formula substitutions in the standard SCKF:

[0097] (6)

[0098] in, The equivalent square root factor matrix satisfies ; for The time-centered measurement volume point matrix remains consistent with the original framework. This represents the operation of obtaining a lower triangular matrix through QR decomposition; T represents matrix transpose.

[0099] (7)

[0100] in, for The equivalent Kalman gain matrix at each time step. for Time-state-observation cross-covariance matrix, subscript Separately identify status and measurement, maintaining consistency with the original framework; here, " "This means solving the corresponding trigonometric equation system (Arasaratnam I, Haykin S. Cubature Kalman Filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269.), rather than matrix inversion.

[0101] (8)

[0102] in, for The posterior state estimate vector at time t; for The prior state estimate vector at time t. for Real measurement vector at any given time. for The prior measurement estimate vector at time t.

[0103] (9)

[0104] in, for The square root factor matrix of the posterior estimate covariance matrix at time t; for The time-centered state volume matrix remains consistent with the original framework.

[0105] Calculated sequentially , , and .

[0106] Step 9: Filtering Iteration Loop;

[0107] Complete the current moment The filtering calculation is performed, and the process returns to the second step to process the next time step, until the measurement sequences of all time steps have been processed.

[0108] Step 10: Output and end;

[0109] Output all time points and The algorithm ends.

[0110] The above ten steps can achieve pure azimuth tracking with resistance to abnormal errors.

[0111] Compared with traditional robust algorithms, the proposed RSCKF scheme has two core advantages: First, it can achieve deep integration of M estimation and SCKF without linearizing the measurement equation, fully balancing estimation accuracy and robustness; second, the solution framework of RSCKF is completely consistent with the standard SCKF, preserving the original SCKF's derivative-free property and numerical stability. Specific Implementation

[0112] The performance of seven filtering algorithms was compared: CKF, SCKF, NRUKF, Maximum Correntropy Criterion Square-root Cubature Kalman Filter (MCCSCKF), Outlier Detection Cubature Kalman Filter (OD-CKF), Robust Student's t-Cucurture Kalman Filter (RSTCKF), and the RSCKF proposed in this invention. All algorithms used the same system model and initialization settings to ensure a fair comparison.

[0113] To describe the filter accuracy, the root mean square error of position and velocity (RMSE) , According to equations (10) and (11), the mean square error of the position for each Monte Carlo run is ( ) is defined according to equation (12). Where, the subscript Short for position, used to identify location; subscript Short for velocity, used to indicate speed. Monte Carlo run count. It is 200.

[0114] (10)

[0115] (11)

[0116] (12)

[0117] in, for The root mean square error of the position at time t. and They represent the first In the Monte Carlo simulation The estimated x-axis and y-axis positions of the target at any given time. and They represent the first In the Monte Carlo simulation The actual x-axis and y-axis positions of the target at any given time; for The root mean square error of the velocity at time t. and They represent the first In the Monte Carlo simulation Estimated x-axis and y-axis velocities of the target at any given time. and They represent the first In the Monte Carlo simulation The actual x-axis and y-axis velocities of the target at any given time; For the first The mean square error of the position in the Monte Carlo simulation. This represents the total simulation time. Used to identify Monte Carlo simulations Used to identify the target.

[0118] The effectiveness of the proposed RSCKF is verified using a pure azimuth tracking system model. The target dynamics model employs a coordinated turning model, with a single maneuvering observation station providing only azimuth measurements. Let the target state be... ,in and These represent the target's x-axis and y-axis position values, respectively. and These represent the target's velocity values ​​along the x and y axes, respectively. A constant turning rate is used. The CT model at ° / min is used as the state transition function, with a sampling period of ° / min. min, total number of time steps The process noise intensity is km 2 / min 3 A single observation station was deployed at its initial location. To ensure observability, the observatory is maneuverable: During this period, it was at a speed Movement at km / min; During this period, it switched to km / min. The standard deviation of the azimuth measurement noise is... rad.

[0119] The target's true initial state is The initial state estimate is set to have a bias. Initial error covariance matrix All filters use the same initialization. Three cases are considered to evaluate performance under different pollution levels. In case 1, the measurement noise follows a standard Gaussian distribution. In cases 2 and 3, approximately The measurements were contaminated by outliers, and their standard deviations were multiples of the nominal values, i.e. and The key parameters of the algorithm are shown in Table 1.

[0120]

[0121] These thresholds are based on a standard Gaussian distribution and derived according to Equation (2), requiring no empirical adjustment. The parameters of the comparison algorithm are selected according to its original references.

[0122] Each filtering algorithm in three cases and Each as Figures 1 to 3 As shown. In During the Monte Carlo run, the average As shown in Table 2, the corresponding variances are... Figure 4 .

[0123]

[0124] In case 1, when no outliers are found in the measurements, as the filter accumulates information from the mobile observation station, all algorithms... and All converge. In robust filters, the RSCKF proposed in this invention achieves... km 2 of The performance of RSCKF is the lowest among the seven algorithms and slightly better than CKF / SCKF. This result shows that RSCKF not only avoids performance loss relative to the benchmark filter but also achieves slightly higher accuracy, which can be attributed to the natural self-regularization effect of the three-segment weighting function on occasional large normal distribution tail events. The performance of NRUKF, MCCSCKF, and OD-CKF is very close to the benchmark algorithm, indicating that these robust mechanisms introduce minimal perturbations under Gaussian noise. RSTCKF's... Slightly higher, for km 2 Furthermore, the variance is significantly larger, reflecting that the VB iteration using the Student's t noise model may introduce additional estimation uncertainty even in the absence of outliers.

[0125] In scenario 2, approximately Wild value sampling from The standard deviation is Under moderate pollution, the performance of non-robust CKF and SCKF significantly degrades, confirming their vulnerability to outliers. Among robust algorithms, the RSCKF proposed in this invention... and All achieved optimal performance, Only increased compared to the case without wild values The M-CKF exhibits superior robustness. This superiority is attributed to two aspects: First, the TLS-based extremum function allows the M-estimation to be naturally incorporated into the SCKF framework without linearization or regression, fully preserving the derivative-free accuracy of the volume rule; second, the three-segment weighting function uses a standardized innovation statistic with a confidence level threshold to detect outliers, achieving principled deweighting of moderate outliers and direct rejection of extreme outliers. OD-CKF and RSTCKF achieve comparable performance through their Bernoulli mixture model and variational Bayesian method, respectively, but still lag behind RSCKF because they do not utilize the TLS structure of the pure orientation tracking measurement model. MCC-SCKF has limited effectiveness due to its inherent sensitivity to kernel bandwidth, with no single value optimal across different contamination levels. In contrast, the threshold in RSCKF is determined based on confidence level statistics, without relying on scene adjustments. NRUKF performs the worst among robust filters because its nonlinear regression process introduces additional linearization errors, which RSCKF inherently avoids by operating within the original SCKF solution framework.

[0126] In scenario 3, approximately Wild value sampling from The standard deviation is Under severe contamination, the advantages of the RSCKF proposed in this invention are most significant. CKF and SCKF fail completely because they treat all measurements equally regardless of contamination. NRUKF also degrades severely because the Huber weights decrease linearly but never reach zero, thus large outliers always have a residual effect on state estimation. In contrast, RSCKF maintains almost the same performance as the case without outliers, due to... Wild values ​​almost always occur These methods completely reject measurement updates, leaving the state and covariance entirely uncontaminated, as if the outlier measurement never occurred. OD-CKF and RSTCKF also maintain reasonable performance; the former benefits from clearer outlier separation in certain cases, while the latter adaptively inflates the noisy covariance through its heavy-tailed Student's t model. However, neither achieves the stability of RSCKF because their soft-weighting mechanism results in remaining partially affected by outliers rather than completely eliminating them. MCC-SCKF shows a significant improvement over case 2 because the Gaussian kernel weights decrease rapidly for large outliers, but this improvement is scenario-dependent, further confirming its sensitivity to mismatches between kernel bandwidth and outlier magnitudes.

[0127] Table 2 summarizes the average values ​​under three pollution scenarios. , Figure 4 For the corresponding variance. The RSCKF proposed in this invention achieves the lowest value in all cases. Furthermore, the performance degradation from case 1 to case 3 is minimal, only increasing by a small amount. In comparison, OD-CKF and RSTCKF increased respectively. and The difference is more than two orders of magnitude greater than that of SCKF. This result quantitatively confirms that the complete rejection mechanism of RSCKF provides stronger protection than the soft weighting strategies employed by other robust filters. Furthermore, even in case 1, RSCKF slightly outperforms SCKF, which can be attributed to the slight regularization effect of the three-segment weighting function on occasional big-tailed events of nominal Gaussian noise.

[0128] The relative computation times of each algorithm are listed in Table 3, with CKF as the normalization benchmark. Two scenarios were tested: Case 1 and Case 3, namely normal measurement and inclusion. of Mixed scenarios with extreme intensity values.

[0129]

[0130] As shown in Table 3, under nominal conditions (Case 1), SCKF and RSCKF are only slightly slower than CKF, while the iterative algorithm requires significantly more time. In fact, the computational complexity of standard SCKF is... This method originates from QR decomposition and matrix operations. The RSCKF proposed in this invention adds a calculation weight matrix. An additional step, involving scalar operations, with a complexity of O(n log n). Therefore, the overall complexity of RSCKF remains O(n). This maintains the efficiency of the standard SCKF. In contrast, iterative robust filters (such as NRUKF and RSTCKF) require multiple iterations per time step, resulting in a complexity of O(n log n). ,in The number of iterations is given. Under severe contamination (case 3), NRUKF and RSTCKF remain computationally expensive because their iteration process is performed regardless of the size of the outlier, while the computation time of RSCKF is reduced to... The cost is close to that of CKF. This reduction stems directly from the rejection mechanism, which is effective for satisfying... The measurement skips the entire Kalman update, saving those time steps of QR decomposition and gain calculation. The non-iterative property of RSCKF stems from the closed-form equivalent weights in equation (4), which depend only on the values ​​available in the estimation step. This ensures the determinism and boundedness of the calculation time for each step.

Claims

1. A pure azimuth tracking method based on the overall least squares criterion, characterized in that, Specifically, the following steps are included: Step 1: Initialization; Initialize all parameters, including the initial state estimation vector. Square root factor matrix of the initial covariance matrix Process noise covariance matrix Measurement noise covariance matrix Type I significance level Second type significance level , actual measurement vector sequence; Step 2: Enter the filtering iteration; Enter the filtering iteration process, for each time step Perform steps three through nine below, where n is the number of iterations, i.e., the total length of the sequence; Step 3: Full time update; Volume points are generated and propagated, calculated based on standard SCKF time updates. Prior state estimation vector at time step and prior estimation of covariance matrix ; Step 4: Partial Measurement Update; Based on the estimation results, the volume points are regenerated and propagated, and the calculations are updated according to the standard SCKF measurements. Time-prior measurement estimated vector The square root factor matrix of the new covariance matrix State-observation cross-covariance matrix ; Step 5: Outlier detection and weight calculation; Within the standard SCKF framework, the noise covariance matrix will be measured. Replace with one Weight matrix at time step ;in , for Time of the first Weights of each measurement channel; , for Time of the first Measurement noise variance for each measurement channel; Used to generate diagonal lines ( A diagonal matrix; To measure the number of channels; For each measurement channel, a standard Euclidean distance between the actual and estimated measurements is introduced to identify outliers. Time of the first Judgment variables for each measurement channel as follows: (1) in, for Real measurement vector at time The One component; for Prior measurement estimate vector at time 1 The One component; for The diagonal elements, This is the square root factor matrix of the new information covariance matrix; When both process noise and measurement noise are zero-mean Gaussian distributed, the judgment variable It approximately follows a standard Gaussian distribution; let Indicates significance level The corresponding two-sided quantiles of the standard normal distribution, then the probability of false positives for outliers. Represented as: (2) like If the value is found to be contaminated by an outlier, then the corresponding measured value is determined to be affected; based on the significance level... Determine the threshold for outlier detection; When wild value is After the demotion, the new Time of the first Judgment variables for each measurement channel It should not exceed the threshold ,Right now: (3) in, for The The diagonal element, i.e. the th element The theoretical equivalent standard deviation of information from each measurement channel. for The square root factor matrix of the time-equivalent innovation covariance matrix; By Replace with We can deduce that: (4) The equivalent weight of RSCKF is calculated according to equation (4); if it is too large... This can lead to RSCKF performance distortion; therefore, when small outliers occur, the equality sign should be used in the calculation. At that time, Set as , for The corresponding two-sided quantiles of the standard normal distribution The significance level is the first type. When large-amplitude outliers appear It will deviate significantly from the true measurement, and the filter is prone to divergence; therefore, when At that time, refuse to perform measurement update, Set to 0, for The corresponding two-sided quantiles of the standard normal distribution The significance level is the second type. Three-segment equivalent weight function The calculation is expressed as: (5) Step 6: Determining extreme outliers; Determine all measurement channels If all values ​​are 0, proceed to step seven; otherwise, proceed to step eight. Step 7: Reject measurement updates; Will Posterior state estimation vector at time step Set as The square root factor matrix of the posterior estimate of the covariance matrix Set as Jump to step nine; Step 8: Normal measurement update; The following are some formula replacements in the standard SCKF: (6) in, The equivalent square root factor matrix satisfies ; for Time-centered measurement volume point matrix; This represents the operation of obtaining a lower triangular matrix through QR decomposition; T represents matrix transpose. (7) in, for The equivalent Kalman gain matrix at each time step. for Time-state-observation cross-covariance matrix; "Indicates solving the corresponding trigonometric equation system; (8) in, for The posterior state estimate vector at time t; for The prior state estimate vector at time t. for Real measurement vector at any given time. for The prior measurement estimate vector at time t; (9) in, for The square root factor matrix of the posterior estimate covariance matrix at time t; for Time-centralized state volume matrix; Calculated sequentially , , and ; Step 9: Filtering Iteration Loop; Complete the current moment The filtering calculation is performed, and the process returns to the second step to process the next time step, until the measurement sequence of all time steps has been processed. Step 10: Output and end; Output all time points and The algorithm ends.

Images