Next Article in Journal
An Enhanced Hybrid Visual–Inertial Odometry System for Indoor Mobile Robot
Next Article in Special Issue
Unmanned Surface Vehicle Collision Avoidance Path Planning in Restricted Waters Using Multi-Objective Optimisation Complying with COLREGs
Previous Article in Journal
A Robust Fire Detection Model via Convolution Neural Networks for Intelligent Robot Vision Sensing
Previous Article in Special Issue
A Hierarchical Stabilization Control Method for a Three-Axis Gimbal Based on Sea–Sky-Line Detection
 
 
Font Type:
Arial Georgia Verdana
Font Size:
Aa Aa Aa
Line Spacing:
Column Width:
Background:
Article

Radar Target Tracking for Unmanned Surface Vehicle Based on Square Root Sage–Husa Adaptive Robust Kalman Filter

1
College of Marine Electrical Engineering, Dalian Maritime University, Dalian 116026, China
2
Key Laboratory of Technology and System for Intelligent Ships of Liaoning Province, Dalian 116026, China
*
Author to whom correspondence should be addressed.
Sensors 2022, 22(8), 2924; https://doi.org/10.3390/s22082924
Submission received: 1 March 2022 / Revised: 3 April 2022 / Accepted: 4 April 2022 / Published: 11 April 2022
(This article belongs to the Special Issue Perception, Planning and Control of Marine Robots)

Abstract

:
Dynamic information such as the position and velocity of the target detected by marine radar is frequently susceptible to external measurement white noise generated by the oscillations of an unmanned surface vehicle (USV) and target. Although the Sage–Husa adaptive Kalman filter (SHAKF) has been applied to the target tracking field, the precision and stability of SHAKF remain to be improved. In this paper, a square root Sage–Husa adaptive robust Kalman filter (SR-SHARKF) algorithm together with the constant jerk model is proposed, which can not only solve the problem of filtering divergence triggered by numerical rounding errors, inaccurate system mathematics, and noise statistical models, but also improve the filtering accuracy. First, a novel square root decomposition method is proposed in the SR-SHARKF algorithm for decomposing the covariance matrix of SHAKF to assure its non-negative definiteness. After that, a three-segment approach is adopted to balance the observed and predicted states by evaluating the adaptive scale factor. Finally, the unbiased and the biased noise estimators are integrated while the interval scope of the measurement noise is constrained to jointly evaluate the measurement and observation noise for better adaptability and reliability. Simulation and experimental results demonstrate the effectiveness of the proposed algorithm in eliminating white noise triggered by the USV and target oscillations.

1. Introduction

Marine radar is widely exploited as a navigational sensing device that can gather dynamic information on surrounding vessels over a wide area while maintaining a decent detection performance [1]. However, its underlying technology is sensitive to external disturbances like ocean waves, which will lead to inaccurate dynamic information being obtained [2,3,4]. To address this issue, target tracking technology has been successfully implemented to predict the dynamic information of the target and produce generally accurate output results. Target tracking is the forecast of the ship’s next possible trajectory as a function of its previous coordinates. The accuracy of the prediction is determined by the accuracy with which the target’s prior and present positions were measured [5]. It is easy to realize the compensation of measurement errors and this plays a vital role in collision avoidance and search and rescue [6].
The Kalman filter (KF) algorithm [7,8] is capable of estimating certain conditions of dynamic systems either in the past, present, or future, or unknown characteristics of the model, and has been widely used in target tracking. Zhang et al. [9] proposed a modified KF in which a novel fading factor is introduced into the cubature KF method to achieve higher robustness and reduce the algorithm complexity simultaneously. The KF is optimized using a new optimization criterion that is integrated with the least-squares method for bearings-only maneuvering target tracking [10]. Lu et al. [11] combined the KF and variational Bayesian method to derive a new robust KF that is more resilient and adaptable than the existing related filters. To tackle the instability problem caused by large-scale application variations, Feng et al. [12] presented a novel filter structure that integrates KF with spatial-temporal regularized correlation filters. To cope with the problem of heavy noise causing dramatic performance degradation, Liu et al. [13] proposed two new maximum correntropy extended KF algorithms that apply the maximum correntropy criterion rather than the minimal mean-square-error criterion to the extended KF. For the preceding target vehicle lateral state estimation, Zhou et al. [14] integrated sensor data and estimated states as measurements and developed a state-input-parameter mixed KF to deal with the cohabitation of uncertain model parameters and unknown control inputs.
The performance of a traditional KF may suffer due to the undervaluation of the noise covariance matrix and the response latency of the gain matrix when the target undergoes an abrupt state change. To overcome the aforementioned problems, SHAKF [15,16] has been introduced. To fulfill the need for greater accuracy and stability of the integrated navigation system, Guo et al. [17] implemented a Sage–Husa adaptive Kalman filter with nonholonomic constraints and forward/backward filtering to an IMU/GPS integrated system. In [18], a novel approach based on the adaptive current statistical model with velocity prediction is proposed, which uses a simplified Sage–Husa estimator to achieve an online measurement noise covariance matrix. At each tracking step, Hou et al. [19] employed a modified Sage–Husa online noise estimator to predict the uncertain process and measurement noise. To improve the performance of state estimation, a state error covariance adaptive five-degree cubature Kalman technique based on the Sage–Husa noise estimation principle is presented in [20]. Yan et al. [21] proposed a tracking method that integrates an improved compressive tracking algorithm with a modified SHAKF to achieve excellent accuracy and robustness in tracking. Luo et al. [22] proposed an adaptive unscented KF based on a modified SHAKF and divergence calculation technique to correct the measurement noise for multi-dimensional vehicle driving state evaluation. To improve the accuracy of an attitude heading reference system, Narasimhappa et al. [23] proposed a modified Sage–Husa adaptive robust KF (MSHARKF) based on adaptive robust estimate theory. However, due to the small size of a USV and its susceptibility to external environmental factors, these methods are not very effective in eliminating white noise generated by the oscillations of the USV and the target, and this will give rise to instability and loss of accuracy when dealing with the filter divergence problem that occurs in target tracking. Therefore, the square root Sage–Husa adaptive robust KF (SR-SHARKF) algorithm is used for target tracking of the constant jerk model in this paper.
In this paper, the contributions are primarily reflected in five aspects as follows:
(1) Use of the constant jerk model can compensate for the dynamic interference caused by a large number of external factors in the marine environment.
(2) A SR-SHARKF algorithm is developed to significantly improve the filtering accuracy while reducing the impact of transient disturbance caused by the large-scale USV jitter problem on subsequent estimation.
(3) A novel square-root decomposition method is introduced into the SHAKF algorithm to decompose the covariance matrix to assure its non-negative definiteness, which can address the issue of filtering divergence triggered by numerical rounding errors.
(4) A three-segment approach is adopted to balance the observations and predicted states by evaluating the adaptive scale factor, which is beneficial to reduce the influence of outliers and state disturbances generated by severe conditions of the USV oscillations.
(5) The measurement noise range is limited to a minimum and maximum threshold, after which the unbiased and biased noise estimators are integrated to evaluate the measurement and observation noise, resulting in increased flexibility and reliability by minimizing filtering divergence.
The remainder of the paper is laid out as follows: Section 2 briefly introduces the state-space model of the target tracking system. Section 3 describes briefly the Sage–Husa adaptive Kalman filter algorithm and the extended Sage–Husa adaptive Kalman filter is proposed. In Section 4, the parameters are determined and we compare the results of simulations with different algorithms. Finally, the conclusions are presented in Section 5.

