Kalman filtering method for target tracking based on noise-induced score function

Abstract

The application discloses a noise-induced score function-based Kalman filtering method for target tracking, and belongs to the technical field of intelligent perception and state estimation. In view of the problem that sensors such as radars and millimeter wave radars are easily interfered by multipath reflection and other pulse interference in a complex environment, leading to abnormal measurement, the application constructs a class of noise-induced M score function, and rewrites the observation update step of Kalman filtering into a generalized M estimation form; meanwhile, an online adaptive update algorithm of a noise scale parameter is designed according to a measurement residual, and collaborative adaptive estimation of system state and noise parameters is realized. The application does not need prior noise distribution assumption, can automatically soft-restrain abnormal measurement under pulse interference, and significantly improves the robustness and precision of state estimation. Compared with traditional Kalman filtering and existing robust filtering methods, the application has higher estimation precision and stronger anti-interference ability in applications such as radar target tracking, automatic driving front vehicle tracking and unmanned aerial vehicle tracking.

Description

Technical Field

[0001] This invention belongs to the field of signal processing and robust state estimation technology, specifically relating to a Kalman filtering method based on a noise-induced score function for target tracking. Background Technology

[0002] Traditional Kalman filtering algorithms are proposed under the assumption that both system noise and observation noise follow a Gaussian distribution, and that the statistical characteristics of the noise are known. However, in practical target tracking engineering applications, observation noise often exhibits non-Gaussian, heavy-tailed, impulse interference, and time-varying statistical characteristics, such as in radar target tracking, autonomous vehicle tracking, and UAV target tracking scenarios, leading to severe degradation of the performance of traditional Kalman filtering.

[0003] To address the aforementioned issues, existing research has proposed incorporating the M-estimation concept into Kalman filtering, forming the M-Kalman filtering method, which suppresses anomalous observations through a nonlinear scoring function. However, existing M-Kalman filtering methods still suffer from the following shortcomings: (1) the form of the scoring function depends on empirical selection and lacks a unified construction principle; (2) the threshold parameter in the scoring function is usually fixed artificially, making it difficult to adapt to time-varying noise environments; and (3) parameter updates and state estimation processes are independent of each other, failing to form a collaborative adaptive mechanism. Therefore, it is necessary to propose a generalized M-Kalman filtering method with a unified theoretical foundation, adaptively adjustable parameters, and applicability to complex noise environments, thereby ensuring the stability and reliability of target tracking. Summary of the Invention

[0004] To address the aforementioned problems in existing technologies, this invention proposes a Kalman filtering method based on a noise-induced score function for target tracking. This method is rationally designed, overcomes the shortcomings of existing technologies, and achieves good results.

[0005] A noise-induced score function-based Kalman filtering method for target tracking includes the following steps: Step 1: Construct a discrete-time state-space model of the target motion; Step 2: Perform inertial extrapolation based on the posterior estimate of the previous time step to obtain the predicted state value and the prediction error covariance; Step 3: Obtain the measurement value at the current moment, and calculate the measurement residual and nominal innovation variance; Step 4: Based on the original M-estimated score function, construct the noise-induced M-score function, and based on the noise-induced M-score function, calculate the Kalman gain, posterior updated state estimate, and covariance; Step 5: Perform online adaptive updates to the noise scale parameters based on the measurement residuals; Step 6: Output the posterior state estimate at the current moment as the target relative motion state estimate result after robust filtering, and write the posterior state estimate, posterior estimation error covariance, and adaptive noise scale parameter at the current moment into the state buffer for recursion in the next measurement cycle.

[0006] Furthermore, in step 1, the state-space model includes state equations and observation equations, expressed as follows: ; : in, For the current moment The system state vector, For the present The measured value output by the time sensor. This is Gaussian process noise with zero mean. For non-Gaussian distributed observation noise containing impulse interference, and These are the state transition matrix and the observation matrix, respectively.

[0007] Furthermore, in step 3, the residual is measured. and nominal innovation variance The expression is: ; ; in, For the current moment State prediction value, For the current moment The prediction error covariance, The nominal noise covariance is used for measurement.

