Pseudo-linear system target tracking method based on dimensionality reduction cubature Kalman filtering

Abstract

The invention discloses a pseudo-linear system target tracking method based on dimensionality reduction cubature Kalman filtering. The method comprises the following steps: step 1, acquiring measurement data for target positioning and tracking; 2, establishing a model of a target tracking and positioning system; 3, solving a target tracking and positioning system model based on a dimensionality reduction volume Kalman filtering algorithm; and 4, updating parameters of the target tracking and positioning system, and obtaining target position information. According to the algorithm, when the volume points are constructed, the volume points are constructed according to the number of elements in the nonlinear subsets of the states, the dimensionality of the system is reduced, the number of the volume points is reduced, the calculation amount is reduced, and therefore the real-time performance of algorithm application is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of target tracking, and in particular to a pseudo-linear system target tracking method based on dimension reduction cubature Kalman filtering. Background Art

[0002] Target tracking and positioning is the process of obtaining real-time estimates of the position, velocity and other motion parameters of the tracked target through measurement data. In the problem of target tracking and positioning, the system state equation is generally a linear equation, while the measurement equation is generally a nonlinear equation. The volumetric Kalman filter algorithm can be used to achieve target tracking and positioning. Although the measurement equation is a nonlinear equation, it can be seen from the modeling of physical laws that the nonlinearity of the nonlinear measurement equation is only caused by part of the state, while the other part of the state is reflected as linear transfer in the system. Therefore, this system can be uniformly converted into a pseudo-linear system model. For pseudo-linear system models, if the volumetric Kalman filter is used for state estimation, it may cause the phenomenon of volume point waste.

[0003] The volumetric Kalman filter is a type of deterministic sampling filter. Compared with the particle filter, its advantage is that the number of sampling points is small, which is convenient for the real-time operation of the algorithm, and thus has engineering application value. In order to obtain higher filtering accuracy, it is necessary to have a higher approximation accuracy for the nonlinear Gaussian weighted integral. Therefore, more volume points are required in the volume criterion, and the number of volume points will increase with the increase of the system dimension. The increase in the number of volume points will lead to an increase in algorithm complexity and a decrease in real-time performance. For high-performance processors, the increase in the number of volume points does not bring too much processing difficulty. However, when the algorithm is deployed on a processor with poor performance, or when the real-time performance of the algorithm is very high, the impact of the number of volume points cannot be ignored. It can be seen that the volumetric Kalman filter method used in the prior art to solve the pseudo-linear system model (such as the target tracking and positioning system) has the problem of increased complexity and decreased real-time performance due to the waste of volume points. Therefore, for this type of pseudo-linear system, how to minimize the number of volume points while keeping the accuracy of the volumetric Kalman filter basically unchanged is a problem that needs to be solved urgently. Summary of the invention

[0004] In view of the above problems, the present invention provides a pseudo-linear system target tracking method based on reduced-dimensional cubature Kalman filtering, which reduces the number of volume points and thus reduces the amount of calculation, thereby improving the real-time performance of the target tracking and positioning system.

[0005] The technical solution adopted by the present invention is as follows:

[0006] A pseudo-linear system target tracking method based on reduced-dimensional cubature Kalman filtering comprises the following steps:

[0007] Step 1: Obtain measurement data for target positioning and tracking;

[0008] Step 2: Establish a model of the target tracking and positioning system;

[0009] Step 3: Run the dimension reduction dimensional Kalman filter algorithm;

[0010] Step 4: Update the target tracking and positioning system parameters and output the target location information.

[0011] Furthermore, the target tracking and positioning system model established in step 2 is:

[0012]

[0013] Among them, x k is the system state, z k is the sensor measurement value, v k is the sensor measurement error, w k-1 is the target motion modeling error, f1 is the system state function 1, f2 is the system state function 2, h1 is the system measurement function 1, and h2 is the system measurement function 2;

[0014] Furthermore, the reduced dimension dimensional Kalman filter algorithm described in step 3 includes the following steps:

[0015] Step 3.1: Initialize the filter to the target state Estimated error covariance initial value and the number of cycles k;

[0016] Step 3.2: Perform time update to obtain the prior covariance matrix;

[0017] Step 3.3: Perform measurement update to obtain the posterior error covariance matrix;

[0018] Step 3.4: Repeat steps 3.2 to 3.3 k times.

[0019] Furthermore, the specific steps of step 3.2 include:

[0020] Step 3.2.1: Split into nonlinear parts and the linear part Right now

[0021] Step 3.2.2: Split into four corresponding matrices, namely

[0022] Step 3.2.3: Use and Calculate volume points according to volume criteria

[0023] Step 3.2.4: Calculate u by the following formula b :

[0024]