2. State-Space Model of Target Tracking System

The acceleration of the target may be constantly changing during the movement, but it can be considered that the change rate of acceleration, i.e., the jerk, remains a constant value over a long period and space, and subject to random systematic errors within a certain range. Therefore, the SR-SHARKF algorithm is applied based on the constant jerk dynamic model. Since the algorithm is proposed for linear systems, it is only applicable to linear systems. The constant jerk dynamic model comprises all the indispensable parameters for target tracking, and it is used to link the radar-measured variables with the state variables.
Traditional KF-based tracking algorithms are typically accomplished by tracking the target’s coordinates, whether applied to shipboard radar systems or other platforms. Since the information measured by the radar includes only the velocity of the target, the range from the USV to the target, the azimuth, and the heading of the target, the aforementioned parameters should be converted to Cartesian coordinates to apply the KF framework. The mapping of angles and ranges to Cartesian space has an uncertainty factor that causes many errors, but they are within the allowable range of the system. Assuming that the initial starting point P init of the USV radar target tracking is the coordinate origin, the orientation of due east is the X-axis, and the orientation of due north is the Y-axis, a 2-D plane coordinate system in the radar plane is established as shown in Figure 1. The coordinates of the USV’s initial starting point P init , and the first point P first to the current point P curt are ( x 0 U , y 0 U ) , ( x 1 U , y 1 U ) ,…, ( x n U , y n U ) , respectively. Considering that P Tcurt ( x , y ) represents the current position of the marine target, the current position of the target can be obtained from the position of the USV as follows:
x = i = 1 n ( x i U x i 1 U ) + d cos β y = i = 1 n ( y i U y i 1 U ) + d sin β
where n represents the number of the radar’s detection periods from the start to the current state for target tracking, d is the distance between the USV and the target, and β is expressed as
β = 5 π 2 θ , i f ( 5 π 2 θ ) 2 π π 2 θ , e l s e w i s e
where θ is the azimuth between the target and the USV.
Let the relative position, relative velocity, relative acceleration, and relative jerk in the X-axis direction be x k , x ˙ k , x ¨ k , x k , and the measured values of the relative position and relative velocity are x k m , x ˙ k m , respectively. The system state-space model of the target can be established as the following discrete formation. The system equation is
X k = Φ X k 1 + G W k 1
where X k is the estimated state vector of the target motion in front of the k-th detection period, Φ is the state transition matrix, G represents the system noise distribution matrix, and W k denotes the system noise vector. The matrix forms of X k , Φ , and G are expressed as follows:
X k = x k , x ˙ k , x ¨ k , x k T
Φ = 1 Δ T Δ T 2 / 2 Δ T 3 / 6 0 1 Δ T Δ T 2 / 2 0 0 1 Δ T 0 0 0 1
G = Δ T 4 / 24 Δ T 3 / 6 Δ T 2 / 2 Δ T T
where Δ T is the radar sampling time. The measurement equation is
Z k = H X k + V k
where Z k is the measurement state vector of the target motion in front of the k-th detection period, H is the measurement matrix, and V k represents the measurement noise vector. The matrix forms of Z k and H are denoted as follows:
Z k = z k , z ˙ k T
H = 1 0 0 0 0 1 0 0
In (3) and (7), both W k and V k denote Gaussian white noise that satisfies W k W N ( q k , Q k ) , V k W N ( r k , R k ) ; they are independent of each other and meet the following requirements:
E W k = q k
C o v W k , W j T = Q k , j = k 0 , j k
E V k = r k
C o v V k , V j T = R k , j = k 0 , j k
C o v W k , V j T = 0
where q k and Q k represent the mean and covariance of the system noise, respectively, and r k , R k denote the mean and covariance of the measurement noise.
The model in the Y-axis direction can be obtained in the same way, so this paper will not describe it in detail here.

3. Proposed Square-Root Sage–Husa Adaptive Robust Kalman Filter (SR-SHARKF)

3.1. The SHAKF Algorithm

SHAKF, one of variants of the conventional KF, has a comparative advantage in eliminating noise that can vary with environmental conditions, which is significant in increasing the target tracking system’s stability and accuracy. The filtering flows of the SHAKF algorithm can be described as follows [24]:
(1) Time update
X k , k 1 = Φ k , k 1 X k 1 + G k 1 q k 1
P k , k 1 = Φ k , k 1 P k 1 Φ k , k 1 T + G k 1 Q k 1 G k 1 T
where P k , k 1 represents the one-step state prediction error covariance matrix and P k 1 denotes the error covariance matrix of state prediction.
(2) Measurement update
ε k = Z k H k X k , k 1 r k 1
X k = X k , k 1 + K k ε k
K k = P k , k 1 H k T ( H k P k , k 1 H k T + R k ) 1
P k = ( I K k H k ) P k , k 1
where ε k is the discrepancy vector and K k is the filter gain matrix. The statistical recursive estimator for time-varying noise is
G k q k = ( 1 d k ) G k 1 q k 1 + d k ( X k Φ k , k 1 X k 1 )
G k Q k G k T = ( 1 d k ) G k 1 Q k 1 G k 1 T + d k ( K k ε k ε k T K k T + P k Φ k , k 1 P k 1 Φ k , k 1 T )
r k = ( 1 d k ) r k 1 + d k ( Z k H k X k , k 1 )
R k = ( 1 d k ) R k 1 + d k ( ε k ε k T H k P k , k 1 H k T )
where d k = ( 1 b ) / ( 1 b k ) ( 0 , 1 ) denotes the amnestic factor, b is the forgetting factor, and 0 < b < 1 .

3.2. The Square-Root Sage–Husa Adaptive Robust Kalman Filter (SR-SHARKF)

In order to operate on the vibration problem of the USV and target, the SR-SHARKF method is proposed, which is based on the SHAKF algorithm. The improvement of the SR-SHARKF algorithm is mainly split into the following several aspects.

3.2.1. Square-Root Decomposition Method

