CN102999696B

CN102999696B - Noise correlation system is based on the bearingsonly tracking method of volume information filtering

Info

Publication number: CN102999696B
Application number: CN201210454749.4A
Authority: CN
Inventors: 文成林; 许大星; 葛泉波; 骆光州
Original assignee: Hangzhou Dianzi University
Current assignee: Hangzhou Dianzi University
Priority date: 2012-11-13
Filing date: 2012-11-13
Publication date: 2016-02-24
Anticipated expiration: 2032-11-13
Also published as: CN102999696A

Abstract

The invention belongs to target tracking domain, relate generally to the bearingsonly tracking method of noise correlation system based on volume information filtering.The nonlinear system method for tracking target of existing volume Kalman carries out under process noise and the incoherent supposed premise of measurement noises, and this just greatly limit its usable range.The present invention is under the prerequisite of the spreading kalman information filter of being correlated with at noise of having derived, the time upgrade with these two processes of measurement updaue in embed volume Kalman information filter.Also just solve the problem that noise is relevant, method practicality of the present invention is strengthened greatly.

Description

Azimuth-only tracking method based on volumetric information filtering for noise correlation systems

技术领域 technical field

本发明属于目标跟踪领域，主要涉及噪声相关系统基于容积信息滤波的纯方位目标跟踪方法。 The invention belongs to the field of target tracking, and mainly relates to a direction-only target tracking method based on volume information filtering in a noise correlation system.

背景技术 Background technique

传感器目标跟踪是一门多学科交叉技术。近年来，随着传感器技术、计算机技术、通信技术和信息处理技术的发展，特别是军事上的迫切需求，多传感器目标跟踪技术的研究内容日益深入和广泛。军事上主要应用于指挥、控制、通信和情报系统，同时在机器人、民航航管等领域也有重要应用价值。目前对目标跟踪有了很多比较好的算法，如卡尔曼滤波算法(KF)，无迹卡尔曼滤波算法(UKF),求容积卡尔曼滤波算法(CKF)等，然而众所周知，当所有传感器测量值到达融合中心进行集中处理时这些算法具有很高的计算复杂度。所以，信息滤波器被提了出来并且得到了广泛的应用由于在计算方面有比卡尔曼滤波算法更优越的性能并且容易初始化。实际上，信息滤波算法本质上是用协方差阵的逆表示的卡尔曼滤波算法。 Sensor target tracking is a multidisciplinary technology. In recent years, with the development of sensor technology, computer technology, communication technology and information processing technology, especially the urgent needs of the military, the research content of multi-sensor target tracking technology has become increasingly in-depth and extensive. In the military, it is mainly used in command, control, communication and intelligence systems, and it also has important application value in the fields of robotics and civil aviation control. At present, there are many better algorithms for target tracking, such as Kalman filter algorithm (KF), unscented Kalman filter algorithm (UKF), volumetric Kalman filter algorithm (CKF), etc. However, as we all know, when all sensor measurements These algorithms have high computational complexity when reaching the fusion center for centralized processing. Therefore, the information filter was proposed and widely used because it has superior performance than the Kalman filter algorithm in calculation and is easy to initialize. In fact, the information filtering algorithm is essentially the Kalman filtering algorithm represented by the inverse of the covariance matrix.

目前关于非线性滤波的目标跟踪算法最新进展是容积信息滤波算法(SCIF)，但由于此算法的前提是任何噪声之间是不相关的，所以大大限制了它的应用范围。在实际当中往往由于天气，跟踪同一个目标，同样的环境，多传感器的异步采样等原因，过程噪声与观测噪声之间可能相关，这就大大限制了SCIF的使用。 At present, the latest development of target tracking algorithm on nonlinear filtering is Volume Information Filtering Algorithm (SCIF), but because the premise of this algorithm is that any noise is uncorrelated, its application range is greatly limited. In practice, due to the weather, tracking the same target, the same environment, and asynchronous sampling of multiple sensors, there may be correlation between process noise and observation noise, which greatly limits the use of SCIF.

