# A New State Estimation Method with Radar Measurement Missing

Authors: Hongjian Wang; Cun Li; Ying Wang; Qing Li; Xicheng Ban
Journal: Complexity
 (2018)

---

## Abstract

This paper describes a method that addresses the transient loss of observations in sea surface target state estimations. A six degrees of freedom swing platform fixed with a MiniRadaScan is used to simulate the loss of observations. The state transition model based on the historical observation data fit prediction is designed because the existing state estimation method can only use the system model prediction while the observation is missing. An observation data sliding window width adaptive adjustment strategy is proposed that can improve the fitting accuracy of the state transition model. To solve the problem where the weight value of the Gaussian components of the Gaussian mixture filter is not changed in the time update stage while the observation is missing, an adaptive adjustment strategy for the weight is proposed based on the Chapman-Kolmogorov equation, which can improve the estimation precision under the conditions of the missing observation. The simulation test demonstrates the proposed accuracy and real-time performance of the proposed algorithm.


---

## Body

## 1. Introduction

The submarine floating on the sea suffers from environmental factors such as the wind, waves, and currents due to the accidental loss of power. To ensure the safety of the submarine personnel and property, it is important to have a timely rescue. The rescue ship detects the target submarine in real time through a sensor and, when close to the target, tracks the target and maintains a relative distance and bearing with the target submarine; then, the rescue ship carries out the effective rescue operation. It is quite difficult and dangerous to carry out the rescue operation in the level four sea conditions because of the transient loss of the sensor. Therefore, it is practical to study the state estimation algorithm with the missing measurement.In the actual ocean environment, the sensor and the responders have certain installation height differences and the scanning range of the sensor is limited. In the level four sea conditions, the relative movement between the rescue ship and target boats, including roll and heave, may produce a transient failure situation. There are many state estimation methods for the missing measurements in the existing literature. Sinopoli et al. [1] studies the problem of tracking applications of wireless sensor networks with intermittent observations, where the measurements are assumed to be received in full or lost completely. Liu and Goldsmith [2] designs a state estimation method for partial observation losses based on [1], where the observation process can be sent in two packets, which are lost separately. A Kalman filter is designed [3] where the time delay is considered simultaneously for the general case that observations are sent over more than two different wireless channels. Feng et al. [4] designed a robust finite horizon Kalman filter for linear discrete time systems subject to norm-bounded uncertainties in the modeling parameters and missing measurements, and the simulation illustrated the effectiveness of the proposed approach. He et al. [5] designed a novel measurement model to represent both the random measurement delays and the stochastic missing data phenomenon. These methods have a common point: when measurements are absent, only use a system model to predict the state.There are some studies that researched the state estimation algorithms with a measurement missing that is used in the field of target tracking [6]. Yan et al. [7] designed the federated Kalman filter for the case of asynchronous multirate multisensor dynamic systems with randomly missing measurements. Li et al. [8] developed a robust filter by applying the basic interacting multiple model approach and the H-infinity technique for the missing measurements based on a maneuvering target tracking example. Pathirana et al. [9] used the nonlinearly modeled Doppler radar measurements to obtain an accurate estimate of the target dynamics in a linear framework utilizing a recently developed robust state estimation approach. Salti and Di Stefano [10] proposed an online learning transition model via support vector regression for the state estimation with the missing measurement. Williams and Maybeck [11] proposed a new method for tracking the whole trajectory of a ballistic missile in a difficult to observe environment with imperfect sensor measurement, incorporating both missing observations and false alarms.Most of the existing methods are based on the premise of an accurate target motion model, but the application is a radar-target-state estimation based on target tracking and tracking due to the lack of prior knowledge of the environment; if there is a difference between the target model parameters and the actual situation, the model will cause the target state estimation performance worse. The state estimation algorithm cannot provide normal control input for the surface target tracking control system, which is very dangerous for surface target rescue. In this paper, an improved Gaussian mixture cubature Kalman filter with a state transition model based on the historical observation data fit prediction and adaptive weight update of a Gaussian component is proposed. The remainder of this paper is organized as follows. First, a six degrees of freedom swing platform fixed with a MiniRadaScan is used to simulate the loss of observations. Second, the state transition model, based on the historical observation data fit prediction, is designed and an observation data sliding window width adaptive adjustment strategy is proposed. Third, an adaptive adjustment strategy for the weight is proposed based on the Chapman-Kolmogorov equation. Finally, a simulation based on a rescue at sea is illustrated.


## 2. Problem Description