[0008] Furthermore, in step 4, a sign function is selected. As the original M-score function, auxiliary induced noise variables are introduced into the residual variables. Construct the noise-induced M-score function : ; in, To introduce the standard probability density function of scale family noise, For noise scale parameters; Based on noise-induced M-score function Based on the residuals, observation confidence weights are generated, and the observation update step in the Kalman filtering algorithm is rewritten into a generalized M-estimation form to achieve automatic soft suppression of multipath impulse interference. The Kalman gain, posterior updated state estimate, and covariance are calculated, expressed as follows: ; ; ; in, The Kalman gain matrix is... For posterior state estimation, The noise-induced score function is constructed, and its input is the measurement residual. and adaptive noise scaling parameters , To estimate the error covariance in the posterior timescale, It is an identity matrix.

[0009] Furthermore, the noise-induced M-score function is constructed as a family of scalable generalized functions whose probability distribution is used to adjust the shape of the score function, thereby achieving smooth and robust adjustment of the score function. Its probability distribution includes, but is not limited to, Gaussian distribution, Laplace distribution, generalized Gaussian distribution, and Student-t distribution.

[0010] Furthermore, in step 5, the noise scale parameter is determined based on the measurement residual. Perform online adaptive updates: ; in, To cut off the amount of innovation, This is a truncation function. The cutoff factor is , These are the weighting coefficients.

[0011] The beneficial technical effects of this invention are as follows: This invention proposes a Kalman filtering method for target tracking based on noise-induced score function modulation. It constructs an M-estimation score function through a noise-induced mechanism, theoretically unifying multiple robust score functions such as Welsch and Student-t. The noise parameter in the score function is used as an adaptively optimizeable variable, achieving collaborative adaptive estimation of state variables and noise parameters. By identifying anomalies in sensor measurement residuals in real time and dynamically adjusting the fusion weights of measurements, it achieves automatic soft suppression of multipath pulse errors, ensuring the stability and reliability of target tracking. Attached Figure Description

[0012] Figure 1 This is a flowchart of the Kalman filter method based on the noise-induced score function used for target tracking in this invention.

[0013] Figure 2 This is a graph showing the convergence curve of the root mean square error under the scenario of sudden outliers.

[0014] Figure 3 for Figure 2 Enlarged view of point A in the middle;

[0015] Figure 4 This is a graph showing the convergence curve of the root mean square error under a four-stage time-varying noise scenario.

[0016] Figure 5 Noise parameters for a four-stage time-varying noise scenario The adaptive update curve. Detailed Implementation

[0017] The specific embodiments of the present invention will be further described below with reference to specific examples: Kalman filtering methods based on noise-induced score functions for target tracking, such as Figure 1 As shown, it includes the following steps: Step 1: Construct a discrete-time state-space model to describe the motion of the target; The state-space model includes state equations and observation equations, expressed as follows: ; : in, For the current moment The system state vector must contain at least relative position and relative velocity. For the current moment The measured value output by the sensor. This is Gaussian process noise with zero mean. For non-Gaussian distributed observation noise containing impulse interference, and These are the state transition matrix and the observation matrix, respectively.

[0018] Step 2: Perform inertial extrapolation based on the posterior estimate from the previous time step to obtain the predicted state value. and prediction error covariance ; ; ; in, This is the state transition matrix from the previous time step. This is the posterior state estimate from the previous time step. Let $\mathbf{a}$ be the process noise covariance matrix of the previous time step. The posterior estimation error covariance of the previous time step; Step 3: Obtain the measurement value at the current moment, and calculate the measurement residual and nominal innovation variance; Measurement residuals and nominal innovation variance The expression is: ; ; in, For the current moment State prediction value, For the current moment The prediction error covariance, The nominal noise covariance is used for measurement.