Based on the SHAKF algorithm, a square-root decomposition method of the state error covariance matrix is proposed. On the SR-SHARKF algorithm, the one-step state prediction error covariance matrix P k , k 1 , the error variance matrix P k 1 , and the system noise covariance matrix Q k 1 are decomposed. This can guarantee that the covariance matrix is non-negative definite to prevent algorithm divergence issues caused by rounding errors [25].
The covariance decomposition matrix can be expressed as
U k , k 1 = [ Φ k , k 1 U k 1 G k 1 S k 1 ]
where U k , k 1 , U k 1 and S k 1 represent the decomposition factors of matrices P k , k 1 , P k 1 and Q k 1 respectively, and satisfy P k , k 1 = U k , k 1 U k , k 1 T , P k 1 = U k 1 U k 1 T , Q k 1 = S k 1 S k 1 T . U k 1 and S k 1 are obtained by Cholesky factorization [26]:
U k 1 = c h o l ( P k 1 )
S k 1 = c h o l ( Q k 1 )
where c h o l ( · ) represents the Cholesky decomposition function.
The corresponding measurement update changes as shown
F k = U k , k 1 T H k T
λ k = [ F k T F k + R k ] 1
K k = U k , k 1 F k λ k
P k = ( I K k H k ) U k , k 1 U k , k 1 T
After decomposition, although the forms of the covariance matrix, gain matrix, and other matrices have changed, the core idea of the KF is still there, which has a significant effect on overcoming the filter divergence triggered by numerical rounding errors.

3.2.2. Three-Segment Method

The SR-SHARKF algorithm introduces an adaptive scale factor to balance the contribution of kinematic model information and measurements on state vector estimation utilizing a three-segment technique followed by learning statistics [27]. In each iteration, the adaptive scale factor is updated as Kalman gain and state estimation error covariance matrix, reducing the uncertainty in the prediction state error model.
The adaptive scale factor α k is calculated as follows
α k = 1 , Δ X k c 0 c 0 Δ X k c 1 Δ X k c 1 c 0 2 , c 0 < Δ X k c 1 0 , Δ X k > c 1
where Δ X k = X k X k , k 1 denotes the residual vector of the predicted state vector, and 1 c 0 1.5 and 3 c 1 4.5 are two constants [23,28]. The learning statistics of the predicted state error model Δ X k are given as
Δ X k = ε k tr ( H k P k , k 1 H k T + R k )
where tr ( · ) represents the trace of the matrix. Then, the gain matrix and the error variance matrix are adjusted to
λ k = [ 1 α k F k T F k + R k ] 1
K k = 1 α k U k , k 1 F k λ k
P k = 1 α k ( I K k H k ) U k , k 1 U k , k 1 T

3.2.3. Noise Covariance Adjustment Method

According to (17), if the practical system measurement noise is lower than the theoretical one and the preceding estimation error is too great, the R-matrix is more likely to lose its positive definiteness, causing the filtering results to diverge. To overcome this problem, the sequential filtering approach is used to limit the size of each element of the R-matrix diagonal, and the biased noise estimator is introduced to regulate the R-matrix and Q-matrix. The biased noise estimator corresponding to the R-matrix and Q-matrix can be expressed as [29]:
R k = ( 1 d k ) R k 1 + d k ( ε k ε k T )
G k Q k G k T = ( 1 d k ) G k 1 Q k 1 G k 1 T + d k ( K k ε k ε k T K k T )
The upper and lower bounds of the R-matrix named R m a x and R m i n are given as follows
R m a x = d i a g ( R m a x 1 , R m a x 2 , , R m a x l )
R m i n = d i a g ( R m i n 1 , R m i n 2 , , R m i n l )
where l is the dimension of measurement vector. The scalar measurement equation is expressed as follows
β k i = ε k i ( ε k i ) T H k i P k , k 1 i ( H k i ) T
where the superscript i denotes that this variable corresponds to the ith vector in the R-matrix. The R-matrix range is limited to meet the following conditions
R k ( i ) = ( 1 d k ) R k 1 ( i ) + d k R m i n ( i ) , β k i < R m i n ( i ) R m a x ( i ) , β k i > R m a x ( i ) ( 1 d k ) R k 1 ( i ) + d k R k , k 1 ( i ) , o t h e r s
where R k ( i ) represents the ith scalar element of the diagonal of the R-matrix at time k[30]. The unbiased and biased noise estimators are merged, and R k , k 1 ( i ) can be denoted as follows
R k , k 1 ( i ) = β k i , i f R k ( i ) i s p o s i t i v e d e f i n i t e ε k i ( ε k i ) T , i f R k ( i ) i s n o t p o s i t i v e d e f i n i t e
The calculation formula of the Q-matrix can be expressed as
G k Q k G k T = Equation ( 22 ) , i f Q k i s p o s i t i v e s e m i d e f i n i t e Equation ( 38 ) , i f Q k i s n o t p o s i t i v e s e m i d e f i n i t e

3.2.4. The SR-SHARKF Algorithm Steps

The SR-SHARKF algorithm validly integrates the square-root decomposition method, three-segment method, and noise covariance adjustment method to obtain a complete set of algorithm update steps, which mainly include the following steps:
Step 1. The initial values X 0 , P 0 , U 0 , r 0 , R 0 , q 0 , Q 0 , S 0 , b, c 0 , c 1 , k = 1 are given.
Step 2. The nominal state and the error covariance matrix are predicted in one step by (15) and (16) to obtain X k , k 1 and P k , k 1 . P k , k 1 is decomposed into U k , k 1 using (25).
Step 3. The new information series d k , r k , ε k are updated sequentially. If there exists β k i that satisfies β k i > R m a x ( i ) or β k i < R m i n ( i ) , then the scalar elements in the R-matrix according to (42) are updated. If not satisfied, then the elements of the R-matrix judge whether the R-matrix is positive definite according to (43) to be updated by (42).
Step 4. The adaptive scale factor α k is obtained by (32), and successively uses (34), (28), (35), (18), and (36) to calculate λ k , F k , K k , X k , and P k , respectively.
Step 5. q k and Q k adopting (21) and (44) are updated, and then P k 1 and Q k 1 are decomposed using (26) and (27) to obtain U k 1 , S k 1 .
Step 6. Skip to step 2.
The SR-SHARKF algorithm’s flow chart is presented in Figure 2.

4. Simulation and Experimental Results Discussion

Simulations and experiments for the existing standard KF, SHAKF, and MSHARKF [23] with the proposed algorithm were performed to verify and evaluate the effectiveness of the proposed SR-SHARKF algorithm. A simulation test was implemented to compare the performances with the existing algorithms under severe interference from a single-Gaussian distribution and a mixed-Gaussian distribution. Unlike the single-Gaussian distribution, the simulation with mixed Gaussian distribution was operated with 1000 Monte Carlo simulations. Finally, the feasibility of the SR-SHARKF algorithm is discussed by a set of real ship experiment data.

