CN116577750A

CN116577750A - Kalman filtering algorithm based on intermediate transition state

Info

Publication number: CN116577750A
Application number: CN202310546341.8A
Authority: CN
Inventors: 程婷; 曹聪冲; 宋佳铭; 刘璐清; 王宇萌; 恒思宇; 王元卿
Original assignee: University of Electronic Science and Technology of China
Current assignee: University of Electronic Science and Technology of China
Priority date: 2023-05-15
Filing date: 2023-05-15
Publication date: 2023-08-11

Abstract

The invention belongs to the field of target tracking, and provides a Kalman filtering algorithm based on an intermediate transition state. When the measurement nonlinearity degree is higher, the measurement error under the Cartesian coordinate system shows non-Gaussian characteristics, so that the algorithm constructs an intermediate transition state under the measurement coordinate system based on the traditional Kalman filtering algorithm, and deduces the conversion relation between the state under the Cartesian coordinate system and the transition state under the measurement coordinate system. The algorithm considers the radial speed, wherein the prediction and iteration processes are performed under a Cartesian coordinate system, and the measurement updating process is performed under a spherical coordinate based on an intermediate transition state; by means of state transition, the whole filtering process is performed in a linear environment. The algorithm solves the problem of measuring nonlinearity, and improves the filtering tracking precision and the stability of the filtering algorithm when the measuring nonlinearity degree is higher.

Description

Kalman filtering algorithm based on intermediate transition state

Technical Field

The invention belongs to the field of target tracking, and provides a Kalman filtering algorithm based on an intermediate transition state.

Background

With the advancement of technology, target tracking has been widely used in the fields of military and civil use. Radar is an important device for acquiring information, in an actual radar system, measurement information such as distance, angle, radial speed and the like of a target is usually acquired in a polar coordinate system or a spherical coordinate system, and a system state equation is often established in a Cartesian coordinate system, so that the problem of measurement nonlinearity exists in the tracking process of the radar target. There are two general solutions to the problem of measuring nonlinearity: nonlinear filtering and measurement conversion methods.

Extended kalman filtering (Extended Kalman Filter, EKF) is a nonlinear filtering algorithm based on taylor expansion (see literature: lcondes, c.t., control and Dynamic System, nonlinear and Kalman Filtering tech.academic Press, 1983). EKF solves the measurement nonlinearity problem, but when the degree of nonlinearity of the equivalent measurement with respect to the system state is high, the tracking error will be large. The literature (Julie S, uhlmann J, durrant Whyte H F. Anew method for the nonlinear transformation of means and covariances in filters and estimations [ J ]. IEEE Transactions on Automatic Control,2000,45 (3): 477-482.) proposes unscented Kalman filtering (Unscented Kalman Filter, UKF), which approximates the posterior probability density of nonlinear system states by unscented transformations. UKF does not need to linearize the nonlinear function, and the calculation precision can reach the Taylor precision of at least three orders, namely UKF has the advantages of high filtering precision, good convergence and the like. However, for a high-dimensional system, the UKF needs to select reasonable parameters to achieve high precision. The Particle Filter (PF) algorithm is a filtering method based on the sequence monte carlo (see literature: blame Meng En, circumnavigation Han Dan. Particle Filter target tracking algorithm overview [ J ] computer engineering and applications 2019, 55 (5): 8-17.). PF is not limited by linearization error or Gaussian noise assumption, performance is similar to UKF algorithm, but after iteration for several times, most particles have larger attenuation, a large number of particles need to be selected or resampled, and calculation amount is large. The literature (ARASARATNAM I, HAYKIN S, eliott R J. Discrete-time nonlinear filtering algorithms using Gauss-Hermite quadrature [ J ]. Proceedings of the IEEE,2007,95 (5): 953-977.) proposes a gaussian-hermite integral filter (Gauss-Hermite Quadrature Filter, GHQF) algorithm, which performs numerical approximation using a gaussian-hermite integral rule, to obtain better target tracking accuracy, but the GHQF expands single-variable gaussian integral to multidimensional integral, whose computational complexity grows exponentially with system dimension, resulting in tracking not having real-time. The literature (I.Arasaratnam, S.Haykin.Cubature Kalman Filters [ J ]. IEEE Transactions on Automatic Control,2009,54 (6): 1254-1269) proposes a volume Kalman filtering (Cubature Kalman Filter, CKF) algorithm, the volume Kalman filtering is based on a third-order spherical-radial volume criterion, the weights of all volume points are equal, the filtering precision is higher, and the calculation complexity is lower. To further improve the stability of CKF, literature (I.Arasaratnam, S Haykin, and T.R. Hurd. Cubature Kalman filtering for continuous-discrete systems: theory and simulations [ J ]. IEEE Trans. Signal Process,2010,58 (10): 4977-4993) introduces an orthogonal decomposition into the covariance matrix of CKF, forming a Square root form of CKF, i.e., square root volume Kalman Filter (Square-Root Cubature Kalman Filter, SRCKF) algorithm. The square root of the covariance matrix is used in the SRCKF iterative operation, so that the symmetry and positive (semi-) qualitative property of the covariance matrix are ensured, and the tracking precision and stability of the filtering algorithm are further improved.

