Robust ellipse expansion target tracking method based on variable center maximum correlation entropy criterion

Abstract

The invention discloses a robust ellipse extended target tracking method based on a variable center maximum correlation entropy criterion, which is used for solving the problem of insufficient tracking precision when an abnormal value exists in radar measurement data in the prior art. Firstly, kernel parameters are estimated, then the motion state is updated, robust estimation of the motion state of an extended target is achieved, shape features are represented through a covariance matrix of radar measurement data, initial estimation of the shape features is obtained, and then recursive updating is carried out under a Kalman filtering framework; according to the method, the variable center maximum correlation entropy criterion is innovatively combined with Kalman filtering, the influence of abnormal values in radar measurement data is effectively suppressed by using a self-adaptive kernel center adjustment mechanism of the variable center maximum correlation entropy criterion, and good robustness is shown.

Description

TECHNICAL FIELD

[0001] The present application relates to a target tracking technology, in particular to a robust ellipse extended object tracking method based on a variable center maximum correlation entropy criterion. BACKGROUND

[0002] Target tracking technology is a basic and key technology in many military and civilian fields such as national defense, autonomous driving, intelligent transportation, and disaster rescue. In traditional target tracking methods, the target is usually simplified as a point, ignoring its extended shape features, and only relying on the measurement data collected at different times to estimate the motion state of the target, such as position and velocity, through Kalman filtering (KF) and other means. However, with the widespread application of modern high-precision sensors, such as millimeter wave radar, laser radar, and high-resolution cameras, multiple measurements on the target can be obtained in a single scan. These measurements can not only be used to estimate the motion state of the target, but also further deduce the shape features of the target, such as size, attitude, and geometric contour. In many application scenarios, the extended shape features of the target play a crucial role in accurate perception and decision-making. For example, in an autonomous driving system, accurately estimating the length, width, and heading angle of a vehicle can significantly improve the accuracy of driving intent prediction; in the field of military reconnaissance, identifying the geometric features of a target can effectively support target classification and threat assessment. In addition, in scenarios where targets form dense formations, such as air refueling formations, sensors may not be able to distinguish individual targets, making it difficult to establish a one-to-one correspondence between targets and measurements. In this case, it is more reasonable to consider the target as an extended target with both motion features and extended shape features. Therefore, it is particularly necessary to develop an extended object tracking (EOT) method based on multiple measurements to simultaneously estimate the motion features and shape features of the target.

[0003] Over the past few decades, EOT methods have been extensively studied. However, this method still faces the following technical challenges:

[0004] Measurement data sparsity and outlier interference: millimeter wave radar, laser radar, and other sensors may only obtain a small number of measurements in a single scan, and the number and spatial distribution of measurements will change dynamically over time. In addition, affected by noise, occlusion, or multipath effects, outliers are often contained in the measurement data. Traditional EOT methods based on least squares or Gaussian assumptions (such as random matrix methods) are sensitive to outliers, which can cause deviations in shape feature estimation.

[0005] The contradiction between the complexity of shape modeling and the real-time requirement of algorithm: The geometric shape of the extended target is usually approximated by models such as ellipses, rectangles, or star-shaped convex bodies. However, in a dynamic environment, the target may change shape, such as the profile change when the vehicle turns. Existing methods, such as particle filtering, can handle complex extended shape features, but have high computational complexity and are difficult to meet real-time requirements.

[0006] Therefore, it is urgent to develop an extended target tracking scheme that takes into account robustness, accuracy and computational efficiency. Among various shape modeling methods, the ellipse / ellipsoid approximation hypothesis is widely used due to its wide applicability and computational efficiency. Based on the ellipse approximation and different physical extension modeling methods, existing ellipse hypothesis extended target tracking methods mainly include particle filtering method, random hyper-surface model method, random matrix method and other derivative methods. These methods have made significant progress in the field of extended target tracking, but still have some technical defects, as follows:

[0007] The particle filtering method represents the state distribution of the target by a large number of particles, has high computational complexity, and is difficult to meet real-time requirements; in practical applications, particle filtering is prone to sample degeneracy, i.e. most particles have weights close to zero, only a few particles have effective weights, leading to a decline in tracking accuracy; particle filtering method usually assumes that the measurement error is centered on zero, and for non-zero center distribution of measurement error, its adaptability is poor, which is easy to cause tracking deviation.

[0008] The random hyper-surface model method needs to model the geometric shape of the target, which has high computational complexity and is difficult to process in real time; in a dynamic environment, the geometric shape of the target may change (such as profile change when the vehicle turns), and the random hyper-surface model method is difficult to quickly adapt to these changes, leading to a decline in tracking accuracy; the random hyper-surface model method usually assumes that the measurement error is centered on zero, and for non-zero center distribution of measurement error, its tracking accuracy will be affected.

[0009] The random matrix method is based on least squares or Gaussian assumption, and is sensitive to outliers in measurement data, which may lead to shape feature estimation deviation; the random matrix method usually assumes that the measurement error is centered on zero, and for non-zero center distribution of measurement error, its adaptability is poor, which is easy to cause tracking accuracy decline.

[0010] Although other derivative methods perform well in some specific scenarios, they lack a unified framework and are difficult to maintain consistent performance in a variety of complex scenarios; many derivative methods perform poorly when dealing with dynamic changes in target shape, making it difficult to quickly adapt to changes in target shape, leading to a decline in tracking accuracy; the derivative method usually assumes that the measurement error is centered on zero, and for non-zero center distribution of measurement error, its adaptability is poor, which is easy to cause tracking deviation.

[0011] In summary, most existing methods assume that the measurement error is centered at zero, which is not always true in practical applications, and the non-zero centered distribution of measurement error will cause the tracking accuracy of these methods to decrease or even fail to track; existing methods lack the ability to dynamically adjust the error center, and cannot effectively handle non-zero centered distribution of measurement error, thereby affecting tracking accuracy and robustness; many existing methods have high computational complexity, making it difficult to meet real-time requirements, especially when dealing with high-dimensional data; existing methods are sensitive to outliers in measurement data, which can cause shape feature estimation to deviate, thereby affecting tracking accuracy; in a dynamic environment, the shape of the target may change, and existing methods are difficult to quickly adapt to these changes, resulting in decreased tracking accuracy. SUMMARY

[0012] The technical problem to be solved by the present application is to provide a robust ellipse extended target tracking method based on a variable center maximum correlation entropy criterion, which can better adapt to non-zero centered distribution of measurement error by jointly estimating kernel bandwidth and kernel center based on the variable center maximum correlation entropy criterion, thereby improving tracking accuracy and robustness.

[0013] The technical solution adopted by the present application to solve the above technical problems is: a robust ellipse extended target tracking method based on a variable center maximum correlation entropy criterion, characterized by the following steps:

[0014] Step 1: In an extended target tracking system, for an extended target with an approximate elliptical shape, its tracking model includes three parts: unknown parameterization, measurement model and dynamic model, wherein the extended target is deployed in a two-dimensional coordinate system;

[0015] Unknown parameterization: the motion state r of the extended target at time k k is composed of position u k and velocity v k , the shape feature p of the extended target at time k k is composed of the angle of rotation α around the x-axis of the two-dimensional coordinate system counterclockwise k and the length and short semi-axis length vector , wherein the initial value of k is 1;

[0016] Measurement model: at time k, the radar obtains the two-dimensional Cartesian coordinates of a set of reflection points on the extended target, these coordinates are taken as measurement values, the jth measurement value obtained at time k is denoted as and the measurement model of is established, wherein j=1,...,J k , J k represents the number of measurement values obtained at time k;

[0017] Dynamic model: assuming that the extended target moves at a constant speed in a straight line within a short observation period, the dynamic model is described as: wherein A k represents the transition matrix at time k, t represents the sampling interval, λ k and ψ k are the process noises at time k, λ k obeys the Gaussian distribution with zero mean and covariance matrix Λ, ψ k obeys the Gaussian distribution with zero mean and covariance matrix Ψ, r k-1 represents the initial motion state of the extended target, p k-1 represents the initial shape feature of the extended target, r k-1 represents the motion state of the extended target at time k-1, p k-1 represents the shape feature of the extended target at time k-1.

[0018] Step 2: based on the dynamic model, one-step prediction is performed on r k and p k to obtain one-step predicted value k of r and one-step predicted value of p k , and further obtain the prediction error covariance matrix of r and the prediction error covariance matrix of p Then, the measurement errors in the measurement models of the prediction errors and are whitened, and the whitened prediction error ξ k and the whitened measurement error are obtained, wherein the whitening process of the prediction error ξ uses the measurement error in the measurement model of r the whitening process of the prediction error ξ

[0019] Step 3: assuming that the estimated value k-1 of r k is an unbiased estimate, based on the assumption, the estimated value of the kernel center of ξ k is set to zero, and the estimated value of the kernel bandwidth of ξ k is set to the standard deviation of ξ k，(n) .

[0020] Step 4: based on the kernel density estimation theory, the kernel bandwidth σ k，(n) of the nth component of r is constructed.and the core center c k，(n) The joint estimation optimization problem is given by the expression $\mathbf{a}$, where $n \in {1, 2}$. The joint estimation optimization problem is decomposed into a kernel bandwidth $σ$. k，(n) The original problem and kernel center c of the estimation subproblem k，(n) Estimate the original problem of the subproblem; then calculate the kernel bandwidth σ. k，(n) The original problem of estimating the subproblem is transformed into kernel bandwidth σ. k，(n) Estimating the convexity of the subproblems, with the kernel center c k，(n) The original problem of estimating subproblems is transformed into the core-center problem c. k，(n) The estimation of the augmented optimization problem of the subproblem, wherein the augmented optimization problem includes... The weight is the center error weight. express The estimated value; then the core center c k，(n) The augmented optimization problem of estimating the subproblem is decomposed into a subproblem concerning the kernel center and a subproblem concerning the center error weights;