[0025] Step 3.2.5: Calculate u by the following formula s :

[0026]

[0027] Step 3.2.6: Let the nonlinear transfer point And Perform weighted summation and calculate the prior state estimate by the following formula

[0028]

[0029] Among them, ω i is the corresponding weight;

[0030] Step 3.2.7: Calculate the matrix M by the following formula k :

[0031]

[0032] Step 3.2.8: Based on the matrix M k , the prior covariance matrix is ​​calculated by the following formula

[0033]

[0034] Furthermore, the specific steps of step 3.3 include:

[0035] Step 3.3.1: Split into nonlinear parts and the linear part Right now

[0036] Step 3.3.2: Convert the prior covariance matrix Split into the corresponding four matrices:

[0037] Step 3.3.3: Use and Calculate volume points according to volume criteria

[0038] Step 3.3.4: Calculate u by the following formula b :

[0039]

[0040] Step 3.3.5: Calculate u by the following formula s :

[0041]

[0042] Step 3.3.6: Let the nonlinear transfer point And Perform weighted summation and calculate the measurement estimate by the following formula

[0043]

[0044] Among them, ω i is the corresponding weight;

[0045] Step 3.3.7: Calculate the matrix N by the following formula k :

[0046]

[0047] Step 3.3.8: Based on the matrix N k Compute the measurement covariance matrix:

[0048]

[0049] Step 3.3.9: Construct vector Calculate the cross covariance matrix P separately xz,k , Kalman filter gain matrix K k , the optimal estimated value of the target position and velocity state at time k and the posterior error covariance matrix

[0050] Furthermore, P in step 3.3.9 xz,k , K k , and The calculation formulas are:

[0051]

[0052] The beneficial effects of the present invention are:

[0053] When constructing volume points, the present invention constructs them according to the number of elements in the nonlinear subset of target position, speed and other states in the target tracking model, thereby reducing the dimension of the system, thereby reducing the number of volume points, reducing the amount of calculation, and improving the real-time performance of tracking and positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1a-1bThey are the positioning RMSE and speed RMSE of the two filtering algorithms respectively.

[0055] Figure 2 Compare the target's true motion trajectory with the target's estimated motion trajectory.

[0056] Figure 3 Comparison results of the number of integration points and running time of the two filtering algorithms. DETAILED DESCRIPTION

[0057] In order to enable those skilled in the art to better understand the technical solution of the present invention, the technical solution of the present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0058] 1. Local nonlinear variable representation of Gaussian weighted integral of pseudo-linear model

[0059] In target tracking and positioning, the measurement equation is a nonlinear equation, and the nonlinearity of the nonlinear measurement equation is caused by the local state, which is a pseudo-linear system model.

[0060] For this type of model, there are:

[0061] The state of the target tracking and positioning system is represented by variable x. Assume that variable x can be decomposed into The nonlinear tracking measurement function g(·) is applied to x. If it can be written as formula (1), then the function g(·) is said to be effective for x. b is pseudo-linear:

[0062] s = g(x) = c(x a )+q(x a )x b (1)

[0063] Among them, x a is a nonlinear subset of the target tracking and positioning system state, x b is a linear subset of the target tracking and positioning system state; and are two subsets of the variable x, and n a +n b =n, c(·) is a nonlinear function, q(·) is a linear function or a nonlinear function;

[0064] Taking the expected value of x, we get:

[0065]

[0066] Among them, Ε(x a ) represents x a The mathematical expectation of Ε(x b ) represents x bThe mathematical expectation of Represents x a The estimated value of Represents x b The estimated value of P a Represents x a The covariance, P b Represents x b The covariance, P ab Represents x a With x b The cross covariance, P x represents the covariance of x.

[0067] From the inverse theorem of the matrix we can get:

[0068]

[0069] Among them, u b , M are intermediate variables for target tracking and positioning calculation.

[0070] Next, we find the expected value of s and get:

[0071]

[0072] Substituting formula (1) into formula (6), we can get:

[0073]

[0074] Among them, u s (x a )=Ε(s|x a )=c(x a )+q(x a ) b (x a ).

[0075] The covariance matrix P of s s Can be expanded to:

[0076]

[0077] The cross covariance matrix P xs Can be expanded to:

[0078]

[0079] Since x follows a Gaussian distribution, some subsets x a It also obeys the Gaussian distribution, that is Therefore, the integral terms in equations (7) to (9) are transformed into the form of "nonlinear function × Gaussian probability density", so when the mean is obtained Covariance Ps and the cross covariance P xs After that, the volume criterion can be used to approximate and obtain the approximate value, thus solving the approximation problem of the posterior probability density of the nonlinear measurement equation in target tracking.