发明内容 Contents of the invention

为了解决噪声相关的情况，本发明提出了噪声相关系统基于容积信息滤波的纯方位跟踪方法，从而达到跟踪目标的目的。为了方便描述本发明的内容，首先本发明针对单传感器目标系统建立模型，它包括2个方程，状态方程和观测方程，分别如下所示： In order to solve the situation of noise correlation, the present invention proposes a pure bearing tracking method based on volume information filtering of the noise correlation system, so as to achieve the purpose of tracking the target. In order to describe the content of the present invention conveniently, at first the present invention establishes a model for the single-sensor target system, and it comprises 2 equations, state equation and observation equation, respectively as follows:

(1) (1)

(2) (2)

这里的是时间指标；是系统的状态向量；是对于状态向量的观测向量；和都是已知的可微函数；过程噪声和测量噪声均为零均值的高斯白噪声，它们的方差分别为和，并且满足： here is the time indicator; is the state vector of the system; is for the state vector the observation vector; and are known differentiable functions; the process noise and measurement noise Both are Gaussian white noise with zero mean, and their variances are and , and satisfy:

(3) (3)

为过程噪声与观测噪声的互协方差矩阵，可以看出过程噪声与测量噪声是相关的，为脉冲函数，即时，，时，。我们令初始状态为，且它的期望值为，令初始状态误差的协方差矩阵为，并且满足 is the cross-covariance matrix of process noise and observation noise, it can be seen that process noise and measurement noise are related, is an impulse function, that is, hour, , hour, . We let the initial state be , and its expected value is , let the covariance matrix of the initial state error be , and satisfy

(4) (4)

针对上面描述的系统模型和初始条件，本发明给出如下迭代算法，具体包括2个模块：时间更新（先）和状态更新（后），从而达到跟踪目标的目的。 For the system model and initial conditions described above, the present invention provides the following iterative algorithm, which specifically includes two modules: time update (first) and state update (after), so as to achieve the purpose of tracking the target.

1.时间更新 1. Time update

步骤1.1分别计算k-1时刻第i个容积点，k-1时刻第i个传播容积点和k-1时刻一步状态预测。 Step 1.1 Calculate the i-th volume point at time k-1 respectively , the i-th propagation volume point at time k-1 and one-step state prediction at time k-1 .

首先，可以假设k-1时刻的状态估计和它的协方差矩阵已知，分解有： First, it can be assumed that the state estimation at time k-1 and its covariance matrix known, decomposed Have:

(5) (5)

其中称为k-1时刻开方值。 in It is called the square root value at time k-1.

其次，从(6)式计算k-1时刻第i个传播容积点， Secondly, calculate the i-th propagation volume point at time k-1 from formula (6) ,

(6) (6)

其中， in,

(7) (7)

并且。这里,是点集合的第i个列向量，例如如果,那么它表示下面的集合： and . here, is a collection of points The ith column vector of , for example if , then it represents the following set:

最后，计算k-1时刻状态的一步预测： Finally, compute a one-step forecast of the state at time k-1:

(8) (8)

步骤1.2根据下式计算k-1时刻一步开方； Step 1.2 Calculate the one-step root at time k-1 according to the following formula ;

(9) (9)

这里表示QR分解，将分解得到的上三角矩阵的转置赋给，是的开方根，即：，并且 here Represents the QR decomposition, and assigns the transpose of the upper triangular matrix obtained by the decomposition to , yes The square root of , that is: ,and

(10) (10)

步骤1.3使用下面的式子得到k-1时刻一步信息矩阵； Step 1.3 Use the following formula to get the one-step information matrix at time k-1 ;

令(11) make (11)

然后利用即可得到k-1时刻一步信息矩阵。 then use One-step information matrix at time k-1 can be obtained .