[0021] Step 5: Propose a criterion for r based on the maximum correlation entropy criterion of the variable center. k The cost function, and the known σ k，(n) and c k，(n) Substitute about r k The cost function is obtained by knowing σ. k，(n) and c k，(n) The optimization problem based on the maximum correlation entropy criterion with varying center is then categorized. This problem is then equivalently transformed into an augmented optimization problem, which includes prediction error weights and measurement error weights. Finally, the augmented optimization problem is decomposed into a function relating r. k Sub-problems concerning prediction error weights and measurement error weights;

[0022] Step 6: Obtain the results using an alternating iterative strategy. The nth component kernel bandwidth σ k，(n) The estimated value and the core center c k，(n) The estimated value and r k The estimated value Specifically, in each outer iteration, for σ... k，(n) Directly solve for the kernel bandwidth σ k，(n) Estimate the convexity of the subproblems to obtain σ at each outer iteration. k，(n) The optimal solution for c; k，(n) During the inner iteration process, the subproblem concerning the kernel center and the subproblem concerning the center error weight are solved alternately. When the first inner iteration condition is satisfied, the value of c in each outer iteration is obtained. k，(n) The optimal solution for r; kDuring the inner iteration process, solutions for r are solved alternately. k The subproblems concerning prediction error weights and measurement error weights, when satisfying the second inner-layer iteration condition, yield r for each outer-layer iteration. k The optimal solution, and r k The estimation error covariance matrix of the optimal solution; σ is obtained when the outer iteration conditions are satisfied. k，(n) The estimated value and c k，(n) The estimated value and r k The estimated value And thus obtain The estimation error covariance matrix R k Then, through the state transition equation, and R k Convert to r respectively k+1 One-step prediction value and its prediction error covariance matrix

[0023] Step 7: Based on the expanded target and Corresponding measurement source The corresponding random vector z k covariance matrix approximation Obtain α k rough estimate Extend the length of the semi-major axis of the target at time k rough estimate Extend the length of the minor semi-axis of the target at time k rough estimate Then obtain p k rough estimate Then calculate The estimation error covariance matrix Then, within the framework of Kalman filtering, update and obtain p. k The estimated value The Kalman gain corresponding to the Kalman filter equation includes...

[0024] Step 8: Following the process of Step 6 and Step 7, obtain the estimated values ​​of the motion state and shape features of the extended target at different times to achieve extended target tracking, where the different times are multiple consecutive times.

[0025] In step 1, Where, r k The dimension is 4×1, p k The dimension is 3×1. a vector composed of the length of the long semi-axis and the length of the short semi-axis of the extended target at time k, superscript "T" represents the transpose of a vector or a matrix; The measurement model is described as: wherein, represents the measurement source corresponding to at time k, u k = Hr k , H represents a selection matrix for extracting u k from r k , H = [I2, 0], I2 represents a 2-order unit matrix, 0 represents a zero matrix with a dimension of 2x2, and represent the length of the long semi-axis and the length of the short semi-axis of the extended target at time k, respectively, represents a corresponding multiplicative noise vector, which is subject to a Gaussian distribution with a mean of zero and a covariance matrix of , is the first element of , is the second element of , and are random quantities for controlling the uniform distribution of the corresponding reflection points in the range of the extended target, represents measurement noise in , which is subject to a Gaussian distribution with a mean of zero and a noise covariance matrix of C n , is the measurement error in the measurement model of .

[0026] In the step 2, wherein, when k = 1, r k-1 , when k > 1, represents the estimated value of r k-1 , when k = 1, p k-1 , when k > 1, represents the estimated value of p k-1 , represents an expected operation, R k-1 represents the estimated error covariance matrix of , Ξ k-1 represents the estimated error covariance matrix of ;

[0027] The acquisition process of k is as follows: for Perform Cholesky decomposition to obtain And thus obtain Where, ξ k The dimension is 4×1;

[0028] The acquisition process is as follows: exist Perform a first-order Taylor expansion at this point: in, Indicates will Substitute into S k V was obtained from 1，k Indicates according to S k The first line S k，(1，：) The calculated Jacobian matrix, Indicates will Substitute into V 1，k V was obtained from 2，k Indicates according to S k The second line S k，(2，：) The calculated Jacobian matrix, Indicates will Substitute into V 2，k Obtain from; then The covariance matrix is ​​represented as M k , Will The covariance matrix is ​​represented as F k , Among them, F k The element F with index (i, t) (i,t) pass We calculate that i, t∈{1, 2}, and tr{·} represents finding the trace of the matrix; then we obtain... The relevant measurement error covariance matrix Q k , Then Q k Perform Cholesky decomposition to obtain And thus obtain The nth component is in, The dimension is 2×1, Π k，(n，：) Represents Π k The nth line.

[0029] In step 4, the joint estimation optimization problem is described as follows: in, express The estimated value, after obtaining r kThe estimated value Under the premise Π k，(n，：) Represents Π k The nth line, Q k express The relevant measurement error covariance matrix, H represents the variance from r k Extract u k The selection matrix;

[0030] In step 4, the kernel bandwidth σ k，(n) The process of obtaining the convex problem of estimating subproblems is as follows: fix the kernel center c. k，(n) The value, use this value as And let The joint estimation optimization problem is transformed into a kernel bandwidth σ problem. k，(n) The original problem for estimating subproblems is described as follows: Then, the kernel bandwidth σ k，(n) The original problem of estimating the subproblem is subjected to a Taylor expansion, and its third-order terms are preserved to ensure the accuracy of the approximation, resulting in the Taylor expansion, which is used as the kernel bandwidth σ. k，(n) The convex problem of estimating subproblems is described as follows: in,

[0031] In step 4, the core center c k，(n) The process of obtaining the augmented optimization problem for estimating subproblems is as follows: fix the kernel bandwidth σ k，(n) The value, use this value as The joint estimation optimization problem is transformed into a kernel-centric c problem. k，(n) The original problem for estimating subproblems is described as follows: Then, based on the properties of convex conjugate functions, the kernel center c is... k，(n) The original problem of estimating subproblems is transformed into the core-center problem c. k，(n) The augmented optimization problem for estimating subproblems is described as follows: in, express The weights are the center error weights, and φ(·) represents the convex conjugate function of the exponential function;

[0032] In step 4, the process of obtaining the sub-problem concerning the kernel center is as follows: [Fixed] The value, denoted as The augmented optimization problem is transformed into a subproblem concerning the kernel center, described as follows:

[0033]

[0034] In step 4, the process of obtaining the sub-problem regarding the center error weight is as follows: fix the kernel center c. k，(n) The value, denoted as The augmentation optimization problem is transformed into a subproblem concerning the center error weights, described as follows:

[0035]

[0036] The specific process of step 5 is as follows:

[0037] Step 5.1: Fix the core bandwidth σ k，(n) The value, use this value as And fix the core center c k，(n) The value, use this value as Then, based on the maximum correlation entropy criterion of the variable center, a proposal is made regarding r. k The cost function is described as follows: in, Indicates about r k The cost function, The kernel function in the equation is a Gaussian kernel function, L = 4 + 2J. k ||·|| denotes the 2-norm, and G1 denotes a kernel function with a kernel bandwidth of 1. Indicates the kernel bandwidth as The kernel function, b k，(i) Indicates the relationship with r k The relevant zero center error vector b k The i-th element in

[0038] Step 5.2: Let Regarding r k The cost function is transformed into a known σ k，(n) and c k，(n) The cost function is described as follows: in, Indicates that σ is known k，(n) and c k，(n) The cost function, Indicates the kernel bandwidth as Gaussian kernel function, ξ k，(m) Indicates ξ k The m-th element, Γ k，(m，：) Indicates Γ k The m-th line;

[0039] Step 5.3: [The sentence is incomplete and requires more context to be translated accurately.] and Substitution In the middle, construct a known σ k，(n) and c k，(n)the optimization problem based on the variable center maximum correlation entropy criterion is described as:

[0040]

[0041] Step 5.4: the optimization problem is equivalently converted into an augmented optimization problem, which is described as: wherein, κ m is a prediction error weight, is a measurement error weight, and φ(·) represents a convex conjugate function of an exponential function,

[0042] Step 5.5: the values of κ m and are fixed, and the two values are denoted as and the augmented optimization problem is converted into a sub-problem about r k , which is described as: the sub-problem about r k is equivalently converted into a linear least square problem about r k , which is described as:

[0043] the value of r k is fixed, and the value is denoted as the augmented optimization problem is converted into a sub-problem about a prediction error weight and a measurement error weight, which is described as: wherein,

[0044] the process of the step 6 is:

[0045] Step 6.1: let s represent the number of outer iterations, and the initial value of s is 1;

[0046] Step 6.2: in the s-th outer iteration process, the values of and are substituted into the convex problem of the kernel bandwidth σ k，(n) estimation sub-problem, and the optimal solution of μ k，(n) in the s-th outer iteration is obtained, that is, the optimal solution of σ k，(n) in the s-th outer iteration is obtained wherein, when s = 1, is the initial value of c k，(n) , is the initial value of r k , is set to 0, is obtained by setting c k，(n) to 0, setting σ k，(n) to 1, setting κ m to 1, and setting are all set to -1 and substituted into s>1, the optimal solution of c s-1, the optimal solution of r k，(n) s-1, the optimal solution of c s-1, the optimal solution of r k s-1, the optimal solution of c