4.1. Simulations and Discussions

4.1.1. Simulations and Analysis of the Noise with Single-Gaussian Distribution

In this section, we compare the proposed algorithm with other existing algorithms by using a maneuvering target tracking example that is generated based on the model (3) and (7) with Δ T = 2 s and verifies the higher estimate performance of our algorithm. The single-Gaussian process and measurement noise are given as follows:
q 1 k N ( 0 , 0.005 ) q 2 k N ( 0 , 0.005 ) q 3 k N ( 0 , 0.005 ) q 4 k N ( 0 , 0.005 ) r 1 k N ( 0 , 1 ) r 2 k N ( 0 , 1 )
In this experiment, the root-mean-square error (RMSE) of position, velocity acceleration, and jerk is used as the comparison criterion in the simulation, which is defined as
RMSE = 1 T k = 1 T ( x k x ^ k ) 2
where x k denotes the real value obtained at time k in a certain direction, T represents the time of filtering iteration, and x ^ k is the estimated value of the filter at time k in a certain direction. For comparison of various aspects, the mean absolute error (MAE) is also used as the comparison criterion in the simulation, which is denoted by
MAE = 1 T k = 1 T x k x ^ k
In the MSHARKF and proposed SR-SHARKF algorithms, the initial values of two constants are chosen as c 0 = 1.2 , c 1 = 4.5 . The parameters of the initial true state are assumed to be X 0 = [ 0 , 1 , 0 . 001 , 0 . 00005 ] T , Y 0 = [ 0 , 4 , 0 . 01 , 0 . 0005 ] T . In addition, the square-root decomposition method (SRD), the three-segment method (TS), and the noise covariance adjustment method (NCA) are introduced to the simulation test as comparative methods to evaluate the advantages of each portion of the proposed algorithm.
Figure 3a shows the actual trajectory, the observation trajectory of the target, and the trajectory estimated by the KF, SHAKF, MSHARKF, SRD, TS, NCA, and proposed SR-SHARKF algorithms. Moreover, the target ship is represented by the small boats in Figure 3a, and their movement on the motion trajectory will be displayed by several boats at various location points, which can reflect the real movement of the target ship more clearly. The actual ship determines the current position of the target ship at each given time by mapping the information of the target ship measured at various times to the coordinates of the Cartesian coordinate system. Figure 3b,c show the actual, observation, and filtering velocities of the X-axis and Y-axis. It can be seen from Figure 3 that the tracking effect in some states is not ideal, but in terms of the overall effect, the proposed SR-SHARKF algorithm achieves satisfactory results in filtering the measurement data compared to other algorithms. Moreover, the runtimes of the KF, SHAKF, MSHARKF, and proposed SR-SHARKF algorithms in a single step run are 9 . 1 × 1 0 4 s , 1 . 16 × 1 0 3 s , 2 . 07 × 1 0 3 s , respectively.
Taking the deviation between the true and the estimated value, the KF, SHAKF, MSHARKF, SRD, TS, NCA, and SR-SHARKF algorithms are used in radar target tracking and the simulation error curves of position, velocity, acceleration, and jerk on the X-axis are shown in Figure 4a–d. It can be seen from Figure 4 that the SRD, TS, and NCA methods for each part of the proposed algorithm have more stable error values compared to SHAKF, and the percentage of states with error values less than SHAKF is higher. As can also be seen from Figure 4, the SR-SHARKF algorithm outperforms other algorithms because the error of most states is kept at a relatively low level and is relatively stable.
According to Table 1, SRD, TS, and NCA have smaller RMSE and MAE values of position, velocity, acceleration, and jerk on the X-axis compared with SHAKF and MSHARKF, indicating that SRD, TS, and NCA can improve SHAKF better. Furthermore, the RMSE and MAE values corresponding to the proposed SR-SHARKF algorithm of position, velocity, acceleration, and jerk on the X-axis are lower compared to other algorithms, which demonstrates that it can obtain the most accurate estimation results than other algorithms in all four state variables and two aspects.
In the Y-axis direction, the ship’s heading varied twice in the opposite direction. It can be seen from Figure 5 that SRD, TS, and NCA are more stable compared to other comparison algorithms, and the error of the algorithm we proposed still remains in a relatively low and stable state compared with other algorithms, which further verifies its relatively outstanding filtering effect. In addition, the RMSE and MAE values of each algorithm are recorded in Table 2, in which it can be seen that the SRD, TS, NCA, and SR-SHARKF algorithms also have obvious advantages in target tracking compared with other algorithms. These results further demonstrate the superior performance of the proposed method.
In the simulation test, compared with the previous ones, the proposed algorithm effectively reduces the appearance rate of non-positive matrices in the X-axis and Y-axis directions, which is due to the effective elimination of numerical rounding errors by the SRD method for covariance decomposition, and the interval restriction of the Q-matrix and R-matrix, as well as the transformation of non-positive to positive matrices by the NCA method together preventing the filtering divergence. Moreover, it can be seen from the simulation tests that the proposed algorithm has significantly improved the speed of adaptation of the time-varying noise covariance compared with the previous ones, which is due to the TS method being employed in the proposed algorithm to balance the observations and predicted states to improve the robustness of the algorithm.
To further verify the robustness of the proposed algorithm, another set of simulation tests was performed for the same model and different trajectories. We only compared the proposed algorithm with the KF, SHAKF, and MSHARKF algorithms. The parameters of the initial true state were chosen as X 0 = [ 2 , 1 , 0 . 001 , 0 . 0002 ] T , Y 0 = [ 1 , 2 , 0 . 0005 , 0 . 0001 ] T .
Figure 6a shows the actual trajectory, the observation trajectory of the target, and the trajectory estimated by the KF, SHAKF, MSHARKF, and proposed SR-SHARKF algorithms. Figure 6b,c show the actual, observation, and filtering velocities of the X-axis and Y-axis. From Figure 6, it can be seen that the proposed SR-SHARKF algorithm outperforms other algorithms in filtering the measurement data.
In the X-axis direction, the USV sails in a positive direction and has a rapid speed change. It can be observed from Figure 7 and Table 3 that the proposed SR-SHARKF has a higher percentage of smaller error values than other algorithms, and the RMSE and MAE values corresponding to the proposed SR-SHARKF algorithm of position, velocity, acceleration, and jerk are lower compared to other algorithms, which shows the superiority of the proposed algorithm over other algorithms.
In the Y-axis direction, the USV sails in a negative direction and the speed changes faster. It can be observed from Figure 8 and Table 4 that the faster the speed change, the worse the tracking effect of all algorithms. However, the proposed SR-SHARKF still outperforms other algorithms, indicating that the algorithm can also bring better performances for different trajectories.

4.1.2. Simulations and Analysis of the Noise with Mixed-Gaussian Distribution