[0019] Step 4: Based on the original M-estimated score function, construct the noise-induced M-score function, and based on the noise-induced M-score function, calculate the Kalman gain, posterior updated state estimate, and covariance; Selecting the sign function As the original M-score function, auxiliary induced noise variables are introduced. ,in For scaled family probability density functions, Construct a noise smoothing score function for the noise scale parameter: ; Normalizing the above scoring function using the zeros of the probability density function yields a class of noise-induced M-score functions: ; in, To introduce the standard probability density function of scale family noise, For noise scale parameters; The noise-induced M-score function is constructed as a family of scalable generalized functions. Its probability distribution is used to adjust the shape of the score function, thereby achieving smooth and robust adjustment of the score function. Its probability distribution includes, but is not limited to, Gaussian distribution, Laplace distribution, generalized Gaussian distribution, and Student-t distribution. When the observation noise exhibits a Gaussian superposition of impulses, the standard probability density function in the scoring function... A Gaussian distribution is selected, and the Gaussian noise-induced score function is: The peak value of this function is at Place, when hour It achieves complete soft suppression of large outliers; when the observed noise exhibits a Cauchy-type heavy-tailed distribution, it reflects the heavy-tailed characteristic of the matched setter, and the standard probability density function in the noise-induced score function... Choosing the Student-t distribution, the corresponding Student-t type score function is constructed as follows: , For degrees of freedom.

[0020] Based on noise-induced M-score function Based on the residuals, observation confidence weights are generated, and the observation update step in the Kalman filtering algorithm is rewritten into a generalized M-estimation form to achieve automatic soft suppression of multipath impulse interference. The Kalman gain, posterior updated state estimate, and covariance are calculated, expressed as follows: ; ; ; in, The Kalman gain matrix is... For posterior state estimation, The noise-induced score function is constructed, and its input is the measurement residual. and adaptive noise scaling parameters , To estimate the error covariance in the posterior timescale, It is an identity matrix.

[0021] Step 5: Apply noise scale parameters based on measurement residuals Perform online adaptive updates: ; in, To cut off the amount of innovation, This is a truncation function. The cutoff factor is , These are weighting coefficients; Step 6: Output the posterior state estimate at the current moment as the target relative motion state estimate result after robust filtering, and write the posterior state estimate, posterior estimation error covariance, and adaptive noise scale parameter at the current moment into the state buffer for recursion in the next measurement cycle.

[0022] Example: This embodiment is applied to autonomous driving vehicle tracking, and is used to process the vehicle-to-vehicle relative distance measurement sequence output by the vehicle-mounted forward millimeter-wave radar, which contains multipath reflection pulse interference, to estimate the relative distance and relative speed between the vehicle and the target vehicle in real time, for use by the vehicle's adaptive cruise control (ACC); it includes the following steps: Step 1: Construct a state-space model to describe the relative motion relationship between the vehicle and the vehicle in front; in, For the current moment The system state vector includes at least relative distance and relative velocity; The relative distance between the workshops is measured by millimeter-wave radar output. Step 2: The onboard autonomous driving domain controller (ADC) performs inertial inference based on the posterior estimate of the previous time step to obtain the state prediction value and prediction error covariance; Step 3: The millimeter-wave radar provides the ADC with the current relative distance measurement value between the workshop and the ADC. Calculate the measurement residuals and nominal innovation variance: Step 4: The robust tracking update module of the onboard ADC is based on the original M-estimated score function. We construct a new class of noise-induced M-score functions and calculate the Kalman gain, posterior updated state estimate, and covariance. Step 5: The onboard ADC adjusts the introduced noise parameters based on the measurement residual. Perform online adaptive updates; however, when radar measurements are affected by multipath reflections from the sides of metal bridges, guardrails, or large truck bodies, abnormally large This automatically increases, causing the weight of the subsequent scoring function on the abnormal measurement to decrease or approach zero, thus isolating multipath pulses from contaminating the workshop distance estimation; Step 6: The onboard ADC outputs the robustly filtered relative distance and relative speed between the vehicles to the ACC controller, and... , , Write the status to the cache for recursion in the next measurement cycle.

[0023] When the noise parameters tend to be infinitesimally small or a fixed constant, the method of this invention degenerates into the traditional Kalman filtering or the classic M-Kalman filtering method, which is suitable for open scenarios with good road conditions. However, when the target vehicle is completely obscured and the radar fails to detect it effectively for several consecutive cycles, Maintaining the maximum value, the measurement update weight approaches zero to ensure the continuity of the target track. When the radar detects the vehicle ahead again, Automatic updates are automatically resumed according to adaptive update rules to ensure seamless continuation of the track after the preceding vehicle reappears.

[0024] I. System Model and Simulation Settings; The state dimension n=2, the observation dimension m=1, and the system parameters are as follows: state transition matrix F=[[0.98,0.02],[0,0.95]]; observation matrix H=[1,0]; process noise covariance. Nominal measurement noise variance .reality .

