A Bayesian dynamic estimation algorithm for nonlinear or non-Gaussian distribution systems

Abstract

The present invention belongs to the technical field of state tracking and prediction of dynamic systems, and is specifically a Bayesian dynamic estimation algorithm for nonlinear or non-Gaussian distribution systems. The present invention first establishes a state model and an observation model for the target to be estimated; then, the above model is used to perform maximum likelihood estimation on the target to obtain an estimated value of the target state at the current moment; the estimation result and its corresponding Cramer-Rao limit are then substituted as intermediate quantities into the state model at the next moment, thereby completing a round of estimation and prediction; through the above iterative process, dynamic estimation of the target is achieved. The algorithm is suitable for nonlinear signal models or non-Gaussian noise. Simulation results show that the algorithm can effectively track targets in a nonlinear state, and is superior to the standard extended Kalman filter and particle filter algorithms in terms of estimation accuracy.

Description

Technical Field

[0001] The present invention belongs to the technical field of state tracking and prediction of dynamic systems, and in particular relates to a Bayesian dynamic estimation algorithm. Background Art

[0002] The application of state prediction and tracking of dynamic systems is very extensive. The most typical application is the positioning and navigation of vehicles or aircraft, including the autonomous driving technology that has attracted widespread attention in recent years. The most commonly used state estimator is the extended Kalman filter [1]. However, for systems with highly nonlinear or non-Gaussian distributions, the standard extended Kalman filter will limit the estimation results. In recent years, the particle filter algorithm has been widely used [2]. It can not only better solve nonlinear and non-Gaussian problems, but also has robustness. However, in the particle filter algorithm, the particle degradation problem occurs, which increases the invalid operation of the system.

[0003] The present invention breaks away from the theoretical framework of the above estimator and proposes a new estimation algorithm, which overcomes some problems existing in the above estimator and improves the accuracy and reliability of the estimation result. Summary of the Invention

[0004] The purpose of the present invention is to provide a high-precision and high-robust Bayesian dynamic estimation algorithm for systems with highly nonlinear or non-Gaussian distributions (not obeying Gaussian distributions), such as positioning and navigation systems of vehicles or aircraft.

[0005] Typically, when dynamically estimating a system, there is a measurement at each moment i, but after the measurement is completed, the measurement result is discarded, which will lead to information loss. In order to make the most of the existing information, it is necessary to integrate the measurement results of the completed estimation into the subsequent estimation process. The Bayesian dynamic estimation algorithm provided by the present invention uses a recursive filter to solve this problem. This filter basically includes two stages: prediction and update. The prediction stage uses the system state model to predict the state probability distribution from the current moment to the next moment. Since the state model is usually affected by interference, the update stage needs to use the latest observations to correct the predicted probability distribution.

[0006] Assume that the state model and observation model of the system are:

[0007] θ i+1 =f i (θ i ,ω i ), (1)

[0008] β i =h i (θ i ,v i), (2) where θ i is the system state at time i, β i is the measurement process at time i, ω i ,v i are all independent noises, f i With h i is a nonlinear function, and f i With h i May change with the change of time i. In addition, we assume that the probability density p(θ0) of the initial state θ0 is known. Using the above state transition function θ i With the measurement function β i , estimate the target quantity (i.e., estimate the target quantity using maximum likelihood).

[0009] Since (1) and (2) are independent of each other, the observation quantity β at different times i are independent of each other, and β i The probability distribution p(β i ) is a constant, so the posterior probability of the estimated quantity satisfies the following relationship:

[0010]

[0011] In order to maximize the posterior probability of the estimated quantity, the above formula is rewritten as:

[0012]

[0013] The optimized objective function is:

[0014] L i (θ i )=logp(θ i |β 1:i-1 )+logp(β i |θ i ), (5)

[0015] A typical method for solving (5) is the sequential convex programming (SCP) algorithm. The specific algorithm flow is shown in Appendix 2; other optimization methods, including gradient descent, can also be used.

[0016] After estimating the target quantity using maximum likelihood, the target quantity θ can be approximated by the asymptotic normality of the estimator. i , the mean is The variance is the corresponding Cramer limit C i Gaussian distribution, that is in, J i is a Fisher matrix [3], and has:

[0017]

[0018] For most probability density functions, finding the mean of the second-order derivative of their log-likelihood form is difficult. In this case, the Monte Carlo method can be used to find a numerical solution. This involves calculating the second-order derivative of the state transfer function and measurement function, given their known mean, variance, and distribution function. This information can then be used to generate sufficient sample values, which are then averaged to obtain an approximate solution to the Fisher matrix. (Although this method can also be used to obtain a numerical solution to the Fisher matrix for the first-order derivative, the sample values required for the second-order derivative are far fewer than those for the first-order derivative when solving a numerical solution of the same accuracy, which helps reduce the computational complexity of the actual estimate.)

[0019] Then, the estimated result at the i-th moment is substituted into the state transfer function at the next moment so that the target state at the i+1-th moment satisfies This completes the dynamic estimation process.