To simulate the loss of observation, a 6 degrees of freedom (DOF) platform (Figure1) is used to simulate the motion of the rescue ship. Figure 2 shows the distance and bearing data of the responder fixed on the forklift, respectively.Figure 1
The 6 DOF platform fixed with MiniRadaScan.Figure 2
The measurements of the responder.It can be seen that when the radar has movement with the 6 DOF platform, the observation data will lose approximately 3~5 seconds periodically. In the test, the transponder is placed on the forklift as the target, unlike the radar with 6 DOF of movement. In a practical application, the target will also be influenced by the external environment, which will increase the probability of a missing observation and insufficient time. Therefore, it is crucial to design the state estimation algorithm with observation loss. In this paper, the design of the state estimation algorithm will be introduced in two parts: the state transition model, which is based on observation, and the Gaussian mixture cubature Kalman filter, which is based on an adaptive weight update.


## 3. The Design of the State Transition Model with Historical Measurement

### 3.1. The State Transition Model

First, the model of the surface target tracking system is given, which is the basis of the state transition model based on observation fitting:(1)xk=Fxk−1+γΔxm+wk−1,zk=1−γhxk+vk,where γ is the parameter of observation loss, γ=1 indicates the loss of the observation, and γ=0 denotes that there is no observation loss. Δxm is the compensation of the observation which needs to be designed in this paper.The position informationxt of the target in the Cartesian coordinate system of the two-dimensional plane can be fitted by some function with variable time t, and then, the state estimation model of the target is established by pt:
(2)xt=pt+wt,where pt denotes the fitting function of xt and wt represents the fitting error. The polynomial fitting pt=a0+a1t+⋯+aktk is usually used to determine the fitting function, where aii=0,…,k is a particular polynomial coefficient.According to the introduction, since the movement of the target when floating on the sea belongs to a slow maneuvering state, greater results with a low polynomial order are available, such as the constant velocity model and the constant acceleration model corresponding to the polynomial with orderk=1 and k=2, respectively. As the order increases, the representative change of state may be more accurate but also leads to an exponential accumulation of computations as the dimension of the equation increases. An improved modeling method for a self-learning state estimation model based on measurement data is proposed.Assuming that the measured data sequence isxn, n=1,2,…,N, and each of them is processed independently, the result is expressed as follows:
(3)Δxn=xn+1−xn.The polynomial functionpn is used to fit the differential function of the measured data, and Δxn can be represented as
(4)Δxn=Δx~n+εn=pn+εn,where Δx~n denotes the fitted estimation values of Δxn and εn represents the fitting errors. Hence, the fitting estimate values x~n+1 of xn+1 can be expressed as
(5)x~n+1=xn+Δx~n.Assume that the sampling time of the observation data isT, and n=t/T; then, the derivative of formula (5) of time t can be calculated:
(6)x~̇n+1=ẋn+Δx~̇n.Then, the discrete form of the state estimation model can be expressed as(7)xn+1ẋn+1=1001xnẋn+Δx~nΔx~̇n+Wxn,where Wxn indicates the process noise of the system.The measurement data of the polynomial fitting method require a certain amount of measured data to achieve the approximate fitting. In the practical application, in order to reflect the state change of the target in real time, one of the most used methods for fitting a coefficient calculation is to point forward continuous observation data from the current. Then, the polynomial prediction equation of the state of the target is modeled, which is called the limited memory data model. To simply distinguish this from formula (7), the index number in the model is changed to m, and formula (7) can be rewritten as
(8)xm+1ẋm+1=1001xmẋm+pmṗm+Wxm,where Δx~n is replaced by polynomial fitting estimates pm.


### 3.2. The Adaptive Scheme of the Sliding Window

To establish the model by fitting the measured data, the main problem is how to select the amount of data. Considering thatM<N, add a sliding window of length M to the observation data. The random error of the system will be decreased while the value of M is large. However, a large value of M will result in a delay of the state estimation; the smaller the value of M, the more accurately it reflects the state and latest trend of the target. However, the state estimation will be easily affected by the measurement noise while the value of M is too small, for which the value of M needs to be compromised.Figure3 gives fitting curves of different widths of the sliding window of the measurements. It can be seen that the fitting value cannot accurately approximate the real value as the length of sliding window increases, but the fitting curve is smooth, which could smooth the observation as a filter. When the length of the sliding window is small, the fitting values can approximate the actual measured value accurately but will be more easily affected by noise. Therefore, the length of the sliding window should be designed according to the estimated requirements in a practical application. The method based on the fitting error of an adaptive design sliding window length will be introduced.Figure 3
The fitting curves of different widths of the sliding window.To improve the influence of the length of sliding window on the fitting accuracy, in this paper, the value ofM is determined according to the fitting error in real time, and the value of M will be larger while the fitting error is relatively stable; on the contrary, the value of M will be smaller. Figure 4 shows the flow chart of the adaptive value of M.Figure 4
Flow chart of the adaptive scheme ofM.Assuming that the initialization of theM is Mmin, XMm is the m-th observation; gMX is the fitting model which can be modeled as
(9)Δxm=pxm=a0+a1m+a2m2,Δẋm=ṗxm=a1+2a2m,Δx¨m=p¨xm=2a2,and it corresponds to the fitting estimation value gMX,m, and the fitting error can be denoted as
(10)WgM,XM=XM1−gMX,1⋮XMM−gMX,M.Assuming that the root mean square fitting error isεM, then
(11)εM=WgM,XM22M.The minimum observation data fitting errorεmin can be expressed as
(12)εmin=εM,M=Mmin,minεM,εmin,M>Mmin.Then, repeat as the flow chart demonstrates, until the following conditions are met:(13)εM>κεmin.