步骤1.4使用式（12）式计算k-1时刻一步预测信息状态向量； Step 1.4 Use formula (12) to calculate the one-step forecast information state vector at time k-1 ;

(12) (12)

2.测量更新 2. Measurement update

步骤2.1分别计算k-1时刻第i个一步容积点,k-1时刻第i个一步传播容积点和k-1时刻一步观测预测。 Step 2.1 Calculate the i-th one-step volume point at time k-1 respectively , the i-th one-step propagation volume point at time k-1 and one-step observation prediction at time k-1 .

首先，计算k-1时刻一步容积点，如下式所示： First, calculate the one-step volume point at time k-1 , as shown in the following formula:

(13) (13)

进而可利用下式计算k-1时刻第i个一步传播容积点， Furthermore, the following formula can be used to calculate the i-th one-step propagation volume point at time k-1,

(14) (14)

然后，利用式(15)计算k-1时刻一步观测预测, Then, use formula (15) to calculate the one-step observation forecast at time k-1 ,

(15) (15)

步骤2.2利用下式计算k-1时刻互协方差矩阵； Step 2.2 Use the following formula to calculate the cross-covariance matrix at time k-1 ;

首先，根据下式计算k-1时刻开方新息协方差矩阵， First, calculate the square root innovation covariance matrix at time k-1 according to the following formula ,

(16) (16)

其中，表示的开方，如 in, express The square root, such as

令 make

然后，利用下式计算k-1时刻互协方差矩阵； Then, use the following formula to calculate the cross-covariance matrix at time k-1 ;

(17) (17)

步骤2.3利用式分别计算k时刻信息矩阵和信息状态向量； Step 2.3 Use the formula to calculate the k time information matrix respectively and information state vector ;

(18) (18)

(19) (19)

步骤2.4利用下式获得k时刻状态估计和相应协方差矩阵. Step 2.4 Use the following formula to obtain the state estimation at time k and the corresponding covariance matrix .

(20) (20)

不断重复上面2个模块的内容，就可实现对目标状态的跟踪估计。 Keep repeating the content of the above two modules to achieve the target state tracking estimates.

本发明的有益效果： Beneficial effects of the present invention:

本发明提出的目标跟踪方法，在时间更新和测量更新中利用容积信息滤波解决了噪声的相关性，从而使本方法能在过程噪声与测量噪声相关的条件下也能对纯方位目标进行很好的跟踪。 The target tracking method proposed by the present invention solves the correlation of noise by using volume information filtering in the time update and measurement update, so that the method can also perform well on the pure orientation target under the condition that the process noise is related to the measurement noise tracking.

附图说明 Description of drawings

图1为本发明跟踪方法的流程图； Fig. 1 is the flowchart of tracking method of the present invention;

图2为本发明中的纯方位跟踪系统图； Fig. 2 is the pure azimuth tracking system figure among the present invention;

图3A为本发明仿真在X方向(正东方向)的跟踪效果图； Fig. 3 A is the tracking effect diagram of the simulation in the X direction (due east direction) of the present invention;

图3B为本发明仿真在Y方向(正北方向)的跟踪效果图； Fig. 3 B is the tracking effect figure of simulation in Y direction (true north direction) of the present invention;

图3C为本发明仿真在X方向(正东方向)的跟踪误差图； Fig. 3 C is the tracking error diagram of the simulation in the X direction (due east direction) of the present invention;

图3D为本发明仿真在X方向(正东方向)的跟踪误差图。 FIG. 3D is a tracking error diagram of the simulation in the X direction (due east direction) according to the present invention.

具体实施方式 detailed description

本发明的实施流程图如图1所示，具体实施方式如下： Implementation flow chart of the present invention is as shown in Figure 1, and specific implementation is as follows:

为了解决噪声相关的情况，本发明提出了噪声相关条件下的容积信息滤波器(SCIF-CN)设计方法，从而达到跟踪目标的目的。为了方便描述本发明的内容，首先本发明针对单传感器目标系统建立模型，它包括2个方程，状态方程和观测方程，分别如下所示： In order to solve the situation of noise correlation, the present invention proposes a volumetric information filter (SCIF-CN) design method under noise correlation conditions, so as to achieve the purpose of tracking the target. In order to describe the content of the present invention conveniently, at first the present invention establishes a model for the single-sensor target system, and it comprises 2 equations, state equation and observation equation, respectively as follows:

(1) (1)

(2) (2)

(3) (3)

(4) (4)

1.时间更新 1. Time update

(5) (5)

其次，从(6)式计算k-1时刻第i个传播容积点，() Secondly, calculate the i-th propagation volume point at time k-1 from formula (6) , ( )

(6) (6)

其中， in,

(7) (7)

并且。这里是点集合的第i个列向量，例如如果,那么它表示下面的集合： and . here is a collection of points The ith column vector of , for example if , then it represents the following set:

(8) (8)

(9) (9)

这里表示QR分解，将分解后的上三角的转置赋给，是的开方根，即：，并且 here Represents the QR decomposition, and assigns the transpose of the decomposed upper triangle to , yes The square root of , that is: ,and

(10) (10)

令(11) make (11)

(12) (12)

2.测量更新 2. Measurement update

(13) (13)

(14) (14)

(15) (15)

(16) (16)

其中，表示的开方，如 in, express The square root, such as

令 make

(17) (17)

(18) (18)

(19) (19)

(20) (20)

方法实验 method experiment

在本试验中，我们采用上述算法对纯方位系统进行目标跟踪估计。为了更好的阐释本实验，首先对图2中的各个参数加以解释：与是2个传感器，为这两个传感器的观测角度，为传感器的坐标位置，为两个传感器的距离。本实验中，目标有4个状态，即，与是目标在正东方向和正北方向的位置坐标，与是目标在正东方向和正北方向的速度大小。假如目标做匀速直线运动，状态方程为：，观测方程为：，本实验参数设置如下：，为目标跟踪系统的融合周期，设置， In this experiment, we use the above algorithm to estimate the target tracking for the bearing-only system. In order to better explain this experiment, first explain each parameter in Figure 2: and are 2 sensors, are the observation angles of the two sensors, is the coordinate position of the sensor, is the distance between the two sensors. In this experiment, the target has 4 states, namely , and are the position coordinates of the target in the direction of true east and true north, and is the speed of the target in the direction of due east and due north. If the target moves in a straight line with uniform speed, the state equation is: , the observation equation is: , the experimental parameters are set as follows: , For the fusion period of the target tracking system, set ,

过程噪声协方差矩阵， Process noise covariance matrix ,

初始状态与协方差矩阵分别为： The initial state and covariance matrix are:

为了显示噪声相关性，我们用产生观测噪声，其中为噪声相关系数，这里设置。 To show noise correlation, we use Observation noise is produced, where is the noise correlation coefficient, set here .

图3A为对X方向的状态估计跟踪效果图，图中X-Displacement为目标在X方向的状态位置，本发明的跟踪方法SCIF-CN的曲线基本与目标在X方向的状态重合，跟踪效果很好。 Figure 3A is a state estimation tracking effect diagram for the X direction, in which X-Displacement is the state position of the target in the X direction, the curve of the tracking method SCIF-CN of the present invention basically coincides with the state of the target in the X direction, and the tracking effect is very good it is good.

图3B为对Y方向的状态估计跟踪效果图，图中Y-Displacement为目标在Y方向的状态位置，本发明的跟踪方法SCIF-CN的曲线基本与目标在Y方向的状态重合，跟踪效果很好。 Figure 3B is a state estimation tracking effect diagram for the Y direction, in which Y-Displacement is the state position of the target in the Y direction, the curve of the tracking method SCIF-CN of the present invention basically coincides with the state of the target in the Y direction, and the tracking effect is very good it is good.