[0020] According to the above introduction, the Bayesian dynamic estimation algorithm for nonlinear or non-Gaussian distribution systems provided by the present invention uses maximum likelihood to estimate the target. According to the asymptotic normality of the estimator, for the target state θ at the current moment, i , so that it approximately obeys the mean as the estimated value The variance is the corresponding Cramer limit C i Gaussian distribution, that is The approximated target state is then used as the intermediate transfer quantity and substituted into the state function at the next moment, so that the estimation result (observation information) at the current moment can be combined with the target state of the next moment prediction stage, thereby improving the accuracy of the target state prediction. The specific operation steps of the algorithm of the present invention are:

[0021] (1) The estimated value of the target initial state θ0 is known The variance of the corresponding noise ω0 Set the number of tracking measurements N;

[0022] (2) For each measurement moment *i|i=1,2,…,N+:

[0023] (3) Using the state model function θ i =f i-1 (θ i-1 ,ω i-1 ), get the target state θ at the i-th moment i The prior probability density function p(θ i |β 1:i-1 );

[0024] (4) By observing the model function β i =hi (θ i ,v i ), we get θ i The posterior probability density function p(β i |θ i );

[0025] (5) According to steps 3 and 4, we get θ i The objective function L i (θ i )=logp(θ i |β 1:i-1 )+logp(β i |θ i ), and use the objective function to i Perform maximum likelihood estimation to obtain and output the estimated value at the i-th moment

[0026] (6) Calculate the θth i Caramel Limited

[0027] (7) The estimated value at time i Carat Meilao Limited C i As θ i The mean and variance of Substitute it into the state function at the i+1th moment to satisfy θ i+1 =f i (θ i ,ω i ), thus completing the i+1 Update of the prior probability density of . See Appendix 1.

[0028] Simulation results show that the algorithm of the present invention can effectively track targets in a nonlinear state, and is superior to the standard extended Kalman filter and particle filter algorithms in terms of estimation accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 It is a comparison of the root mean square error of the velocity and position estimation results between the algorithm of the present invention and the extended Kalman filter and the particle filter.

[0030] Figure 2 It is the comparison between the estimated trajectory of the algorithm of the present invention and the actual trajectory of the target. DETAILED DESCRIPTION

[0031] The present invention is described below through specific implementation examples.

[0032] As an application of this algorithm, we combine the target position and velocity estimation based on distance and Doppler shift. There is a moving target to be estimated, whose position and velocity information are unknown. At each time i, there is a known position information The sensor observes the target and obtains the relative distance d between the target and the sensor i and Doppler shift Δf i We will use the above information to estimate the actual position x of the target i With speed y i Make an estimate.

[0033] make The relative distance and Doppler shift between the target and the sensor are:

[0034]

[0035]

[0036] Among them, f c is the carrier frequency, c is the carrier propagation speed, and They are all Gaussian white noise and are independent of each other.

[0037] Because f c and c are constants in a fixed environment, so using (8), we can get the component of the velocity of the target on the line connecting it and the sensor, that is, the radial velocity:

[0038]

[0039] in, but Obey the Gaussian distribution, that is

[0040] In order to facilitate the subsequent calculation, (7) and (9) are integrated. The observation quantity obtained by the sensor can be expressed as:

[0041]

[0042] in,

[0043] The algorithm is used in t i The state variables at this moment are set to:

[0044]

[0045] in, are the acceleration, velocity and position of the target respectively; ω i~N(0,Ψ i ) is the state error of the system.

[0046] The acceleration follows a ternary mixed Gaussian model, and the probability density distribution of each dimension obeys:

[0047] a d ~0.3×N(1,0.1)+0.2×N(-1,0.1)+0.5×0, where *d|d=1,2,3+.

[0048] The variance of the relative distance noise is: σ i =1m 2 ;

[0049] The variance of the radial phase velocity noise is:

[0050] The time interval between two measurements is Δt i =0.1s.

[0051] The sample size of the particle filter is 100.

[0052] The number of samples for calculating the Cramer-Rao limit of this method is 100. Based on the mean of 1000 Monte Carlo experiments, we compare the root mean square error of the proposed algorithm, the extended Kalman filter, and the particle filter, and compare it with the Cramer-Rao limit. Figure 1 It can be seen that the estimation results of the target velocity by the three methods are similar; and for the estimation of the target position, compared with EKF and PF, the estimation accuracy of the method of the present invention is the highest, and the dynamic error peak is the smallest. Figure 2 The comparison between the estimated trajectory of the proposed algorithm and the actual trajectory of the target is shown, and it can be seen that the proposed algorithm can accurately estimate the target path.

[0053] Appendix 1, nonlinear Bayesian iterative algorithm, the specific steps are:

[0054] 1: Estimated value of the known initial point θ0 and variance Set the number of tracking measurements N;

[0055] 2: for i=1:N:

[0056] 3: Using the state transfer function, we get θ i The prior probability density function of ;

[0057] 4: Through the observation quantity, that is, θ i The posterior probability density function of ;

