An improved strong tracking square-root cubature kalman filter algorithm

Abstract

The application belongs to the field of target tracking, and proposes an improved strong tracking square-root cubature Kalman filter algorithm. The algorithm introduces chi-square detection to judge target motion state mutation on the basis of the strong tracking algorithm, effectively improves the false alarm problem of the traditional strong tracking algorithm to state mutation; at the same time, based on the orthogonality principle of the strong tracking, the application proposes an improved fading factor calculation method, which uses the average of the ratio of diagonal elements instead of the ratio of the sum of diagonal elements or the maximum value of the ratio of diagonal elements in the traditional method, thereby overcoming the problem that the traditional fading factor is sensitive to the measurement of each dimension. The algorithm can make the system more accurately judge the motion state of the target, and improve the tracking accuracy when the target motion state is mutated and not mutated.

Description

Technical Field

[0001] This invention belongs to the field of target tracking and proposes an improved strong tracking square root commensurate Kalman filter algorithm. Background Technology

[0002] With the advancement of technology, target tracking has been widely applied in military and civilian fields. Radar is an important device for acquiring information. In actual radar systems, the target's range, angle, radial velocity, and other measurement information are usually obtained in polar or spherical coordinate systems, while the system state equations are often established in Cartesian coordinate systems, which means that there is a nonlinearity problem in radar measurements.

