A Kalman filtering algorithm based on an intermediate transition state

Abstract

The present invention belongs to the field of target tracking, and proposes a Kalman filtering algorithm based on an intermediate transition state. When the measurement non-linearity degree is relatively high, the measurement error in the Cartesian coordinate system exhibits non-Gaussian characteristics. Therefore, the algorithm of the present invention is based on the traditional Kalman filtering algorithm, constructs an intermediate transition state in the measurement coordinate system, and derives the conversion relationship between the state in the Cartesian coordinate system and the transition state in the measurement coordinate system. The algorithm takes the radial velocity into account, the prediction and iteration processes are carried out in the Cartesian coordinate system, and the measurement update process is carried out in the spherical coordinate system based on the intermediate transition state; through the method of state conversion, the entire filtering process is carried out in a linear environment. The algorithm of the present invention solves the problem of measurement non-linearity, and at the same time improves the filtering tracking accuracy and the stability of the filtering algorithm when the measurement non-linearity degree is relatively high.

Description

Technical Field

[0001] The present invention belongs to the field of target tracking, and proposes a Kalman filtering algorithm based on an intermediate transition state. Background Art

[0002] With the progress of technology, target tracking has been widely applied in military, civilian and other fields. Radar is an important device for obtaining information. In an actual radar system, measurement information such as the distance, angle, and radial velocity of a target is usually obtained in a polar coordinate system or a spherical coordinate system, while the system state equation is often established in a Cartesian coordinate system. Therefore, there is a problem of non-linearity in measurement during the radar target tracking process. There are generally two solutions to the problem of measurement non-linearity: non-linear filtering and measurement conversion methods.

[0003] The Extended Kalman Filter (EKF) is a nonlinear filtering algorithm based on Taylor expansion (see reference: Lcondes, C.T., Control and Dynamic System, Nonlinear and Kalman Filtering Technique. Academic Press, 1983). EKF solves the problem of measurement nonlinearity. However, when the degree of nonlinearity of the measurement relative to the system state is high, it will lead to a large tracking error. The literature (Julier S, Uhlmann J, Durrant Whyte H F. A new method for the nonlinear transformation of means and covariances in filters and estimations[J]. IEEE Transactions on Automatic Control, 2000, 45(3): 477-482.) proposed the Unscented Kalman Filter (UKF). UKF approximates the posterior probability density of the nonlinear system state through the unscented transformation. UKF does not require linearization of the nonlinear function, and the calculation accuracy can reach at least the Taylor accuracy of the third order or higher, that is, UKF has the advantages of high filtering accuracy and good convergence. However, for high-dimensional systems, UKF needs to select reasonable parameters to achieve high accuracy. The Particle Filter (PF) algorithm is a filtering method based on sequential Monte Carlo (see reference: Jiu Mengen, Zhou Hang, Han Dan. A review of particle filter target tracking algorithms[J]. Computer Engineering and Applications, 2019, 55(5): 8-17.). PF is not restricted by linearization errors or Gaussian noise assumptions, and its performance is similar to that of the UKF algorithm. However, after several iterations, most particles have a large attenuation, and a large number of particles need to be selected or resampled, resulting in a large amount of calculation.The literature (ARASARATNAM I, HAYKINS, ELLIOTT R J. Discrete-time nonlinear filtering algorithms using Gauss-Hermite quadrature[J]. Proceedings of the IEEE, 2007, 95(5): 953-977.) proposed the Gauss-Hermite Quadrature Filter (GHQF) algorithm. The GHQF uses the Gauss-Hermite quadrature rule for numerical approximation and can obtain better target tracking accuracy. However, the GHQF extends the univariate Gaussian integral to the multi-dimensional integral, and its computational complexity increases exponentially with the system dimension, resulting in non-real-time tracking. The literature (I. Arasaratnam, S. Haykin. Cubature Kalman Filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269) proposed the Cubature Kalman Filter (CKF) algorithm. The cubature Kalman filter is based on the third-order spherical-radial cubature criterion, and the weights of each cubature point are equal, with higher filtering accuracy and lower computational complexity. To further improve the stability of the CKF, the 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.) introduced orthogonal decomposition into the covariance matrix of the CKF to form the square-root form of the CKF, that is, the Square-Root Cubature Kalman Filter (SRCKF) algorithm. The square root of the covariance matrix is used in the SRCKF iterative operation, which ensures the symmetry and positive (semi)-definiteness of the covariance matrix and further improves the tracking accuracy and stability of the filtering algorithm.