## 3.1. The State Transition Model

First, the model of the surface target tracking system is given, which is the basis of the state transition model based on observation fitting:(1)xk=Fxk−1+γΔxm+wk−1,zk=1−γhxk+vk,where γ is the parameter of observation loss, γ=1 indicates the loss of the observation, and γ=0 denotes that there is no observation loss. Δxm is the compensation of the observation which needs to be designed in this paper.The position informationxt of the target in the Cartesian coordinate system of the two-dimensional plane can be fitted by some function with variable time t, and then, the state estimation model of the target is established by pt:
(2)xt=pt+wt,where pt denotes the fitting function of xt and wt represents the fitting error. The polynomial fitting pt=a0+a1t+⋯+aktk is usually used to determine the fitting function, where aii=0,…,k is a particular polynomial coefficient.According to the introduction, since the movement of the target when floating on the sea belongs to a slow maneuvering state, greater results with a low polynomial order are available, such as the constant velocity model and the constant acceleration model corresponding to the polynomial with orderk=1 and k=2, respectively. As the order increases, the representative change of state may be more accurate but also leads to an exponential accumulation of computations as the dimension of the equation increases. An improved modeling method for a self-learning state estimation model based on measurement data is proposed.Assuming that the measured data sequence isxn, n=1,2,…,N, and each of them is processed independently, the result is expressed as follows:
(3)Δxn=xn+1−xn.The polynomial functionpn is used to fit the differential function of the measured data, and Δxn can be represented as
(4)Δxn=Δx~n+εn=pn+εn,where Δx~n denotes the fitted estimation values of Δxn and εn represents the fitting errors. Hence, the fitting estimate values x~n+1 of xn+1 can be expressed as
(5)x~n+1=xn+Δx~n.Assume that the sampling time of the observation data isT, and n=t/T; then, the derivative of formula (5) of time t can be calculated:
(6)x~̇n+1=ẋn+Δx~̇n.Then, the discrete form of the state estimation model can be expressed as(7)xn+1ẋn+1=1001xnẋn+Δx~nΔx~̇n+Wxn,where Wxn indicates the process noise of the system.The measurement data of the polynomial fitting method require a certain amount of measured data to achieve the approximate fitting. In the practical application, in order to reflect the state change of the target in real time, one of the most used methods for fitting a coefficient calculation is to point forward continuous observation data from the current. Then, the polynomial prediction equation of the state of the target is modeled, which is called the limited memory data model. To simply distinguish this from formula (7), the index number in the model is changed to m, and formula (7) can be rewritten as
(8)xm+1ẋm+1=1001xmẋm+pmṗm+Wxm,where Δx~n is replaced by polynomial fitting estimates pm.


## 3.2. The Adaptive Scheme of the Sliding Window

To establish the model by fitting the measured data, the main problem is how to select the amount of data. Considering thatM<N, add a sliding window of length M to the observation data. The random error of the system will be decreased while the value of M is large. However, a large value of M will result in a delay of the state estimation; the smaller the value of M, the more accurately it reflects the state and latest trend of the target. However, the state estimation will be easily affected by the measurement noise while the value of M is too small, for which the value of M needs to be compromised.Figure3 gives fitting curves of different widths of the sliding window of the measurements. It can be seen that the fitting value cannot accurately approximate the real value as the length of sliding window increases, but the fitting curve is smooth, which could smooth the observation as a filter. When the length of the sliding window is small, the fitting values can approximate the actual measured value accurately but will be more easily affected by noise. Therefore, the length of the sliding window should be designed according to the estimated requirements in a practical application. The method based on the fitting error of an adaptive design sliding window length will be introduced.Figure 3
The fitting curves of different widths of the sliding window.To improve the influence of the length of sliding window on the fitting accuracy, in this paper, the value ofM is determined according to the fitting error in real time, and the value of M will be larger while the fitting error is relatively stable; on the contrary, the value of M will be smaller. Figure 4 shows the flow chart of the adaptive value of M.Figure 4
Flow chart of the adaptive scheme ofM.Assuming that the initialization of theM is Mmin, XMm is the m-th observation; gMX is the fitting model which can be modeled as
(9)Δxm=pxm=a0+a1m+a2m2,Δẋm=ṗxm=a1+2a2m,Δx¨m=p¨xm=2a2,and it corresponds to the fitting estimation value gMX,m, and the fitting error can be denoted as
(10)WgM,XM=XM1−gMX,1⋮XMM−gMX,M.Assuming that the root mean square fitting error isεM, then
(11)εM=WgM,XM22M.The minimum observation data fitting errorεmin can be expressed as
(12)εmin=εM,M=Mmin,minεM,εmin,M>Mmin.Then, repeat as the flow chart demonstrates, until the following conditions are met:(13)εM>κεmin.