[0003] The Extended Kalman Filter (EKF) is a nonlinear filtering algorithm based on Taylor expansion (see: Lcondes, CT, Control and Dynamic System, Nonlinear and Kalman Filtering Technique. Academic Press, 1983). However, when the 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 FA 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.) proposes the Unscented Kalman Filter (UKF). UKF approximates the posterior probability density of the nonlinear system state through unscented transformation. UKF does not require linearization of the nonlinear function, and the computational accuracy can reach at least third-order Taylor accuracy. That is, UKF has the advantages of high filtering accuracy and good convergence. However, for high-dimensional systems, UKF requires the selection of appropriate 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 limited by linearization error or Gaussian noise assumptions, and its performance is similar to that of the UKF algorithm. However, after several iterations, most particles have a large decay, requiring the selection of a large number of particles or resampling, resulting in a large computational load.The literature (ARASARATNAM I, HAYKIN S, ELLIOTT RJ. 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. GHQF uses the Gauss-Hermite integral rule for numerical approximation, which can obtain better target tracking accuracy. However, GHQF extends the single-variable Gaussian integral to a multi-dimensional integral, and its computational complexity increases exponentially with the system dimension, resulting in non-real-time tracking.

[0004] The literature (I. Arasaratnam, S. Haykin. Cubature Kalman Filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269) proposes the Cubature Kalman Filter (CKF) algorithm. The Cubature Kalman Filter is based on the third-order spherical-radial volume criterion, with equal weights at each volume point, resulting in higher filtering accuracy and lower computational complexity. To further improve the stability of Cubic Kalman Filtering (CKF), the literature (I. Arasaratnam, S. Haykin, and TR. Hurd. Cubature Kalman filtering for continuous-discrete systems: Theory and simulations[J]. IEEE Trans. Signal Process, 2010, 58(10): 4977-4993.) introduces orthogonal decomposition into the covariance matrix of CKF, forming the square root form of CKF, namely the Square-Root Cubic Kalman Filter (SRCKF) algorithm. The SRCKF iterative operation uses the square root of the covariance matrix, ensuring the symmetry and positive (semi-)definiteness of the covariance matrix, further improving the tracking accuracy and stability of the filtering algorithm.

[0005] When the system model is inaccurate, such as when the target's motion state suddenly changes or the model noise abruptly changes, the performance of the above nonlinear filters will deteriorate. The literature (Zhou DH, Xi YG, Zhang ZJ. A suboptimal multiple extended Kalman filter[J]. Chinese J of Automation, 1992, 4(2): 145-152) introduces a strong tracking filter (STF) algorithm on the EKF framework, that is, introduces a suboptimal fading factor into the prediction covariance matrix, and adjusts the Kalman gain by adjusting the prediction covariance matrix to improve the tracking accuracy of the algorithm for highly maneuvering targets. However, due to the limitations of the EKF algorithm itself, such as large computational load and large tracking error, the tracking accuracy of the EKF combined with ST algorithm is still not high. The literature (Bo Han, Hanqiao Huang, Lei Lei. An Improved IMMAlgorithm Based on STSRCKF for Maneuvering Target Tracking[J].IEEE Access,2019,7:57795-57804) introduces a fading factor on the basis of SRCKF to construct a strong tracking filter. This algorithm also introduces a fading factor into the prediction covariance matrix, thereby adjusting the Kalman gain to meet the conditions of the strong tracking filter algorithm. However, the fading factor is calculated by taking the trace of the matrix and then taking the ratio, that is, by using the ratio of the sum of the measured residual variances in different dimensions to the theoretical output as the value of the fading factor. This method has shortcomings: the units of the measurements in each dimension are different, and directly summing the diagonal elements of the covariance matrix is ​​meaningless; if the abrupt change mainly occurs in the angular dimension, while no abrupt change occurs in the distance dimension, then the fading factor calculated by taking the summation form above becomes invalid, and the tracking performance of the algorithm for maneuvering targets will also drop sharply. To address this issue, the literature (Haowei Zhang, Junwei Xie, Jiang Ge, Adaptive Strong Tracking Square-Root Cubature Kalman Filter for Maneuvering Aircraft Tracking[J]. Journal of IEEE Access, 2018, 6: 10052-10061) redefines the introduction of the fading factor in the algorithm, namely, introducing it into the cross-covariance matrix and the residual covariance matrix. The literature also re-derives a new method for calculating the fading factor, using the maximum value of the ratio of the vector formed by the diagonal elements of the measured residual variance to the vector formed by the diagonal elements of the filter output theoretical value as the fading factor, thus overcoming the limitations of the traditional fading factor calculation method.However, there are still problems with using the above method as the fading factor: if the system state does not change abruptly, that is, the target is in a non-maneuvering state, the sudden increase in measurement error will also increase the fading factor obtained by the above calculation method; that is, the above method is more likely to introduce the fading factor, even when the target state does not change abruptly.

[0006] To address the aforementioned issues, this invention proposes an improved strong tracking square root capacitive Kalman filter algorithm. The calculation method for the fading factor is redefined, and the strong tracking capacitive Kalman filter algorithm is combined with chi-square detection, so that the strong tracking filter algorithm can effectively track the target whether the motion state changes abruptly or not. Summary of the Invention

[0007] Assume that the state estimate of the target has been obtained at time k-1. And estimation error covariance P k-1|k-1 The specific steps of an improved strong tracking square root capacitive Kalman filter algorithm for one iteration from time k-1 to k are as follows:

[0008] Step 1: Time Update:

[0009] (1) Decompose the state estimation error covariance matrix at time k-1:

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

[0011] In the formula, S k-1|k-1 Let be the square root of the state estimation error matrix at time k-1, and chole(·) be the Cholesky decomposition of the matrix.

[0012] (2) Constructing a volume point and propagating it through a nonlinear state equation:

[0013]

[0014]

[0015] In the formula, Let be the initial volume point constructed at time k-1. for via nonlinear state equations The propagation yields predicted volume points, where m is the number of volume points, satisfying m = 2n, and n is the dimension of the state vector. [1] i Let i be the i-th column of the point set [1]. The point set [1] is:

[0016]

[0017] (3) Predict the state at time k:

[0018]

[0019] In the formula, Let be the state prediction vector at time k.

[0020] (4) Calculate the square root of the prediction error covariance matrix:

[0021]

[0022] In the formula, S k|k-1 Let R be the square root of the prediction error covariance matrix at time k; the Tria(·) operation is defined as follows: Let R be a matrix A T The upper triangular matrix obtained by QR decomposition is then represented as S = Tria(A) = R. T Where S is a lower triangular matrix, i.e., S = R T In the formula, S Q,k-1 =chol(Q) k-1 ), Q k-1 Let k-1 be the process noise covariance matrix and the weighted center matrix. for:

[0023]

[0024] Step 2: Measurement Update

[0025] (1) Construct a new volume point and propagate it using nonlinear measurement equations:

[0026]

[0027]