[0004] Another method to solve measurement non-linearity is measurement transformation. The traditional Converted Measurement Kalman Filter (CMKF) algorithm uses coordinate transformation to convert measurement information in polar coordinate system or spherical coordinate system to Cartesian coordinate system, and then performs filtering processing through KF (see reference: P. Suchomski. Explicit Expressions for Debiased Statistics of 3D Converted Measurements[J]. IEEE Trans. Aerospace and Electronic System, 35(1): 368 - 370.). However, due to the non-linearity of coordinate transformation, the conversion result is biased and the tracking accuracy is relatively low. The Debiased Converted Measurement Kalman Filter (DCMKF) algorithm performs additive debiasing processing on CMKF, eliminating the error of the traditional converted measurement Kalman filter algorithm (see reference: Bordonaro S, Willett P, Bar-Shalom Y. Decorrelated unbiased converted measurement Kalman filter[J]. IEEE Transactions on Aerospace and Electronic Systems, 2014, 50(2): 1431 - 144). However, the above research content on measurement transformation only includes position measurement information. The literature (A. Farina and F. A. Studer. Rader Data Processing. vol.Ⅰ: Introduction and tracking, vol.Ⅱ: Advanced Topics and Applications[M]. Research Studies Press, Letchworth, 1985) proposes that if measurement information such as Doppler is fully utilized, the tracking accuracy of the algorithm can be further improved.When introducing the radial velocity, the problem faced is that the non-linearity between the radial velocity measurement and the target state is very large. At the same time, the literature (Y. Bar-Shalom. Negative Correlation and Optimal Tracking with Doppler Measurements[J]. IEEE Trans. Aerospace and Electronic Systems, 2001, 37(3): 1117-1120) points out that for some waveforms, the error statistical characteristics of the radial distance and the radial velocity are correlated. The Sequential Filter (SQ) constructs a pseudo-measurement using the product of the radial distance and the radial velocity to reduce the non-linearity of the radial velocity relative to the target operating state, and then filters the position measurement and the pseudo-measurement separately: after decorrelating the conversion errors of the position measurement and the pseudo-measurement, the position measurement is processed by the Kalman filter, and the pseudo-measurement is processed by the second-order extended Kalman filter (see the literature: Duan Z, Han C Li X R 2007 Sequential Nonlinear Tracking Filter with Range-rate Measurements in Spherical Coordinates IEEE Trans. Aerospace and Electronic Systems 43(1) 239-250.). However, using the conversion error statistical characteristics derived conditional on the measurement value results in a certain correlation between the filtering gain and the measurement error. The literature (Peng Han, Cheng Ting. Measurement Conversion Sequential Filter Target Tracking Based on Prediction Information[J]. Systems Engineering and Electronics, 2019, 41(3): 549-554) adopted the measurement conversion sequential filtering algorithm based on prediction information, using the nested conditional method to eliminate the correlation between the measurement information and the state estimation, and further improving the tracking accuracy of the filtering algorithm.