图3C为本发明跟踪方法SCIF-CN在X方向对目标状态估计跟踪的误差，其误差在2.5左右震荡，误差在实际允许范围内。 Figure 3C shows the error of the tracking method SCIF-CN of the present invention on the estimated tracking of the target state in the X direction. The error fluctuates around 2.5, and the error is within the actual allowable range.

图3D为本发明跟踪方法SCIF-CN在Y方向对目标状态估计跟踪的误差，其误差在1.2左右震荡，误差在实际允许范围内。 Figure 3D shows the tracking method SCIF-CN of the present invention estimates and tracks the target state in the Y direction. The error fluctuates around 1.2, and the error is within the actual allowable range.

Claims

1. The pure bearing tracking method based on the volume information filtering of the noise correlation system is characterized in that:

A model is established for a single-sensor target system, which includes two equations, the state equation and the observation equation, as follows:

x _k ＝f _k-1 (x _k-1 )+w _k,k-1 (1)

z _k ＝h _k (x _k )+v _k (2)

Here k is the time index; x _k ∈ R ^n×1 is the state vector of the system; z _k ∈ R ^m×1 is the observation vector for the state vector x _k ; f _k-1 : R ^n×1 →R ^{n× 1} and h _k : R ^n×1 →R ^m×1 are known differentiable functions, n is the dimension of the state vector, m is the dimension of the measurement vector; process noise w _k,k-1 and measurement noise v _k are Gaussian white noise with zero mean, their variances are Q _{k, k-1} and R _{k respectively} , and satisfy:

E E. {{(\begin{matrix} {w w}_{k k,, k k - - 11} \\ {v v}_{k k} \end{matrix}) (\begin{matrix} {w w}_{b b,, b b - - 11}^{T T} & {v v}_{b b}^{T T} \end{matrix})}} = = [\begin{matrix} {Q Q}_{k k,, k k - - 11} & {D D.}_{k k} \\ {D D.}_{k k}^{T T} & {R R}_{k k} \end{matrix}] {δ δ}_{k k,, b b} - - - - - - ((33))

D _k is the cross-covariance matrix of process noise and observation noise. It can be seen that process noise and measurement noise are related, and δ _k,b is an impulse function, that is, when k=b, δ _k,b =1, k≠b , δ _k,b ＝0; let the initial state be x ₀ , and its expected value is Let the covariance matrix of the initial state error be P _0|0 , and satisfy

E E. {{{x x}_{00}}} = = {\overset{^^}{x x}}_{00 | | 00},, E E. {{[[{x x}_{00} - - {\overset{^^}{x x}}_{00 | | 00}]] {[[{x x}_{00} - - {\overset{^^}{x x}}_{00 | | 00}]]}^{T T}}} = = {P P}_{00 | | 00} - - - - - - ((44))

According to the system model and initial conditions described above, the following iterative algorithm is given, which specifically includes two modules: time update and state update, so as to achieve the purpose of tracking the target;

Ⅰ. Time update

Step 1.1 Calculate the i-th volume point X _i,k-1|k- 1 at time k-1 respectively, and the i-th propagation volume point at time k-1 and one-step state prediction at time k-1

First, it can be assumed that the state estimation at time k-1 And its covariance matrix P _k-1|k-1 is known, and the decomposition of P _k-1|k-1 has:

{P P}_{k k - - 11 | | k k - - 11} = = {S S}_{k k - - 11 | | k k - - 11} {S S}_{k k - - 11 | | k k - - 11}^{T T} - - - - - - ((55))

Among them, S _k-1|k-1 is called the square root value at time k-1;

Secondly, calculate the i-th propagation volume point at time k-1 from formula (6) i=1,2,...,M; M=2n