[0028] In the formula, For the new volume point after the time update, for Through nonlinear measurement equation The volume point obtained after propagation.

[0029] (2) Predict the measurement at time k:

[0030]

[0031] In the formula, Let be the measurement prediction vector at time k.

[0032] (3) Calculate the square root of the residual covariance matrix:

[0033]

[0034] In the formula, S is the square root of the residual covariance matrix at time k. R,k =chol(R) k ), R k Let Z be the measurement noise covariance matrix at time k, and Z be the weighted central matrix. k|k-1 for:

[0035]

[0036] (4) Calculate the residual covariance matrix and cross covariance matrix:

[0037]

[0038]

[0039] In the formula, Let k be the residual covariance matrix without the introduction of the fading factor. Let X be the cross-covariance matrix at time k without introducing the fading factor, and the weighted centrality matrix X. k|k-1 for:

[0040]

[0041] Step 3: Perform a chi-square test to determine if the system has experienced a sudden change.

[0042]

[0043] In the formula, M k v is the square of the residual Mahalanobis distance. k The residual at time k: Z k Let be the measurement vector at time k.

[0044] (1) If Let the fading factor λ k =1, continue the calculation according to step 5.

[0045] (2) If Calculate the fading factor λ according to step 4. k .

[0046] in, α represents the chi-square test value with l degrees of freedom and a significance level of α, where l is the measurement dimension.

[0047] Step 4: Calculate the improved fading factor:

[0048]

[0049]

[0050]

[0051]

[0052] In the formula, diag(·) is the vector formed by the diagonal elements of the matrix, [·] i This indicates retrieving the i-th element from the vector. The calculation method is as follows:

[0053]

[0054] In the formula, ρ is the forgetting factor.

[0055] Step 5: Calculate the residual covariance matrix and cross-covariance matrix after introducing the fading factor.

[0056]

[0057]

[0058] In the formula, and Introducing the fading factor λ respectively k The residual covariance matrix and cross covariance matrix are then obtained.

[0059] Step 6: Calculate the Kalman gain and update the state estimate and the autocorrelation matrix of the estimation error at time k:

[0060]

[0061]

[0062] S k|k =Tria(X k|k-1 -K k Z k|k-1 ,S R,k (26)

[0063]

[0064] In the formula, K k The Kalman gain at time k, Let P be the state estimation vector at time k. k|k Let S be the autocorrelation matrix of the state estimation error at time k. k|k Let P be the estimated error autocorrelation matrix at time k. k|k The square root of.

[0065] Inventive Principles

[0066] The Square Root Capacitive Kalman Filter (SRCKF) is a nonlinear filtering method based on volume points. SRCKF introduces orthogonal triangular decomposition into the CKF algorithm. During the filtering process, iterative calculations are performed using the square root of the error covariance matrix, ensuring the symmetry and positive definiteness of the error covariance matrix, resulting in higher stability compared to CKF. The SRCKF algorithm includes time updates and measurement updates, as shown in steps 1 and 2.

[0067] When the system model is unknown, inaccurate, or the target suddenly maneuvers, the system tracking performance will degrade. Therefore, to effectively detect whether the system state has changed abruptly, this invention introduces a chi-square test with 1 degrees of freedom. The following assumptions are made:

[0068] Assumption 0H0: The target's motion state has not undergone a sudden change;

[0069] Assumption 1H1: The target's motion state undergoes a sudden change.

[0070] When the target's motion state does not change abruptly, according to the orthogonality principle of Kalman filtering, the squared residual Mahalanobis distance follows a chi-square distribution with l degrees of freedom (see reference: DA R. Failure detection of dynamical objects with the state chi-square test[J]. Journal of Guidance Control & Dynamics, 1994, 17(2): 271-277), that is,

[0071]

[0072] In the formula, M k The squared Mahalanobis distance, l is the measurement dimension, and v k It represents the residual.

[0073] By introducing chi-square detection, it is determined whether the target's motion state has undergone a sudden change, as shown in step 3. If If the target's motion state does not change abruptly, no fading factor is introduced, and the fading factor is set to 1, meaning the target is tracked using the SRCKF algorithm; if If the target's motion state changes abruptly, a fading factor is introduced using the ST algorithm, and step 4 is performed.

[0074] When the chi-square detects a sudden change in the target's motion state, indicating a mismatch between the actual target motion model and the system model, a strong tracking strategy is introduced. This strategy stabilizes tracking performance by introducing a fading factor to ensure the following two conditions are met:

[0075]

[0076]

[0077] In the formula, min represents taking the minimum value, v k This is the residual vector.

[0078] It is known that Theorem 1 exists (see reference: Guo Z, Miao LJ, Zhao HS. An improved strongtracking UKF algorithm and its application in SINS initial alignment underlarge azimuth misalignment angles[J]. Acta Aeronautica et Astronautica Sinica, 2014, 35(1): 203-214.):

[0079] Theorem 1: The state estimation error is expressed as If there is O(|ε) k | 2 ) << O(ε| k If |) holds true, then the covariance of the residual sequence at different times can be expressed as:

[0080]

[0081] In the formula, H k+j for h k+j The Jacobian matrix, F k-1+j f k-1+j The Jacobian matrix.

[0082] According to equations (30) and (31), we can obtain:

[0083]

[0084] Simplify the cross-covariance matrix without introducing the fading factor:

[0085]

[0086] In the formula, V k The measurement noise at time k is denoted as .

[0087] Therefore, equation (32) can be expressed as:

[0088]

[0089] According to the Kalman gain formula have have:

[0090]

[0091] Substituting equation (35) into equation (34), we get:

[0092]

[0093] To make equations (34) and (36) hold, in and Introducing the fading factor λ k Then we obtain the matrix after introducing the fading factor. and As shown in equations (22) and (23) in step 5.

[0094] Substituting the cross-covariance matrix after introducing the fading factor into equation (36) yields:

[0095]

[0096] In the formula, The calculation method is shown in equation (21).

[0097] It can be seen that the fading factor λ k It is necessary to satisfy equation (37), but it is impossible to obtain a unique scalar solution λ that satisfies equation (37). k To ensure that the strong tracking algorithm has the same sensitivity across all measurement dimensions, an improved strong tracking algorithm is proposed, as shown in step 4. Equations (19) and (20) are introduced based on equation (37), and the fading factor is calculated by summing and averaging the ratios of each dimension based on equation (18). Furthermore, to further reduce false alarms caused by sudden changes in the target's motion state, a sudden change in motion state is only considered to have occurred when the mean calculated by equation (18) exceeds 1; otherwise, the fading factor is set to 1, as shown in equation (17).

[0098] Based on the improved fading factor, adjust the residual covariance matrix and cross-covariance matrix; calculate the Kalman gain, and update the target's current state and estimation error covariance, as shown in steps 5 and 6. Attached Figure Description

[0099] Figure 1 For the target motion trajectory diagram

[0100] Figure 2 A comparison chart of positional RMSE values ​​under different chi-square test values.

[0101] Figure 3 This is a comparison chart of the position RMSE of the algorithm of this invention and existing algorithms. Detailed Implementation

[0102] Assume a radar is tracking a moving target in a two-dimensional plane. The radar is located at the origin, the sampling period is 1 second, the target's initial position is (20km, 25km), and the initial velocity is (25m / s, 20m / s). The target's motion is as follows: uniform motion from 1 to 100s, 201 to 350s, and 401 to 500s; uniform turning motion from 101 to 200s and 351 to 400s, with angular velocities of -0.9 rad / s and 6 rad / s, respectively. Figure 1 The target's trajectory is shown. Radar measurements include range, azimuth, and Doppler measurements. The measurement noise is zero-mean Gaussian white noise, with standard deviations of σ... r =5m, σ β =5mrad and σ r =0.05m / s.

[0103] The target is tracked using an improved ST-SRCKF algorithm proposed in this invention, with different chi-square detection threshold values ​​selected: and The mean square error (RMSE) of the position estimation at time k is used as a metric for the filtering algorithm:

[0104]

[0105] in, and To give the true and estimated values ​​of the target's x-direction position at time k in the m-th Monte Carlo experiment, and Let M represent the true and estimated position of the target in the y-direction at time k in the m-th Monte Carlo simulation. The following are the statistical results of 500 Monte Carlo simulations.