Another approach to solving measurement nonlinearities is measurement transformation. The traditional measurement conversion kalman filtering (Converted Measurement Kalman Filter, CMKF) algorithm converts measurement information in a polar coordinate system or a spherical coordinate system into a cartesian coordinate system by using coordinate transformation, and performs filtering processing by KF (see document: p.suchomski.explicit Expressions for Debiased Statistics of 3D Converted Measurements[J ]. IEEE trans.aerospace and Electronic System,35 (1): 368-370). However, the conversion result is biased due to the nonlinearity of the coordinate conversion, and the tracking accuracy is low. The deskewing measurement and conversion Kalman filter (Debiased Converted Measurement Kalman Filter, DCMKF) algorithm performs additive deskewing treatment on the CMKF, so that errors of the traditional measurement and conversion Kalman filter algorithm are eliminated (see literature: bordonaro S, willett P, bar-Shalom Y.decorrellated unbiased converted measurement Kalman filter [ J ]. IEEE Transactions on Aerospace and Electronic Systems,2014,50 (2): 1431-144). However, the above-mentioned research content about measurement conversion only contains position measurement information, and the literature (A.Farina and F.A. studio Data processing, vol. I: introduction and tracking, vol. II: advanced Topics and Applications [ M ]. Research Studies Press, letchworth, 1985) suggests that the tracking accuracy of the algorithm can be further improved if Doppler equivalent measurement information is fully utilized. When introducing radial velocity, the problem faced is that radial velocity measurements are very nonlinear to the target state, while the literature (Y. Bar-Shalm. Negative Correlation and Optimal Tracking with Doppler Measurements [ J ]) IEEE Trans. Aerospace and Electronic Systems,2001,37 (3): 1117-1120) states that radial distance is related to the error statistics of radial velocity for some waveforms. Sequential filtering (Sequential Filter, SQ) constructs pseudo-measurements using the product of radial distance and radial velocity to reduce the nonlinearity of radial velocity relative to the target operating state, and then separately filtering the position measurements and the pseudo-measurements: after the decorrelation process of the position measurement and the pseudo measurement conversion error, the position measurement is processed by a Kalman filter process, and the pseudo measurement is processed by a second-order extended Kalman filter process (see literature: duan Z, han CLi X R2007 Sequential Nonlinear Tracking Filter with Range-rate Measurements in Spherical Coordinates IEEE Trans. Aerospace and Electronic Systems (1) 239-250.). However, the statistical characteristics of conversion errors derived on the basis of measurement values are adopted, so that the filter gain and the measurement errors have certain correlation (Peng Han, cheng Ting. Measurement conversion sequential filtering target tracking [ J ] based on prediction information is adopted by system engineering and electronic technology 2019,41 (3): 549-554), measurement conversion sequential filtering algorithm based on prediction information is adopted, and the nesting condition method is utilized, so that the correlation between the measurement information and state estimation is eliminated, and the tracking precision of the filtering algorithm is further improved.