{X x}_{i i,, k k | | k k - - 11}^{* *} = = {f f}_{k k - - 11} (({X x}_{i i,, k k - - 11 | | k k - - 11})) - - - - - - ((66))

in,

{X x}_{i i,, k k - - 11 | | k k - - 11} = = {S S}_{k k - - 11 | | k k - - 11} {ξ ξ}_{i i} + + {\overset{^^}{x x}}_{k k - - 11 | | k k - - 11} - - - - - - ((77))

and Here [1] _i is the ith column vector of the point set [1];

Finally, compute a one-step forecast of the state at time k-1:

{\overset{^^}{x x}}_{k k | | k k - - 11} = = \frac{11}{M m} {Σ Σ}_{i i = = 11}^{M m} {X x}_{i i,, k k | | k k - - 11}^{* *} - - - - - - ((88))

Step 1.2 Calculate the one-step root S _k|k- 1 at time k-1 according to the following formula;

{S S}_{k k | | k k - - 11} = = T T r r i i a a (([\begin{matrix} {γ γ}_{k k | | k k - - 11}^{* *} & {S S}_{{Q Q}_{k k,, k k - - 11}} \end{matrix}])) - - - - - - ((99))

Here Tria means QR decomposition, assign the transpose of the upper triangular matrix obtained by the decomposition to S _k|k-1 , is the square root of Q _k,k-1 , namely: and

{γ γ}_{k k | | k k - - 11}^{* *} = = \frac{11}{\sqrt{M m}} [\begin{matrix} {X x}_{11,, k k | | k k - - 11}^{* *} - - {\overset{^^}{x x}}_{k k | | k k - - 11} & {X x}_{22,, k k | | k k - - 11}^{* *} - - {\overset{^^}{x x}}_{k k | | k k - - 11} & ... ... & {X x}_{M m,, k k | | k k - - 11}^{* *} - - {\overset{^^}{x x}}_{k k | | k k - - 11} \end{matrix}] - - - - - - ((1010))

Step 1.3 uses the following formula to obtain the one-step information matrix Y _{k|k-1 at time k-1} ;

make

γ_{k | k - 1} = \frac{1}{\sqrt{m}} [\begin{matrix} x_{1, k | k - 1} - {\hat{x}}_{k | k - 1} & x_{2, k | k - 1} - {\hat{x}}_{k | k - 1} & ... & x_{m, k | k - 1} - {\hat{x}}_{k | k - 1} \end{matrix}] - - - (11)

then use The one-step information matrix Y _k|k- 1 at time k-1 can be obtained;

Step 1.4 Use formula (12) to calculate the one-step prediction information state vector at time k-1