The same model as above is used to generate a maneuvering target tracking example except that the process and measurement noise are white noise satisfying the mixed-Gaussian distribution. The mixed-Gaussian process and measurement noise are given as follows:
q 1 k 0.9 N ( 0 , 0.01 ) + 0.1 N ( 0 , 0.1 ) q 2 k 0.9 N ( 0 , 0.01 ) + 0.1 N ( 0 , 0.1 ) q 3 k 0.9 N ( 0 , 0.01 ) + 0.1 N ( 0 , 0.1 ) q 4 k 0.9 N ( 0 , 0.01 ) + 0.1 N ( 0 , 0.1 ) r 1 k 0.6 N ( 0 , 4 ) + 0.4 N ( 0 , 20 ) r 2 k 0.6 N ( 0 , 4 ) + 0.4 N ( 0 , 20 )
In this experiment, the initial values of two constants with respect to the MSHARKF and proposed SR-SHARKF algorithms are also chosen as c 0 = 1.2 , c 1 = 4.5 . The parameters of the initial true state are assumed to be X 0 = [ 0 , 0 , 0 . 001 , 0 . 00001 ] T , Y 0 = [ 0 , 0 , 0 . 005 , 0 . 0005 ] T . The RMSE is employed as the comparison criterion, but the true value x k i and the estimated value x ^ k i at the kth moment of the ith MC run are used instead of x k and x ^ k , and we replace T by the number of MC runs M c . The averaged root-mean-squared error (ARMSE) of position, velocity acceleration, and jerk is also used as the evaluation criterion to test the filter performance, which is defined as
ARMSE = 1 T M c k = 1 T i = 1 M c ( x k i x ^ k i ) 2
To evaluate the dispersion of position, velocity, acceleration, and jerk for all algorithms over 1000 Monte Carlo runs, the standard deviation (STD) and the averaged STD (ASTD) as the evaluation criteria are defined as follows
STD = 1 M c i = 1 M c ( x k i x ¯ k i ) ASTD = 1 T M c k = 1 T i = 1 M c ( x k i x ¯ k i ) 2
where x ¯ k i represents the mean of the algorithm estimates for position, velocity, acceleration, or jerk.
Figure 9a–d and Figure 10a–d show that the RMSE and STD of position, velocity, acceleration, and jerk on the X-axis are calculated as the average and standard deviation of each state over 1000 Monte Carlo runs. It can be seen from Figure 9a–d that the RMSEs of SHAKF and MSHARKF have different degrees of divergence in terms of position, velocity, acceleration, or jerk, which is caused by their inability to estimate the noise covariance accurately in 1000 Monte Carlo simulations, while the proposed SR-SHARKF can effectively prevent the divergence and the RMSE remains relatively stable and lower, indicating that the performance of SR-SHARKF outperforms KF, SHAKF, and MSHARKF. As shown in Figure 10a–d and the ASTD of Table 5, we can see that the dispersion of the estimates of position, velocity, and acceleration for all algorithms increases with time since the changes in position, velocity, and acceleration become larger after the amount that can be changed becomes larger. The dispersions estimated by all algorithms are similar.
The ARMSE values were evaluated for all algorithms in both directions and are given in Table 5. We can see from these tables that the SR-SHARKF algorithm gives more competitive results than the KF, SHAKF, and MSHARKF algorithms.
In the Y-direction, we may obtain similar results as in the X-direction by Figure 11a–d and Figure 12a–d and Table 6. It is further verified that the proposed algorithm has better performance compared to other existing algorithms.

4.2. Experiments and Discussion

To further verify the superiority of the proposed SR-SHARKF method, the experimental data were collected by the LanXin USV in the waters along the Dalian Xinghai Cross-Sea Bridge. The experimental flow chart is shown in Figure 13. The USV’s velocity was 3.3657 m/s, the heading angle was 138.71°, and the target information sensed by the radar was within the range of 1852 m. An experimental target ship was installed with integrated navigation to save its position and velocity information as the assumed true position and velocity of the target. The assumed accuracy of position and velocity were, respectively, ± 15 m and ± 5 m / s . The SR-SHARKF algorithm proposed in this paper was compared with the tracking filtering effect of other existing algorithms.
Figure 14a shows the true, radar observation trajectories of the target and the trajectory estimated by the KF, SHAKF, MSHARKF, and SR-SHARKF algorithms. It can be seen from Figure 14a that the combined filtering effect of the SR-SHARKF method in the X-axis and Y-axis directions has a better smoothing impact on the measured data, which makes it closer to the true value. Figure 14b,c show the velocity filtering effects of these algorithms in the X-axis and Y-axis directions, respectively. It can be seen from them that the SR-SHARKF algorithm can obtain a stable filtering effect on the whole. Compared with other algorithms, no matter whether in terms of trajectory or velocity, the SR-SHARKF algorithm proposed in this paper has a better filtering effect than other algorithms.
Figure 15a–d show the error curves of position and velocity on the X-axis and the Y-axis by the KF, SHAKF, MSHARKF, and SR-SHARKF algorithms. It can be seen from Figure 15 that the majority of the states of the error curves of the SR-SHARKF algorithm are smaller than other algorithms, demonstrating that this algorithm’s tracking filtering effect is superior to other algorithms.
According to Table 7 and Table 8, the RMSE and MAE values corresponding to the proposed SR-SHARKF algorithm of position and velocity on the X-axis and the Y-axis are given. This demonstrates that the SR-SHARKF algorithm’s error for actual ship experimental data can also obtain a smaller value compared with other algorithms.

5. Conclusions

In this study, the SR-SHARKF algorithm was developed based on adaptive theories, which can not only solve the problem of filtering divergence triggered by numerical rounding errors, inaccurate system mathematics, and noise statistical models, but also improve the filtering accuracy. First, a novel square-root decomposition method was proposed to decompose the covariance matrix of SHAKF to ensure its non-negative definiteness. Second, an adaptive scale factor was introduced into the SHAKF algorithm and updated at each iteration. Third, the noise covariance adjustment method was applied by combining noise-biased estimation, unbiased estimation, and a fixed observation noise interval. Simulation and experimental results demonstrate that the proposed algorithm has stronger stability and robustness than the KF, SHAKF, and MSHARKF algorithms. In practical application, the proposed algorithm may be widely used in collision risk evaluation and collision avoidance, etc. In our future work, the algorithm proposed in this paper will be applied to multi-target tracking scenarios, and considering the motion of multiple targets at the same time is critical to avoiding ship collisions.

Author Contributions

The work presented here was performed in collaboration among all authors. S.Q. designed, analyzed, and wrote the paper. Y.F. guided the full text. G.W. conceived the idea. D.M. analyzed the data. Z.H. processed the data. All authors have read and agreed to the published version of the manuscript.

Funding