However, when radar ranging accuracy is high and angular accuracy is low, the statistical properties of the measurement errors in the Cartesian coordinate system are contrary to the assumed Gaussian distribution (see, for example, K.Romeo, P.Willett and Y. Bar-Shalm, "Particle filter tracking for banana and contact lens problems," in IEEE Transactions on Aerospace and Electronic Systems, vol.51, no.2, pp.1098-1110,April 2015,doi:10.1109/TAES. 2014.130670.). Although this document proposes an improved PF algorithm to solve the non-gaussian problem, the PF requires a large number of particle sampling and screening processes, resulting in a complex algorithm calculation and a low tracking real-time. Meanwhile, when the Gaussian assumption is against the back, the tracking performance of the measurement conversion method is drastically reduced, and even the situations of filtering divergence and the like occur. Therefore, in order to solve the above problems, the present invention provides a kalman filtering algorithm based on an intermediate transition state, wherein radial velocity measurement is considered, the prediction and iteration processes are performed under a cartesian coordinate system, and the measurement update process is performed under a measurement coordinate system based on the intermediate transition state.

Disclosure of Invention

Assuming that at time k-1, the state estimation and estimation error covariance matrices of the obtained target are respectively And P _k-1|k-1 Wherein->And->Representing the position estimate of the system in x, y, z direction of the object at time k-1,/, respectively>And->Is a velocity estimate for the corresponding direction. Assume that the measurement vector for the target obtained at time k is +.>Wherein->And->Measuring distance, pitch angle, azimuth angle and radial velocity respectively, and measuring noise variance is +.>Andand assuming that the azimuth angle, pitch angle measurement noise and radial velocity measurement noise are mutually independent, and the correlation coefficient between the distance and radial velocity measurement noise is ρ, an iterative process from k-1 to k time based on the Kalman filtering algorithm of the intermediate transition state specifically comprises the following steps:

step 1: and (5) updating time:

(1) Predicting the state at the moment k:

wherein F is a state transition matrix,a state prediction vector at time k.

(2) Calculating a prediction error covariance matrix:

P _k|k-1 ＝FP _k-1|k-1 F ^T +Q _k-1 (2)

in which Q _k-1 Process noise for time k-1Covariance matrix, P _k|k-1 Is a prediction error covariance matrix.

Step 2: state transition:

(1) Decomposing a prediction estimation error covariance matrix:

S _k|k-1 ＝chol(P _k|k-1 ) (3)

wherein S is _k|k-1 For the square root of the prediction error covariance matrix, chol (·) is Cholesky decomposition of the matrix.

(2) Constructing an equal weight sampling point:

in the method, in the process of the invention,for the predicted sampling point constructed at time k, m is the sampling point, satisfying m=2n, n is the dimension of the state vector, +.>[1] _i Is a point set [1 ]]Is the ith column of (1) point set]The method comprises the following steps:

(3) Sampling points are predictedDenoted as->Wherein->And->For predicting the components for the position in x, y and z directions,/->And->For the velocity prediction component of the corresponding direction, it is converted into a measurement coordinate system:

in the method, in the process of the invention,and->Predicted components of distance, pitch and azimuth, respectively, < >>And->For the radial distance, pitch velocity and azimuth velocity components, these components are represented by intermediate transition state prediction sampling points in the measured coordinate system after conversion +.>

(4) Calculating a predicted value of the intermediate transition state under a measurement coordinate system:

in the method, in the process of the invention,is a predicted value for measuring the intermediate transition state in the coordinate system.

(5) Calculating a prediction error covariance matrix under a measurement coordinate system:

in the method, in the process of the invention,an error covariance matrix is estimated for the predictions in the metrology coordinate system.

Step 3: and (5) measurement and update:

(1) Innovation covariance matrix:

in the method, in the process of the invention,for the innovation covariance matrix, H _Mid And R is _k The measurement matrix and the measurement noise covariance matrix, respectively, can be expressed as:

(2) Calculating Kalman gain:

wherein K is _Mid Is a Kalman gain matrix.

(3) Calculating an estimated vector of intermediate transition states in a metrology coordinate system:

in the method, in the process of the invention,a vector is estimated for the intermediate transition state at time k.

(4) Calculating an estimated error covariance matrix under a measurement coordinate system:

step 4: state transition:

(1) Decomposing an estimation error covariance matrix under a measurement coordinate system:

in the method, in the process of the invention,is the square root of the error covariance matrix.

(2) Constructing an equal weight sampling point:

in the method, in the process of the invention,an estimated sampling point is constructed for time k.

(3) Sampling points will be estimatedDenoted as->Wherein->And->Estimating components for range, pitch and azimuth, +.>And->For radial distance, pitch angle velocity and azimuth angle velocity components, it is converted into the original cartesian coordinate system:

in the method, in the process of the invention,and->For estimating the components in the x, y and z directions, +.>And->For the velocity estimation components of the corresponding direction, the components are represented by state estimation sampling points in the original Cartesian coordinate system

(4) Calculating a state estimation vector at the moment k:

in the method, in the process of the invention,is the state estimate at time k.

(5) Calculating an estimation error covariance matrix:

wherein P is _k|k Is the estimated error covariance matrix at time k.

Principle of the invention

In the target tracking process, it is generally assumed that the measurement noise follows a gaussian distribution of zero mean, however, after the measurement is converted from the measurement coordinate system to the rectangular coordinate system, the noise no longer follows the gaussian distribution. The degree of nonlinearity of the measurement with respect to the state can be measured by gamma (see D.Lerro and Y.Bar-Shalom, "Tracking with debiased consistent converted measurements versus EKF," in IEEE Transactions on Aerospace and Electronic Systems, vol.29, no.3, pp.1015-1022,July 1993,doi:10.1109/7.220948.), and the nonlinear measurement index gamma is expressed as

As shown in fig. 1 (a), when γ is small, the measurement nonlinearity is small, and when converted into a cartesian coordinate system, the noise distribution relatively conforms to the gaussian distribution. As shown in fig. 1 (b), when γ is large, the degree of nonlinearity of the measurement is large, and the measurement after conversion to the cartesian coordinate system no longer follows gaussian distribution. Therefore, when the measurement nonlinearity is small, the target can be effectively tracked by adopting the traditional nonlinear filtering and measurement conversion method; and when the measurement nonlinearity degree is large, the filtering method based on the assumed zero-mean Gaussian measurement noise fails. Based on the problems, the invention provides a Kalman filtering algorithm based on an intermediate transition state.

The Kalman filtering algorithm (Mid-KF) based on the intermediate transition state is a nonlinear filtering algorithm based on sampling points. The Mid-KF time updating is performed under the original coordinate system, the measurement updating is performed under the measurement coordinate system by using state conversion, so that errors caused by linearization of nonlinear measurement are avoided, meanwhile, the complicated statistical characteristics of measurement conversion errors are not required to be deduced, the tracking precision is improved, and meanwhile, the complexity of a filtering algorithm is reduced.

The Mid-KF algorithm firstly performs time update to obtain a prediction state and a prediction error covariance matrix, as shown in step 1. Measurement nonlinearity problems exist because the target motion state is modeled in a Cartesian coordinate system, whereas the measurement is acquired in a spherical coordinate system. To solve this problem, while maintaining the gaussian distribution of measurement noise, the Mid-KF algorithm updates the measurement by establishing an intermediate transition state in the measurement coordinate system. If the state of the target motion is set as follows:

the intermediate transition state is constructed as follows:

from the above, the intermediate transition state X _Mid And the measurement Z is in a linear relationship, and the measurement matrix is shown as a formula (15). Thus, the filtering in the intermediate transition state is linear filtering. State X to transition state X _Mid The conversion relation of (2) is:

then, equation (6) in step 2 is obtained.

After the state transition is completed, measurement updating is performed under the measurement coordinate system, as shown in step 3. Since there is no nonlinear relationship between the measurement and the transition state and the measurement noise conforms to the assumed gaussian distribution, this process is a standard kalman filter update process. Thus, the estimation of the transition state obtained in this state is the optimal linear estimation. In obtaining transition state X _Mid Optimal estimation of (a)Then, the transition state needs to be converted into a Cartesian coordinate system again for the next iteration operation. Intermediate transition state X _Mid The conversion relation of the original state X is as follows:

x＝rcosβcosθ (39)

y＝rcosβsinθ (41)

z＝rsinβ (43)

then, equation (22) in step 4 is obtained.

And obtaining the optimal estimation and estimation error covariance matrix of the target state under the original coordinate system after conversion, as shown in the formulas (28) and (29) in the step 4.

Drawings

Fig. 1 (a) and 1 (b)) shows an uncertainty region distribution diagram corresponding to γ=0.01 and γ=1 in a cartesian coordinate system

FIG. 2 is a diagram of a target motion trajectory in a non-maneuver scenario

FIG. 3 (FIGS. 3 (a) through 3 (d)) is a comparison of location, velocity, distance, pitch angle and azimuth angle RMSE at the scene

FIG. 4 (FIGS. 4 (a) through 4 (d)) is a comparison of position, velocity, distance, pitch angle and azimuth angle RMSE under scenario two

FIG. 5 (FIGS. 5 (a) through 5 (d)) is a comparison of position, velocity, distance, pitch angle and azimuth angle RMSE under field three

Detailed Description

Suppose that radar tracks maneuvering targets in a two-dimensional planeThe radar is located at the origin of coordinates, the sampling period is 1s, the initial position of the target is (15 km,10 km), and the initial speed is (100 m/s,100m/s,0 m/s). Assuming that the target moves linearly at a uniform speed, the tracking time is 200s. Fig. 2 shows a motion trajectory of the object. Radar acquisition measurements include range r, azimuth angle beta, azimuth angle theta, and doppler measurementsThe measured noise is zero-mean Gaussian white noise. The simulation experiment contains the following 3 different scenarios as shown in table 1.

Table 1 simulation scene parameter settings

The present invention provides an improved intermediate transition state based Kalman filtering algorithm (Mid-KF) to track the target and compare it with a predicted value based de-biased decorrelation metrology conversion Kalman filtering algorithm (DCMKF-P) (see document Peng Han, cheng Ting. Metrology conversion sequential filtering target tracking based on predicted information [ J ]. Systems engineering and electronics, 2019,41 (03): 549-554;Duan Z,Han C Li X R2007Sequential Nonlinear Tracking Filter with Range-rate Measurements in Spherical Coordinates IEEE Trans. Aerospace and Electronic Systems (1) 239-250) and a square root volume Kalman filtering algorithm (SRCKF) (see document I. Araatnam, S Haykin, and T.R. Hurd. Cubature Kalman filtering for continuous-discrete systems: theory and simulations [ J ]. IEEE. Trans. Signal Process,2010,58 (10): 4977-4993.).

The method comprises the steps of taking a position estimation mean square error (position RMSE), a distance estimation root mean square error (distance RMSE), a pitch angle estimation root mean square error (pitch angle RMSE) and an azimuth angle estimation root mean square error (azimuth angle RMSE) at the moment k as measurement indexes of a filtering algorithm, wherein the specific expression of the position RMSE is shown in the following formula:

wherein, the liquid crystal display device comprises a liquid crystal display device,and->For the true and estimated values of the position of the target in the x-direction at time k in the mth monte carlo experiment,and->For the true and estimated values of the position of the target y-direction at time k in the mth Monte Carlo experiment, +.>And->And M is the simulation times of Monte Carlo for the true value and the estimated value of the position of the target in the z direction at the k moment in the mth Monte Carlo experiment. The distance, pitch and azimuth RMSE are calculated in a similar way as the position RMSE. The statistics of 500 monte carlo simulations are given below. The simulated time consumption comparison table is as follows:

table 2 time-consuming comparison table