[0058] 5: According to steps 3 and 4, θ i Perform joint maximum likelihood estimation to obtain and output the estimated value at the i-th moment

[0059] 6: Calculate θ i Carat Mei Lao C i ;

[0060] 7: Estimated value Carat Meilao Limited C i As θ i The mean and variance of Substitute it into the state function at the i+1th moment to complete the calculation of θ i+1 Update of the prior probability density.

[0061] 8: end.

[0062] Appendix 2, SCP iterative algorithm, the specific steps are:

[0063] 1:while 1do:

[0064] 2: Calculate for θ i The first derivative of With the second derivative

[0065] 3: Calculate Newton direction u i ;like is a positive semidefinite matrix, then:

[0066] otherwise:

[0067] For the general Remove the part with eigenvalue less than 0 to make it a semi-positive definite matrix;

[0068] 4: Find the step length s by backtracking the straight line method so that L i maximize;

[0069] 5: Through θ i +su nt Approximating the true θ i , that is, θ i ←θ i +su nt ;

[0070] 6: When Newton decreases Enough hours to record the estimated value at this time and jump out of the iteration;

[0071] 7:end while.

[0072] References

[0073] [1]M.I.Ribeiro,“Kalman and extended Kalman filters:Concept,derivationand properties,”Institute for Systems and Robotics,vol.43,p.46,2004.

[0074] [2]X.Ma,P.Karkus,D.Hsu,and W.S.Lee,“Particle filter recurrent neuralnetworks,”in Proceedings of the AAAI Conference on ArtificialIntelligence,vol.34,pp.5101–5108,2020.

[0075] [3]P.Tichavsky,C.H.Muravchik,and A.Nehorai,“Posterior Cram′er-Raobounds for discrete-time nonlinear filtering,”IEEE Transactions on signalprocessing,vol.46,no.5,pp.1386–1396,1998。

Claims

1. A Bayesian dynamic estimation method for a vehicle or aircraft positioning and navigation system, characterized in that: Use a recursive filter that consists of two stages: prediction and update; where: In the prediction stage, the state model of the target is used to predict the state probability distribution from the current moment to the next moment; In the update phase, since the state model is usually affected by disturbances, the latest observations are used to correct the predicted probability distribution; Assume that the state model and observation model of the system are: i i+1 =f i (i i ,oh i ) (1) b i =h i (i i ,v i ) (2) Among them, θ i is the state of the target to be estimated at time i, β i is the measurement process at time i, ω i ,v i are all independent noises, f i With h i is a nonlinear function, and f i With h i As the time i changes, it is assumed that the probability density p(θ0) of the target initial state θ0 is known. Using the above state transfer function θ i With the measurement function β i Estimate the target quantity, that is, estimate the target quantity using maximum likelihood; Since (1) and (2) are independent of each other, the observation quantity β at different times i are independent of each other, and β i The probability distribution p(β i ) is a constant, so the posterior probability of the estimated quantity satisfies the following relationship: In order to maximize the posterior probability of the quantity to be estimated, the above formula is rewritten as: The optimized objective function is: L i (i i )=logp(θ i |b 1:i-1 )+logp(β i |θ i ) (5) Solve (5) and get the optimal solution; After estimating the target quantity using maximum likelihood, the target quantity θ can be approximated by the asymptotic normality of the estimator. i , the mean is The variance is the corresponding Cramer limit C i Gaussian distribution, that is in, J i is a Fisher matrix, and: The Monte Carlo method is used to solve the numerical solution of formula (6), that is, for the state transfer function and the measurement function, when their mean, variance, and distribution function are known, their second-order derivatives are calculated, and then the above information is used to generate enough sample values, and the sample values are averaged to obtain the approximate solution of the Fisher matrix; Then, the estimated result at the i-th moment is substituted into the state transfer function at the next moment so that the target state at the i+1-th moment satisfies This completes the dynamic estimation process.

2. The Bayesian dynamic estimation method according to claim 1, characterized in that: The specific steps are: (1) The estimated value of the target initial state θ0 is known The variance of the corresponding noise ω0 Set the number of tracking measurements N; (2) For each measurement moment {i|i=1,2,…,N}: (3) Using the state model function θ i =f i-1 (θ i-1 ,ω i-1 ), get the target state θ at the i-th moment i The prior probability density function p(θ i |β 1:i-1 ); (4) By observing the model function β i =h i (θ i ,v i ), we get θ i The posterior probability density function p(β i |θ i ); (5) According to steps (3) and (4), we can get θ i The objective function L i (θ i )=logp(θ i |β 1:i-1 )+logp(β i |θ i ), and use the objective function to i Perform maximum likelihood estimation to obtain and output the estimated value at the i-th moment (6) Calculate the θth i Caramel Limited (7) The estimated value at time i Carat Meilao Limited C i As θ i The mean and variance of Substitute it into the state function at the i+1th moment to satisfy θ i+1 =f i (θ i ,ω i ), thus completing the i+1 Update of the prior probability density.

Images