## 4. The Design of the Improved Gaussian Mixture Cubature Kalman Filter

### 4.1. The Gaussian Mixture Cubature Kalman Filter

The probability density function of the initial system state is given as (1):
(14)px0=∑i=1Iω0siNx0i;x^0i,P0i.The state and covariance can be propagated using the CKF time update stage, as follows:(15)x^k∣k−1r=∑c=1mωcξc,k∣k−1i+wkj,(16)Pk∣k−1r=∑c=1mωcξc,k∣k−1iξc,k∣k−1Ti−x^k∣k−1r−wkjx^k∣k−1r−wkjT+Qkj,(17)ωk∣k−1tr=ωk∣k−1si⋅ωkpj,(18)ωk∣k−1si=ωk−1∣k−1s,where m is the number of cubature points, r=i−1J+j, ωk∣k−1tr denotes the weight of Gaussian components after time update, ωkpj is the weight of the j-th Gaussian noise of process noise, ωk−1∣k−1si is the weight of i-th state Gaussian component in time k−1, and ωk∣k−1s denotes the weight after propagation.The state and covariance are updated by the CKF measurement update stage. The final outputs of the filter and the weights of components are updated as follows:(19)x^k=∑n=1I⋅J⋅Lωknx^kn,(20)Pk=∑n=1I⋅J⋅Lω¯knPkn+x^kn−x^kx^kn−x^kT,where x^kn is the n-th Gaussian component with the weighted value ωkn, which can be updated as follows:
(21)ωkn=ωk∣k−1trωkmlpzk∣xk,n∑r=1I⋅J∑l=1Lωk∣k−1trωkmlpzk∣xk,n,where n=r−1L+l, ωkml is the weight of l-th Gaussian components of observation noise, and pzk∣xk,n is the observation likelihood distribution of the n-th Gaussian component and can be calculated as follows:
(22)pzk∣xk,n=12πσn2exp−12zk−z^k∣k−1r,lσn2.Notice that the weightsωk∣k−1si do not change during the propagation from (7), which is valid if the system has a precise model. The reason that the weight keeps constant is that it is assumed that the covariance is small enough. This is particularly a problem if the uncertainty in the system model is large and the measurements are not frequently available.


### 4.2. Adaptive Weight Update

