Ship tracking based on the fusion of Kalman filter and particle filter

Aiming at the problem of low filtering accuracy and even divergence caused by model mismatch when using extended Kalman filter in ship GPS navigation and positioning state estimation, a positioning ship state estimation algorithm based on the fusion of improved unscented Kalman filter and particle filter is proposed. Compared with the traditional particle filtering algorithm, the algorithm has two improvements: first, the algorithm uses untraced Kalman as the main framework, and uses the optimal estimation of particle updating state by particle algorithm; Secondly, in the resampling process, a resampling algorithm based on weight optimization is proposed to increase the diversity of particles. The simulation results show that not only the particle degradation degree of the particle filter is reduced, but also the particle tracking accuracy is improved.


Introduction
Global Position System (GPS) is widely used in military and national economy. Ship GPS navigation and positioning is to install a GPS [1] receiver on the moving target (ship) to carry out navigation and positioning calculation. GPS receiver can receive the signal broadcast by the navigation satellite in orbit in real time, and calculate the position and speed of the receiving carrier (ship). The high frequency oscillatory random jamming signal is artificially added into the ground signal transmitted by GPS navigation satellite in civil neighborhood, which results in the high frequency jitter of all derived ground satellite signals. In order to improve the positioning accuracy, it is necessary to filter the GPS observation signal about the ship's position and velocity. The artificial high frequency random jamming signal in GPS system can be regarded as GPS positioning observation noise. The observation noise intensity (variance) can be obtained from GPS observation signal using system identification method [2].

Target motion model
In order to simplify the model, it is assumed that the ship sails in a straight direction when leaving the port. The starting point of the port and wharf is used as the origin of coordinates, and the sampling time is set to T0. S(k) represents the real position of the ship at sampling time kT0, and Y (k) represents the observation value of GPS positioning at sampling time kT0, so there is an observation model.
a k is composed of maneuvering acceleration ( ) u k and random acceleration ( ) w k , namely: ( ) u k is the control signal of the ship power system, which is ( ) u k known maneuver signal output artificially; ( ) w k is the random acceleration caused by sea breeze and waves, assuming that it is a white noise independent of ( ) v k with zero mean and variance of . The state ( ) x k of the system at sampling time 0 kT is defined as the position and speed of the ship, i.e The equation of state of ship motion can be obtained as Namely, the state space model of the system is

3.1.UKF filter algorithm
The concrete realization process of the algorithm. Consider the nonlinear system model with additive noise as follows [4]: Where, the state vector of the system at time K is Step 1: Initialize. Given the filter's initial state 0 x , initial covariance 0 P , process noise variance 0 Q , observation noise variance 0 R .
Step 2: According to the estimated mean  1 k x  and covariance Step 3: Calculate the one-step prediction of the sigma point set.
Step 4: Calculate the predicted mean value and covariance matrix of the system state quantity.
Step 6: Perform nonlinear transformation of Sigma points according to the observation model and calculate the observation quantity to predict the sampling points.
Step 7: Calculate the predicted mean value of system observations.
Step 8: Calculate the new information covariance matrix, the cross-covariance matrix between the state and the observation.
Step 9: Calculate the filter gain matrix.
Step 11: set k= K +1 and repeat step 2 to Step 10 for the next filtering calculation. Since UKF filter algorithm was put forward, and has extensive application prospect in engineering, but the UKF algorithm also has its disadvantages, first of all, in addressing the problem of high dimension, because at this time no trace filter auxiliary scale parameter in 0   , thus makes some sigma point weights 0 w  , this leads to the filter covariance appeared in the process of the positive definite, Thus, the filter value is unstable and even diverges. Secondly, the parameter selection problem of UKF algorithm has not been effectively solved, and the filtering effect of the algorithm will also be affected by the initial filtering value.
Suppose that the state space model of a continuous nonlinear dynamic system can be expressed as Equation (15)      represents the state of the system of nonlinear transfer function, nonlinear observation   h  said the state of the system function, k u and k v are independent of each other and has nothing to do with the system state, respectively state noise and observation noise, m, n state vector dimension and observation vector dimension respectively. The standard particle filter (SIR) algorithm is SIS algorithm which introduces resampling and selects the state transition probability distribution with prior properties as the reference distribution. The SIR algorithm process is as follows: Step 1: Initialize Step 2: Importance sampling is obtained by sampling from the proposed distribution (2) Calculate the weight of particles at time K, as shown in Equation (17) :

3.3.UKF and PF fusion algorithm
The specific steps of the filtering algorithm are shown in Figure 1 (20) Then calculate the one-step prediction of the sigma point set: Calculate the predicted mean value and covariance matrix of the system state quantity: The predicted mean  1 k k x   and covariance matrix (24) Further, the nonlinear transformation of Sigma points was carried out according to the observation model, and the observation quantity was calculated to predict the sampling points: (31) When the estimated value of the mixed filter A and the covariance matrix B at time K are worked out, the particle at the next moment is updated and the mixed filter calculation at time K +1 continues[10].

Analysis of simulation result
It is assumed that the ship is moving on a two-dimensional horizontal plane, the initial position is (-100m,200m), the horizontal movement speed is 2m/s, the vertical movement speed is 20m/s, the scanning period of GPS receiver is T=1s, the mean value of observation noise is 0, and the variance is 100. The closer the target is to uniform linear motion; Otherwise, it is a curved motion.   It can be seen that the maximum observed noise of displacement is close to 35m. For the target sports field (about 1800m, about 250m wide), this noise is very large. Of course, it is only for simulation, and the measurement error of the actual sensor cannot be so large. After Kalman filtering, the position deviation is reduced to below 10m. You can see that. Kalman filter can not eliminate noise completely, but it has reduced the influence of noise to the maximum extent.
Therefore, Kalman filter has achieved a good effect. Compared with Figure 2 and Figure 3, it can be concluded that PF-KF filter has a stronger tracking effect, and most of its deviation is almost  Figure 5 that the filtered tracking track is quite consistent with the real track.

Conclusion
This paper mainly studies and analyzes the current main filtering theory and related algorithms. Firstly, linear KF algorithm based on KF framework is introduced. Then, in order to solve the state estimation problem of nonlinear and non-Gaussian systems, PF algorithm based on Monte Carlo method is further introduced. Combining KF algorithm with PF algorithm, a hybrid filtering algorithm combining KF and PF is proposed, which is introduced from ship target tracking related problems. Ship target tracking model based on KF algorithm, KF and PF hybrid filtering algorithm is established respectively. The specific principles and steps of target tracking by using the above two algorithms are described in detail. The simulation experiment is carried out by comparing the trajectory simulation and tracking error. Experimental results show that the estimation result has higher accuracy than that of kalman filter and particle filter alone, and the target tracking algorithm based on KF and PF hybrid filter has the best tracking effect and the smallest error.