[0047] Step 6.3: In the s-th outer iteration, inner iteration is performed on c k，(n) , which is as follows:

[0048] Step 6.3.1: Let q represent the number of inner iteration, and the initial value of q is 1;

[0049] Step 6.3.2: In the q-th inner iteration in the s-th outer iteration, substitute and into the sub-problem about the kernel center, and the optimal solution of c k，(n) in the q-th inner iteration in the s-th outer iteration is obtained wherein, when q=1, is the initial value of c at the beginning of inner iteration in the s-th outer iteration is set to -1, and when q>1, s-1, the optimal solution of c in the q-1-th inner iteration in the s-th outer iteration, is obtained by substituting into ;

[0050] In the q-th inner iteration in the s-th outer iteration, substitute and into the sub-problem about the center error weight, and the optimal solution of r in the q-th inner iteration in the s-th outer iteration is obtained

[0051] Step 6.3.3: Determine whether the first inner iteration condition is met, if met, take as the optimal solution of c k，(n) in the s-th outer iteration and execute Step 6.4 again, if not met, let q=q+1, and then return to Step 6.3.2 for continuous execution, wherein, when q=1, is the initial value of c k，(n) at the beginning of inner iteration in the s-th outer iteration is equal to ​denotes the optimal solution of r k，(n) at the qth inner iteration in the st outer iteration;

[0052] Step 6.4: in the st outer iteration, inner iteration is performed on r k , as follows:

[0053] Step 6.4.1: let q denote the number of inner iterations, and the initial value of q is 1;

[0054] Step 6.4.2: in the qth inner iteration in the st outer iteration, substitute and into the linear least squares problem about r k , to obtain the optimal solution of r k at the qth inner iteration in the st outer iteration wherein, when q = 1, is the initial value of κ m at the beginning of inner iteration in the st outer iteration is the initial value of at the beginning of inner iteration in the st outer iteration and are both set as -1, when q > 1, denotes the optimal solution of κ m at the q-1th inner iteration in the st outer iteration, denotes the optimal solution of at the q-1th inner iteration in the st outer iteration, denotes the Kalman gain, then the estimation error covariance matrix of is calculated wherein, I denotes the unit matrix;

[0055] in the qth inner iteration in the st outer iteration, substitute into the sub-problem about the prediction error weight and the measurement error weight, to obtain the optimal solution of κ m at the qth inner iteration in the st outer iteration and the optimal solution of at the qth inner iteration in the st outer iteration wherein,

[0056] Step 6.4.3: judge whether the second inner iteration condition is established, if yes, take as the optimal solution of r k at the st outer iteration Let be the estimate error covariance matrix of Step 6.5 is performed again, if not, let q = q + 1, then return to step 6.4.2 to continue, where q = 1 is the initial value of r k at the beginning of the inner iteration in the s-th outer iteration process is equal to represents the optimal solution of r k at the (q-1)-th inner iteration in the s-th outer iteration;

[0057] Step 6.5: judge whether the outer iteration condition is established, if yes, let correspond to the estimate value of k，(n) the estimate value of k，(n) the estimate value of k then let be the estimate error covariance matrix of R k , and convert and R k to the one-step prediction value of r k+1 and its prediction error covariance matrix R if not, let s = s + 1, then return to step 6.2 to continue, where the outer iteration condition is whether s is less than or equal to the preset number of times.

[0058] The specific process of the step 7 is as follows:

[0059] Step 7.1: let z k represent the corresponding random vector, let y k represent the corresponding random vector, let represent the covariance matrix of z k , let represent the covariance matrix of y k , and have the following relationship: where n k represents the corresponding random vector of the set ; then calculate the approximate value of where represents the mean of ,​​​​​​ And calculate approximation in, It is positive definite;

[0060] Step 7.2: According to Obtain α k rough estimate rough estimate rough estimate in, express The element in the first row and first column, express The element in the 1st row and 2nd column, express The element in the second row and second column; thus, obtain p. k rough estimate

[0061] Step 7.3: Calculation The estimation error covariance matrix Specifically as follows:

[0062] Step 7.3.1: Definition The estimation error vector Δp k ,

[0063] Step 7.3.2: Let

[0064] Step 7.3.3: Definition in, Corresponding representation The element in the first row and first column, the element in the first row and second column, and the element in the second row and second column. Corresponding representation The element in the first row and first column, the element in the first row and second column, and the element in the second row and second column. S k,(1,：) S represents k The first row vector, S k,(2,：) S represents k The second row vector, express The first element, express The second element, Cn,(1,1) represents the element of the first row and the first column of C n n，(1,2) represents the element of the first row and the second column of C n n，(2，2) represents the element of the second row and the second column of C n

[0065] Step 7.3.4: Calculate the covariance matrix C σ of Δσ σ the element of the first row and the first column of C σ the element of the first row and the second column of C σ the element of the second row and the second column of C σ the element of the first row and the third column of C σ the element of the second row and the third column of C σ the element of the third row and the third column of C

[0066] Step 7.3.5: Substitute into and perform a first-order Taylor expansion on respectively at σ, to obtain: Δp k = DΔσ, wherein the first row of D is the second row of D is the third row of D is

[0067] Step 7.3.6: Calculate

[0068] Step 7.4: Update the estimate of p k in the framework of Kalman filtering wherein K p represents the Kalman gain corresponding to the Kalman filtering equation, then calculate the estimation error covariance matrix Ξ k of

[0069] Compared with the prior art, the present application has the following advantages:

[0070] ​​​​​​​​​1) Propose a robust extended object tracking framework based on MCC-VC: innovatively introduce MCC-VC criterion into the extended object tracking (EOT) problem, and significantly improve the robustness of the method to measurement outliers through the estimation of the error center.

[0071] 2) Design an alternating iteration strategy under the MCC-VC framework: first whiten the prediction error and the measurement error to make them compatible with the MCC-VC framework, and then realize the robust estimation of the motion state of the extended object through the alternating optimization between the kernel bandwidth estimated by the kernel density estimation theory and the kernel center and the motion state.

[0072] 3) Reconstruct the Kalman filter recursion formula based on the MCC-VC theory, effectively suppress the influence of the non-zero center distribution of the measurement error, and improve the tracking accuracy and robustness of the extended object.

[0073] 4) Establish a pseudo-measurement updating mechanism for the shape feature of the extended object: develop a two-step strategy of "initial estimation + Kalman filter", take the initial estimation of the shape feature as the pseudo-measurement value, calculate the estimation error covariance matrix, and then update and optimize it in the Kalman filter framework. BRIEF DESCRIPTION OF DRAWINGS

[0074] Figure 1 is the overall implementation block diagram of the method of the present application;

[0075] Figure 2 is the root mean square error (RMSE) of the length estimation of the major and minor axes of the extended object by the method of the present application under small noise as a function of the sample index (sample index);

[0076] Figure 3 is the root mean square error (RMSE) of the position estimation of the extended object by the method of the present application under small noise as a function of the sample index (sample index);

[0077] Figure 4 is the root mean square error (RMSE) of the angle estimation of the extended object rotating counterclockwise around the x-axis of the two-dimensional coordinate system by the method of the present application under small noise as a function of the sample index (sample index);

[0078] Figure 5 is the root mean square error (RMSE) of the velocity estimation of the extended object by the method of the present application under small noise as a function of the sample index (sample index);

[0079] Figure 6Figure for average GWD of all parameter estimation of extended target by the method of the present application in case of small noise versus sample index;

[0080] Figure 7 Figure for root mean square error (RMSE) of long and short semi-axis length estimation of extended target by the method of the present application in case of large noise versus sample index;

[0081] Figure 8 Figure for root mean square error (RMSE) of position estimation of extended target by the method of the present application in case of large noise versus sample index;

[0082] Figure 9 Figure for root mean square error (RMSE) of angle estimation of extended target rotating counterclockwise around x axis of two-dimensional coordinate system by the method of the present application in case of large noise versus sample index;

[0083] Figure 10 Figure for root mean square error (RMSE) of velocity estimation of extended target by the method of the present application in case of large noise versus sample index;

[0084] Figure 11 Figure for average GWD of all parameter estimation of extended target by the method of the present application in case of large noise versus sample index;

[0085] Figure 12 Figure for trajectory of extended target tracking in automatic driving multi-modal data set by the method of the present application, wherein, x direction represents x direction, y direction represents y direction;

[0086] Figure 13 Figure for GWD of all parameter estimation of extended target in automatic driving multi-modal data set by the method of the present application versus (sample index). DETAILED DESCRIPTION

[0087] The present application will be further described in detail below with reference to the embodiments of the drawings.

[0088] The present application proposes a robust ellipse extended target tracking method based on variable center maximum correlation entropy criterion, the overall implementation block diagram is as shown in Figure 1 The method comprises the following steps:

[0089] Step 1: In the extended target tracking system, for the extended target with approximate elliptical shape, its tracking model contains three parts: unknown parameterization, measurement model and dynamic model, which together constitute a complete tracking framework for estimating the motion state and shape feature of the extended target, wherein the extended target is deployed in a two-dimensional coordinate system.

[0090] Unknown parameterization: the motion state r of the extended target at time k k consists of position u k and velocity v k , the shape feature p of the extended target at time k k consists of angle a of counterclockwise rotation around the x-axis of the two-dimensional coordinate system k and length and short semi-axis length vector , wherein the initial value of k is 1.

[0091] Measurement model: the radar obtains the two-dimensional Cartesian coordinates of a set of reflection points on the extended target at time k, which are taken as measurement values, the jth measurement value obtained at time k is denoted as and the measurement model of is established, wherein j = 1,..., J k , J k denotes the number of measurement values obtained at time k.