\{\begin{matrix} {\overset{^^}{y the y}}_{k k | | k k - - 11} = = {P P}_{k k | | k k - - 11}^{- - 11} {\overset{^^}{x x}}_{k k | | k k - - 11} = = \frac{11}{M m} (({Y Y}_{k k | | k k - - 11} {Σ Σ}_{i i = = 11}^{M m} {X x}_{i i,, k k | | k k - - 11}^{* *})) \\ {v v}_{k k} = = {z z}_{k k} - - {\overset{^^}{z z}}_{k k | | k k - - 11} = = {z z}_{k k} - - \frac{11}{M m} {Σ Σ}_{i i = = 11}^{M m} {Z Z}_{i i,, k k | | k k - - 11} \end{matrix} - - - - - - ((1212))

Ⅱ. Status update

Step 2.1 Calculate the i-th one-step volume point X _i,k|k- 1 at time k-1, the i-th one-step propagation volume point Z _i,k|k -1 at time k-1, and one-step observation prediction at time k-1

First, calculate the one-step volume point X _i,k|k- 1 at time k-1, as shown in the following formula:

{X x}_{i i,, k k | | k k - - 11} = = {S S}_{k k | | k k - - 11} {ξ ξ}_{i i} + + {\overset{^^}{x x}}_{k k | | k k - - 11} - - - - - - ((1313))

Furthermore, the following formula can be used to calculate the i-th one-step propagation volume point at time k-1,

Z _i,k|k-1 ＝h _k (X _i,k|k-1 )(14)

Then, use formula (15) to calculate the one-step observation forecast at time k-1

{\overset{^^}{z z}}_{k k | | k k - - 11} = = \frac{11}{M m} {Σ Σ}_{i i = = 11}^{M m} {Z Z}_{i i,, k k | | k k - - 11} - - - - - - ((1515))

Step 2.2 Use the following formula to calculate the cross-covariance matrix at time k-1

First, calculate the square root innovation covariance matrix at time k-1 according to the following formula

{S S}_{\overset{~ ~}{z z} \overset{~ ~}{z z},, k k | | k k - - 11} = = T T r r i i a a (([\begin{matrix} {ζ ζ}_{k k | | k k - - 11}^{* *} & {S S}_{{\overset{&OverBar; &OverBar;}{R R}}_{k k}} \end{matrix}])) - - - - - - ((1616))

in, Indicates the root of R _k ;

make

ζ_{k | k - 1} = \frac{1}{\sqrt{m}} [\begin{matrix} Z_{1, k | k - 1} - {\hat{z}}_{k | k - 1} & Z_{2, k | k - 1} - {\hat{z}}_{k | k - 1} & ... & Z_{m, k | k - 1} - {\hat{z}}_{k | k - 1} \end{matrix}]

Then, use the following formula to calculate the cross-covariance matrix at time k-1

{P P}_{\overset{~ ~}{x x} \overset{~ ~}{z z},, k k | | k k - - 11} = = {γ γ}_{k k | | k k - - 11} {ζ ζ}_{k k | | k k - - 11}^{T T} - - - - - - ((1717))

Step 2.3 Use formula (19) to calculate the information matrix Y _k|k and the information state vector at time k respectively

{\overset{~ ~}{R R}}_{k k} = = {R R}_{k k} - - {D D.}_{k k}^{T T} {P P}_{k k | | k k - - 11}^{- - 11} {D D.}_{k k} - - - - - - ((1818))

\{\begin{matrix} {Y Y}_{k k | | k k} = = {Y Y}_{k k | | k k - - 11} + + {Y Y}_{k k | | k k - - 11} {P P}_{\overset{~ ~}{x x} \overset{~ ~}{z z},, k k | | k k - - 11} {\overset{~ ~}{R R}}_{k k}^{- - 11} {P P}_{\overset{~ ~}{x x} \overset{~ ~}{z z},, k k | | k k - - 11}^{T T} {Y Y}_{k k | | k k - - 11}^{T T} \\ {\overset{^^}{y the y}}_{k k | | k k} = = {\overset{^^}{y the y}}_{k k | | k k - - 11} + + {Y Y}_{k k | | k k - - 11} {P P}_{\overset{~ ~}{x x} \overset{~ ~}{z z},, k k | | k k - - 11} {\overset{~ ~}{R R}}_{k k}^{- - 11} (({v v}_{k k} + + {P P}_{\overset{~ ~}{x x} \overset{~ ~}{z z},, k k | | k k - - 11}^{T T} {Y Y}_{k k | | k k - - 11}^{T T} {\overset{^^}{x x}}_{k k | | k k - - 11})) \end{matrix} - - - - - - ((1919))

Step 2.4 Use the following formula to obtain the state estimation at time k And the corresponding covariance matrix P _k|k ;

\{\begin{matrix} {P P}_{k k | | k k} = = {Y Y}_{k k | | k k}^{- - 11} \\ {\overset{^^}{x x}}_{k k | | k k} = = {P P}_{k k | | k k} {\overset{^^}{y the y}}_{k k | | k k} \end{matrix} - - - - - - ((2020))

Repeat the content of the above two modules of time update and status update to achieve the target state tracking estimates.