Consider the nonlinear system (1) with the probability density function of the initial conditions px0. Given the Gaussian mixture approximation of the probability density function,
(23)p^xk=∑i=1IωksiNxki;x^ki,Pki.Given the true probability density function of the system state by the Chapman-Kolmogorov equation,(24)pxk=∫ℝnpxk∣xk−1pxk−1dxk−1.Mean-square optimal new weights can be obtained by minimizing the following integral square difference between the true probabilitypxk and the approximated probability p^xk in the least square algorithm:
(25)wk∣k−1=argmin12∫ℝnpxk−p^xk2dxk,where wk∣k−1=ωk∣k−11ωk∣k−12…ωk∣k−1IT denotes the vector of the weights of every Gaussian component at time k. To resolve formula (18), the cost function is given as follows [11]:
(26)Jwk∣k−1=Jppwk∣k−1−2Jpp^wk∣k−1+Jp^p^wk∣k−1,where the first term denotes the self-likeness of the true probability density function of the system state [12] and Jppwk∣k−1=∫ℝnpxkpxkdxk. The second term denotes the cross-likeness of the true probability and the approximation one, and Jpp^wk∣k−1=∫ℝnpxkp^xkdxk. The last term is the self-likeness of the approximation probability of the system state, and Jp^p^wk∣k−1=∫ℝnp^xkp^xkdxk.The formula (17) rests on the assumption that the Gaussian mixture approximation is equal with the true probability density function at time k, namely, p^xk=pxk, and the proof procedure is as follows.Substitute the formula (14) into formula (24) and the following relationship can be given:
(27)pxk=∑i=1Iωk−1si∫ℝnNxk−1i;x^k−1i,Pk−1iNxk;fxk−1+wk−1,Qk−1dxk−1=∑i=1Iωk−1si∫ℝnNxk−1i;x^k−1i,Pk−1iNxk;xk−1−x^k−1i+fxk−1+wk,Qkdxk−1+∑i=1Iωk=1si∫ℝnεk−1dxk−1,where
(28)εk−1=Nxk−1i;x^k−1i,Pk−1iNxk;fxk−1+wk−1,Qk−1−Nxk;xk−1−x^k−1i+fxk−1+wk,Qk.Assuming that all covariancePk−1i of the Gaussian components is small enough so that the linearization around a mean is a representative for the dynamics in the vicinity of the respective mean and that there are sufficient number of Gaussian components, and then Pk−1i⟶0 implies ∫ℝnεk−1dxk−1⟶0. pxk can be rewritten as
(29)pxk≈∑i=1Iωk−1si∫ℝnNxk−1i;x^k−1i,Pk−1iNxk;xk−1−x^k−1i+fxk−1+wk,Qkdxk−1=∑i=1Iωk−1siNxk−1;fxk−1+wk−1,Qk−1=∑i=1Iωk−1siNxki;x^ki,Pki=p^xk−1.Now the derivation of the terms of cost function is given as follows [13, 14]:
(30)Jppwk∣k−1=∫ℝnpxk2dxk=∫ℝn∫ℝnpxk∣xk−1pxk−1dxk−12dxk≈∫ℝn∫ℝnpxk∣xk−1p^xk−1dxk−12dxk=∫ℝn∫ℝnNxk;fxk−1+wk,Qkp^xk−1dxk−12dxk=∫ℝn∫ℝnNxk;fxk−1+wk,Qk∑i=1Iωk−1siNxk−1i;x^k−1i,Pk−1idxk−12dxk=wk−1TJppwk−1.Similarly,(31)Jpp^wk∣k−1=wk∣k−1TJpp^wk−1,Jp^p^wk∣k−1=wk∣k−1TJp^p^wk∣k−1.The elements of the matrices are given as follows:(32)Jijpp=∫∫Nxk;fxk−1+wk,QkNxk−1i;x^k−1i,Pk−1idxk−12dxk=∫ENxk−1i;x^k−1i,Pk−1iNxk;fxk−1+wk,Qk2dxk,Jijpp^=∫Nfxk−1;xk∣k−1i,Pk∣k−1i+Qk−1Nxk−1;xk−1j,Pk−1jdxk−1=ENxk−1;xk−1j,Pk−1jNfxk−1;xk∣k−1i,Pk∣k−1i+Qk−1=∑c=1mωcNξc,k∣k−1;xk∣k−1i,Pk∣k−1i,where ωc,ξc are the cubature points.
(33)Jijp^p^=Nxk∣k−1i;xk∣k−1j,Pk∣k−1i+Pk∣k−1j=2πPk∣k−1i+Pk∣k−1i−1/2exp−12xk∣k−1i−xk∣k−1jTPk∣k−1i+Pk∣k−1i−1xk∣k−1i−xk∣k−1j.The first term in the cost function (15) is not needed in the optimization process and is used only to provide an overall magnitude of the uncertainty propagation error. According to the above relations, the final formulation of the formula (15) can be obtained as follows:
(34)wk∣k−1=argmin12wk∣k−1TJp^p^wk∣k−1−wk∣k−1TJpp^wk−1.Based on the above derivation, the summary of the improved Gaussian mixture CKF with adaptive weight update can be given as shown in Table1.Table 1
Computation steps of IGM-CKF.
Time update
x^k∣k−1r=∑c=1mωcξc,k∣k−1i+wkjPk∣k−1r=∑c=1mωcξc,k∣k−1iξc,k∣k−1Ti−x^k∣k−1r−wkjx^k∣k−1r−wkjT+Qkjwk∣k−1=argmin12wk∣k−1TJp^p^wk∣k−1−wk∣k−1TJpp^wk−1
Measurement update
Pzz,k∣k−1r,l=∑c=1mωcεc,k∣k−1rεc,k∣k−1Tr−z^k∣k−1r−vklz^k∣k−1r−vklT+RklPxz,k∣k−1r,l=∑c=1mωcξc,k∣k−1rεc,k∣k−1Tr−x^k∣k−1rz^k∣k−1r−vklTKkr,l=Pxz,k∣k−1r,lPzz,k∣k−1−1r,lx^kn=x^k∣k−1r+Kkr,lz^k−z^k∣k−1r,lPkn=Pk∣k−1r−Kkr,lPzz,k∣k−1r,lKkTr,l
Filter output
Equations (19), (20), (21), and (22)


## 4.1. The Gaussian Mixture Cubature Kalman Filter