As can be seen from fig. 3 to fig. 5, compared with the DCMKF-P algorithm using measurement conversion and the srkf algorithm based on volume points, the Mid-KF algorithm provided by the invention has higher tracking accuracy, and the advantages of the Mid-KF algorithm are more obvious along with the increase of the measurement nonlinearity degree. In the first scene, the performances of the three algorithms are not poor, and the tracking precision of Mid-KF is slightly superior to that of the other two algorithms; however, as the angular velocity measurement error increases, i.e., the degree of nonlinearity of the measurement relative to the target state increases, the tracking performance of the SRCKF and the DCMKF-P gradually deteriorates. In scenario two, it can be seen that the position RMSE of the srkf has diverged due to the increase in the degree of measurement nonlinearity, and the position RMSE curve diverges due to the increase in tracking error of the pitch angle and the azimuth angle, although the tracking accuracy of the srkf to the distance remains high. Meanwhile, in the second scene, it can be found that although the location RMSE of the DCMKF-P still has a converging tendency, its distance RMSE is always larger. As the measurement error continues to increase, the tracking performance of DCMKF-P also gradually deteriorates. In scenario three, it can be found that only Mid-KF has high tracking accuracy, and that RMSE of the other two algorithms has diverged.

The present simulation uses MATLAB 2019a to verify the tracking performance of the algorithm with an AMD Ryzen 7 5800h processor and 16.0GB memory. As can be seen from Table 2, mid-KF is less time consuming and less complex than SRCKF. When the measurement nonlinearity degree is small, the tracking performance of the two is equivalent; and when the measurement nonlinearity degree is larger, the Mid-KF can ensure the tracking precision and reduce the calculated amount.

In conclusion, compared with the existing nonlinear filtering algorithm, the Kalman filtering algorithm based on the intermediate transition state has the advantages of improved tracking precision and lower calculation complexity, and is an effective target tracking nonlinear filtering method.

Claims

1. The Kalman filtering algorithm based on the intermediate transition state is characterized in that:

assuming that at time k-1, the state estimation and estimation error covariance matrices of the obtained target are respectively And P _k-1|k-1 Wherein->And->Representing the position estimate of the system in x, y, z direction of the object at time k-1,/, respectively>And->Estimating the speed of the corresponding direction; assume that the measurement vector for the target obtained at time k is +.>Wherein->And->Measuring distance, pitch angle, azimuth angle and radial velocity respectively, and measuring noise variance is +.>And->And assuming that the azimuth angle, pitch angle measurement noise and radial velocity measurement noise are mutually independent, and the correlation coefficient between the distance and radial velocity measurement noise is ρ, an iterative process from k-1 to k time based on the Kalman filtering algorithm of the intermediate transition state specifically comprises the following steps:

step 1: and (5) updating time:

(1) Predicting the state at the moment k:

wherein F is a state transition matrix,a state prediction vector at the moment k;

(2) Calculating a prediction error covariance matrix:

P _k|k-1 ＝FP _k-1|k-1 F ^T +Q _k-1 (2)

in which Q _k-1 Process noise covariance matrix for k-1 time, P _k|k-1 A covariance matrix of the prediction error;

step 2: state transition:

(1) Decomposing a prediction estimation error covariance matrix:

wherein S is _k|k-1 For the square root of the prediction error covariance matrix, chol (·) is Cholesky decomposition of the matrix;

(2) Constructing an equal weight sampling point:

(3) Sampling points are predictedDenoted as->Wherein->Andfor predicting the components for the position in x, y and z directions,/->And->For the velocity prediction component of the corresponding direction, it is converted into a measurement coordinate system:

in the method, in the process of the invention,the method is used for measuring a predicted value of an intermediate transition state in a coordinate system;

in the method, in the process of the invention,estimating an error covariance matrix for prediction under a measurement coordinate system;

step 3: and (5) measurement and update:

(1) Innovation covariance matrix:

(2) Calculating Kalman gain:

wherein K is _Mid Is a Kalman gain matrix;

in the method, in the process of the invention,estimating a vector for the intermediate transition state at time k;

step 4: state transition:

in the method, in the process of the invention,is the square root of the error covariance matrix;

(2) Constructing an equal weight sampling point:

in the method, in the process of the invention,an estimated sampling point constructed for time k;

in the method, in the process of the invention,and->For estimating the components in the x, y and z directions, +.>And->For the speed estimation components of the corresponding direction, these components are represented by state estimation sampling points in the original Cartesian coordinate system +.>

(4) Calculating a state estimation vector at the moment k:

in the method, in the process of the invention,state estimation at time k;

(5) Calculating an estimation error covariance matrix:

wherein P is _k|k Is the estimated error covariance matrix at time k.