This work was supported in part by the National Natural Science Foundation of China under Grants 61976033 and 51609033, the Key Development Guidance Program of Liaoning Province of China under Grant 2019JH8/10100100, the Soft Science Research Program of Dalian City of China under Grant 2019J11CY014, and the Fundamental Research Funds for the Central Universities under Grants 3132021106 and 3132020110.

Institutional Review Board Statement

Not applicable.

Informed Consent Statement

Not applicable.

Data Availability Statement

Not applicable.

Conflicts of Interest

The authors declare no conflict of interest.

Abbreviations

The following abbreviations are used in this manuscript:
USVUnmanned surface vehicle
SHAKFSage–Husa adaptive Kalman filter
SR-SHARKFSquare root Sage–Husa adaptive robust Kalman filter
KFKalman filter
MSHARKFModified Sage–Husa adaptive robust KF
RMSERoot-mean-square error
MAEMean absolute error
SRDSquare root decomposition
TSThree segment
ARMSEAveraged root-mean-squared error
STDStandard deviation
ASTDAveraged standard deviation

References

  1. Han, J.; Cho, Y.; Kim, J.; Kim, J.; Son, N.S.; Kim, S.Y. Autonomous collision detection and avoidance for ARAGON USV: Development and field tests. J. Field Robot. 2020, 37, 987–1002. [Google Scholar] [CrossRef]
  2. Choi, W.Y.; Kang, C.M.; Lee, S.H.; Chung, C.C. Radar accuracy modeling and its application to object vehicle tracking. Int. J. Control Autom. Syst. 2020, 18, 3146–3158. [Google Scholar] [CrossRef]
  3. Li, C.; Sun, Z.; Yeo, T.S. STGRFT for detection of maneuvering weak target with multiple motion models. IEEE Trans. Signal Process. 2019, 67, 3676–3688. [Google Scholar] [CrossRef]
  4. Wei, S.; Zhang, L.; Liu, H.; Wang, K. Signal-domain Kalman filtering: An approach for maneuvering target surveillance with wideband radar. Signal Process. 2020, 177, 107724. [Google Scholar] [CrossRef]
  5. Raboaca, M.S.; Dumitrescu, C.; Manta, I. Aircraft trajectory tracking using radar equipment with fuzzy logic algorithm. Mathematics 2020, 8, 207. [Google Scholar] [CrossRef] [Green Version]
  6. Zhou, Y.; Wang, T.; Hu, R. Multiple kernelized correlation filters (MKCF) for extended object tracking using x-band marine radar datas. IEEE Trans. Signal Process. 2019, 67, 3676–3688. [Google Scholar] [CrossRef]
  7. Hashmi, A.J.; Eftekhar, A.; Adibi, A.; Amoozegar, F. A Kalman filter based synchronization scheme for telescope array receivers in deep-space optical communication links. Opt. Commun. 2012, 285, 5037–5043. [Google Scholar] [CrossRef]
  8. Nadzilah, A.; Gandana, D.M.; Muliadi, J.; Daryanto, Y. Application of Kalman filter to track ship maneuver. In Proceedings of the 5th Cyber and IT Service Management (CITSM), Denpasar, Indonesia, 8–10 August 2017. [Google Scholar]
  9. Zhang, A.; Shuida, B.A.O.; Fei, G.A.O.; Wenhao, B.I. A novel strong tracking cubature Kalman filter and its application in maneuvering target tracking. Chin. J. Aeronaut. 2019, 32, 2489–2502. [Google Scholar] [CrossRef]
  10. Li, L.Q.; Sun, Y.C.; Liu, Z.X. Maximum fuzzy correntropy Kalman filter and its application to bearings-only maneuvering target tracking. Int. J. Fuzzy Syst. 2021, 23, 405–418. [Google Scholar] [CrossRef]
  11. Lu, C.; Zhang, Y.; Ge, Q. Kalman filter based on multiple scaled multivariate skew normal variance mean mixture distributions with application to target tracking. IEEE Trans. Circuits Syst. II-Express Briefs 2021, 68, 802–806. [Google Scholar] [CrossRef]
  12. Feng, S.; Hu, K.; Fan, E.; Zhao, L.; Wu, C. Kalman filter for spatial-temporal regularized correlation filters. IEEE Trans. Image Process. 2021, 30, 3263–3278. [Google Scholar] [CrossRef]
  13. Liu, X.; Ren, Z.; Lyu, H.; Jiang, Z.; Ren, P.; Chen, B. Linear and nonlinear regression-based maximum correntropy extended Kalman filtering. IEEE Trans. Syst. Man Cybern.-Syst. 2021, 51, 3093–3102. [Google Scholar] [CrossRef]
  14. Zhou, Z.; Zhou, G.; Wang, Y.; Du, H.; Hu, J.S.; Yin, C. PTV longitudinal-lateral state estimation considering unknown control inputs and uncertain model parameters. IEEE Trans. Veh. Technol. 2021, 70, 4366–4376. [Google Scholar] [CrossRef]
  15. He, Z.; Li, Y.; Sun, Y.; Zhao, S.; Lin, C.; Pan, C.; Wang, L. State-of-charge estimation of lithium ion batteries based on adaptive iterative extended Kalman filter. J. Energy Storage 2021, 39, 102593. [Google Scholar] [CrossRef]
  16. Cai, Q.; Yang, C.; Li, J.; Yang, G.; Song, N.; Yin, H.; Liu, Y. An eL-SHAKF-based integration scheme for gyro thermal-magnetic coupling heading-effect drift compensation in INS. IEEE Sens. J. 2020, 20, 11508–11517. [Google Scholar] [CrossRef]
  17. Guo, H.; Guo, J.; Yu, M.; Hong, H.; Xiong, J.; Tian, B. A weighted combination filter with nonholonomic constrains for integrated navigation systems. Adv. Space Res. 2015, 55, 1470–1476. [Google Scholar] [CrossRef]
  18. Jiang, B.; Sheng, W.; Zhang, R.; Han, Y.; Ma, X. Range tracking method based on adaptive current statistical model with velocity prediction. Signal Process. 2017, 131, 261–270. [Google Scholar] [CrossRef]
  19. Hou, X.; Zhou, J.; Yang, Y.; Yang, L.; Qiao, G. Adaptive two-Step bearing-only underwater uncooperative target tracking with uncertain underwater disturbances. Entropy 2021, 23, 907. [Google Scholar] [CrossRef]
  20. Wan, J.; Ren, P.; Guo, Q. Application of interactive multiple model adaptive five-degree cubature Kalman algorithm based on fuzzy logic in target tracking. Symmetry 2019, 11, 767. [Google Scholar] [CrossRef] [Green Version]
  21. Yan, W.; Ding, Q.; Chen, J.; Liu, Y.; Cheng, S.S. Needle tip tracking in 2D ultrasound based on improved compressive tracking and adaptive Kalman filter. IEEE Robot. Autom. Lett. 2021, 6, 3224–3231. [Google Scholar] [CrossRef]
  22. Luo, Z.; Fu, Z.; Xu, Q. An adaptive multi-dimensional vehicle driving state observer based on modified Sage-Husa UKF algorithm. Sensors 2020, 20, 6889. [Google Scholar] [CrossRef]
  23. Narasimhappa, M.; Mahindrakar, A.D.; Guizilini, V.C.; Terra, M.H.; Sabat, S.L. MEMS-based IMU drift minimization: Sage Husa adaptive robust Kalman filtering. IEEE Sens. J. 2020, 20, 250–260. [Google Scholar] [CrossRef]
  24. Wang, Y.; Liu, J.; Wang, J.; Zeng, Q.; Shen, X.; Zhang, Y. Micro aerial vehicle navigation with visual-inertial integration aided by structured light. J. Navig. 2020, 73, 16–36. [Google Scholar] [CrossRef]
  25. Jiang, C.; Wang, S.; Wu, B.; Fernandez, C.; Xiong, X.; Coffie-Ken, J. A state-of-charge estimation method of the power lithium-ion battery in complex conditions based on adaptive square root extended Kalman filter. Energy 2021, 219, 119603. [Google Scholar] [CrossRef]
  26. Nan, D.; Wang, W.; Wang, K.; Mahfoud, R.J.; Haes Alhelou, H.; Siano, P. Dynamic State Estimation for Synchronous Machines Based on Adaptive Ensemble Square Root Kalman Filter. Appl. Sci. 2019, 9, 5200. [Google Scholar] [CrossRef] [Green Version]
  27. Yang, Y.; Gao, W. An optimal adaptive Kalman filter. J. Geod. 2006, 80, 177–183. [Google Scholar] [CrossRef]
  28. Yang, Y.; Xu, T. An adaptive Kalman filter based on Sage windowing weights and variance components. J. Navig. 2003, 56, 231–240. [Google Scholar] [CrossRef]
  29. Wang, T.; Huang, S.; Gao, M.; Wang, Z. Adaptive extended Kalman filter based dynamic equivalent method of PMSG wind farm cluster. IEEE Trans. Ind. Appl. 2021, 57, 2908–2917. [Google Scholar] [CrossRef]
  30. Huang, T.; Jiang, H.; Zou, Z.; Ye, L.; Song, K. An integrated adaptive Kalman filter for high-speed UAVs. Appl. Sci. 2019, 9, 1916. [Google Scholar] [CrossRef] [Green Version]