The probability density function of the initial system state is given as (1):
(14)px0=∑i=1Iω0siNx0i;x^0i,P0i.The state and covariance can be propagated using the CKF time update stage, as follows:(15)x^k∣k−1r=∑c=1mωcξc,k∣k−1i+wkj,(16)Pk∣k−1r=∑c=1mωcξc,k∣k−1iξc,k∣k−1Ti−x^k∣k−1r−wkjx^k∣k−1r−wkjT+Qkj,(17)ωk∣k−1tr=ωk∣k−1si⋅ωkpj,(18)ωk∣k−1si=ωk−1∣k−1s,where m is the number of cubature points, r=i−1J+j, ωk∣k−1tr denotes the weight of Gaussian components after time update, ωkpj is the weight of the j-th Gaussian noise of process noise, ωk−1∣k−1si is the weight of i-th state Gaussian component in time k−1, and ωk∣k−1s denotes the weight after propagation.The state and covariance are updated by the CKF measurement update stage. The final outputs of the filter and the weights of components are updated as follows:(19)x^k=∑n=1I⋅J⋅Lωknx^kn,(20)Pk=∑n=1I⋅J⋅Lω¯knPkn+x^kn−x^kx^kn−x^kT,where x^kn is the n-th Gaussian component with the weighted value ωkn, which can be updated as follows:
(21)ωkn=ωk∣k−1trωkmlpzk∣xk,n∑r=1I⋅J∑l=1Lωk∣k−1trωkmlpzk∣xk,n,where n=r−1L+l, ωkml is the weight of l-th Gaussian components of observation noise, and pzk∣xk,n is the observation likelihood distribution of the n-th Gaussian component and can be calculated as follows:
(22)pzk∣xk,n=12πσn2exp−12zk−z^k∣k−1r,lσn2.Notice that the weightsωk∣k−1si do not change during the propagation from (7), which is valid if the system has a precise model. The reason that the weight keeps constant is that it is assumed that the covariance is small enough. This is particularly a problem if the uncertainty in the system model is large and the measurements are not frequently available.


## 4.2. Adaptive Weight Update