[0092] Dynamic model: in general, the dynamic model of the time series evolution process of the motion state and shape feature of the extended target is not subject to specific restrictions, in order to simplify the analysis, the present application sets that the extended target moves at a constant speed in a straight line within a short observation period, and the dynamic model is described as: wherein A k denotes the transition matrix at time k, t denotes the sampling interval, λ k and ψ k are both process noise at time k, λ k obeys Gaussian distribution with mean of zero and covariance matrix Λ, ψ k obeys Gaussian distribution with mean of zero and covariance matrix Ψ, r k-1 denotes the initial motion state of the extended target, p k-1 denotes the initial shape feature of the extended target, r k-1 denotes the motion state of the extended target at time k-1, p k-1 denotes the shape feature of the extended target at time k-1.

[0093] In this embodiment, in step 1, wherein the dimension of r k is 4x1, and the dimension of p k is 3x1, A vector consisting of the lengths of the major and minor axes of the extended target at time k, with the superscript "T" indicating the transpose of the vector or matrix; The measurement model is described as follows: in, Indicates the extension target and The corresponding measurement source, i.e., the true two-dimensional Cartesian coordinates of the reflection point, is used for modeling. This invention utilizes multiplicative noise vectors Will Related to the shape characteristics of the extended target at time k: u k =Hr k H represents from r k Extract u k The selection matrix is ​​H = [I2, 0], where I2 represents the 2-dimensional identity matrix and 0 represents the zero matrix with dimension 2×2. and The corresponding symbols represent the lengths of the major and minor axes of the extended target at time k. express The corresponding multiplicative noise vector follows a property with zero mean and a covariance matrix of... Gaussian distribution, for The first element, for The second element, and All are used for control The corresponding reflection points are random quantities that are uniformly distributed within the extended target range. In this invention, a widely used spatial distribution model is used to represent the extended target range. express The measurement noise in the data follows a zero-mean pattern and has a noise covariance matrix of C. n Gaussian distribution, for Measurement errors in the measurement model.

[0094] Step 2: Based on the dynamic model, for r k and p k Perform a prediction step to obtain r k One-step prediction value and p k One-step prediction value thereby obtaining Prediction error covariance matrix and Prediction error covariance matrix For the motion state estimation of the extended target, there are two errors, which are prediction error and measurement error, then in order to make the motion state estimation of the extended target compatible with the variable center maximum correlation entropy criterion (MCC-VC), the prediction error and the measurement error in the measurement model of is whitened, and the whitened prediction error ξ k and the whitened measurement error are obtained, wherein the whitening process of the prediction error uses the measurement error in the measurement model of the whitening process of the measurement error uses and

[0095] In this embodiment, in step 2, wherein when k = 1 r k-1 , and when k > 1 represents the estimated value of r k-1 , when k = 1 p k-1 , and when k > 1 represents the estimated value of p k-1 , represents the expected operation, R k-1 represents the estimated error covariance matrix of Ξ k-1 represents the estimated error covariance matrix of ξ k The obtaining process of is as follows: Cholesky decomposition is performed on to obtain and then is obtained, wherein the dimension of ξ k is 4 × 1. The obtaining process of is as follows: since in the measurement model of the shape feature is not linear, the first-order Taylor expansion is performed on at : wherein represents that is substituted into S k to obtain, V 1，k represents the Jacobian matrix calculated according to S k , the first row S k，(1,：) , represents that is substituted into V 1，k to obtain, V 2，k represents the Jacobian matrix calculated according to S k , the second row Sk，(2，：) The calculated Jacobian matrix, Indicates will Substitute into V 2，k Obtain from; then The covariance matrix is ​​represented as M k , Will The covariance matrix is ​​represented as F k , Among them, F k The element F with index (i, t) (i，t) pass We calculate that i, t∈{1, 2}, and tr{·} represents finding the trace of the matrix; then we obtain... The relevant measurement error covariance matrix Q k , Then Q k Perform Cholesky decomposition to obtain And thus obtain The nth component is in, The dimension is 2×1, Π k，(n,；) Represents Π k The nth line.

[0096] Step 3: Assume that when k > 1, r k-1 The estimated value As an unbiased estimate, based on this assumption, it is easy to know that ξ k It has the property of zero mean, therefore ξ k Setting the estimated value of the core center to zero is reasonable; because A single sample is insufficient to perform ξ analysis. k The kernel bandwidth estimate, therefore ξ k The estimated value of the kernel bandwidth is set as ξ. k Standard deviation, ξ k The standard deviation is 1.

[0097] Step 4: Due to the influence of multiplicative error, Since the mean of the kernel is not zero, its kernel bandwidth and kernel center need to be estimated. Therefore, a joint estimation optimization problem needs to be constructed. Based on the kernel density estimation (KDE) theory, a joint estimation optimization problem is constructed. The nth component kernel bandwidth σ k，(n) and the core center c k，(n)The joint estimation optimization problem is given by n∈{1,2}. The joint estimation of kernel bandwidth and kernel center in the joint estimation optimization problem may lead to local convergence due to the non-convexity of the problem. To solve this problem, this invention decomposes the joint estimation optimization problem into two sub-problems, estimating a parameter for each sub-problem. Specifically, the joint estimation optimization problem is decomposed into kernel bandwidth σ. k，(n) The original problem and kernel center c of the estimation subproblem k，(n) Estimate the original problem of the subproblem; then calculate the kernel bandwidth σ. k，(n) The original problem of estimating the subproblem is transformed into kernel bandwidth σ. k，(n) Estimating the convexity of the subproblems, with the kernel center c k，(n) The original problem of estimating subproblems is transformed into the core-center problem c. k，(n) The estimation of the augmented optimization problem of the subproblem, wherein the augmented optimization problem includes... The weight is the center error weight. express The estimated value; then the core center c k，(n) The augmented optimization problem of the estimation subproblem is decomposed into a subproblem concerning the kernel center and a subproblem concerning the center error weight.

[0098] In this embodiment, the Maximum Correlation Entropy (MCC) criterion has been widely applied to nonlinear non-Gaussian signal processing problems involving impulse noise. Its core idea is to assign appropriate weights to each measurement value, thus reducing the impact of outliers by assigning them smaller weights. Compared to the traditional MCC method (assuming a zero kernel center and calculating the kernel bandwidth using specific rules such as the Silverman criterion), the Variable Center Maximum Correlation Entropy (MCC-VC) criterion, based on kernel density estimation (KDE) theory, achieves joint optimization of kernel bandwidth and kernel center. When the kernel bandwidth and kernel center are accurately estimated, MCC-VC typically exhibits superior performance. Since the actual errors in real-world engineering problems often have non-zero center characteristics, a more precise estimation method is needed to estimate the Gaussian kernel function. Given the kernel center and kernel bandwidth, based on kernel density estimation theory, the following optimization problem can be constructed: in, and These represent the kernel bandwidth and kernel center, respectively. and These represent the optimal estimates of kernel bandwidth and kernel center, respectively. This represents the whitening measurement error of the j-th measurement, where J represents the number of measurements. Indicates the kernel bandwidth as The Gaussian kernel function. Therefore, in step 4, the joint estimation optimization problem is described as: in, Expressing the requirement to make When σ is at its minimum k，(n) and ck，(n) the value of exp(·) represents an exponential function with the natural constant as the base, the natural constant is 2.71..., represents the estimated value of k the estimated value of under the premise that r Π k，(n，：) represents the nth row of Π k Q k represents the related measurement error covariance matrix, H represents the selection matrix of extracting u k from r k .

[0099] In this embodiment, in step 4, the kernel bandwidth σ k，(n) The obtaining process of the convex problem of the kernel bandwidth σ k，(n) estimation sub-problem is: fix the value of the kernel center c and let convert the joint estimation optimization problem into the original problem of the kernel bandwidth σ k，(n) estimation sub-problem, described as: Wherein, min is the minimum value function, the original problem is non-convex; then perform Taylor expansion on the original problem of the kernel bandwidth σ k，(n) estimation sub-problem and retain its third order term to ensure the accuracy of the approximation, get the Taylor expansion formula, and as the convex problem of the kernel bandwidth σ k，(n) estimation sub-problem, described as: Wherein, The Taylor expansion formula is proved to be strictly convex in (0~+∞), so the global optimal solution can be obtained, σ k，(n) can be obtained by taking the reciprocal of the estimation of μ k，(n) , represents the 3rd power of μ k，(n) .

[0100] In this embodiment, in step 4, the kernel center c k，(n) The obtaining process of the augmented optimization problem of the kernel center c k，(n) estimation sub-problem is: fix the value of the kernel bandwidth σ convert the joint estimation optimization problem into the original problem of the kernel center c k，(n) estimation sub-problem, described as: The original problem is non-convex; then according to the properties of convex conjugate functions, the original problem of the kernel center c k，(n) estimation sub-problem is converted into the augmented optimization problem of the kernel center c k，(n) estimation sub-problem, described as: ​where max is the max function, denotes the weight of the center error, and φ(·) denotes the exponential function e -y .

[0101] From the augmented optimization problem, it can be seen that by the property of the convex conjugate function, the original problem only about one unknown c k，(n) is transformed into an augmented optimization problem about one unknown c k，(n) and J k unknown center error weights, although the number of unknowns has increased, the augmented optimization problem is easier to handle, and can be solved by alternating iteration of two sub-problems, so the augmented optimization problem of the kernel center c k，(n) estimation sub-problem is decomposed into a sub-problem about the kernel center and a sub-problem about the center error weight.