[0025] Algorithm hyperparameters: , , , .

[0026] Simulation settings: Monte Carlo simulation is used. Second-rate, Step, comparison method: The Kalman filtering algorithm based on the scoring function of Gaussian noise-induced modulation proposed in this invention ( Maximum correlation entropy Kalman filter (MCC-KF, Gaussian kernel width) =1), Huber-Kalman filter (Huber-KF, The overall root mean square error (RMSE) of each algorithm (PF, 150 particles) and the improvement of the method of this invention are compared.

[0027] II. Simulation Verification of Road Scenarios and Noise Models; (1) Sudden multipath pulse scenario on a metal overpass; Basic radar measurement noise: ; Sudden multipath pulse: amplitude 25m (typical multipath magnitude for metal steel truss bridges), duration L=10 measurement cycles, occurrence probability p=0.08, unidirectional pulse (fixed metal surface reflection path). (2) Four-stage time-varying multipath interference scenario; The simulated vehicle passes through the following routes in sequence: open main road → section with dense metal guardrails → open main road → overpass complex, T=600 steps, with each stage consisting of 150 steps (i.e., Stage 1: Ph1 [0, 150) steps, Stage 2: Ph2 [150, 300) steps, Stage 3: Ph3 [300, 450) steps, Stage 4: Ph4 [450, 600) steps). Light pollution stage (stages 1 and 3): ε=0.02, occasional slight multipath; Heavy pollution stages (stages 2 and 4): ε=0.55, frequent strong multipath (Cauchy distribution simulates extreme disturbances with infinite variance).

[0028] III. Simulation Verification 1 – Sudden Multipath Pulse Scenario on Metal Overpass; Algorithm initialization: , [[100,0],[0,10]], .

[0029] like Figure 2 and Figure 3 As shown, the method of the present invention ( The relative distance root mean square error curve under sudden multipath pulse interference gradually converges, and it is superior to the comparative methods such as Standard KF, PF, and Huber-KF in terms of convergence speed and steady-state accuracy. The root mean square error of the method in the present invention is always lower than that of the MCC-KF method under the impulse noise environment, realizing adaptive robust state estimation under impulse interference scenarios.

[0030] Table 1 Comparison of RMSE of algorithms in the scenario of sudden multipath pulses on metal overpasses

[0031] ; Simulation results are shown in Table 1: This invention The RMSE for shop floor distance tracking using the AMKF method is 0.93, while that of the fixed-parameter Huber-KF method is 1.52, the fixed-parameter MCC-KF method is 1.93, the particle filter method is 3.12, and the standard KF method is 10.41. Compared to Huber-KF, the method of this invention reduces the overall RMSE by 38.8%; compared to the fixed-parameter MCC-KF method, the overall RMSE is reduced by 51.9%; compared to the particle filter method, the overall RMSE is reduced by 70.2%; and compared to the standard KF method, the overall RMSE is reduced by 91.1%. The results show that the method of this invention achieves robust state estimation in impulse interference scenarios without relying on any prior noise parameter information, significantly outperforming fixed-parameter robust filters and traditional Kalman series methods.

[0032] IV. Simulation Verification of Two-Four Stage Time-Varying Multipath Scenarios; Algorithm initialization: , , .

[0033] like Figure 4 As shown, in a four-stage time-varying noise scenario, the method of the present invention ( The convergence speed of the method of this invention is significantly better than other comparative methods. The root mean square error curve of the method remained at a low level in all stages of the time-varying non-Gaussian noise environment, while the errors of Huber-KF and PF increased significantly in the heavy pollution stage (stages 2 and 4), and the standard KF diverged completely, which confirms that the method of the present invention has good robustness in time-varying noise scenarios.

[0034] Table 2 Comparison of Tracking RMSE of Various Methods in Cauchy-type Interference Scenarios

[0035] ; Simulation results are shown in Table 2: The overall RMSE of the method of this invention is 0.46, achieving a qualitative improvement in safety compared to the standard KF (divergent) method, and improving RMSE performance by approximately 10.6% and 30.6% compared to Huber-KF and MCC-KF, respectively. Compared to PF, the performance improvement is approximately 55.1%. Due to parameters... By adaptively updating to adapt to the noise environment, the method of this invention has stronger robustness and steady-state accuracy in time-varying non-Gaussian noise environments compared to other methods.

[0036] Figure 5 The noise parameters in the method of the present invention are shown. Adaptive update curves in a four-stage scenario. In the light pollution stages (stages 1 and 3). Maintain within the normal range of 1.0 to 1.2; during the heavy pollution phase (phases 2 and 4). Automatically rises to the 2.1~2.7 range, effectively shielding against Cauchy-type infinite variance interference; stage switching time It completes convergence in approximately 8 steps, which is equivalent to a response distance of about 5 meters for a vehicle traveling at high speeds, meeting the safety following response time requirements of the ACC system.

[0037] Of course, the above description is not intended to limit the present invention, and the present invention is not limited to the examples given above. Any changes, modifications, additions or substitutions made by those skilled in the art within the scope of the present invention should also fall within the protection scope of the present invention.

Claims

1. A Kalman filter method based on a noise-induced score function for target tracking, characterized in that, Includes the following steps: Step 1: Construct a discrete-time state-space model to describe the relative motion relationship between the vehicle and the vehicle in front; Step 2: The onboard autonomous driving domain controller ADC performs inertial inference based on the posterior estimate of the previous time step to obtain the state prediction value and prediction error covariance; Step 3: The millimeter-wave radar provides the ADC with the current relative distance measurement value between the workshop and the ADC, and calculates the measurement residual and nominal innovation variance; Step 4: The robust tracking update module of the vehicle ADC constructs a noise-induced M-score function based on the original M-estimate score function, and calculates the Kalman gain, posterior update state estimate, and covariance based on the noise-induced M-score function. Step 5: The onboard ADC performs online adaptive updates to the noise scale parameters based on the measurement residuals; Step 6: The on-board ADC outputs the posterior state estimate of the current moment to the ACC controller as the robustly filtered relative distance and relative speed between the two vehicles, and writes the posterior state estimate of the current moment, the posterior estimation error covariance, and the adaptive noise scale parameter into the state buffer for recursion in the next measurement cycle. In step 4, the symbol function is selected. As the original M-score function, auxiliary induced noise variables are introduced into the residual variables. ,in For scaled family probability density functions, Construct a noise smoothing score function for the noise scale parameter. : ； Normalizing the noise smoothing score function using the zeros of the probability density function yields the noise-induced M-score function. : ； in, To introduce the standard probability density function of scale family noise, For noise scale parameters; Based on noise-induced M-score function Based on the residuals, observation confidence weights are generated, and the observation update step in the Kalman filtering algorithm is rewritten into a generalized M-estimation form to achieve automatic soft suppression of multipath impulse interference. The Kalman gain, posterior updated state estimate, and covariance are calculated, expressed as follows: ； ； ； in, The Kalman gain matrix is... For posterior state estimation, The noise-induced score function is constructed, and its input is the measurement residual. and adaptive noise scaling parameters , To estimate the error covariance in the posterior timescale, It is the identity matrix; The noise-induced M-score function is constructed as a family of scalable generalized functions whose probability distribution is used to adjust the shape of the score function, thereby achieving smooth and robust adjustment of the score function. Its probability distribution includes, but is not limited to, Gaussian distribution, Laplace distribution, generalized Gaussian distribution, and Student-t distribution. In step 5, the noise scale parameter is determined based on the measurement residual. Perform online adaptive updates: ； in, To cut off the amount of innovation, This is a truncation function. The cutoff factor is , These are the weighting coefficients.

2. The Kalman filtering method for target tracking based on a noise-induced score function according to claim 1, characterized in that, In step 1, the state-space model includes state equations and observation equations, expressed as follows: ； ： in, For the current moment The system state vector, For the present The measured value output by the time sensor. This is Gaussian process noise with zero mean. For non-Gaussian distributed observation noise containing impulse interference, and These are the state transition matrix and the observation matrix, respectively.

3. The Kalman filtering method for target tracking based on a noise-induced score function according to claim 1, characterized in that, In step 3, the residual is measured. and nominal innovation variance The expression is: ； ； in, For the current moment State prediction value, For the current moment The prediction error covariance, The nominal noise covariance is used for measurement.

Images