Consider the nonlinear system (1) with the probability density function of the initial conditions px0. Given the Gaussian mixture approximation of the probability density function,
(23)p^xk=∑i=1IωksiNxki;x^ki,Pki.Given the true probability density function of the system state by the Chapman-Kolmogorov equation,(24)pxk=∫ℝnpxk∣xk−1pxk−1dxk−1.Mean-square optimal new weights can be obtained by minimizing the following integral square difference between the true probabilitypxk and the approximated probability p^xk in the least square algorithm:
(25)wk∣k−1=argmin12∫ℝnpxk−p^xk2dxk,where wk∣k−1=ωk∣k−11ωk∣k−12…ωk∣k−1IT denotes the vector of the weights of every Gaussian component at time k. To resolve formula (18), the cost function is given as follows [11]:
(26)Jwk∣k−1=Jppwk∣k−1−2Jpp^wk∣k−1+Jp^p^wk∣k−1,where the first term denotes the self-likeness of the true probability density function of the system state [12] and Jppwk∣k−1=∫ℝnpxkpxkdxk. The second term denotes the cross-likeness of the true probability and the approximation one, and Jpp^wk∣k−1=∫ℝnpxkp^xkdxk. The last term is the self-likeness of the approximation probability of the system state, and Jp^p^wk∣k−1=∫ℝnp^xkp^xkdxk.The formula (17) rests on the assumption that the Gaussian mixture approximation is equal with the true probability density function at time k, namely, p^xk=pxk, and the proof procedure is as follows.Substitute the formula (14) into formula (24) and the following relationship can be given:
(27)pxk=∑i=1Iωk−1si∫ℝnNxk−1i;x^k−1i,Pk−1iNxk;fxk−1+wk−1,Qk−1dxk−1=∑i=1Iωk−1si∫ℝnNxk−1i;x^k−1i,Pk−1iNxk;xk−1−x^k−1i+fxk−1+wk,Qkdxk−1+∑i=1Iωk=1si∫ℝnεk−1dxk−1,where
(28)εk−1=Nxk−1i;x^k−1i,Pk−1iNxk;fxk−1+wk−1,Qk−1−Nxk;xk−1−x^k−1i+fxk−1+wk,Qk.Assuming that all covariancePk−1i of the Gaussian components is small enough so that the linearization around a mean is a representative for the dynamics in the vicinity of the respective mean and that there are sufficient number of Gaussian components, and then Pk−1i⟶0 implies ∫ℝnεk−1dxk−1⟶0. pxk can be rewritten as
(29)pxk≈∑i=1Iωk−1si∫ℝnNxk−1i;x^k−1i,Pk−1iNxk;xk−1−x^k−1i+fxk−1+wk,Qkdxk−1=∑i=1Iωk−1siNxk−1;fxk−1+wk−1,Qk−1=∑i=1Iωk−1siNxki;x^ki,Pki=p^xk−1.Now the derivation of the terms of cost function is given as follows [13, 14]:
(30)Jppwk∣k−1=∫ℝnpxk2dxk=∫ℝn∫ℝnpxk∣xk−1pxk−1dxk−12dxk≈∫ℝn∫ℝnpxk∣xk−1p^xk−1dxk−12dxk=∫ℝn∫ℝnNxk;fxk−1+wk,Qkp^xk−1dxk−12dxk=∫ℝn∫ℝnNxk;fxk−1+wk,Qk∑i=1Iωk−1siNxk−1i;x^k−1i,Pk−1idxk−12dxk=wk−1TJppwk−1.Similarly,(31)Jpp^wk∣k−1=wk∣k−1TJpp^wk−1,Jp^p^wk∣k−1=wk∣k−1TJp^p^wk∣k−1.The elements of the matrices are given as follows:(32)Jijpp=∫∫Nxk;fxk−1+wk,QkNxk−1i;x^k−1i,Pk−1idxk−12dxk=∫ENxk−1i;x^k−1i,Pk−1iNxk;fxk−1+wk,Qk2dxk,Jijpp^=∫Nfxk−1;xk∣k−1i,Pk∣k−1i+Qk−1Nxk−1;xk−1j,Pk−1jdxk−1=ENxk−1;xk−1j,Pk−1jNfxk−1;xk∣k−1i,Pk∣k−1i+Qk−1=∑c=1mωcNξc,k∣k−1;xk∣k−1i,Pk∣k−1i,where ωc,ξc are the cubature points.
(33)Jijp^p^=Nxk∣k−1i;xk∣k−1j,Pk∣k−1i+Pk∣k−1j=2πPk∣k−1i+Pk∣k−1i−1/2exp−12xk∣k−1i−xk∣k−1jTPk∣k−1i+Pk∣k−1i−1xk∣k−1i−xk∣k−1j.The first term in the cost function (15) is not needed in the optimization process and is used only to provide an overall magnitude of the uncertainty propagation error. According to the above relations, the final formulation of the formula (15) can be obtained as follows:
(34)wk∣k−1=argmin12wk∣k−1TJp^p^wk∣k−1−wk∣k−1TJpp^wk−1.Based on the above derivation, the summary of the improved Gaussian mixture CKF with adaptive weight update can be given as shown in Table1.Table 1
Computation steps of IGM-CKF.
Time update
x^k∣k−1r=∑c=1mωcξc,k∣k−1i+wkjPk∣k−1r=∑c=1mωcξc,k∣k−1iξc,k∣k−1Ti−x^k∣k−1r−wkjx^k∣k−1r−wkjT+Qkjwk∣k−1=argmin12wk∣k−1TJp^p^wk∣k−1−wk∣k−1TJpp^wk−1
Measurement update
Pzz,k∣k−1r,l=∑c=1mωcεc,k∣k−1rεc,k∣k−1Tr−z^k∣k−1r−vklz^k∣k−1r−vklT+RklPxz,k∣k−1r,l=∑c=1mωcξc,k∣k−1rεc,k∣k−1Tr−x^k∣k−1rz^k∣k−1r−vklTKkr,l=Pxz,k∣k−1r,lPzz,k∣k−1−1r,lx^kn=x^k∣k−1r+Kkr,lz^k−z^k∣k−1r,lPkn=Pk∣k−1r−Kkr,lPzz,k∣k−1r,lKkTr,l
Filter output
Equations (19), (20), (21), and (22)


## 5. Simulation

