Automobile trajectory tracking method based on variational Bayesian particle filtering
By using the variational Bayesian particle filtering method in car trajectory tracking, iterative updates are performed using the inverse Viagra distribution and coordinate ascending method, the problem of low accuracy caused by difficult to obtain prior distribution and unknown noise is solved, and more efficient State estimation and lower computational complexity.
Patent Information
- Application Number
- CN202411576176.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-11-06
- Publication Date
- 2025-02-18
AI Technical Summary
The existing particle filtering method in automobile trajectory tracking has problems such as low estimation accuracy due to the difficulty in accurately obtaining a priori distribution and unknown system noise.
By using the particle filtering method based on variational Bayes, the inverse Viagra distribution is selected as the conjugate prior distribution of state prediction error covariance, and iteratively updates using the coordinate rise method to construct the Gaussian distribution as the importance density function of the system. , from which new particles are sampled, weights are calculated and normalized and resampled.
It improves the estimation accuracy of automotive trajectory tracking, can handle state estimation problems in nonlinear and non-Gaussian systems more accurately, and significantly reduces the computational complexity.
Smart Images
Figure CN119474647A_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the technical field of nonlinear filtering, and in particular relates to a vehicle trajectory tracking method based on variational Bayesian particle filtering. Background Art
[0002] With the continuous advancement of intelligent driving technology, vehicle trajectory tracking technology has become one of the key technologies for realizing intelligent driving. The main goal of trajectory tracking technology is to ensure that the car can accurately and stably follow the predetermined trajectory during driving, thereby improving driving safety and comfort. In order to achieve this goal, it is necessary to adopt efficient and accurate filtering and estimation methods to obtain the vehicle's position, speed and other status information in real time and perform precise control.
[0003] Particle filtering is a non-parametric filtering technology based on the Monte Carlo method. It approximates the probability distribution of the system state by generating a large number of particle states, and estimates the system state in real time by updating and adjusting the weights of these particle states. Since particle filtering can handle nonlinear and non-Gaussian problems, it can estimate the position, speed and other state information of the car in real time, thereby providing an accurate basis for trajectory tracking control. Therefore, it has been widely used in vehicle trajectory tracking. Because there are environmental factors such as non-line-of-sight, interference, and occlusion when the car is driving, the influence of complex system characteristics such as nonlinearity, non-Gaussianity, and unknown parameters cannot be avoided in the process of system modeling, estimation, and identification. However, in the traditional particle filtering method, the weight update of the particle usually depends on the likelihood function and prior distribution of the observed value, but in practical applications, the prior distribution of the system state is often difficult to obtain accurately, which will affect the estimation accuracy of the particle filter. In order to solve this problem, it is necessary to design a new particle filter algorithm to make it more widely used, thereby improving the accuracy of state estimation. Summary of the invention
[0004] The purpose of the present invention is to provide a vehicle trajectory tracking method based on variational Bayesian particle filtering, which solves the problem in existing practical engineering that it is difficult to accurately assign its parameters due to insufficient prior modeling information and the low prediction accuracy of the estimated vehicle driving trajectory due to unknown system noise.
[0005] The technical solution adopted by the present invention is a vehicle trajectory tracking method based on variational Bayesian particle filtering, which is specifically implemented according to the following steps:
[0006] Step 1: Initialize particles;
[0007] Step 2: For the unknown system noise distribution problem, select IW distribution as the state prediction error covariance The conjugate prior distribution of ;
[0008] Step 3: Use the coordinate ascent method to Perform iterative updates;
[0009] Step 4: Update the Substitute Gaussian distribution As the importance density function of the system, new particles are sampled from it, and then the weights are calculated and normalized;
[0010] Step 5, resample the particle set, and select polynomial resampling as the resampling method;
[0011] Step 6: Get the optimal estimate and track the vehicle's trajectory.
[0012] The present invention is also characterized in that:
[0013] Step 1 is implemented according to the following steps:
[0014] Initialize the number of particles N to define the initial values of the parameters, including x0 and P0, using Gaussian distribution N(Fx0, BP0B T ) Randomly generate N initial particles i=1,2,...,N.
[0015] Step 2 is implemented as follows:
[0016] Step 2.1: Initialize the variational Bayes parameters, including the degrees of freedom parameter of the IW distribution The inverse scaling matrix of the IW distribution The hyperparameter τ, And reinitialize
[0017] Step 2.2: According to the parameterized inverse Wishart IW distribution, the state prediction error covariance The prior distribution of is expressed as:
[0018]
[0019] z k is the observed value;
[0020] Step 2.3: For the inverse Wishart distribution, when When n is the system dimension, the mean of the state prediction error covariance is Expressed as
[0021]
[0022] Let the state prediction error covariance be The prior distribution parameters of Expressed as
[0023]
[0024] Step 2.4, formula (4) is further transformed into
[0025]
[0026] τ represents the tuning parameter. By using formula (3) to formula (6), the shape of the state prediction covariance prior distribution and its corresponding distribution parameters are derived.
[0027] Step 3 is implemented as follows:
[0028] Step 3.1: First calculate the parameters
[0029]
[0030] Step 3.2: IW distribution parameters for the jth iteration and Update by
[0031]
[0032]
[0033] Step 3.3: j+1th iteration update of the state prediction error covariance Expressed as
[0034]
[0035] Step 4 is implemented according to the following steps:
[0036] Step 4.1: Update the result from the previous step Constructed Gaussian distribution As the system's prior probability distribution and importance density function q(x k |x k-1 ,z k ),Right now Combined with the observation data z of the positioning sensor installed on the vehicle at time k k and the observation likelihood distribution p(z k |x k ), calculate particle weights
[0037]
[0038] Step 4.2: Normalize the particle weights
[0039]
[0040] Step 6 is implemented according to the following steps:
[0041] Step 6.1: Use the weighted sum of the particles after weight normalization in step 4 and resampling in step 5 to approximate the true posterior probability density function, so that the estimated value of the final state is
[0042]
[0043] Update at the same time
[0044]
[0045] Step 6.2: Calculate the estimated vehicle state at the jth moment And the vehicle state estimation result at the j-1th moment The relative error of the vehicle state estimation result at the j+1th moment And the vehicle state estimation result at the jth moment The relative error is compared and the maximum value is taken. If the value is still greater than the set error value, go to step 3. Otherwise,
[0046]
[0047] Step 6.2, let k=k+1, and return to step 2;
[0048] Through the above steps, formula (15) obtains the optimal estimate of the system state at time k, locates the position, velocity and acceleration of the car in the x and y directions at any time, and realizes the trajectory tracking of the car.
[0049] The beneficial effect of the present invention is that the vehicle trajectory tracking method based on variational Bayesian particle filtering can handle the state estimation problem of nonlinear and non-Gaussian systems. In vehicle trajectory tracking, due to the complexity and uncertainty of the vehicle's motion state, traditional filtering methods are often difficult to achieve ideal tracking effects. The present invention can more accurately estimate the vehicle's motion state by iteratively updating the particle distribution, thereby improving the tracking accuracy, and by finding a simple distribution family that is closest to the true posterior distribution, the computational complexity can be significantly reduced. Therefore, the variational Bayesian particle filter not only ensures the tracking accuracy, but also improves the computational efficiency. These effects make this method have broad application prospects in vehicle trajectory tracking. BRIEF DESCRIPTION OF THE DRAWINGS
[0050] Figure 1 This is a comparison chart of the filtering results of the standard particle filter algorithm (Particle Filter, PF), the cross-resolution prediction filter (Cross-Resolution Prediction Filter, CRPF) and the vehicle trajectory tracking method of the variational Bayesian particle filter (VBPF);
[0051] Figure 2(a) and Figure 2(b) are the comparison diagrams of the root mean square error of x and y of the standard particle filter algorithm (Particle Filter, PF), the cost reference particle filter (Cross-Resolution Prediction Filter, CRPF) and the vehicle trajectory tracking method of variational Bayesian particle filter (VBPF);
[0052] FIG3(a) and FIG3(b) are comparison diagrams of the root mean square error of x′ and y′ of the standard particle filter algorithm (Particle Filter, PF), the cost reference particle filter (Cross-Resolution Prediction Filter, CRPF) and the vehicle trajectory tracking method of the variational Bayesian particle filter (VBPF);
[0053] FIG4(a) and FIG4(b) are comparison diagrams of the root mean square error of x″ and y″ of the standard particle filter algorithm (Particle Filter, PF), the cost reference particle filter (Cross-Resolution Prediction Filter, CRPF) and the vehicle trajectory tracking method of the variational Bayesian particle filter (VBPF). DETAILED DESCRIPTION
[0054] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments.
[0055] Aiming at the situation that the prior distribution of system state is unknown, the present invention designs a variational Bayesian particle filter, which combines particle filtering and variational Bayesian method, and utilizes the non-parametric characteristics of particle filtering and the modeling ability of variational Bayesian method to realize accurate tracking of vehicle trajectory. Firstly, assuming that the prior distribution of system state is Gaussian distribution, the parameterized IW distribution is introduced as the conjugate distribution of the state prediction error covariance matrix to establish a prior information model, and the parameters are updated in combination with the iterative method of coordinate ascent. Finally, it is combined with the particle filter algorithm to calculate the particle weights and resample them, so that the particles are weighted to obtain state estimation. In the case of unknown prior distribution, the present invention significantly improves the precision and accuracy of the filtering results, effectively reduces the system filtering deviation, optimizes the filtering performance, provides strong support for the in-depth analysis and control of the vehicle driving trajectory positioning and tracking system, and realizes high-precision estimation of nonlinear systems under non-Gaussian conditions with time-varying process noise.
[0056] The vehicle trajectory tracking method based on variational Bayesian particle filtering of the present invention is implemented in the following steps:
[0057] Step 1: Initialize particles;
[0058] Step 1 is implemented according to the following steps:
[0059] Initialize the number of particles N to define the initial values of the parameters, including x0 and P0, using Gaussian distribution N(Fx0, BP0B T ) Randomly generate N initial particles i=1,2,...,N.
[0060] Step 2: For the unknown system noise distribution problem, select IW distribution as the state prediction error covariance The conjugate prior distribution of ;
[0061] Step 2 is implemented as follows:
[0062] Step 2.1: Initialize the variational Bayes parameters, including the degrees of freedom parameter of the IW distribution The inverse scaling matrix of the IW distribution The hyperparameter τ, And reinitialize
[0063] Step 2.2: According to the parameterized inverse-Wishart distribution, the state prediction error covariance The prior distribution of is expressed as:
[0064]
[0065] z k is the observed value;
[0066] Step 2.3: For the inverse Wishart distribution, when When n is the system dimension, the mean of the state prediction error covariance is Expressed as
[0067]
[0068] Let the state prediction error covariance be The prior distribution parameters of Expressed as
[0069]
[0070] Step 2.4, formula (4) is further transformed into
[0071]
[0072] τ represents the tuning parameter. By using formula (3) to formula (6), the shape of the state prediction covariance prior distribution and its corresponding distribution parameters are derived.
[0073] Step 3: In the variational Bayesian optimization process, the coordinate ascent method is used to Perform iterative updates;
[0074] Step 3 is implemented as follows:
[0075] Step 3.1: Calculate the parameters first
[0076]
[0077] Step 3.2: IW distribution parameters for the jth iteration and Update by
[0078]
[0079]
[0080] Step 3.3: j+1th iteration update of the state prediction error covariance Expressed as
[0081]
[0082] Step 4: Update the Substitute Gaussian distribution As the importance density function of the system, new particles are sampled from it, and then the weights are calculated and normalized;
[0083] Step 4 is implemented according to the following steps:
[0084] Step 4.1: Update the result from the previous step Constructed Gaussian distribution As the system's prior probability distribution and importance density function q(x k |x k-1 ,z k ),Right now Combined with the observation data z of the positioning sensor installed on the vehicle at time k k and the observation likelihood distribution p(z k |x k ), calculate particle weights
[0085]
[0086] Step 4.2: Normalize the particle weights
[0087]
[0088] Step 5, resample the particle set, and select polynomial resampling as the resampling method;
[0089] Step 6: Get the optimal estimate and track the vehicle's trajectory.
[0090] Step 6 is implemented according to the following steps:
[0091] Step 6.1: Use the weighted sum of the particles after weight normalization in step 4 and resampling in step 5 to approximate the true posterior probability density function, so that the estimated value of the final state is
[0092]
[0093] Update at the same time
[0094]
[0095] Step 6.2: Calculate the estimated vehicle state at the jth moment And the vehicle state estimation result at the j-1th moment The relative error of the vehicle state estimation result at the j+1th moment And the vehicle state estimation result at the jth moment The relative error is compared and the maximum value is taken. If the value is still greater than the set error value, go to step 3. Otherwise,
[0096]
[0097] Step 6.2, let k=k+1, and return to step 2;
[0098] Through the above steps, formula (15) obtains the optimal estimate of the system state at time k, locates the position, velocity and acceleration of the car in the x and y directions at any time, and realizes the trajectory tracking of the car.
[0099] Example 1
[0100] The vehicle trajectory tracking method based on variational Bayesian particle filtering of the present invention is specifically implemented according to the following steps: Under the particle filtering framework, for the problem of unknown prior distribution, the IW distribution is selected as the conjugate prior distribution of the state covariance and the state prediction error covariance is updated, and then the Gaussian distribution constructed by the error covariance is used as the system importance distribution.
[0101] Step 1: Initialize particles;
[0102] Step 2: For the unknown system noise distribution problem, select IW distribution as the state prediction error covariance The conjugate prior distribution of ;
[0103] Step 3: In the variational Bayesian optimization process, the coordinate ascent method is used to Perform iterative updates;
[0104] Step 4: Update the Substitute Gaussian distribution As the importance density function of the system, new particles are sampled from it, and then the weights are calculated and normalized;
[0105] Step 5, resample the particle set, and select polynomial resampling as the resampling method;
[0106] Step 6: Get the optimal estimate and track the vehicle's trajectory.
[0107] Example 2
[0108] The vehicle trajectory tracking method based on variational Bayesian particle filtering of the present invention is implemented in the following steps:
[0109] Step 1: Initialize particles;
[0110] Step 1 is implemented according to the following steps:
[0111] Initialize the number of particles N to define the initial values of the parameters, including x0 and P0, using Gaussian distribution N(Fx0, BP0B T ) Randomly generate N initial particles i=1,2,...,N.
[0112] Step 2: For the unknown system noise distribution problem, select IW distribution as the state prediction error covariance The conjugate prior distribution of ;
[0113] Step 2 is implemented as follows:
[0114] Step 2.1: Initialize the variational Bayes parameters, including the degrees of freedom parameter of the IW distribution The inverse scaling matrix of the IW distribution The hyperparameter τ, And reinitialize
[0115] Step 2.2: According to the parameterized inverse-Wishart distribution, the state prediction error covariance The prior distribution of is expressed as:
[0116]
[0117] z k is the observed value;
[0118] Step 2.3: For the inverse Wishart distribution, when When n is the system dimension, the mean of the state prediction error covariance is Expressed as
[0119]
[0120] Let the state prediction error covariance be The prior distribution parameters of Expressed as
[0121]
[0122] Step 2.4, formula (4) is further transformed into
[0123]
[0124] τ represents the tuning parameter. By using formula (3) to formula (6), the shape of the state prediction covariance prior distribution and its corresponding distribution parameters are derived.
[0125] Step 3: In the variational Bayesian optimization process, the coordinate ascent method is used to Perform iterative updates;
[0126] Step 4: Update the Substitute Gaussian distribution As the importance density function of the system, new particles are sampled from it, and then the weights are calculated and normalized;
[0127] Step 5, resample the particle set, and select polynomial resampling as the resampling method;
[0128] Step 6: Get the optimal estimate and track the vehicle's trajectory.
[0129] Example 3
[0130] The vehicle trajectory tracking method based on variational Bayesian particle filtering of the present invention is implemented in the following steps:
[0131] Step 1: Initialize particles;
[0132] Step 1 is implemented according to the following steps:
[0133] Initialize the number of particles N to define the initial values of the parameters, including x0 and P0, using Gaussian distribution N(Fx0, BP0B T ) Randomly generate N initial particles i=1,2,...,N.
[0134] Step 2: For the unknown system noise distribution problem, select IW distribution as the state prediction error covariance The conjugate prior distribution of ;
[0135] Step 2 is implemented as follows:
[0136] Step 2.1: Initialize the variational Bayes parameters, including the degrees of freedom parameter of the IW distribution The inverse scaling matrix of the IW distribution The hyperparameter τ, And reinitialize
[0137] Step 2.2: According to the parameterized inverse-Wishart distribution, the state prediction error covariance The prior distribution of is expressed as:
[0138]
[0139] z k is the observed value;
[0140] Step 2.3: For the inverse Wishart distribution, when When n is the system dimension, the mean of the state prediction error covariance is Expressed as
[0141]
[0142] Let the state prediction error covariance be The prior distribution parameters of Expressed as
[0143]
[0144] Step 2.4, formula (4) is further transformed into
[0145]
[0146] τ represents the tuning parameter. By using formula (3) to formula (6), the shape of the state prediction covariance prior distribution and its corresponding distribution parameters are derived.
[0147] Step 3: In the variational Bayesian optimization process, the coordinate ascent method is used to Perform iterative updates;
[0148] Step 3 is implemented as follows:
[0149] Step 3.1: First calculate the parameters
[0150]
[0151] Step 3.2: IW distribution parameters for the jth iteration and Update by
[0152]
[0153]
[0154] Step 3.3: j+1th iteration update of the state prediction error covariance Expressed as
[0155]
[0156] Step 4: Update the Substitute Gaussian distribution As the importance density function of the system, new particles are sampled from it, and then the weights are calculated and normalized;
[0157] Step 5, resample the particle set, and select polynomial resampling as the resampling method;
[0158] Step 6: Get the optimal estimate and track the vehicle's trajectory.
[0159] Example 4
[0160] The vehicle trajectory tracking method based on variational Bayesian particle filtering of the present invention is implemented in the following steps:
[0161] Step 1: Initialize particles;
[0162] Step 1 is implemented according to the following steps:
[0163] Initialize the number of particles N to define the initial values of the parameters, including x0 and P0, using Gaussian distribution N(Fx0, BP0B T ) Randomly generate N initial particles i=1,2,...,N.
[0164] Step 2: For the unknown system noise distribution problem, select IW distribution as the state prediction error covariance The conjugate prior distribution of ;
[0165] Step 2 is implemented as follows:
[0166] Step 2.1: Initialize the variational Bayes parameters, including the degrees of freedom parameter of the IW distribution The inverse scaling matrix of the IW distribution The hyperparameter τ, And reinitialize
[0167] Step 2.2: According to the parameterized inverse-Wishart distribution, the state prediction error covariance The prior distribution of is expressed as:
[0168]
[0169] z k is the observed value;
[0170] Step 2.3: For the inverse Wishart distribution, when When n is the system dimension, the mean of the state prediction error covariance is Expressed as
[0171]
[0172] Let the state prediction error covariance be The prior distribution parameters of Expressed as
[0173]
[0174] Step 2.4, formula (4) is further transformed into
[0175]
[0176] τ represents the tuning parameter. By using formula (3) to formula (6), the form of the state prediction covariance prior distribution and its corresponding distribution parameters are derived.
[0177] Step 3: In the variational Bayesian optimization process, the coordinate ascent method is used to Perform iterative updates;
[0178] Step 3 is implemented as follows:
[0179] Step 3.1: Calculate the parameters first
[0180]
[0181] Step 3.2: IW distribution parameters for the jth iteration and Update by
[0182]
[0183]
[0184] Step 3.3: j+1th iteration update of the state prediction error covariance Expressed as
[0185]
[0186] Step 4: Update the Substitute Gaussian distribution As the importance density function of the system, new particles are sampled from it, and then the weights are calculated and normalized;
[0187] Step 4 is implemented according to the following steps:
[0188] Step 4.1: Update the result from the previous step Constructed Gaussian distribution As the system's prior probability distribution and importance density function q(x k |x k-1 ,z k ),Right now Combined with the observation data z of the positioning sensor installed on the vehicle at time k k and the observation likelihood distribution p(z k |x k ), calculate particle weights
[0189]
[0190] Step 4.2: Normalize the particle weights
[0191]
[0192] Step 5, resample the particle set, and select polynomial resampling as the resampling method;
[0193] Step 6: Get the optimal estimate and track the vehicle's trajectory.
[0194] Example 5
[0195] The vehicle trajectory tracking method based on variational Bayesian particle filtering of the present invention is implemented in the following steps:
[0196] Step 1: Initialize particles;
[0197] Step 1 is implemented according to the following steps:
[0198] Initialize the number of particles N to define the initial values of the parameters, including x0 and P0, using Gaussian distribution N(Fx0, BP0B T ) Randomly generate N initial particles i=1,2,...,N.
[0199] Step 2: For the unknown system noise distribution problem, select IW distribution as the state prediction error covariance The conjugate prior distribution of ;
[0200] Step 2 is implemented as follows:
[0201] Step 2.1: Initialize the variational Bayes parameters, including the degrees of freedom parameter of the IW distribution The inverse scaling matrix of the IW distribution The hyperparameter τ, And reinitialize
[0202] Step 2.2: According to the parameterized inverse-Wishart distribution, the state prediction error covariance The prior distribution of is expressed as:
[0203]
[0204] z k is the observed value;
[0205] Step 2.3: For the inverse Wishart distribution, when When n is the system dimension, the mean of the state prediction error covariance is Expressed as
[0206]
[0207] Let the state prediction error covariance be The prior distribution parameters of Expressed as
[0208]
[0209] Step 2.4, formula (4) is further transformed into
[0210]
[0211] τ represents the tuning parameter. By using formula (3) to formula (6), the form of the state prediction covariance prior distribution and its corresponding distribution parameters are derived.
[0212] Step 3: In the variational Bayesian optimization process, the coordinate ascent method is used to Perform iterative updates;
[0213] Step 4: Update the Substitute Gaussian distribution As the importance density function of the system, new particles are sampled from it, and then the weights are calculated and normalized;
[0214] Step 4 is implemented according to the following steps:
[0215] Step 4.1: Update the result from the previous step Constructed Gaussian distribution As the system's prior probability distribution and importance density function q(x k |x k-1 ,z k ),Right now Combined with the observation data z of the positioning sensor installed on the vehicle at time k k and the observation likelihood distribution p(z k |x k ), calculate particle weights
[0216]
[0217] Step 4.2: Normalize the particle weights
[0218]
[0219] Step 5, resample the particle set, and select polynomial resampling as the resampling method;
[0220] Step 6: Get the optimal estimate and track the vehicle's trajectory.
[0221] Step 6 is implemented according to the following steps:
[0222] Step 6.1: Use the weighted sum of the particles after weight normalization in step 4 and resampling in step 5 to approximate the true posterior probability density function, so that the estimated value of the final state is
[0223]
[0224] Update at the same time
[0225]
[0226] Step 6.2: Calculate the estimated vehicle state at the jth moment And the vehicle state estimation result at the j-1th moment The relative error of the vehicle state estimation result at the j+1th moment And the vehicle state estimation result at the jth moment The relative error is compared and the maximum value is taken. If the value is still greater than the set error value, go to step 3. Otherwise,
[0227]
[0228] Step 6.2, let k=k+1, and return to step 2;
[0229] Through the above steps, formula (15) obtains the optimal estimate of the system state at time k, locates the position, velocity and acceleration of the car in the x and y directions at any time, and realizes the trajectory tracking of the car.
[0230] Example 6
[0231] In order to verify the effectiveness of the proposed method, a multidimensional nonlinear model system is used to analyze the accuracy and stability of the state estimation of each algorithm and the nonlinear model. The state equation and observation equation of the model are
[0232]
[0233]
[0234] In the formula,
[0235]
[0236]
[0237] x k =[x(k),y(k),x′(k),y′(k),x″(k),y″(k)] T is the target state vector. The components of the target state vector represent the horizontal position, vertical position, horizontal velocity, vertical velocity, horizontal acceleration and vertical acceleration respectively. The initial value of the experiment is Maneuvering frequency The sampling period Δ=0.1s, the simulation step size L=400, the number of particles N=100, and the resampling steps all use the polynomial resampling method.
[0238] The standard particle filter algorithm (Particle Filter, PF), cross-resolution prediction filter (Cross-Resolution Prediction Filter, CRPF) and the vehicle trajectory tracking method of variational Bayesian particle filter (VBPF) were compared and tested.
[0239] The present invention is used to estimate the driving state of the car and obtain the filtering result of the system, such as Figure 1 shown.
[0240] In order to accurately evaluate the effectiveness of the present invention, RMSE (root mean square error) is used as an indicator for evaluating system performance for comparative analysis. Figures 2(a) and 2(b) are respectively the root mean square deviation comparison diagrams of standard PF, CRPF and x and y of the present invention; Figures 3(a) and 3(b) are respectively the root mean square deviation comparison diagrams of standard PF, CRPF and x' and y' of the present invention; Figures 4(a) and 4(b) are respectively the root mean square deviation comparison diagrams of standard PF, CRPF and x" and y" of the present invention; Table 1 shows the experimental root mean square error of each algorithm. According to the figure and table, it can be seen that the RMSE of the present invention after multiple experiments is the smallest, and the CRPF algorithm leads to tracking divergence as time increases, while the method of the present invention has good estimation accuracy and stability.
[0241] Table 1
[0242]
Claims
1. A vehicle trajectory tracking method based on variational Bayesian particle filtering, characterized in that: Follow the steps below to implement it: Step 1: Initialize particles; Step 2: For the unknown system noise distribution problem, select IW distribution as the state prediction error covariance The conjugate prior distribution of ; Step 3: Use the coordinate ascent method to Perform iterative updates; Step 4: Update the Substitute Gaussian distribution As the importance density function of the system, new particles are sampled from it, and then the weights are calculated and normalized; Step 5, resample the particle set, and select polynomial resampling as the resampling method; Step 6: Get the optimal estimate and track the vehicle's trajectory.
2. The vehicle trajectory tracking method based on variational Bayesian particle filtering according to claim 1 is characterized in that: The step 1 is specifically implemented according to the following steps: Initialize the number of particles N to define the initial values of the parameters, including x0 and P0, using Gaussian distribution N(Fx0, BP0B T ) Randomly generate N initial particles 3. The vehicle trajectory tracking method based on variational Bayesian particle filtering according to claim 2 is characterized in that: The step 2 is specifically implemented according to the following steps: Step 2.1: Initialize the variational Bayes parameters, including the degrees of freedom parameter of the IW distribution The inverse scaling matrix of the IW distribution The hyperparameter τ, And reinitialize Step 2.2: According to the parameterized inverse Wishart IW distribution, the state prediction error covariance The prior distribution of is expressed as: z k is the observed value; Step 2.3: For the inverse Wishart distribution, when When n is the system dimension, the mean of the state prediction error covariance is Expressed as Let the state prediction error covariance be The prior distribution parameters of Expressed as Step 2.4, formula (4) is further transformed into τ represents the tuning parameter. By using formula (3) to formula (6), the form of the state prediction covariance prior distribution and its corresponding distribution parameters are derived.
4. The vehicle trajectory tracking method based on variational Bayesian particle filtering according to claim 3 is characterized in that: The step 3 is specifically implemented according to the following steps: Step 3.1: First calculate the parameters Step 3.2: IW distribution parameters for the jth iteration and Update by Step 3.3: j+1th iteration update of the state prediction error covariance Expressed as 5. The vehicle trajectory tracking method based on variational Bayesian particle filtering according to claim 4 is characterized in that: The step 4 is specifically implemented according to the following steps: Step 4.1: Update the result obtained in the previous step Constructed Gaussian distribution As the system's prior probability distribution and importance density function q(x k |x k-1 ,z k ); Step 4.2: Normalize the weights of the particles.
6. The vehicle trajectory tracking method based on variational Bayesian particle filtering according to claim 5 is characterized in that: The step 4.1 is specifically implemented according to the following steps: Right now Combined with the observation data z of the positioning sensor installed on the vehicle at time k k and the observation likelihood distribution p(z k |x k ), calculate particle weights 7. The vehicle trajectory tracking method based on variational Bayesian particle filtering according to claim 6 is characterized in that: The weight normalization process in step 4.2 is specifically as follows:
8. The vehicle trajectory tracking method based on variational Bayesian particle filtering according to claim 7 is characterized in that: The step 6 is specifically implemented according to the following steps: Step 6.1: Use the weighted sum of the particles after weight normalization in step 4 and resampling in step 5 to approximate the true posterior probability density function, so that the estimated value of the final state is Update at the same time Step 6.2: Calculate the estimated vehicle state at the jth moment And the vehicle state estimation result at the j-1th moment The relative error of the vehicle state estimation result at the j+1th moment And the vehicle state estimation result at the jth moment The relative error is compared and the maximum value is taken. If the value is still greater than the set error value, go to step 3. Otherwise, Step 6.2, let k=k+1, and return to step 2; Through the above steps, formula (15) obtains the optimal estimate of the system state at time k, locates the position, velocity and acceleration of the car in the x and y directions at any time, and realizes the trajectory tracking of the car.