[0102] In this embodiment, in step 4, the obtaining process of the sub-problem about the kernel center is: fixing the value of c , and recording this value as c The augmented optimization problem is transformed into a sub-problem about the kernel center, described as: This sub-problem is a linear least squares problem about the kernel center c k，(n) , and the solution can be easily obtained by setting the derivative to zero,

[0103] In this embodiment, in step 4, the obtaining process of the sub-problem about the center error weight is: fixing the value of the kernel center c k，(n) , and recording this value as c The augmented optimization problem is transformed into a sub-problem about the center error weight, described as: According to the property of the convex conjugate function, for a fixed When , the sub-problem reaches the maximum value, denotes the weight of the center error.

[0104] Step 5: based on the variable center maximum correlation entropy criterion (MCC-VC), a cost function about r k is proposed, and σ k，(n) and c k，(n) known are substituted into the cost function about r k , to obtain an optimization problem based on the variable center maximum correlation entropy criterion with known σ k，(n) and c k，(n) ; then the optimization problem is equivalently transformed into an augmented optimization problem, wherein the augmented optimization problem contains a prediction error weight and a measurement error weight; and then the augmented optimization problem is decomposed into a sub-problem about r ksub-problems and sub-problems about the prediction error weights and the measurement error weights.

[0105] In this embodiment, the specific process of step 5 is as follows:

[0106] Step 5.1: Fix the value of the kernel bandwidth σ k，(n) , and take this value as Step 5.2: Fix the value of the kernel center c k，(n) , and take this value as Then, based on the variable center maximum correlation entropy criterion (MCC-VC), a cost function about r k is proposed, described as: wherein, represents the cost function about r k , The kernel function in is the Gaussian kernel function, L = 4 + 2J k , ||·|| represents the 2-norm, G1 represents the kernel function with a kernel bandwidth of 1, represents the kernel function with a kernel bandwidth of , b k，(i) represents the zero-centered error vector b k related to r k , and the i-th element in b k ,

[0107] Step 5.2: Let convert the cost function about r k into a cost function with known σ k，(n) and c k，(n) , described as: wherein, represents the cost function with known σ k，(n) and c k，(n) , represents the Gaussian kernel function with a kernel bandwidth of , ξ k，(m) represents the m-th element of ξ k , Γ k，(m，：) represents the m-th row of Γ k .

[0108] Step 5.3: Substitute and into , and construct an optimization problem based on the variable center maximum correlation entropy criterion with known σ k，(n) and c k，(n) , described as:

[0109]

[0110] Step 5.4: Due to the non-convexity of the optimization problem, direct solution is quite challenging. Observation reveals that the objective function of the optimization problem contains an exponential function term. Therefore, we can refer to the kernel center c. k，(n) Estimation of subproblem handling methods. The optimization problem is equivalently transformed into an augmented optimization problem, described as: Among them, κ m For prediction error weights, For the measurement error weights, φ(·) represents the exponential function e -y convex conjugate function,

[0111] Step 5.5: As can be seen from the augmented optimization problem, by utilizing the properties of the convex conjugate function, the problem can be transformed from one unknown vector r to a single unknown vector r. k The optimization problem is transformed into an optimization problem involving an unknown vector r. k and 4+2J k This is an augmented optimization problem with unknown error weights (including prediction error weights and measurement error weights). Note that although the number of unknowns increases, this augmented optimization problem is easier to handle and can be solved by iteratively solving two subproblems.

[0112] Fixed κ m and The values ​​are denoted as and these two values ​​are recorded accordingly. and The augmentation optimization problem is transformed into a problem concerning r. k The subproblems are described as follows: because and With r k It is irrelevant, therefore we can consider r k The subproblem is equivalently transformed into a problem concerning r. k The linear least squares problem is described as follows: The solution can be easily obtained by setting the derivative to zero.

[0113] Fixed r k The value, denoted as The augmentation optimization problem is transformed into a subproblem concerning the prediction error weights and the measurement error weights, described as follows: in, According to the properties of convex conjugate functions, when At that point, the subproblem reaches its maximum value.

[0114] Step 6: Obtain the results using an alternating iterative strategy. The nth component kernel bandwidth σ k，(n) The estimated value and the core center c k，(n)The estimated value and r k The estimated value Specifically, in each outer iteration, for σ... k，(n) Directly solve for the kernel bandwidth σ k，(n) Estimate the convexity of the subproblems to obtain σ at each outer iteration. k，(n) The optimal solution for c; k，(n) During the inner iteration process, the subproblem concerning the kernel center and the subproblem concerning the center error weight are solved alternately. When the first inner iteration condition is satisfied, the value of c in each outer iteration is obtained. k，(n) The optimal solution for r; k During the inner iteration process, solutions for r are solved alternately. k The subproblems concerning prediction error weights and measurement error weights, when satisfying the second inner-layer iteration condition, yield r for each outer-layer iteration. k The optimal solution, and r k The estimation error covariance matrix of the optimal solution; σ is obtained when the outer iteration conditions are satisfied. k，(n) The estimated value and c k，(n) The estimated value and r k The estimated value And thus obtain The estimation error covariance matrix R k Then, through the state transition equation, and R k Convert to r respectively k+1 One-step prediction value and its prediction error covariance matrix

[0115] In this embodiment, step 6 is as follows:

[0116] Step 6.1: Let s represent the number of outer iterations, and the initial value of s is 1.

[0117] Step 6.2: During the s-th outer iteration, and Substitute kernel bandwidth σ k，(n) Convex problems for estimating subproblems In the solution, μ is obtained at the s-th outer iteration. k，(n) The optimal solution, i.e., obtaining σ at the s-th outer iteration. k，(n) optimal solution Where s = 1 For c k，(n) initial value For r k initial value Set to 0, c k，(n) is set to 0, σ k，(n) is set to 1, κ m and are all set to -1, and substituted into , it is found that s > 1 represents the optimal solution of c k，(n) in the s-th outer iteration, represents the optimal solution of r k in the s-1-th outer iteration.

[0118] Step 6.3: In the s-th outer iteration, inner iteration is performed on c k，(n) , which is as follows:

[0119] Step 6.3.1: Let q represent the number of inner iteration, and the initial value of q is 1.

[0120] Step 6.3.2: In the q-th inner iteration in the s-th outer iteration, substitute and into the sub-problem about the kernel center , and solve to obtain the optimal solution of c k，(n) in the q-th inner iteration in the s-th outer iteration wherein, when q = 1, is the initial value of c at the beginning of inner iteration in the s-th outer iteration is set to -1, and when q > 1, represents the optimal solution of c in the q-1-th inner iteration in the s-th outer iteration, is obtained by substituting into .

[0121] In the q-th inner iteration in the s-th outer iteration, substitute and into the sub-problem about the center error weight , and solve to obtain the optimal solution of r in the q-th inner iteration in the s-th outer iteration

[0122] Step 6.3.3: Determine whether the first inner iteration condition is met, if yes, then is taken as the optimal solution of c k，(n) in the s-th outer iteration Then execute step 6.4. If it fails, set q = q + 1, and then return to step 6.3.2 to continue execution. Here, when q = 1... When c starts during the s-th outer iteration... k，(n) initial value equal hour c represents the value of c in the (q-1)th inner iteration within the s-th outer iteration. k，(n) The optimal solution.

[0123] Step 6.4: During the s-th outer iteration, for r k Perform inner layer iterations as follows:

[0124] Step 6.4.1: Let q represent the number of inner iterations, and the initial value of q is 1.

[0125] Step 6.4.2: During the q-th inner iteration within the s-th outer iteration, ... and Substitute about r k Linear least squares problem In the solution, we obtain the value of r during the q-th inner iteration within the s-th outer iteration. k optimal solution Where q = 1 κ is the value at the start of the inner iteration during the s-th outer iteration. m initial value When the inner iteration begins during the s-th outer iteration... initial value and All are set to -1, when q>1 κ represents the value of κ in the (q-1)th inner iteration within the s-th outer iteration. m The optimal solution This indicates the time of the (q-1)th inner iteration within the s-th outer iteration. The optimal solution. Indicates Kalman gain, Then calculate The estimation error covariance matrix Where I represents the identity matrix.

[0126] During the q-th inner iteration within the s-th outer iteration, Substitute the subproblems concerning prediction error weights and measurement error weights. In the solution, we obtain κ when the inner iteration is q within the s-th outer iteration. m optimal solution and optimal solution in,

[0127] Step 6.4.3: Determine the second inner layer iteration condition Is it true? If it is true, then... When r is the s-th outer iteration k optimal solution Will As The estimation error covariance matrix Then execute step 6.5. If it fails, set q = q + 1, and then return to step 6.4.2 to continue execution. Here, when q = 1... When r starts during the s-th outer iteration... k initial value equal hour This indicates that when r is in the (q-1)th inner iteration within the s-th outer iteration... k The optimal solution.

[0128] Step 6.5: Determine if the outer iteration condition is true. If it is true, then... Corresponding as σ k，(n) The estimated value c k，(n) The estimated value r k The estimated value Then As The estimation error covariance matrix R k Then, through the state transition equation, and R k Convert to r respectively k+1 One-step prediction value and its prediction error covariance matrix If the condition is not met, then let s = s + 1, and return to step 6.2 to continue execution. The outer iteration condition is whether s is less than or equal to a preset number of iterations, such as 2.

[0129] Step 7: For shape feature estimation, given a dataset in a two-dimensional plane, the covariance matrix is ​​naturally considered to characterize the data dispersion and thus estimate the shape features. In statistics and data analysis, the covariance matrix is ​​a fundamental tool for describing data distribution and variability. The eigenvalues ​​and eigenvectors of the covariance matrix can reveal the geometric characteristics of data points, specifically: by quantifying the variability of data in different directions, the main direction of the data and its corresponding shape features can be determined. In this invention, measured values ​​are used. The shape features are estimated and then updated based on Kalman filtering theory. As mentioned earlier, estimating the shape features requires calculating the covariance matrix of the data points.