A series of simulations are carried out to evaluate the performance of the method proposed in this paper. The content is divided into two parts: the first part is the simulation of adaptive adjustment of the sliding window and the second part is the simulation of the target state estimation method with the loss of observation. The data set used in the simulation is to simulate a certain submarine target floating freely affected by the wind, wave, and currents. The environmental parameters are set as follows: the speed of wind is 13.8 m/s, the average wind angle is 60°, the wave height is 2 m, the average wave angle is 50°, the speed of the current is 1 m/s, and the average current angle is 55°. The initialization position of the submarine is0,0, and the float trajectory is the black solid line in Figure 5.Figure 5
The trajectory of the target with missing measurements.The length of sliding window is designed by formula (24). As shown in Figure 6, the length of sliding windows can be initialized by M=16, while εmin=0.5. In addition, Figure 7 shows the adaptive adjustment process of the sliding windows during the target tracking.Figure 6
The relationship between the sliding windows and fitting error.Figure 7
The curves of lengths of sliding windows.As shown in Figure7, the length of the sliding widow is initialized as 16, and as the experiment progresses, the length of the sliding window changes adaptively. The sliding window becomes larger because the movement of the target becomes steady, and the state estimation of the target converges to a small range of error.Figures8 and 9 show the fitting results of position and velocity, respectively.Figure 8
The position fitting error.Figure 9
The velocity fitting error.Figures10 and 11 show the fitting error of position and velocity, respectively.Figure 10
The RMSE of position fitting.Figure 11
The RMSE of velocity fitting.As shown in Figure10, the error of the position fitting estimation could be limited to 0.5 m when M=16. The fitting error of the estimation could be converged to approximately 0.2 m, which makes the sliding window become large. Figure 11 shows that the error of the north velocity fitting estimation could be limited to 0.02 m/s, and the error of the east velocity could be limited to 0.006 m/s.To ensure the reliability of the observation, it is assumed that the initial observation of the target can be observed three times continuously, so the initial value of the filter can be determined by the observational data at the first three moments:(35)x3∣3=x3,ẋ3,x¨3,y3,ẏ3,y¨3=x3,x3−x2,x3−2x2+x1,y3,y3−y2,y3−2y2+y1.Three observation loss times are set in the simulation (250 s, 300 s), (450 s, 520 s), and (650 s, 700 s). In the period of time for formulaγ=1, the adaptive state transfer model will be used to predict the target state, and the Gaussian component weights are adaptively updated. Figure 5 shows the estimated trajectory curve of various algorithms with missing measurements. In the figure, the black solid line denotes the real target trajectory, the green cross denotes the measurements, the blue dashed line denotes the estimation of GM-CKF, and the red dash-dot line denotes the estimation trajectory of IGM-CKF. It can be seen from the figure that the estimated trajectories of two algorithms are consistent with normal measurement, and GM-CKF will appear as a larger deviation when the measurement is missing. There are 3 observation loss times in the simulation, and the curve of GM-CKF corresponds to the emergence of 3 section-estimated trajectory deviations. The IGM-CKF proposed in this paper, with an adaptive state transition model and adaptive Gaussian component weight update, can maintain a high estimation accuracy in the whole simulation.The RMSE of the position of the two methods are given as Figures12 and 13. It can be seen from the figure that when the measurements are lost, the GM-CKF algorithm, without an adaptive state transition model of the linear estimation error, increases. This is because the method only uses the conventional state transfer model to predict the target state, similar to the dead reckoning, so there will be an increasing cumulative error. When the measurement is normal, because of the observation update, the estimation could jump to normal. The adaptive state transition model and adaptive Gaussian component weight update IGM-CKF can predict the target state according to the target motion trend, measured when measurements are missing, and improve the accuracy of the estimate.Figure 12
The RMSE of north position.Figure 13
The RMSE of east position.The RMSE of the velocity of the two methods are given as Figures14 and 15. It can be seen from the figure that there are little differences on the RMSE of the north velocity of the two methods, because the speed on the north of the floating wrecked boat is relatively low and the estimation error will be relatively small. The state transition model used contains the acceleration item, so the estimation of the speed east of GM-CKF will be showing a gradual increase, and the RMSE of the IGM-CKF proposed in this paper can be kept in a low range when measurements are missing.Figure 14
The RMSE of north velocity.Figure 15
The RMSE of east velocity.Table2 shows the average RMSE and running time of the two methods. It can be seen from the table that the GM-CKF has a larger RMSE and shorter running time because the algorithm predicts the target state only using the state transition model and cannot carry out the measurement model when the measurement is missing. The IGM-CKF proposed in this paper could use the historical data to predict the motion trend of the target and needs to carry the adaptive Gaussian component weight adjustment when the measurement is missing, which has a higher estimation accuracy and a longer running time. However, the running time of the millisecond level could completely satisfy the need for real-time target tracking.Table 2
The average RMSE and running time of various algorithms.
North position/m
East position/m
North velocity/ms−1
East velocity/ms−1
Running time/ms
GM-CKF
1.89
1.01
0.21
0.33
209
IGM-CKF
1.25
0.55
0.11
0.16
368


## 6. Conclusion

An improved GM-CKF algorithm is proposed to solve the problem of state estimation of the rescue on the sea when the measurement is missing. The state transition model based on the historical measurements is designed to improve the accuracy of the prediction, and a novel adaptive scheme of the sliding window has been proposed to adjust the length of sliding window adaptively. The adaptive weight update scheme has been designed to improve the estimation accuracy when the measurement is missing. The simulation result illustrates that the proposed algorithm has high estimation accuracy and could satisfy the need of the real-time target tracking.



---

*Source: 5327637-2018-12-02.xml*