[0106] like Figure 2 As shown, the Improved ST-SRCKF tracking algorithm is compared under different chi-square test values. When the selected chi-square detection threshold is small, such as... The RMSE curve of the improved ST-SRCKF(0.5) shows the largest tracking error at the three thresholds when the target's motion state does not change abruptly. When the target's motion state changes, the RMSE of the improved ST-SRCKF(0.5) does not change significantly during the 100–200s weak maneuvering period, but increases during the 350–400s strong maneuvering period, although the tracking error is smallest at the three thresholds. This indicates that the lower the chi-square test value, the easier it is for the tracking algorithm to pass the threshold, thus determining that the target's motion state has changed abruptly, resulting in a relatively large tracking error when the target's motion state does not change abruptly; when the selected chi-square detection threshold is large, such as… When the target's motion state does not change abruptly, the RMSE curve of Improved ST-SRCKF(0.005) is the lowest among the three RMSE curves. However, in the 350-400s range when the system state changes abruptly and the maneuvering characteristics are strong, the RMSE of Improved ST-SRCKF(0.005) is slightly higher than the tracking error under the other two threshold algorithms. This indicates that the higher the chi-square threshold, the less likely the system is to determine that the target's motion state has changed abruptly. In this case, the probability of introducing the fading factor in the Improved ST-SRCKF(0.005) algorithm is relatively low, and the algorithm performs better in tracking targets with relatively stable motion states. When the target's motion state changes abruptly, because the chi-square threshold is high, the system considers the probability of the target maneuvering to be low, and the probability of the algorithm introducing the fading factor is low. The RMSE obtained by averaging after 500 Monte Carlo runs is slightly higher than the RMSE of the algorithms corresponding to the other two thresholds. As can be observed from the figure, when a chi-square test value with a significance level of α = 0.95 is selected, the algorithm of the present invention can better balance the two cases where the target motion state does not change and changes occur.

[0107] Next, the algorithm of this invention is compared with two existing algorithms. The two algorithms are: the traditional strong tracking algorithm (ST-SRCKF) (see reference: Han B, Huang HQ, Lei L, et al. An Improved IMM Algorithm Based on STSRCKF for Maneuvering Target Tracking[J]. Journal of IEEE Access, 2019, 7: 57795-57804) and a combined ST and SRCKF algorithm (ST-SRCKF-max) where the fading factor takes the maximum value of the ratio of diagonal elements (see reference: Haowei Zhang, Junwei Xie, Jiang Ge, Adaptive Strong Tracking Square-Root Cubature Kalman Filter for Maneuvering Aircraft Tracking[J]. Journal of IEEE Access, 2018, 6: 10052-10061). During the algorithm comparison, the algorithm proposed in this invention uses a chi-square test threshold with a significance level of α = 0.95.

[0108] Figure 3 The performance comparison results of the algorithms are presented, showing that Improved ST-SRCKF has a smaller RMSE than ST-SRCKF and T-SRCKF-max. This is because the ST-SRCKF algorithm calculates the ratio of the trace of the measurement residual variance matrix to the theoretical output as the fading factor. However, since the units of each measurement dimension are different, calculating the trace and taking the ratio results in varying sensitivities of the fading factor to different measurement dimensions in the ST-SRCKF algorithm, increasing the tracking error. ST-SRCKF-max takes the maximum value of the ratio of each dimension of the matrix. If the measurement error of a certain dimension increases, even if the target's motion state has not changed abruptly, the system is more likely to judge that the system state has changed abruptly at that moment, and the fading factor is greater than 1. This leads to a generally higher RMSE curve for ST-SRCKF-max. The Improved ST-SRCKF algorithm, on the other hand, takes the mean of the diagonal elements of the matrix, considers each measurement dimension to determine whether the target has maneuvered, and the weight of each dimension is equal. This allows for a more accurate determination of whether the target has maneuvered, thus the algorithm has a smaller tracking error. Furthermore, the algorithm of this invention introduces chi-square detection to detect sudden changes in the target state, thereby improving the accuracy of change detection. Figure 3As shown, when the target's motion state does not change abruptly, the RMSE curves of Improved ST-SRCKF are lower in the ranges of 0–100s, 200–350s, and 401–500s. Only after passing the chi-square test (step 3) will the algorithm proceed to the stage of calculating the fading factor. If the chi-square test is not passed, the fading factor is not introduced, and the filtering algorithm becomes the SRCKF algorithm. Therefore, when the target's motion does not change abruptly, the tracking error of Improved ST-SRCKF is minimized.