[0080] 2. Construction of pseudo-linear system target tracking method based on reduced dimension cubature Kalman filter

[0081] (1) Rewrite the random dynamic system model

[0082] Rewrite the random dynamic system model in the target tracking and positioning system:

[0083]

[0084] Among them, x k is the target position, speed and other system states, z k is the sensor measurement value, f is the target motion model function, h is the nonlinear measurement model function, w k-1 Modeling error for target motion, v k is the sensor measurement error and k represents the time.

[0085] Assume that both the state equation and the measurement equation are pseudo-linear models. For the state equation, assume that there are m elements in the nonlinear subset, that is:

[0086]

[0087] For the measurement equation, it is assumed that there are p elements in the nonlinear subset, namely:

[0088]

[0089] Based on formula (1), formula (10) is rewritten as formula (13):

[0090]

[0091] Since the system model (10) can be written as the pseudo-linear system model shown in (13), combined with the local nonlinear state integral equations given by (7), (8) and (9), and approximated by the volume criterion, we can obtain P s and P xs This process is called the reduced dimensional cubature Kalman filter (RDCKF).

[0092] The recursive calculation steps of the reduced dimension volumetric Kalman filter algorithm are specifically as follows:

[0093] Step 1: Initialize the filter to the target state Estimated error covariance initial value and the number of cycles k, where k = 1, 2, ...;

[0094] Step 2: Time Update

[0095] ① Split into nonlinear parts and the linear part Right now

[0096] ② Split into four corresponding matrices, namely

[0097] ③ Use separately and Calculate volume points according to volume criteria

[0098] ④Calculation

[0099] ⑤Calculation

[0100] ⑥Let the nonlinear transfer point

[0101] 7. Pair Perform a weighted sum to calculate the prior state estimate ω i is the corresponding weight;

[0102] ⑧Calculation matrix

[0103] ⑨ Calculate the prior covariance matrix:

[0104]

[0105] Step 3: Measurement Update

[0106] ① Split into nonlinear parts and the linear part Right now

[0107] ② Split into four corresponding matrices, namely

[0108] ③ Use separately and Calculate volume points according to the selected volume criterion

[0109] ④Calculation

[0110] ⑤Calculation

[0111] ⑥Let the nonlinear transfer point

[0112] 7. Pair Perform weighted sum calculation to measure the estimate ω i is the corresponding weight;

[0113] ⑧Calculation matrix

[0114] ⑨ Calculate the measurement covariance matrix:

[0115]

[0116] Among them, ω i is the corresponding weight;

[0117] ⑩Construct vector

[0118] Compute the cross-covariance matrix:

[0119]

[0120] Among them, ω i is the corresponding weight;

[0121] Calculate the Kalman filter gain matrix:

[0122] Compute the posterior estimate of the state:

[0123] Compute the posterior error covariance matrix:

[0124] in, is the optimal estimate of the target position and velocity state at time k, that is, the target tracking positioning value.

[0125] Repeat step 2-step 3 k times to obtain the target tracking and positioning value at each moment.

[0126] The above steps are the calculation process of the pseudo-linear system target tracking method based on the dimension-reduced dimensional Kalman filter. When constructing volume points, they are all constructed according to the number of elements in the nonlinear subset of the state. From this perspective, it is equivalent to reducing the dimension of the system, thereby reducing the number of volume points and the amount of calculation, thereby improving the real-time performance of the algorithm application.

[0127] Example

[0128] In order to verify the effect of the dimension reduction cubature Kalman filter algorithm proposed in the present invention, a target uniform turning tracking model is used to verify the performance of the algorithm.

[0129] 1. Target motion equation of target uniform turning tracking model

[0130] x k =F CT x k-1 +G CT w k-1

[0131] in, is the target motion state vector, F CT is the state transfer matrix, G CT is the noise driving matrix, which is shown as follows, w k is the process noise, and its covariance matrix is ​​Q k-1 :

[0132]

[0133] Where, Ω is the target turning angular velocity, T is the sampling interval;

[0134] The measurement equation of the sensor is:

[0135]

[0136] Among them, arctan2 is the four-quadrant inverse tangent function, is the coordinate of the sensor, v k To measure noise;

[0137] In the simulation experiment, it is assumed that the initial position of the target is (x0, y0) = (50m, 2m) and the initial velocity is Angular velocity Ω = 5° / s, ranging accuracy is 2m, angular accuracy is 0.02°, and the sensor coordinates are The simulation time is 40s, and the Monte-Carlo simulation is run 200 times. The standard CKF algorithm and the RDCKF algorithm proposed in this invention are compared, and the RMSE is used to describe the target filtering tracking accuracy. The simulation results are shown in Figures 1-3. Figure 1a-1b It can be seen that CKF and RDCKF obtain almost the same positioning RMSE and fixed speed RMSE, indicating that the target tracking accuracy of the two algorithms is almost the same; Figure 3 It can be seen that the number of volume points required by RDCKF is 4, which is significantly less than the 8 integration points required by CKF, which makes the filtering calculation time of the RDCKF algorithm shorter than that of the CKF algorithm. Figure 2 It can be seen that the real motion trajectory of the target is basically consistent with the tracking and positioning trajectory, which proves that the present invention has a good tracking and positioning effect.

