Track prediction method based on improved Kalman filtering

Abstract

The invention provides a trajectory prediction method based on improved Kalman filtering, which can improve reaction time and prediction accuracy. Comprising the following steps: step 1, substituting a position velocity vector of a target object and a noise vector possibly causing the state change of the target object at the current moment into a state prediction equation to obtain the target velocity and position information at the next moment; 2, substituting the state transition matrix into a covariance prediction equation to obtain an error covariance matrix; 3, performing Kalman gain according to the error covariance matrix obtained in the step 2 to obtain a Kalman gain matrix; 4, performing state updating according to the Kalman gain matrix obtained in the step 3 to obtain a state updating equation; 5, substituting the state updating equation into the covariance matrix in the step 2 for updating, and recalculating the covariance matrix; and step 6, obtaining a trajectory prediction state vector from the state xk obtained in the step 4 based on a BP neural network and a Kalman gain equation.

Description

Technical Field

[0001] The present invention relates to a target tracking prediction technology, and in particular to a trajectory prediction method based on an improved Kalman filter and fused with a BP neural network. Background Art

[0002] Target tracking prediction is a typical problem in the field of engineering technology. It is a method for predicting the track / motion trajectory of an observed object (target). By observing and recording the current and past spatial position information of the target, the trajectory prediction method is used to predict the spatial motion trajectory of the target at the next moment. In order to accurately estimate the motion trajectory of the target, so as to implement simple trajectory correction and predict the flight time of the projectile, the Kalman filter method is applied, and at the same time, the BP neural network prediction model is added in combination with the adaptive method to establish an adaptive Kalman filter model, thereby inferring the target's motion position at the next moment and the projectile's flight time.

[0003] The power system of current objects is constantly updated. The flight of objects is no longer a simple linear motion. The acceleration and turning of objects are faster, and it is becoming more and more difficult to predict the trajectory of a target object. Kalman filtering is essentially a data fusion algorithm, which fuses data with the same measurement purpose, from different sensors, and (possibly) with different units to obtain a more accurate target measurement value. Its biggest advantage is that it has a small amount of calculation and can use the state of the previous moment (and possible measurement values) to obtain the optimal estimate of the state at the current moment. However, the limitation of Kalman filtering is that it can only fit linear Gaussian systems.

[0004] Therefore, there is an urgent need to introduce BP neural network into adaptive Kalman filtering, which is different from the typical Kalman filter application technology, so as to achieve more accurate prediction accuracy. Summary of the invention

[0005] The present invention proposes a trajectory prediction method based on an improved Kalman filter, which can improve reaction time and prediction accuracy.

[0006] The present invention is achieved through the following technical solutions.

[0007] A trajectory prediction method based on an improved Kalman filter comprises the following steps:

[0008] Step 1: Substitute the position and velocity vector of the target object and the noise vector that may cause the state change of the target object at the current moment into the state prediction equation to obtain the target velocity and position information at the next moment;

[0009] Step 2: Substitute the state transfer matrix into the covariance prediction equation to obtain the error covariance matrix;

[0010] Step 3: Perform Kalman gain according to the error covariance matrix obtained in step 2 to obtain a Kalman gain matrix;

[0011] Step 4: Update the state according to the Kalman gain matrix obtained in step 3 to obtain the state update equation;

[0012] Step 5: Substitute the state update equation into the covariance matrix in step 2 to update and recalculate the covariance matrix;

[0013] Step 6: The state x obtained in step 4 k The trajectory prediction state vector is obtained based on the BP neural network and the Kalman gain equation;

[0014] y k =(x k +B k ) / 2

[0015] Among them B k is the state vector predicted by the BP neural network, y k is the state vector predicted for the obtained trajectory.

[0016] Beneficial effects of the present invention:

[0017] 1. Compared with the traditional Kalman filter method which does not consider the Kalman gain, that is, only has the state prediction equation, that is, in the prediction of the motion trajectory, each step takes a constant value, the present invention updates the prediction value of each step according to the observation data, which can achieve higher prediction accuracy and more accurate prediction values ​​for the speed and acceleration of the target object;

[0018] 2. Kalman filtering is suitable for linear trajectory prediction and calibration, but not for nonlinear problem prediction. BP neural network is suitable for large data volume problems. Combining the two can solve both problems and predict nonlinear small data volume problems.

[0019] 3. The present invention adds BP neural network model prediction to the adaptive Kalman filter model and takes the average as the true prediction value, which can improve the reaction time and prediction accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 This is a flow chart of a trajectory prediction method based on an improved Kalman filter according to the present invention;

[0021] Figure 2 This is a schematic diagram of Embodiment 1 of the present invention;

[0022] Figure 3 This is a schematic diagram of Embodiment 2 of the present invention when only Kalman filtering is considered;

[0023] Figure 4 This is a schematic diagram of Example 2 of the present invention combined with a BP network. DETAILED DESCRIPTION

[0024] The exemplary embodiments of the present invention are described in detail below with reference to the accompanying drawings. It should be understood that the embodiments shown and described in the accompanying drawings are only exemplary and are intended to illustrate the principles and spirit of the present invention, rather than to limit the scope of the present invention.

[0025] like Figure 1 As shown, the trajectory prediction method based on the improved Kalman filter of the present invention specifically includes the following steps:

[0026] Step 1: Substitute the position and velocity vector of the target object and the noise vector that may cause the state change of the target object at the current moment into the state prediction equation to obtain the target velocity and position information at the next moment;

[0027] In this embodiment, the state prediction equation is specifically:

[0028]

[0029] Among them, F k|k-1 is the state transfer matrix, B k-1 is the control matrix, u k-1 is the control vector, w k is the noise vector, x k is the system state vector; the system state vector describes the position, velocity, acceleration and other information of the system;

[0030] In a specific implementation, the noise vector that may cause the target object state to change includes but is not limited to the current wind factor.

[0031] Step 2: Substitute the state transfer matrix into the covariance prediction equation to obtain the error covariance matrix;

[0032] In this embodiment, the covariance prediction equation is specifically:

[0033]

[0034] Among them, F k|k-1 is the state transfer matrix, P k|k-1 is x k|k-1 The corresponding error covariance matrix represents the uncertainty of the state estimate, Q k-1 is the state transfer (prediction noise) covariance matrix;

[0035] The covariance prediction equation is introduced in this step. The main purpose is to introduce the adaptive method into the Kalman filter method. By introducing the adaptive method, different weights are given to the observed data and the formula predicted data, so that the prediction can be more accurate.

[0036] Step 3: Perform Kalman gain according to the error covariance matrix obtained in step 2 to obtain a Kalman gain matrix;

[0037] In this embodiment, the Kalman gain equation is specifically:

[0038]

[0039] Where R is the bias matrix, which assigns different weights; K is the Kalman gain matrix at time k, which represents the proportion of actual observations in correcting predicted values.

[0040] Step 4: Update the state according to the Kalman gain matrix obtained in step 3 to obtain the state update equation;

[0041] In this embodiment, the state update equation is specifically:

[0042]

[0043] Where Z k is the state observation vector;

[0044] Step 5: Substitute the state update equation into the covariance matrix in step 2 to update and recalculate the covariance matrix, that is,

[0045] P k =P k|k-1 -KH k P k|k-1 (5)

[0046] It can be seen that after this step, the observed speed and acceleration are substituted into the covariance prediction of equation (2), so that equations (3)(4)(5) can be further calculated, and then the state prediction equation can be updated.

[0047] Step 6: The state x obtained in step 4 k The trajectory prediction state vector is obtained based on the BP neural network and the Kalman gain equation;

[0048] y k =(x k +B k ) / 2

[0049] Among them B k is the state vector predicted by the BP neural network, y k is the state vector predicted for the obtained trajectory.

[0050] Embodiment 1:

[0051] Predictive linear problem x = 1 / 2*u*t**2

[0052] like Figure 2 As shown, by adding Kalman filtering and BP neural network, the one-dimensional data can be predicted more accurately.

[0053] The important part of the program code in Case 1 is as follows

[0054]

[0055]

[0056] Example 2

[0057] Predicting nonlinear problems x = (1 / 2*u*t**2)sin(t)

[0058] If only the Kalman filter method is considered without combining it with the BP neural network prediction, the prediction results are as follows: Figure 3 shown.

[0059] The prediction result after the present invention combines the Kalman filter method with the BP neural network prediction is as follows: Figure 4 As shown. It can be seen that, through this method, both linear and nonlinear problems can be well predicted.

[0060] Those skilled in the art will appreciate that the embodiments described herein are intended to help readers understand the principles of the present invention, and should be understood that the protection scope of the present invention is not limited to such specific statements and embodiments. Those skilled in the art can make various other specific variations and combinations that do not deviate from the essence of the present invention based on the technical revelations disclosed by the present invention, and these variations and combinations are still within the protection scope of the present invention.

Claims

1. A trajectory prediction method based on improved Kalman filtering, characterized in that: The following steps are involved: Step 1: Substitute the position and velocity vector of the target object and the noise vector that may cause the state change of the target object at the current moment into the state prediction equation to obtain the target velocity and position information at the next moment; Step 2: Substitute the state transfer matrix into the covariance prediction equation to obtain the error covariance matrix; Step 3: Perform Kalman gain according to the error covariance matrix obtained in step 2 to obtain a Kalman gain matrix; Step 4: Update the state according to the Kalman gain matrix obtained in step 3 to obtain the state update equation; Step 5: Substitute the state update equation into the covariance matrix in step 2 to update and recalculate the covariance matrix; Step 6: The state x obtained in step 4 k The trajectory prediction state vector is obtained based on the BP neural network and the Kalman gain equation; y k =(x k +B k ) / 2 Among them B k is the state vector predicted by the BP neural network, y k is the state vector predicted for the obtained trajectory.

2. A trajectory prediction method based on improved Kalman filtering as claimed in claim 1, characterized in that: The state prediction equation in step 1 is specifically: Among them, F k|k-1 is the state transfer matrix, B k-1 is the control matrix, u k-1 is the control vector, w k is the noise vector, x k is the system state vector.

3. A trajectory prediction method based on improved Kalman filtering as claimed in claim 1 or 2, characterized in that: The covariance prediction equation in step 2 is specifically: Among them, F k|k-1 is the state transfer matrix, P k|k-1 is x k|k-1 The corresponding error covariance matrix represents the uncertainty of the state estimate, Q k-1 is the state transfer covariance matrix.

4. A trajectory prediction method based on improved Kalman filtering as described in claim 1 or 2, characterized in that: The Kalman gain equation in step 3 is specifically: Where R is the bias matrix, which assigns different weights; K is the Kalman gain matrix at time k, which represents the proportion of actual observations in correcting predicted values.

5. A trajectory prediction method based on improved Kalman filtering as claimed in claim 1 or 2, characterized in that: The state update equation in step 4 is specifically: Where Z k is the state observation vector.

Images