[0130] Based on the expansion target and Corresponding measurement source The corresponding random vector z k covariance matrix approximation Obtain α k rough estimate Extend the length of the semi-major axis of the target at time k rough estimate Extend the length of the minor semi-axis of the target at time k rough estimate Then obtain p k rough estimate Then calculate The estimation error covariance matrix Then, within the framework of Kalman filtering, update and obtain p. k The estimated value The Kalman gain corresponding to the Kalman filter equation includes...

[0131] In this embodiment, the specific process of step 7 is as follows:

[0132] Step 7.1: Let z k Represents a set of data points The corresponding random vector, let y k Represents a set of data points The corresponding random vector, let Indicate z k Let the covariance matrix be... Indicates y k The covariance matrix, and The relationship is as follows: Where, n k Represents a set The corresponding random vector; in practice It can be approximated by the sample covariance, and then calculated. approximation in, Represents a set of data points The mean, And calculate approximation in, It is positive definite, note. must be positive definite, but this requirement can not be satisfied when the measurements are particularly sparse, in which case one can directly use to approximate

[0133] Step 7.2: Obtain the rough estimate of k where denotes the element in the first row and the first column of denotes the element in the first row and the second column of denotes the element in the second row and the second column of ; and further obtain the rough estimate of k

[0134] Step 7.3: One can regard as a measurement of k , i.e. a pseudo-measurement, and its covariance matrix can be approximately obtained by the following way. Calculate the estimation error covariance matrix of

[0135] Step 7.3.1: Define the estimation error vector of k

[0136] Step 7.3.2: Let

[0137] Step 7.3.3: Define where corresponds to the element in the first row and the first column, the element in the first row and the second column, and the element in the second row and the second column of corresponds to the element in the first row and the first column, the element in the first row and the second column, and the element in the second row and the second column of S k,(1,：) denotes the first row vector of k S k，(2， : ) denotes the first row vector of k ​​​​​​​​​​the second row of A, denotes the first element of A, denotes the second element of A, C n,(1,1) denotes C n the first column element of the first row of A, C n，(1，2) denotes C n the second column element of the first row of A, C n，(2，2) denotes C n the second column element of the second row of A.

[0138] Step 7.3.4: Calculate the covariance matrix C σ of the Δσ, σ which is a symmetric matrix, C σ the first column element of the first row of C is σ the second column element of the first row of C is σ the second column element of the second row of C is σ the third column element of the first row of C is σ the third column element of the second row of C is σ the third column element of the third row of C

[0139] Step 7.3.5: Substitute into and perform a first-order Taylor expansion of at σ, respectively, to obtain: Δp k = DΔσ, where the first row of D is the third row of D

[0140] Step 7.3.6: Calculate

[0141] Step 7.4: Update the estimate k of p in the framework of Kalman filtering, where K p denotes the Kalman gain corresponding to the Kalman filtering equation, and then calculate the estimation error covariance matrix Ξ k of

[0142] ​Step 8: Obtain the estimated value of the motion state and the estimated value of the shape feature of the extended target at different time points, which are continuous multiple time points, to realize the tracking of the extended target according to the process of steps 6 and 7.

[0143] To verify the feasibility and effectiveness of the method, simulation tests are carried out under small noise and large noise conditions, and real tests are carried out using the existing automatic driving multi-modal data set (nuScenes data set).

[0144] Simulation test:

[0145] The true motion trajectory of the extended target is the same as that set by the MEM-EKF method proposed in the existing literature, and the long and short semi-axes of the ellipse shape are 170 meters and 40 meters, respectively. During the movement, the lengths of the long and short semi-axes of the extended target remain unchanged, but the angle of counterclockwise rotation around the x-axis of the two-dimensional coordinate system changes dynamically with time. The extended target moves at a constant speed of 50 km / h along the -π / 4 direction of the coordinate origin at the initial time. The measurement data is sampled at intervals of 10 seconds, a total of 103 times. The measurement points at each time are uniformly distributed in the elliptical region, and the number of single-time measurement points follows a Poisson distribution with a mean of 30. The true initial motion state is r0=[0m, 0m, 50 / 3.6×cos(-π / 4)m / s, 50 / 3.6×sin(-π / 4)m / s] t , the true initial shape feature is p0=[-π / 4rad, 170m, 40m] T , the covariance matrix of r0 is R0=diag{900m 2 , 900m 2 , 16(m / s) 2 , 16(m / s) 2}, and the covariance matrix of p0 is Ξ0=diag{0.2rad 2 , 400m 2 , 400m 2}, where diag{} represents a diagonal matrix. The estimated value of the initial motion state is randomly generated according to the Gaussian distribution , and the estimated value of the initial shape feature is randomly generated according to the Gaussian distribution . The multiplicative noise vector is assumed to follow a Gaussian distribution with a mean of zero and a covariance matrix of . In addition, the covariance matrix Λ of the process noise λ k is set to Λ=diag{100m 2 , 100m 2 , 1(m / s) 2, 1 (m / s) 2} process noise ψ k The covariance matrix Ψ of the process noise ψ is set as Ψ = diag{0.01 rad 2 , 0.001 m 2 , 0.001 m 2}. The noise covariance matrix of the small noise is set as C n = diag{200 m 2 , 80 m 2}, and the noise covariance matrix of the large noise is set as C n = diag{1000 m 2 , 200 m 2}.

[0146] Based on the above parameter settings, the simulation test tests the tracking performance of the extended target of the method under different measurement noise powers. The root mean square error (RMSE) of the speed, position, long and short semi-axis length and the angle of counterclockwise rotation around the x-axis of the two-dimensional coordinate system of the extended target obtained by 1000 Monte Carlo experiments respectively, and the average GWD are used to evaluate the tracking performance of the method. The GWD is defined as: wherein Φ k represents a symmetric positive definite shape matrix, represents the estimated value of u k , and Φ represents the estimated value of Φ k .

[0147] Figure 2 The root mean square error (RMSE) of the long and short semi-axis length estimation of the extended target of the method under the condition of small noise is given as a function of the sampling time (sample index), Figure 3 The root mean square error (RMSE) of the position estimation of the extended target of the method under the condition of small noise is given as a function of the sampling time (sample index), Figure 4 The root mean square error (RMSE) of the angle estimation of the counterclockwise rotation of the extended target around the x-axis of the two-dimensional coordinate system of the method under the condition of small noise is given as a function of the sampling time (sample index), Figure 5 The root mean square error (RMSE) of the speed estimation of the extended target of the method under the condition of small noise is given as a function of the sampling time (sample index). From Figures 2 to 5As can be seen from the data, when the sampling time changes, the method of the present invention can accurately estimate the position, velocity, major and minor axis lengths, and counterclockwise rotation angle of the extended target under the condition of low measurement noise power. Except for the estimation of the major and minor axis, the tracking performance of other parameters will be slightly worse when the extended target turns.

[0148] Figure 6 The graph shows the variation of the average Gauss-Wasestein distance (GWD) for all parameter estimates of the extended target using the method of this invention under low noise conditions, as a function of the sampling index. Figure 6 As can be seen, when the noise power is low, the average Gauss-Wasestein distance (GWD) estimated for all parameters of the extended target by the method of the present invention will reach the best tracking accuracy in a short time, and the tracking performance will deteriorate slightly when the extended target turns.

[0149] Figure 7 The graph shows the root mean square error (RMSE) of the estimation of the major and minor semi-axis lengths of the extended target using the method of the present invention under high noise conditions, as a function of the sampling time (sample index). Figure 8 The graph shows the variation of the root mean square error (RMSE) of the method of the present invention for the position estimation of the extended target under high noise conditions with the sampling time (sample index). Figure 9 The graph shows the root mean square error (RMSE) of the method of the present invention for estimating the angle of counterclockwise rotation of the extended target around the x-axis of the two-dimensional coordinate system under high noise conditions, as a function of sampling time (sample index). Figure 10 The graph shows the root mean square error (RMSE) of the velocity estimation of the extended target using the method of this invention under high noise conditions, as a function of the sampling time (sample index). From... Figures 7 to 10 As can be seen from the data, when the sampling time changes, the method of the present invention can accurately estimate the position, velocity, major and minor axis lengths, and counterclockwise rotation angle of the extended target when the measurement noise power is large. Except for the estimation of the major and minor axis, the tracking performance of other parameters will be slightly worse when the extended target turns.

[0150] Figure 11 The graph shows the variation of the average Gauss-Wasestein distance (GWD) for all parameter estimates of the extended target under high noise conditions using the method of this invention, as a function of the sampling index. Figure 11It can be seen that the average GWD of all parameter estimates of the extended target is optimal in a short time under the condition of small measurement noise, and the tracking performance is slightly worse when the extended target turns.

[0151] Real test experiment:

[0152] In the experiment using the automatic driving multi-modal data set, the scene-0757 scene in the "mini" subset of the nuScenes data set is selected, which contains a bus with a size of 13.818 m x 3.132 m turning right at a crossroads, and the reflection points generated by the radar when irradiating the vehicle. The scene extracts 11 time points of radar measurement data from the "mini" subset, which is collected by the front radar every 0.5 seconds. The measurement noise covariance matrix is set as C n = diag{0.5 2 m 2 , 0.5 2 m 2}, the real initial motion state is r0 = [300 m 2 , 600 m 2 , -0.5 m / s, 1 m / s], the real initial shape feature is p0 = [-π / 3 rad 2 , 7 m 2 , 1 m 2 ], the covariance matrix of r0 is R0 = diag{100 m 2 , 100 m 2 , 1 (m / s) 2 , 1 (m / s) 2}, the covariance matrix of p0 is Ξ0 = diag{0.1 rad 2 , 1 m 2 , 1 m 2}, the estimated value of the initial motion state is randomly generated according to the Gaussian distribution , and the estimated value of the initial shape feature is randomly generated according to the Gaussian distribution . Based on the above parameter settings, the real test experiment tests the average GWD of the speed, position, long and short semi-axis length and the angle of counterclockwise rotation around the x axis of the two-dimensional coordinate system of the extended target obtained by 1000 times of Monte Carlo experiments of the method to evaluate the tracking performance of the method.

[0153] Figure 12 The trajectory diagram of the method for tracking the extended target in the automatic driving multi-modal data set is given, wherein the x direction represents the x direction, and the y direction represents the y direction,Figure 13 A figure showing the change of Gaussian Waldstein distance (GWD) of the method of the present application for all parameter estimation of the extended target in the automatic driving multi-modal dataset with (sample index) is given. From Figure 12 It can be seen from the figure that the trajectory consistency of the method of the present application for extended target tracking is high. From Figure 13 It can be seen from the figure that as the sampling time changes, the data points obtained by sampling the extended target become less, so the Gaussian Waldstein distance (GWD) changes with the sampling time and rises, but the estimation is still reliable.

Claims

1. A robust elliptical extended object tracking method based on variable center maximum correlation entropy criterion, characterized in that The method comprises the following steps: Step 1: in an extended target tracking system, for an extended target with an approximate elliptical shape, a tracking model of the extended target comprises three parts: an unknown parameterization, a measurement model and a dynamic model, wherein the extended target is arranged in a two-dimensional coordinate system; Unknown parameterization: the motion state r of the extended target at time k k consists of position u k and velocity v k The shape feature p of the extended target at time k k consists of the angle of rotation α counterclockwise around the x-axis of the two-dimensional coordinate system k and the length and semi-axis length vector l k , where the initial value of k is 1; Measurement model: At time k, the radar obtains a set of two-dimensional Cartesian coordinates of the reflection points on the extended target, which are taken as measurement values. The jth measurement value obtained at time k is denoted as and a measurement model of is established, where j = 1,..., J k , J k represents the number of measurement values obtained at time k. Dynamic model: if the extended target is assumed to move at a constant velocity in a straight line in a short observation period, the dynamic model is described as: where A k represents the transition matrix at time k, t represents the sampling interval, λ k and ψ k are the process noises at time k, λ k obeys the Gaussian distribution with zero mean and covariance matrix Λ, ψ k obeys the Gaussian distribution with zero mean and covariance matrix Ψ, r k-1 represents the initial motion state of the extended target, p k-1 represents the initial shape feature of the extended target, r k-1 represents the motion state of the extended target at time k-1, p k-1 represents the shape feature of the extended target at time k-1. Step 2: Based on the dynamic model, for r k and p k Perform a prediction step to obtain r k One-step prediction value and p k One-step prediction value thereby obtaining Prediction error covariance matrix and Prediction error covariance matrix Then, the prediction error was analyzed. and The measurement error in the measurement model is whitened, and the corresponding whitened prediction error ξ is obtained. k Measurement error after whitening Among them, prediction error The whitening process used The measurement error whitening process in the measurement model used and Step 3: Assume that k > 1, then the estimate of r k-1 is unbiased, based on this assumption, set the estimate of the kernel center of ξ k to zero; set the estimate of the kernel bandwidth of ξ k to the standard deviation of ξ k ;​ Step 4: Based on kernel density estimation theory, construct... The nth component kernel bandwidth σ k，(n) and the core center c k，(n) The joint estimation optimization problem is given by the expression $\mathbf{a}$, where $n \in {1, 2}$. The joint estimation optimization problem is decomposed into a kernel bandwidth $σ$. k，(n) The original problem and kernel center c of the estimation subproblem k，(n) Estimate the original problem of the subproblem; then calculate the kernel bandwidth σ. k，(n) The original problem of estimating the subproblem is transformed into kernel bandwidth σ. k，(n) Estimating the convexity of the subproblems, with the kernel center c k，(n) The original problem of estimating subproblems is transformed into the core-center problem c. k，(n) The estimation of the augmented optimization problem of the subproblem, wherein the augmented optimization problem includes... The weight is the center error weight. express The estimated value; then the core center c k，(n) The augmented optimization problem of estimating the subproblem is decomposed into a subproblem concerning the kernel center and a subproblem concerning the center error weights; Step 5: Propose a criterion for r based on the maximum correlation entropy criterion of the variable center. k The cost function, and the known σ k，(n) and c k，(n) Substitute about r k The cost function is obtained by knowing σ. k，(n) and c k，(n) The optimization problem based on the maximum correlation entropy criterion with varying center is then categorized. This problem is then equivalently transformed into an augmented optimization problem, which includes prediction error weights and measurement error weights. Finally, the augmented optimization problem is decomposed into a function relating r. k Sub-problems concerning prediction error weights and measurement error weights; Step 6: Obtain the results using an alternating iterative strategy. The nth component kernel bandwidth σ k，(n) The estimated value and the core center c k，(n) The estimated value and r k The estimated value Specifically, in each outer iteration, for σ... k，(n) Directly solve for the kernel bandwidth σ k，(n) Estimate the convexity of the subproblems to obtain σ at each outer iteration. k，(n) The optimal solution for c; k，(n) During the inner iteration process, the subproblem concerning the kernel center and the subproblem concerning the center error weight are solved alternately. When the first inner iteration condition is satisfied, the value of c in each outer iteration is obtained. k，(n) The optimal solution for r; k During the inner iteration process, solutions for r are solved alternately. k The subproblems concerning prediction error weights and measurement error weights, when satisfying the second inner-layer iteration condition, yield r for each outer-layer iteration. k The optimal solution, and r k The estimation error covariance matrix of the optimal solution; σ is obtained when the outer iteration conditions are satisfied. k，(n) The estimated value and c k，(n) The estimated value and r k The estimated value And thus obtain The estimation error covariance matrix R k Then, through the state transition equation, and R k Convert to r respectively k+1 One-step prediction value and its prediction error covariance matrix Step 7: Based on the expanded target and Corresponding measurement source The corresponding random vector z k covariance matrix approximation Obtain α k rough estimate The length l of the major semi-axis of the extended target at time k k,(1) rough estimate The length l of the minor semi-axis of the extended target at time k k，(2) rough estimate Then obtain p k rough estimate Then calculate The estimation error covariance matrix Then, within the framework of Kalman filtering, update and obtain p. k The estimated value The Kalman gain corresponding to the Kalman filter equation includes... Step 8: according to the processes of step 6 and step 7, the estimated values of the motion states and the estimated values of the shape features of the extended target at different time points are obtained to realize tracking of the extended target, wherein the different time points are a plurality of continuous time points.

2. The robust ellipse extended object tracking method based on variable center maximum correlation entropy criterion according to claim 1, characterized in that In step 1, Where, r k The dimension is 4×1, p k The dimension is 3×1, l k A vector consisting of the lengths of the major and minor axes of the extended target at time k, with the superscript "T" indicating the transpose of the vector or matrix; The measurement model is described as follows: in, Indicates the extension target and The corresponding measurement source, u k =Hr k H represents from r k Extract u k The selection matrix, H = [I2, 0], where I2 represents the 2x2 identity matrix, 0 represents the zero matrix of dimension 2×2, and l k，(1) and l k，(2) The corresponding symbols represent the lengths of the major and minor axes of the extended target at time k. express The corresponding multiplicative noise vector follows a property with zero mean and a covariance matrix of... Gaussian distribution, for The first element, for The second element, and All are used for control The corresponding reflection points are random quantities that are uniformly distributed within the extended target range. express The measurement noise in the data follows a zero-mean pattern and has a noise covariance matrix of C. n Gaussian distribution, For measurement error in the measurement model.

3. The robust elliptical extended object tracking method based on variable center maximum correlation entropy criterion according to claim 2, characterized in that In step 2, Where k=1 That is, r k-1 When k > 1 Indicates r k-1 The estimated value when k=1 That is, p k-1 When k > 1 p k-1 The estimated value, R represents the desired operation. k-1 express The estimated error covariance matrix, Ξ k-1 express The estimation error covariance matrix; ξ k The acquisition process is as follows: Perform Cholesky decomposition to obtain And thus obtain Where, ξ k The dimension is 4×1; The acquisition process is: substituting into S and performing a first-order Taylor expansion at where is obtained by substituting into S k , V l，k is the Jacobian matrix calculated according to the first row S k of S k，(1，：) , is obtained by substituting into V 1，k , V 2，k is the Jacobian matrix calculated according to the second row S k of S k，(2，：) , is obtained by substituting into V 2，k ; then the covariance matrix of is denoted as M k , the covariance matrix of is denoted as F k , where the element F k with index (i, t) in F (i，t) is calculated by , i, t ∈ {1, 2}, tr{·} denotes the trace of a matrix; then the measurement error covariance matrix Q k related to is obtained k Cholesky decomposition is performed on Q , and the nth component of is obtained where the dimension of k，(n,；) is 2 × 1, and Π k denotes the nth row of Π k .