[0005] However, when the radar ranging accuracy is high and the angle measurement accuracy is low, the statistical characteristics of the measurement error in the Cartesian coordinate system are inconsistent with the assumed Gaussian distribution (see the literature: K. Romeo, P. Willett and Y. Bar-Shalom, "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 literature proposes an improved PF algorithm to solve the non-Gaussian problem, since PF requires a large number of particle sampling and screening processes, this algorithm is computationally complex and has low tracking real-time performance. At the same time, when this Gaussian assumption is violated, the tracking performance of the measurement conversion method will drop sharply, and even filtering divergence and other situations will occur. Therefore, in view of the above problems, the present invention proposes a Kalman filtering algorithm based on an intermediate transition state, taking into account the radial velocity measurement, with the prediction and iteration processes carried out in the Cartesian coordinate system, and the measurement update process carried out in the measurement coordinate system based on the intermediate transition state. The algorithm of the present invention solves the problem of measurement non-linearity, and at the same time improves the filtering tracking accuracy and the stability of the filtering algorithm when the measurement non-linearity degree is high. Summary of the Invention

[0006] Assume that at the (k - 1)th moment, the state estimate and the estimated error covariance matrix of the target have been obtained as and P k-1|k-1 respectively, where and represent the system's position estimates of the target in the x, y, and z directions at the (k - 1)th moment respectively, and are the velocity estimates in the corresponding directions. Assume that the measurement vector of the target is obtained at the kth moment as where and are the range, elevation angle, azimuth angle, and radial velocity measurements respectively, and the measurement noise variances are and respectively. It is also assumed that the azimuth angle, elevation angle measurement noise, and radial velocity measurement noise are independent of each other, and the correlation coefficient between the range and radial velocity measurement noise is ρ. Then, the specific steps of an iteration process of a Kalman filtering algorithm based on an intermediate transition state from the (k - 1)th moment to the kth moment are as follows:

[0007] Step 1: Time update:

[0008] (1) Predict the state at time k:

[0009]

[0010] Where F is the state transition matrix, is the state prediction vector at time k.

[0011] (2) Calculate the prediction error covariance matrix:

[0012] P k|k-1 = F P k-1|k-1 F T + Q k-1 (2)

[0013] Where Q k-1 is the process noise covariance matrix at time k-1, and P k|k-1 is the prediction error covariance matrix.

[0014] Step 2: State transformation:

[0015] (1) Decompose the prediction estimation error covariance matrix:

[0016] S k|k-1 = chol(P k|k-1 ) (3)

[0017] Where S k|k-1 is the square root of the prediction error covariance matrix, and chol(·) is the Cholesky decomposition of the matrix.

[0018] (2) Construct equally weighted sampling points:

[0019]

[0020] Where are the prediction sampling points constructed at time k, m is the number of sampling points, satisfying m = 2n, where n is the dimension of the state vector, [1] i is the i-th column of the point set [1], and the point set [1] is:

[0021]

[0022] (3) Represent the prediction sampling points as where and are the position prediction components along the x, y, and z directions, and are the velocity prediction components in the corresponding directions, and then transform them to the measurement coordinate system:

[0023]

[0024] In the formula, and are the predicted components of distance, elevation angle, and azimuth angle respectively, and are the components of radial distance, elevation angular velocity, and azimuth angular velocity. These components are represented by the predicted sampling points of the intermediate transition state in the measurement coordinate system after transformation.

[0025] (4) In the measurement coordinate system, calculate the predicted value of the intermediate transition state:

[0026]

[0027] In the formula, is the predicted value of the intermediate transition state in the measurement coordinate system.

[0028] (5) Calculate the predicted error covariance matrix in the measurement coordinate system:

[0029]

[0030] In the formula, is the predicted estimation error covariance matrix in the measurement coordinate system.

[0031] Step 3: Measurement update:

[0032] (1) Innovation covariance matrix:

[0033]

[0034] In the formula, is the innovation covariance matrix, H Mid and R k are the measurement matrix and the measurement noise covariance matrix respectively, and can be expressed as:

[0035]

[0036] (2) Calculate the Kalman gain:

[0037]

[0038] In the formula, K Mid is the Kalman gain matrix.

[0039] (3) Calculate the estimated vector of the intermediate transition state in the measurement coordinate system:

[0040]

[0041] In the formula, Is the intermediate transition state estimation vector at time k.

[0042] (4) Calculate the estimated error covariance matrix in the measurement coordinate system:

[0043]

[0044] Step 4: State transformation:

[0045] (1) Decompose the estimated error covariance matrix in the measurement coordinate system:

[0046]

[0047] In the formula, Is the square root of the error covariance matrix.

[0048] (2) Construct equal-weight sampling points:

[0049]

[0050] In the formula, Is the estimated sampling point constructed at time k.

[0051] (3) Express the estimated sampling point As Where And Are the distance, elevation angle, and azimuth angle estimation components, And Are the radial distance, elevation angular velocity, and azimuth angular velocity components, then transform them to the original Cartesian coordinate system:

[0052]

[0053] In the formula, And Are the position estimation components along the x, y, and z directions, And Are the velocity estimation components in the corresponding directions, and express these components using the state estimation sampling points in the original Cartesian coordinate system

[0054] (4) Calculate the state estimation vector at time k:

[0055]

[0056] In the formula, Is the state estimation at time k.

[0057] (5) Calculate the estimated error covariance matrix:

[0058]

[0059] where P k|k is the estimated error covariance matrix at time k.

[0060] Principle of the Invention

[0061] During the target tracking process, it is usually assumed that the measurement noise follows a Gaussian distribution with zero mean. However, after the measurement is transformed from the measurement coordinate system to the Cartesian coordinate system, the noise no longer follows a Gaussian distribution. The degree of nonlinearity of the measurement with respect to the state can be measured by γ (see the literature: 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 nonlinearity metric γ is expressed as

[0062]

[0063] As shown in Fig. 1(a), when γ is small, the degree of measurement nonlinearity is small, and when transformed to the Cartesian coordinate system, its noise distribution relatively conforms to the Gaussian distribution. As shown in Fig. 1(b), when γ is large, the degree of measurement nonlinearity is large, and the measurement after transformation to the Cartesian coordinate system no longer follows the Gaussian distribution. Therefore, when the measurement nonlinearity is small, traditional nonlinear filtering and measurement transformation methods can effectively track the target; when the degree of measurement nonlinearity is large, the filtering method based on the assumption of zero-mean Gaussian measurement noise fails. Based on the above problems, the present invention proposes a Kalman filtering algorithm based on an intermediate transition state.

[0064] The Kalman filtering algorithm based on an intermediate transition state (Mid-KF) is a sampling-point-based nonlinear filtering algorithm. The time update of Mid-KF is carried out in the original coordinate system, and the measurement update is carried out in the measurement coordinate system using state transformation, avoiding the error caused by the linearization of the nonlinear measurement. At the same time, it is not necessary to deduce the statistical characteristics of the complex measurement transformation error, which not only improves the tracking accuracy but also reduces the complexity of the filtering algorithm.

[0065] The Mid-KF algorithm first performs a time update to obtain the predicted state and the predicted error covariance matrix, as shown in Step 1. Since the target motion state is modeled in the Cartesian coordinate system, while the measurements are obtained in the spherical coordinate system, there exists a problem of measurement nonlinearity. To solve this problem and maintain the Gaussian distribution of the 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:

[0066]

[0067] Construct the intermediate transition state as:

[0068]

[0069] From the above equation, it can be seen that there is a linear relationship between the intermediate transition state X Mid and the measurement Z, and the measurement matrix is shown in Equation (15). Therefore, the filtering in the intermediate transition state is linear filtering. The conversion relationship from state X to the transition state X Mid is:

[0070]

[0071] Thus, Equation (6) in Step 2 is obtained.

[0072] After the state conversion is completed, the measurement update is performed in 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. Therefore, the estimation of the transition state obtained in this state is the optimal linear estimation. After obtaining the optimal estimation Mid of the transition state X , it is necessary to convert this transition state back to the Cartesian coordinate system for the next iteration operation. The conversion relationship between the intermediate transition state X Mid and the original state X is:

[0073] x = rcosβcosθ (39)

[0074]

[0075] y = rcosβsinθ (41)

[0076]

[0077] z = rsinβ (43)

[0078]

[0079] Thus, Equation (22) in Step 4 is obtained.

[0080] The optimal estimate of the target state and the estimated error covariance matrix in the original coordinate system are obtained after the transformation, as shown in Equations (28) and (29) in Step 4. Description of the Drawings

[0081] Figures 1(a) and (b) are respectively the distribution diagrams of the uncertainty regions corresponding to γ = 0.01 and γ = 1 in the Cartesian coordinate system

[0082] Figure 2 is the target motion trajectory diagram for the non-maneuvering scenario

[0083] Figures 3(a), (b), (c) and (d) are respectively the comparison diagrams of RMSE of position, velocity, distance, elevation angle and azimuth angle in Scenario 1

[0084] Figures 4(a), (b), (c) and (d) are respectively the comparison diagrams of RMSE of position, velocity, distance, elevation angle and azimuth angle in Scenario 2

[0085] Figures 5(a), (b), (c) and (d) are respectively the comparison diagrams of RMSE of position, velocity, distance, elevation angle and azimuth angle in Scenario 3 Detailed Implementation Manner

[0086] It is assumed that in a two-dimensional plane, a radar tracks a maneuvering target. The radar is located at the origin of coordinates, the sampling period is 1 s, the initial position of the target is (15 km, 15 km, 10 km), and the initial velocity is (100 m / s, 100 m / s, 0 m / s). It is assumed that the target moves in a uniform straight line and the tracking duration is 200 s. Figure 2 The motion trajectory diagram of the target is given. The measurements obtained by the radar include the distance r, azimuth angle β, elevation angle θ and Doppler measurement The measurement noise is zero-mean Gaussian white noise. The simulation experiment includes the following 3 different scenarios as shown in Table 1.

[0087] Table 1 Simulation Scenario Parameter Settings

[0088]

[0089] An improved Kalman filtering algorithm based on the intermediate transition state (Mid-KF) proposed in the present invention is used to implement the tracking of the target, and it is compared with the debiased and decorrelated measurement conversion Kalman filtering algorithm based on the predicted value (DCMKF-P) (see the literature: Peng Han, Cheng Ting. Target tracking of measurement conversion sequential filtering based on prediction information [J]. Systems Engineering and Electronics, 2019, 41(03): 549-554; Duan Z, Han C, Li X R. 2007. Sequential Nonlinear Tracking Filter with Range-rate Measurements in Spherical Coordinates. IEEE Trans. Aerospace and Electronic Systems, 43(1): 239-250.) and the square root cubature Kalman filtering algorithm (SRCKF) (see the 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.).

[0090] The mean square error of position estimation (position RMSE), root mean square error of distance estimation (distance RMSE), root mean square error of pitch angle estimation (pitch angle RMSE), and root mean square error of azimuth angle estimation (azimuth angle RMSE) at time k are used as the measurement indicators of the filtering algorithm. The specific expression of the position RMSE is shown as follows:

[0091]

[0092] where and are the true value and estimated value of the target's position in the x direction at time k in the m-th Monte Carlo experiment, and are the true value and estimated value of the target's position in the y direction at time k in the m-th Monte Carlo experiment, and are the true value and estimated value of the target's position in the z direction at time k in the m-th Monte Carlo experiment, and M is the number of Monte Carlo simulations. The calculation methods of the distance, pitch angle, and azimuth angle RMSE are similar to that of the position RMSE. The statistical results of 500 Monte Carlo simulations are given below. The comparison table of simulation time consumption is as follows:

[0093] Table 2 Comparison table of time consumption

[0094]

[0095] As can be seen from Figures 3 - 5, the Mid-KF algorithm proposed by the present invention has higher tracking accuracy compared with the DCMKF-P algorithm using measurement transformation and the SRCKF algorithm based on cubature points. Moreover, as the degree of measurement non-linearity increases, the advantages of the Mid-KF algorithm become more obvious. In Scenario 1, from the position dimension (see Figure 3(a)), distance dimension (see Figure 3(b)), pitch angle (see Figure 3(c)), and azimuth angle dimension (see Figure 3(d)), the performance of the three algorithms is not much different, and the Mid-KF has a slight advantage in tracking accuracy compared with the other two algorithms. However, as the angular velocity measurement error increases, that is, as the degree of measurement non-linearity relative to the target state increases, the tracking performance of the SRCKF and DCMKF-P gradually deteriorates. In Scenario 2, it can be seen that due to the increase in the degree of measurement non-linearity, the position RMSE of the SRCKF has diverged (see Figure 4(a)). Although the SRCKF maintains a high level of tracking accuracy for distance (see Figure 4(b)), the divergence of the position RMSE curve is caused by the increase in tracking errors for pitch angle and azimuth angle (see Figure 4(c) and Figure 4(d)). At the same time, in Scenario 2, it can be found that although the position RMSE of the DCMKF-P still has a tendency to converge (see Figure 4(a)), its distance RMSE is always large (see Figure 4(b)). As the measurement error continues to increase, the tracking performance of the DCMKF-P also gradually deteriorates (see Figure 5(a)-(d)). In Scenario 3, it can be found that only the Mid-KF has high tracking accuracy in each dimension, and the RMSE of the other two algorithms has diverged (see Figure 5(a)-(d)).

[0096] This simulation uses MATLAB 2019a under an AMD Ryzen 7 5800H processor and 16.0GB of memory to verify the tracking performance of the algorithm. As can be seen from Table 2, the Mid-KF takes less time and has lower complexity compared with the SRCKF. When the degree of measurement non-linearity is small, the tracking performance of the two is comparable; while when the degree of measurement non-linearity is large, the Mid-KF can ensure tracking accuracy while reducing the computational load.

[0097] In summary, a Kalman filtering algorithm based on an intermediate transition state proposed by the present invention has improved tracking accuracy compared with existing non-linear filtering algorithms and has a lower computational complexity, and is an effective non-linear filtering method for target tracking.

Claims

1. A Kalman filtering algorithm based on an intermediate transition state, characterized in that: Assume that at time k-1, the state estimate of the target and the estimation error covariance matrix are respectively and P k-1|k-1 , where and respectively represent the system's position estimates of the target in the x, y, and z directions at time k-1, and are the velocity estimates in the corresponding directions; assume that the measurement vector of the target obtained at time k is where and are respectively the range, elevation angle, azimuth angle, and radial velocity measurements, and the measurement noise variances are respectively and Assume that the azimuth angle measurement noise, elevation angle measurement noise, and radial velocity measurement noise are independent of each other, and the correlation coefficient between the range and radial velocity measurement noises is ρ. Then, the specific steps of an iterative process of a Kalman filtering algorithm based on an intermediate transition state from time k-1 to time k are as follows: Step 1: Time update: (1) Predict the state at time k: where F is the state transition matrix, is the state prediction vector at time k; (2) Calculate the predicted error covariance matrix: P k|k-1 = FP k-1|k-1 F T + Q k-1 (2) where Q k-1 is the process noise covariance matrix at time k-1, and P k|k-1 is the prediction error covariance matrix; Step 2: State transformation: (1) Decompose the predicted estimation error covariance matrix: where S k|k-1 is the square root of the prediction error covariance matrix, and chol(·) performs the Cholesky decomposition on the matrix; (2) Construct equally weighted sampling points: wherein, is the predicted sampling point constructed at the k-th moment, m is the number of sampling points, satisfying m = 2n, and n is the dimension of the state vector, [1] i is the i-th column of the point set [1], and the point set [1] is: (3) Represent the predicted sampling point as where and are the position prediction components along the x, y, and z directions, and are the velocity prediction components in the corresponding directions, and then convert them to the measurement coordinate system: In the formula, and are the predicted components of distance, elevation angle, and azimuth angle respectively, and are the components of radial distance, elevation angular velocity, and azimuth angular velocity. These components are represented by the predicted sampling points of the intermediate transition state in the measurement coordinate system after conversion. (4) Calculate the predicted value of the intermediate transition state in the measurement coordinate system: In the formula, is the predicted value of the intermediate transition state in the measurement coordinate system; (5) Calculate the predicted error covariance matrix in the measurement coordinate system: wherein, is the predicted estimation error covariance matrix in the measurement coordinate system; Step 3: Measurement update: (1) Innovation covariance matrix: In the formula, is the innovation covariance matrix, H Mid and R k are the measurement matrix and the measurement noise covariance matrix respectively, and can be expressed as: (2) Calculate the Kalman gain: where K Mid is the Kalman gain matrix; (3) Calculate the estimated vector of the intermediate transition state in the measurement coordinate system: In the formula, is the intermediate transition state estimation vector at time k; (4) Calculate the estimated error covariance matrix in the measurement coordinate system: Step 4: State transformation: (1) Decompose the estimated error covariance matrix in the measurement coordinate system: In the formula, is the square root of the error covariance matrix; (2) Construct equally weighted sampling points: wherein, is the estimated sampling point constructed at the k-th moment; (3) Express the estimated sampling points as where and are the distance, elevation angle, and azimuth angle estimation components, and are the radial distance, elevation angular velocity, and azimuth angular velocity components, then transform them to the original Cartesian coordinate system: wherein, and are the position estimation components along the x, y, and z directions, and are the velocity estimation components in the corresponding directions. These components are represented by the state estimation sampling points in the original Cartesian coordinate system (4) Calculate the state estimated vector at time k: In the formula, is the state estimate at time k; (5) Calculate the estimated error covariance matrix: where P k|k is the estimated error covariance matrix at time k.

Images