[0109] In summary, the improved strong tracking square root commensurate Kalman filter algorithm proposed in this invention improves tracking accuracy compared to existing strong tracking algorithms and is an effective radar target tracking method.

Claims

1. An improved strong tracking square root capillary Kalman filter algorithm, characterized by: Assume that the state estimate of the target has been obtained at time k-1. And estimation error covariance P k-1|k-1 The specific steps of an improved strong tracking square root capacitive Kalman filter algorithm for one iteration from time k-1 to k are as follows: Step 1: Time Update: (1) Decompose the state estimation error covariance matrix at time k-1: S k-1|k-1 =chol(P k-1|k-1 ) (1) In the formula, S k-1|k-1 Let be the square root of the state estimation error matrix at time k-1, and chole(·) be the Cholesky decomposition of the matrix. (2) Constructing a volume point and propagating it through a nonlinear state equation: In the formula, Let be the initial volume point constructed at time k-1. for via nonlinear state equations The propagation yields predicted volume points, where m is the number of volume points, satisfying m = 2n, and n is the dimension of the state vector. [1] i Let i be the i-th column of the point set [1]. The point set [1] is: (3) Predict the state at time k: In the formula, Let be the state prediction vector at time k; (4) Calculate the square root of the prediction error covariance matrix: In the formula, S k|k-1 Let R be the square root of the prediction error covariance matrix at time k; the Tria(·) operation is defined as follows: Let R be a matrix A T The upper triangular matrix obtained by QR decomposition is then represented as S = Tria(A) = R. T Where S is a lower triangular matrix, i.e., S = R T In the formula, S Q,k-1 =chol(Q) k-1 ), Q k-1 Let k-1 be the process noise covariance matrix and the weighted center matrix. for: Step 2: Measurement Update (1) Construct a new volume point and propagate it using nonlinear measurement equations: In the formula, For the new volume point after the time update, for Through nonlinear measurement equation The measurement volume point obtained after propagation; (2) Predict the measurement at time k: In the formula, Let k be the measurement prediction vector at time k; (3) Calculate the square root of the residual covariance matrix: In the formula, S is the square root of the residual covariance matrix at time k. R,k =chol(R) k ), R k Let Z be the measurement noise covariance matrix at time k, and Z be the weighted central matrix. k|k-1 for: (4) Calculate the residual covariance matrix and cross covariance matrix: In the formula, Let k be the residual covariance matrix without the introduction of the fading factor. Let X be the cross-covariance matrix at time k without introducing the fading factor, and the weighted centrality matrix X. k|k-1 for: Step 3: Perform a chi-square test to determine if the system has experienced a sudden change. In the formula, M k v is the square of the residual Mahalanobis distance. k The residual at time k: Z k Let be the measurement vector at time k; (1) If Let the fading factor λ k =1, continue the calculation according to step 5; (2) If Calculate the fading factor λ according to step 4. k ; in, The chi-square test value is given with l degrees of freedom and a significance level of α, where l is the measurement dimension. Step 4: Calculate the improved fading factor: In the formula, diag(·) is the vector formed by the diagonal elements of the matrix, [·] i This indicates retrieving the i-th element from the vector; The calculation method is as follows: In the formula, ρ is the forgetting factor; Step 5: Calculate the residual covariance matrix and cross-covariance matrix after introducing the fading factor. In the formula, and Introducing the fading factor λ respectively k The residual covariance matrix and cross covariance matrix are then obtained. Step 6: Calculate the Kalman gain and update the state estimate and the autocorrelation matrix of the estimation error at time k: S k|k =Tria(X k|k-1 -K k Z k|k-1 ,S R,k ) (26) In the formula, K k The Kalman gain at time k, Let P be the state estimation vector at time k. k|k Let S be the autocorrelation matrix of the state estimation error at time k. k|k Let P be the estimated error autocorrelation matrix at time k. k|k The square root of.

Images