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.
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 (Figure 1) 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.
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: where is the parameter of observation loss, indicates the loss of the observation, and denotes that there is no observation loss. is the compensation of the observation which needs to be designed in this paper.
The position information of the target in the Cartesian coordinate system of the two-dimensional plane can be fitted by some function with variable time , and then, the state estimation model of the target is established by : where denotes the fitting function of and represents the fitting error. The polynomial fitting is usually used to determine the fitting function, where 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 order and , 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 is , , and each of them is processed independently, the result is expressed as follows:
The polynomial function is used to fit the differential function of the measured data, and can be represented as where denotes the fitted estimation values of and represents the fitting errors. Hence, the fitting estimate values of can be expressed as
Assume that the sampling time of the observation data is , and ; then, the derivative of formula (5) of time can be calculated:
Then, the discrete form of the state estimation model can be expressed as where 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 , and formula (7) can be rewritten as where is replaced by polynomial fitting estimates .
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 that , add a sliding window of length to the observation data. The random error of the system will be decreased while the value of is large. However, a large value of will result in a delay of the state estimation; the smaller the value of , 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 is too small, for which the value of needs to be compromised.
Figure 3 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.
To improve the influence of the length of sliding window on the fitting accuracy, in this paper, the value of is determined according to the fitting error in real time, and the value of will be larger while the fitting error is relatively stable; on the contrary, the value of will be smaller. Figure 4 shows the flow chart of the adaptive value of .
Assuming that the initialization of the is , is the -th observation; is the fitting model which can be modeled as and it corresponds to the fitting estimation value , and the fitting error can be denoted as
Assuming that the root mean square fitting error is , then
The minimum observation data fitting error can be expressed as
Then, repeat as the flow chart demonstrates, until the following conditions are met:
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):
The state and covariance can be propagated using the CKF time update stage, as follows: where is the number of cubature points, , denotes the weight of Gaussian components after time update, is the weight of the -th Gaussian noise of process noise, is the weight of -th state Gaussian component in time , and 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: where is the -th Gaussian component with the weighted value , which can be updated as follows: where , is the weight of -th Gaussian components of observation noise, and is the observation likelihood distribution of the -th Gaussian component and can be calculated as follows:
Notice that the weights 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 . Given the Gaussian mixture approximation of the probability density function,
Given the true probability density function of the system state by the Chapman-Kolmogorov equation,
Mean-square optimal new weights can be obtained by minimizing the following integral square difference between the true probability and the approximated probability in the least square algorithm: where denotes the vector of the weights of every Gaussian component at time . To resolve formula (18), the cost function is given as follows [11]: where the first term denotes the self-likeness of the true probability density function of the system state [12] and . The second term denotes the cross-likeness of the true probability and the approximation one, and . The last term is the self-likeness of the approximation probability of the system state, and .
The formula (17) rests on the assumption that the Gaussian mixture approximation is equal with the true probability density function at time , namely, , and the proof procedure is as follows.
Substitute the formula (14) into formula (24) and the following relationship can be given: where
Assuming that all covariance 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 implies . can be rewritten as
Now the derivation of the terms of cost function is given as follows [13, 14]:
Similarly,
The elements of the matrices are given as follows: where are the cubature points.
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:
Based on the above derivation, the summary of the improved Gaussian mixture CKF with adaptive weight update can be given as shown in Table 1.
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 is , and the float trajectory is the black solid line in Figure 5.
The length of sliding window is designed by formula (24). As shown in Figure 6, the length of sliding windows can be initialized by , while . In addition, Figure 7 shows the adaptive adjustment process of the sliding windows during the target tracking.
As shown in Figure 7, 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.
Figures 8 and 9 show the fitting results of position and velocity, respectively.
Figures 10 and 11 show the fitting error of position and velocity, respectively.
As shown in Figure 10, the error of the position fitting estimation could be limited to 0.5 m when . 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:
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 , 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 Figures 12 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.
The RMSE of the velocity of the two methods are given as Figures 14 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.
Table 2 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.
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.
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that there is no conflict of interest regarding the publication of this paper.
Acknowledgments
This research work is supported by the key project of the China Natural Science Foundation (no. 61633008), Harbin Science and Technology Innovation Talents of Special Fund Project (Outstanding Subject Leaders) under Grant 2012RFXXG083, and Natural Science Foundation of Heilongjiang Province under Grant F2015035.