Figure 1. Coordinate systems for target parameterization with radar sensor measurements.
Figure 1. Coordinate systems for target parameterization with radar sensor measurements.
Sensors 22 02924 g001
Figure 2. Flow chart of the SR-SHARKF algorithm.
Figure 2. Flow chart of the SR-SHARKF algorithm.
Sensors 22 02924 g002
Figure 3. Filtering effect in terms of position and velocity. (a) Target trajectory. (b) Target velocity in the X-axis direction. (c) Target velocity in the Y-axis direction.
Figure 3. Filtering effect in terms of position and velocity. (a) Target trajectory. (b) Target velocity in the X-axis direction. (c) Target velocity in the Y-axis direction.
Sensors 22 02924 g003
Figure 4. Dynamic parameter error in the X-axis direction. (a) Position error. (b) Velocity error. (c) Acceleration error. (d) Jerk error.
Figure 4. Dynamic parameter error in the X-axis direction. (a) Position error. (b) Velocity error. (c) Acceleration error. (d) Jerk error.
Sensors 22 02924 g004
Figure 5. Dynamic parameter error in the Y-axis direction. (a) Position error. (b) Velocity error. (c) Acceleration error. (d) Jerk error.
Figure 5. Dynamic parameter error in the Y-axis direction. (a) Position error. (b) Velocity error. (c) Acceleration error. (d) Jerk error.
Sensors 22 02924 g005
Figure 6. Filtering effect in terms of position and velocity. (a) Target trajectory. (b) Target velocity in the X-axis direction. (c) Target velocity in the Y-axis direction.
Figure 6. Filtering effect in terms of position and velocity. (a) Target trajectory. (b) Target velocity in the X-axis direction. (c) Target velocity in the Y-axis direction.
Sensors 22 02924 g006
Figure 7. Dynamic parameter error in the X-axis direction. (a) Position error. (b) Velocity error. (c) Acceleration error. (d) Jerk error.
Figure 7. Dynamic parameter error in the X-axis direction. (a) Position error. (b) Velocity error. (c) Acceleration error. (d) Jerk error.
Sensors 22 02924 g007
Figure 8. Dynamic parameter error in the Y-axis direction. (a) Position error. (b) Velocity error. (c) Acceleration error. (d) Jerk error.
Figure 8. Dynamic parameter error in the Y-axis direction. (a) Position error. (b) Velocity error. (c) Acceleration error. (d) Jerk error.
Sensors 22 02924 g008
Figure 9. Dynamic parameter RMSE in the X-axis direction. (a) Position RMSE. (b) Velocity RMSE. (c) Acceleration RMSE. (d) Jerk RMSE.
Figure 9. Dynamic parameter RMSE in the X-axis direction. (a) Position RMSE. (b) Velocity RMSE. (c) Acceleration RMSE. (d) Jerk RMSE.
Sensors 22 02924 g009
Figure 10. Dynamic parameter STD in the X-axis direction. (a) Position STD. (b) Velocity STD. (c) Acceleration STD. (d) Jerk STD.
Figure 10. Dynamic parameter STD in the X-axis direction. (a) Position STD. (b) Velocity STD. (c) Acceleration STD. (d) Jerk STD.
Sensors 22 02924 g010
Figure 11. Dynamic parameter RMSE in the Y-axis direction. (a) Position RMSE. (b) Velocity RMSE. (c) Acceleration RMSE. (d) Jerk RMSE.
Figure 11. Dynamic parameter RMSE in the Y-axis direction. (a) Position RMSE. (b) Velocity RMSE. (c) Acceleration RMSE. (d) Jerk RMSE.
Sensors 22 02924 g011
Figure 12. Dynamic parameter STD in the Y-axis direction. (a) Position STD. (b) Velocity STD. (c) Acceleration STD. (d) Jerk STD.
Figure 12. Dynamic parameter STD in the Y-axis direction. (a) Position STD. (b) Velocity STD. (c) Acceleration STD. (d) Jerk STD.
Sensors 22 02924 g012
Figure 13. Lanxin USV experiment flow chart.
Figure 13. Lanxin USV experiment flow chart.
Sensors 22 02924 g013
Figure 14. Filtering effect comparison of multiple algorithms in terms of position and velocity. (a) Target trajectory comparison. (b) Target velocity comparison in the X-axis direction. (c) Target velocity comparison in the Y-axis direction.
Figure 14. Filtering effect comparison of multiple algorithms in terms of position and velocity. (a) Target trajectory comparison. (b) Target velocity comparison in the X-axis direction. (c) Target velocity comparison in the Y-axis direction.
Sensors 22 02924 g014
Figure 15. Dynamic parameter error. (a) Position error in the X-axis direction. (b) Velocity error in the X-axis direction. (c) Position error in the Y-axis direction. (d) Velocity error in the Y-axis direction.
Figure 15. Dynamic parameter error. (a) Position error in the X-axis direction. (b) Velocity error in the X-axis direction. (c) Position error in the Y-axis direction. (d) Velocity error in the Y-axis direction.
Sensors 22 02924 g015
Table 1. Dynamic parameter error evaluation in the X-axis direction.
Table 1. Dynamic parameter error evaluation in the X-axis direction.
State
Variables
KFSHAKFMSHARKFSRDTSNCASR-SHARKF
RMSEMAERMSEMAERMSEMAERMSEMAERMSEMAERMSEMAERMSEMAE
Pos-X0.82740.67161.10950.91250.97620.76240.94120.70390.88360.71910.78450.63010.78060.6246
Vel-X0.44480.34710.50500.38010.52510.41460.40580.29420.49560.40340.38580.30610.35430.2659
Vcc-X0.13990.10970.15100.11840.19920.15120.12360.09510.18160.14940.11550.09220.10910.0849
Jer-X0.02360.01990.02180.01860.03630.02810.02130.01690.02890.02510.01970.01640.01930.0159
Table 2. Dynamic parameter error evaluation in the Y-axis direction.
Table 2. Dynamic parameter error evaluation in the Y-axis direction.
State
Variables
KFSHAKFMSHARKFSRDTSNCASR-SHARKF
RMSEMAERMSEMAERMSEMAERMSEMAERMSEMAERMSEMAERMSEMAE
Pos-Y0.90570.74440.88040.64690.80480.61500.76910.61110.75070.60380.83660.64080.72370.5737
Vel-Y0.41210.34130.41740.32790.40790.32100.33300.27000.40890.31240.41280.33060.39640.3013
Vcc-Y0.14080.11610.14330.10800.14720.11770.11190.09100.13030.09750.14490.12160.12890.1022
Jer-Y0.03220.02660.03110.02500.03410.02590.02850.02420.02830.02150.03200.02720.03040.0250
Table 3. Dynamic parameter error evaluation in the X-Axis direction.
Table 3. Dynamic parameter error evaluation in the X-Axis direction.
State VariablesKFSHAKFMSHARKFSR-SHARKF
RMSEMAERMSEMAERMSEMAERMSEMAE
Pos-X0.62800.47850.68470.51430.79370.66570.57860.4448
Vel-X0.36940.29940.34520.26860.30660.24130.29470.2446
Vcc-X0.16110.13180.12000.09230.15490.12560.12540.0981
Jer-X0.03970.03430.03140.02570.04100.03450.03210.0270
Table 4. Dynamic parameter error evaluation in the Y-axis direction.
Table 4. Dynamic parameter error evaluation in the Y-axis direction.
State VariablesKFSHAKFMSHARKFSR-SHARKF
RMSEMAERMSEMAERMSEMAERMSEMAE
Pos-Y0.70020.55090.67490.53770.76180.64320.65670.5410
Vel-Y0.49120.39120.47850.40570.49520.39000.44380.3562
Vcc-Y0.18630.15620.23600.18350.29730.24220.15970.1286
Jer-Y0.04200.03520.03240.02720.04290.03820.03310.0287
Table 5. Dynamic parameter ARMSE and ASTD evaluation in the X-Axis direction.
Table 5. Dynamic parameter ARMSE and ASTD evaluation in the X-Axis direction.
State VariablesKFSHAKFMSHARKFSR-SHARKF
ARMSEASTDARMSEASTDARMSEASTDARMSEASTD
Pos-X9.361517.59214.08017.6358.654317.6388.023517.641
Vel-X2.49251.13663.50651.13802.78881.13622.28811.1394
Vcc-X0.58280.05260.62390.04990.67950.05250.50250.0515
Jer-X0.07970.00240.07360.00140.08760.00240.07040.0018
Table 6. Dynamic parameter ARMSE and ASTD evaluation in the Y-axis direction.
Table 6. Dynamic parameter ARMSE and ASTD evaluation in the Y-axis direction.
State VariablesKFSHAKFMSHARKFSR-SHARKF
ARMSEASTDARMSEASTDARMSEASTDARMSEASTD
Pos-Y9.376290.52512.80790.2608.845290.3908.082190.366
Vel-Y2.48903.83973.47203.82022.81473.84032.24553.8377
Vcc-Y0.58200.12020.62390.11560.69030.11200.48780.1179
Jer-Y0.07950.00260.07420.00180.09010.00260.06940.0019
Table 7. Dynamic parameter error evaluation in the X-axis direction.
Table 7. Dynamic parameter error evaluation in the X-axis direction.
State VariablesKFSHAKFMSHARKFSR-SHARKF
RMSEMAERMSEMAERMSEMAERMSEMAE
Pos-X9.14317.36198.8597.12649.21736.97317.49725.8540
Vel-X1.33251.04821.02610.85321.05040.80560.48040.3839
Table 8. Dynamic parameter error evaluation in the Y-axis direction.
Table 8. Dynamic parameter error evaluation in the Y-axis direction.
State VariablesKFSHAKFMSHARKFSR-SHARKF
RMSEMAERMSEMAERMSEMAERMSEMAE
Pos-Y8.88647.04128.56596.79269.32077.30586.99255.3837
Vel-Y0.94180.68980.81130.64950.82830.62830.73110.5845
Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

Share and Cite

MDPI and ACS Style

Qiao, S.; Fan, Y.; Wang, G.; Mu, D.; He, Z. Radar Target Tracking for Unmanned Surface Vehicle Based on Square Root Sage–Husa Adaptive Robust Kalman Filter. Sensors 2022, 22, 2924. https://doi.org/10.3390/s22082924

AMA Style

Qiao S, Fan Y, Wang G, Mu D, He Z. Radar Target Tracking for Unmanned Surface Vehicle Based on Square Root Sage–Husa Adaptive Robust Kalman Filter. Sensors. 2022; 22(8):2924. https://doi.org/10.3390/s22082924

Chicago/Turabian Style

Qiao, Shuanghu, Yunsheng Fan, Guofeng Wang, Dongdong Mu, and Zhiping He. 2022. "Radar Target Tracking for Unmanned Surface Vehicle Based on Square Root Sage–Husa Adaptive Robust Kalman Filter" Sensors 22, no. 8: 2924. https://doi.org/10.3390/s22082924

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Metrics

Back to TopTop