[0138] In order to quantitatively describe the filter estimation accuracy of the two algorithms, the average RMSE results are statistically calculated, as shown in Table 1. It can be seen from Table 1 that within the accuracy range, the average positioning RMSE and the average fixed speed RMSE of the two filters are completely consistent, and compared with the CKF algorithm, RDCKF reduces the running time by 7.56%, which shows that the RDCKF proposed in the present invention can reduce the filter calculation time, improve the real-time performance of the algorithm, and effectively maintain the filtering accuracy of CKF.

[0139] Table 1 Average RMSE of state estimation for two filtering algorithms

[0140]

[0141] The above shows and describes the basic principles, main features and advantages of the present invention. It should be understood by those skilled in the art that the present invention is not limited to the above embodiments, and the above embodiments and descriptions are only for explaining the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention to be protected. The scope of protection of the present invention is defined by the attached claims and their equivalents.

Claims

1. A pseudo-linear system target tracking method based on reduced-dimensional cubature Kalman filtering, characterized in that: The following steps are involved: Step 1: Obtain measurement data for target positioning and tracking; Step 2: Establish a model of the target tracking and positioning system; Step 3: Run the dimension reduction dimensional Kalman filter algorithm; Step 4: Update the target tracking and positioning system parameters and output the target location information.

2. The pseudo-linear system target tracking method based on reduced dimension cubature Kalman filtering according to claim 1, characterized in that: The target tracking and positioning system model established in step 2 is: Among them, x k is the system state, z k is the sensor measurement value, v k is the sensor measurement error, w k-1 is the target motion modeling error, f1 is the system state function 1, f2 is the system state function 2, h1 is the system measurement function 1, and h2 is the system measurement function 2.

3. The pseudo-linear system target tracking method based on reduced dimension dimensional Kalman filtering as claimed in claim 2, characterized in that: The reduced dimension volumetric Kalman filter algorithm described in step 3 includes the following steps: Step 3.1: Initialize the filter to the target state Estimated error covariance initial value and the number of cycles k; Step 3.2: Perform time update to obtain the prior covariance matrix; Step 3.3: Perform measurement update to obtain the posterior error covariance matrix; Step 3.4: Repeat steps 3.2 to 3.3 k times.

4. The pseudo-linear system target tracking method based on reduced dimension dimensional Kalman filtering as claimed in claim 3, characterized in that: The specific steps of step 3.2 include: Step 3.2.1: Split into nonlinear parts and the linear part Right now Step 3.2.2: Split into four corresponding matrices, namely Step 3.2.3: Use and Calculate volume points according to volume criteria Step 3.2.4: Calculate u by the following formula b : Step 3.2.5: Calculate u by the following formula s : Step 3.2.6: Let the nonlinear transfer point And Perform weighted summation and calculate the prior state estimate by the following formula Among them, ωi is the corresponding weight; Step 3.2.7: Calculate the matrix M by the following formula k : Step 3.2.8: Based on the matrix M k , the prior covariance matrix is ​​calculated by the following formula 5. The pseudo-linear system target tracking method based on reduced dimension cubature Kalman filtering as claimed in claim 4, characterized in that: The specific steps of step 3.3 include: Step 3.3.1: Split into nonlinear parts and the linear part Right now Step 3.3.2: Convert the prior covariance matrix Split into the corresponding four matrices: Step 3.3.3: Use and Calculate volume points according to volume criteria Step 3.3.4: Calculate u by the following formula b : Step 3.3.5: Calculate u by the following formula s : Step 3.3.6: Let the nonlinear transfer point And Perform weighted summation and calculate the measurement estimate by the following formula Among them, ωi is the corresponding weight; Step 3.3.7: Calculate the matrix N by the following formula k : Step 3.3.8: Based on the matrix N k Compute the measurement covariance matrix: Step 3.3.9: Construct vector Calculate the cross covariance matrix P separately xz,k , Kalman filter gain matrix K k , the optimal estimated value of the target position and velocity state at time k and the posterior error covariance matrix 6. The pseudo-linear system target tracking method based on reduced-dimensional cubature Kalman filtering as claimed in claim 5, characterized in that: P in step 3.3.9 xz,k , K k , and The calculation formulas are:

Images