4. The robust elliptical extended object tracking method based on variable center maximum correlation entropy criterion according to claim 3, characterized in that In the step 4, the joint estimation optimization problem is described as: wherein denotes an estimate of k an estimate of under the premise that Π k，(n，：) denotes the nth row of k Q k denotes a measurement error covariance matrix associated with H denotes a selection matrix that extracts k u k from r​ The kernel bandwidth σ k，(n) The kernel bandwidth σ k，(n) is fixed, the original problem of the subproblem is described as: and let The joint estimation optimization problem is converted into the kernel bandwidth σ k，(n) The original problem of the subproblem is described as: The Taylor expansion is then performed on the original problem of the subproblem and the third order term is retained to ensure the accuracy of the approximation, and the Taylor expansion is obtained, and the kernel bandwidth σ k，(n) The kernel bandwidth σ k，(n) The kernel bandwidth σ wherein, In step 4, the kernel center c k，(n) The acquisition process of the augmented optimization problem of the estimation sub-problem is: fixing the value of the kernel bandwidth σ k，(n) The joint estimation optimization problem is converted into the original problem of the kernel center c k，(n) The estimation sub-problem is described as: Then according to the properties of the convex conjugate function, the original problem of the estimation sub-problem is converted into the kernel center c k，(n) The augmented optimization problem of the estimation sub-problem is described as: k，(n) Wherein, Indicates the weight of , that is, the center error weight, and φ(·) indicates the convex conjugate function of the exponential function.​​ In step 4, the acquisition process for the subproblem on the kernel centers is: fix the value of , and let this value be Convert the augmented optimization problem into a subproblem on the kernel centers, described as: In step 4, the subproblem regarding the center error weight is obtained by fixing the value of the kernel center c k，(n) and denoting this value as The augmented optimization problem is converted into a subproblem regarding the center error weight, described as:

5. The robust elliptical extended object tracking method based on variable center maximum correlation entropy criterion according to claim 4, characterized in that The specific process of the step 5 is: Step 5.1: Fixing the kernel bandwidth σ k，(n) The value of σ is fixed to be and the kernel center c k，(n) is fixed to be Then a cost function on r k is proposed based on the variable center maximum correlation entropy criterion, described as: where, denotes the cost function on r k , and The kernel function in (5) is a Gaussian kernel function, L = 4 + 2J k , ||·|| denotes the 2-norm, G1 denotes the kernel function with kernel bandwidth 1, denotes the kernel function with kernel bandwidth , and b k，(i) denotes the zero-centered error vector b k related to r k , the i-th element in b Step 5.2: Let The cost function for r k is transformed into a cost function with known σ k，(n) and c k，(n) is described as: where, represents the cost function with known σ k，(n) and c k，(n) , represents the Gaussian kernel function with kernel bandwidth , ξ k，(m) represents the mth element of ξ k , and Γ k，(m，：) represents the mth row of Γ k . Step 5.3: Substitute and into to construct the optimization problem based on the variable-centred maximum correlation entropy criterion with known k，(n) and k，(n) c Step 5.4: Transform the optimization problem equivalently into an augmented optimization problem, described as: where κ m is the prediction error weight, is the measurement error weight, and φ(·) denotes the convex conjugate function of the exponential function, Step 5.5: Fixing Kappa m and The values of these two quantities are denoted and The augmented optimization problem is transformed into a subproblem in r k which is described as: The subproblem in r k is equivalently transformed into a linear least squares problem in r k which is described as: The value of r is fixed at 0.5 k This value is denoted as The augmented optimization problem is converted into sub-problems on the prediction error weights and the measurement error weights, described as: where, 6. The robust elliptical extended object tracking method based on variable center maximum correlation entropy criterion according to claim 5, characterized in that The process of the step 6 is: Step 6.1: let s represent the number of outer iterations, and the initial value of s is 1; Step 6.2: During the s-th outer iteration, and Substitute kernel bandwidth σ k，(n) In the convex problem of estimating subproblems, the solution yields μ at the s-th outer iteration. k，(n) The optimal solution, i.e., obtaining σ at the s-th outer iteration. k，(n) optimal solution Where s = 1 For c k，(n) initial value For r k initial value Set to 0, To pass through c k，(n) Set to 0, σ k，(n) Let it be 1, κ m and Set all to -1 and substitute them into Solving for s, we find that when s>1 c represents the value of c in the (s-1)th outer iteration. k，(n) The optimal solution This indicates that r represents the value of the outermost iteration in the (s-1)th iteration. k The optimal solution; Step 6.3: In the s-th outer iteration, inner iterations are performed for c k，(n) as follows: Step 6.3.1: let q represent the number of inner iterations, and the initial value of q is 1; Step 6.3.2: In the s-th outer iteration, the q-th inner iteration, we have and Substitute into the subproblem for the core center, we obtain the optimal solution of c k，(n) in the q-th inner iteration of the s-th outer iteration where q = 1 is the initial value of c in the beginning of the inner iteration in the s-th outer iteration is set to -1, q > 1 denotes the optimal solution of c in the q-1-th inner iteration of the s-th outer iteration is obtained by substituting into ; In the qth inner iteration in the st outer iteration, we have and Substitute into the subproblem for the center error weight, we obtain the optimal solution in the qth inner iteration in the st outer iteration Step 6.3.3: judge the first inner iteration condition if yes, then set as the optimal solution of c k，(n) at the s-th outer iteration Step 6.4 is executed again, if no, then set q = q + 1, and return to Step 6.3.2 to continue, wherein q = 1 is the initial value of c k，(n) at the beginning of the inner iteration in the s-th outer iteration process equals to q > 1 represents the optimal solution of c k，(n) at the q-1-th inner iteration in the s-th outer iteration Step 6.4: In the s-th outer iteration, an inner iteration is performed for r k as follows: Step 6.4.1: let q represent the number of inner iterations, and the initial value of q is 1; Step 6.4.2: In the qthinner iteration in the stouter iteration, calculate and Substitute into the linear least square problem for r k , and solve to obtain the optimal solution for r k in the qthinner iteration in the stouter iteration where q = 1 is the initial value of κ m at the beginning of the inner iteration in the stouter iteration is the initial value of at the beginning of the inner iteration in the stouter iteration and are both set to -1, q > 1 denotes the optimal solution for κ m in the q-1thinner iteration in the stouter iteration, denotes the optimal solution for in the q-1thinner iteration in the stouter iteration, denotes the Kalman gain, Then calculate the estimation error covariance matrix of where I denotes the identity matrix; In the qth inner iteration in the st outer iteration, we have Substituting into the sub-problems for the prediction error weight and the measurement error weight, we obtain the optimal solution for κ m and where ​​ Step 6.4.3: judge the second inner iteration condition if it is true, then set as the optimal solution of r k in the s-th outer iteration set as the estimated error covariance matrix of r perform step 6.5 again, if it is not true, then set q = q + 1, and then return to step 6.4.2 to continue, wherein q = 1 is the initial value of r k at the beginning of the inner iteration in the s-th outer iteration process equal to q > 1 is the optimal solution of r k in the q-1-th inner iteration in the s-th outer iteration​ Step 6.5: Determine if the outer iteration condition is true. If it is true, then... Corresponding to σ k，(n) The estimated value c k，(n) The estimated value r k The estimated value Then As The estimation error covariance matrix R k Then, through the state transition equation, and R k Convert to r respectively k+1 One-step prediction value and its prediction error covariance matrix If the condition is not met, let s = s + 1, and then return to step 6.2 to continue execution. The outer iteration condition is whether s is less than or equal to the preset number of iterations.

7. The robust elliptical extended object tracking method based on variable center maximum correlation entropy criterion according to claim 6, characterized in that The specific process of the step 7 is: Step 7.1: Let z k denote the corresponding random vector, let y k denote the corresponding random vector, let denote the covariance matrix of z k , let denote the covariance matrix of y k , and let denote the relation between z and y where n k denotes the set of integers from 1 to n ; then compute an approximation of z as where denotes the mean of z , and compute an approximation of y as where is positive definite;​ Step 7.2: According to Obtain α k rough estimate l k，(1) rough estimate l k，(2) rough estimate in, express The element in the first row and first column, express The element in the 1st row and 2nd column, express The element in the second row and second column; thus, obtain p. k rough estimate Step 7.3: Calculation of the estimated error covariance matrix In detail as follows: Step 7.3.1: Definition of the estimated error vector Δp k , Step 7.3.2: let Step 7.3.3: Definition wherein, corresponding to the first row, first column element, the first row, second column element, the second row, second column element, corresponding to the first row, first column element, the first row, second column element, the second row, second column element, corresponding to the first row, first column element, the first row, second column element, the second row, second column element, corresponding to the first row, first column element, the first row, second column element, the second row, second column element, S k，(1，：) represents the first row vector of S k represents the second row vector of S k，(2，：) represents the first row vector of S k represents the second row vector of S represents the first element of represents the second element of represents the first element of represents the second element of n,(1,1) represents the first column element of the first row of C n represents the first column element of the first row of C n，(1，2) represents the second column element of the first row of C n represents the second column element of the first row of C ，(2,2) represents the second column element of the second row of C n represents the second column element of the second row of C Step 7.3.4: Compute the covariance matrix Cσof Δσ, the element in the first row, first column of Cσis the element in the first row, second column of Cσis the element in the second row, second column of Cσis ， The element in the first row and third column of Cσ is The element in the second row and third column of Cσ is The element in the 3rd row and 3rd column of Cσ is Step 7.3.5: The following compounds were prepared according to the procedure described in Step 7.3.4: Substitution of In the mean time, and for Performing a first order Taylor expansion at σ, respectively, gives: Δp k = D Δσ, where the first row of D is The second row of D is ， The third row of D is Step 7.3.6: Calculation Step 7.4: Update the estimate of p k where K p represents the Kalman gain corresponding to the Kalman filter equation, and then calculate the estimation error covariance matrix Ξ k ​​​

Images