INS/GNSS Tightly-Coupled Integration Using Quaternion-Based AUPF for USV

Abstract

This paper addresses the problem of integration of Inertial Navigation System (INS) and Global Navigation Satellite System (GNSS) for the purpose of developing a low-cost, robust and highly accurate navigation system for unmanned surface vehicles (USVs). A tightly-coupled integration approach is one of the most promising architectures to fuse the GNSS data with INS measurements. However, the resulting system and measurement models turn out to be nonlinear, and the sensor stochastic measurement errors are non-Gaussian and distributed in a practical system. Particle filter (PF), one of the most theoretical attractive non-linear/non-Gaussian estimation methods, is becoming more and more attractive in navigation applications. However, the large computation burden limits its practical usage. For the purpose of reducing the computational burden without degrading the system estimation accuracy, a quaternion-based adaptive unscented particle filter (AUPF), which combines the adaptive unscented Kalman filter (AUKF) with PF, has been proposed in this paper. The unscented Kalman filter (UKF) is used in the algorithm to improve the proposal distribution and generate a posterior estimates, which specify the PF importance density function for generating particles more intelligently. In addition, the computational complexity of the filter is reduced with the avoidance of the re-sampling step. Furthermore, a residual-based covariance matching technique is used to adapt the measurement error covariance. A trajectory simulator based on a dynamic model of USV is used to test the proposed algorithm. Results show that quaternion-based AUPF can significantly improve the overall navigation accuracy and reliability.

1. Introduction

The development of unmanned surface vehicles (USVs) for scientific, military and commercial purpose in applications such as oil and gas exploration, oceanographic data collection, hydrographic, oceanographic and environmental survey, mine counter measure, surveillance and reconnaissance, anti-submarine warfare and fast inshore attack craft for combat training require the corresponding development of navigation systems [1]. The need of the robustness, accuracy and reliability is a guarantee of long duration, unmanned operations for USVs. The sensor units on board, which serve as environment perceptions, are of critical importance for automation operation. A reliable and accurate navigation system is very important for a USV.

The integration of Global Navigation Satellite System (GNSS) and Inertial Navigation System (INS) is widely used for positioning and attitude determination for vehicles. The development of micro-electromechanical system (MEMS) technology has brought low-cost INS/GNSS integration approaches into practice [2,3]. A growing number of research groups are developing integrated navigation systems utilizing INS and GNSS due to the complementary nature of INS and GNSS principles. The INS/GNSS integration can be classified into loosely-coupled, tightly-coupled and deeply-coupled [4]. For the loosely-coupled manner, independent redundant solutions are available from the GNSS receiver and INS process. However, the disadvantage is that typically four satellites have to be in view to obtain position and velocity solutions from the GNSS receiver. In addition, cascaded filtering problems may occur when one local Kalman filter (KF) is used in GPS data processing and the other is used in integration. These problems can be easily solved in tightly-coupled integration. In tightly-coupled integration, a centralized KF is employed. The pseudorange and delta pseudorange (or Doppler) measurements are used as observations to update the navigation filter. Furthermore, the system does not need a full GNSS solution to assist the INS. This means that the error correlation of inertial measurement unit (IMU) measurements can maintain the update of the estimated solution by the INS even if the number of the tracked satellites is less than four. However, the resulting system observation model turns out to be nonlinear, which should be carefully treated in the design of the integration KF.

The EKF (extended Kalman filter) is known as state-of-the-art for fusion INS and GNSS data in tightly-coupled integration. The linearization should be implemented in both the nonlinear system model and the observation model first in order to apply EKF. This approximation will result in large errors when EKF calculates a posterior mean and covariance, which may lead to suboptimal performance or even divergence of the filter. UKF (unscented Kalman filter) is a recursive MMSE (Minimum Mean Square Error) estimator based on optimal Gaussian approximation Kalman filter framework. Unlike the EKF, the UKF uses the true nonlinear model and approximates the distribution of the state random variables [5]. The state distribution in the UKF is specified using a minimal set of deterministically chosen sample points to capture the posterior mean and covariance accurately to 2nd order for any nonlinearity. It is based on the assumption that it is much easier to approximate a Gaussian distribution than to simulate an arbitrary nonlinear function [6]. Recently, with the development of the computer technology, particle filter (PF) turns out to be more attractive for nonlinear and non-Gaussian applications, and has been successfully used in [7] to recursively update the posterior distribution by sequential importance sampling and resampling. However, the large computational burden impeded the practical use of PF. In addition, the sample impoverishments accompanying the degeneracy of the system performance are primary disadvantages of the basic PF [8]. To overcome these problems, in [3], the strategy of combining the UKF with PF was proposed.

To achieve better performance from the KF framework, the stochastic information provided to the filter must be as accurate as possible. It is therefore necessary to adapt the stochastic model to accommodate for changes in vehicle dynamics and environment conditions. Insufficient or incorrect knowledge about statistics may lead to degradation of the system performance or even divergence of the filter. In [9], a filter innovation sequence based on adaptive Kalman filtering (AKF) was introduced, which showed a major improvement in adjusting process noise error covariance and sensor measurement error covariance adaptively. In this paper, an adaptive unscented particles filter (AUPF) algorithm based on quaternion is proposed using a residual-based covariance matching technique.

In the remainder of this paper, the content is organized as follows. In Section 2, a trajectory simulator with corresponding sensors measurements allocated in a USV is developed. In Section 3, quaternion-based propagation and an observation model are introduced. Furthermore, a quaternion-based GPS/INS integration algorithm using AUPF is proposed. Simulation results are analyzed and compared to illustrate the performance of the proposed AUPF algorithm.

2. Sensor Measurement Simulation

In this part, a USV trajectory simulator is designed. We briefly summarize the different component parts of the USV platform. These include the mathematical dynamic state-space models used for the simulator, the sensors subsystem and the controller design. The core components of such a simulator are schematically depicted in Figure 1, which include a control system, a guidance system and a navigation system (GNC). The GNS system takes the noisy GPS and MEMS-IMU measurements as inputs, and then fuses them from the dynamics model of USV to estimate the optimal vehicle navigation state solutions. There state estimations together with desired trajectories from guidance systems are then adopted by the controller, which generates an optimal control law to drive the thruster of the USV.

2.1. Trajectory Simulator of the USV

As in [10], the marine craft equations of motion in six degrees of freedom can be written in vectorial setting form:

$$\dot{\eta }=J_{\Theta }\left(\eta \right)\nu$$

(1)

$$M\dot{\nu }+C\left(\nu \right)\nu +D\left(\nu \right)\nu +g\left(\eta \right)={\tau }_{wave}+{\tau }_{wind}+{\tau }_{currents}+\tau$$

(2)

where $\eta \in {\text{ℝ}}^6$ denotes the position and the orientation vector. $\nu \in {\text{ℝ}}^6$ denotes the linear and angular velocity vectors that are decomposed in the body-fixed reference frame. $\tau \in {\text{ℝ}}^6$ describes the forces and moments acting on the craft in the body-fixed fame. The generalized position, velocity and force vectors have the form that represented as Equation (3):

$$\begin{array}{ccc}\eta =\left[\begin{array}{c}{p}_{b/n}^n\\ {\Theta }_{nb}\end{array}\right],& v=\left[\begin{array}{c}{\upsilon }_{b/n}^b\\ {\omega }_{b/n}^b\end{array}\right],& \tau =\left[\begin{array}{c}{f}_b\\ m_b\end{array}\right]\end{array}$$

(3)

The same notation as in [10] is used, $p_{b/n}^n\in {\text{ℝ}}^3$ is the position expressed in the north east down (NED) frame, {N}. ${\Theta }_{nb}\in S^3$ represents the Euler angles. ${\upsilon }_{b/n}^b$ and ${\omega }_{b/n}^b$ represent the linear and angular velocity of body frame expressed in{B} frame with respect to {N} frame. $f_b\in {\text{ℝ}}^3$ and $m_b\in {\text{ℝ}}^3$ are the forces and moments acting on the vehicle, respectively. ${\text{ℝ}}^3$ and $S^3$ denote the Euclidean space of dimension three and the sphere, respectively. $J_{\Theta }\left(\eta \right)$ is the transformation matrix. $M$, $C\left(v\right)$, and $D\left(v\right)$ represent the inertial, Coriolis-Centripetal and damping matrices, respectively. $g\left(\eta \right)$ represents the restoring forces and moments.

A sliding mode trajectory tracking controller is designed to track the reference trajectory for USV as in [11]. All relevant position, orientation, linear and angular velocities, acceleration and forces describing the USV’s trajectory are calculated.

2.2. GPS Date Simulation

There are three kinds of measurements from GPS receiver, i.e., pesudorange, Doppler and carrier phase. In this subsection, we will introduce the measurement mode of pesudorange and Doppler. The first step is to develop the GPS constellation model, which can be used to generate the position of the satellites in the simulation. GPS archive data are available from the website of the International GNSS service (IGS). The next step is to model the signal transmission from the satellites. The main GPS measurement errors include ionospheric errors, tropospheric errors, etc. Further details can be found in [12]. Considering all of these aspects, the biased pseudorange measurement from one satellite vehicle at one instance is formulated as:

$$\tilde{\rho }=\rho +c\left(t_u-t_s\right)+T_{iono}+T_{tropo}+{\epsilon }_{\rho }$$

(4)

where $\tilde{\rho }$ denotes a measured range of user to satellite, $\rho$ is a true range of user to satellite, $t_u$ and $t_s$ denote receiver clock bias and satellite clock bias, respectively, $T_{iono}$ and $T_{tropo}$ denote ionospheric delay and tropospheric delay, respectively, ${\epsilon }_{\rho }$ denotes other un-modeled errors, i.e., multipath delay.

The relative motion between the satellite and the receiver results in change of the observed frequency of satellite signal. The Doppler can be used to estimate the user velocities from the satellite velocities. As in [3], the Doppler shift can be written as a projection of the relative velocity vector on the satellite line-of-sight vector:

$$\tilde{\dot{\rho }}={\left(I_u^s\right)}^T\left(v_s-v_u\right)+c{\dot{t}}_u+{\epsilon }_{\dot{\rho }}$$

(5)

where $I_u^s$ is the use-to-satellite line-of-sight unit vector, $v_s$ and $v_u$ represent the satellite and receiver velocity respectively. $I_u^s$ can be expressed as:

$$I_u^s=\frac{1}{\Vert p_s-p_u\Vert }{\left[x_s-x_u,y_s-y_u,z_s-z_u\right]}^T$$

(6)

where $p_s=\left(x_s,y_s,z_s\right)$ and $p_u=\left(x_u,y_u,z_u\right)$ denote the position vector of satellite and receiver expressed in Earth-Central Earth Fixed (ECEF) coordinate.

2.3. IMU Date Simulation

For the purpose of simulating IMU data, raw measurements of accelerometer and gyroscope are needed. The trajectory simulator data are used as the basis in simulating the sensor data. Moreover, the velocity and angular rate data for the USVs are used.

The IMU measurements are provided by extracting the acceleration and angular velocity from the simulator model of USVs, which can be modeled as:

$$f_{IMU}=R_n^b(\Theta )\left({\dot{v}}_{nb}^n-g_n\right)+f_b^{bias}+{\epsilon }_{acc}^b$$

(7)

$${\omega }_{IMU}={\omega }_{nb}^b+{\omega }_b^{bias}+{\epsilon }_{gyro}^b$$

(8)

where $g_n$ represents gravity expressed in a navigation frame. ${\epsilon }_{acc}^b$ and ${\epsilon }_{gyro}^b$ denote the zero mean Gaussian distribute noise. $f_b^{bias}$ and ${\omega }_b^{bias}$ are bias errors of the specific forces and angular rate measurements, respectively. For the low cost MEMS-IMU, the sensor errors have the non-Gaussian characteristics. The bias errors in the gyroscope and accelerometer measurements in body frame will be transformed to be the position and velocity drifts in the navigation frame.

3. Quaternion-Based INS/GNSS Integration

3.1. Quaternion-Based Propagration and Observation Models

Define the vector form of quaternions as $q={\left[\begin{array}{cc}{q}_0& {\overline{q}}^T\end{array}\right]}^T$ with one real part $q_0$ and three imaginary parts given by the vector $\overline{q}={\left[\begin{array}{ccc}{q}_1& q_2& q_3\end{array}\right]}^T$. Quaternions are represented as a complex number with four bases and are used to compute the rotation from navigation frame to body frame. Based on Euler’s theorem, every change in the relative orientation of two rigid bodies or reference frame can be produced by means of a simple rotation from one frame to another along fixed axes. Given the invariant axis (rotation axis) and rotation angle (with magnitude $\Vert \phi \Vert$), the quaternion vector can be represented as:

$$q=\left[\begin{array}{l}\cos \left(0.5\Vert \phi \Vert \right)\\ \sin \left(0.5\Vert \phi \Vert \right)u\end{array}\right], \text{with} u={\left[u_x,u_y,u_z\right]}^T$$

(9)

where $u$ denotes the unit vector along the invariant axis. Apparently, $q$ has the normality property, that is to say $\Vert q\Vert =1$. It can be concluded that the quaternion vector has only three degrees of freedom, although $q$ has four elements. Given a rotation vector $\phi ={\left[{\phi }_1,{\phi }_2,{\phi }_3\right]}^T$, the quaternion vector can be computed as:

$$\Vert \phi \Vert =\sqrt{{\phi }_1^2+{\phi }_2^2+{\phi }_3^2},u=\frac{\phi }{\Vert \phi \Vert },q=\left[\begin{array}{l}\cos \left(0.5\Vert \phi \Vert \right)\\ \sin \left(0.5\Vert \phi \Vert \right)\frac{\phi }{\Vert \phi \Vert }\end{array}\right]$$

(10)

Conventionally, with a sufficiently small time interval, the quaternion vector is updated using vectors added together in discrete time domain. However, it should be noticed that the unit sphere defined by quaternion is not a Euclidean vector space. That is to say, the common definition of addition and scaling cannot be applied directly. In the AUPF algorithm, we will apply the quaternion product rule to update the quaternion vector. Thus, the system propagation model in discrete time domain can be expressed as:

$$\begin{array}{l}{p}_{n,k+1}=p_{n,k}+v_{n,k}\cdot T\\ v_{n,k+1}=v_{n,k}+\left[R_b^n\left(q_k\right)f_{ib,k}^b+g_n\right]\cdot T\\ q_{k+1}=q_k\otimes \left(\Delta q_k\right)\\ f_{b,k+1}^{bias}=f_{b,k}^{bias}+w_f\\ {\omega }_{b,k+1}^{bias}={\omega }_{b,k}^{bias}+w_{\omega }\\ c\Delta t_{k+1}=c\Delta t_k+c\Delta {\dot{t}}_k\cdot T+w_{cbk}\\ c\Delta {\dot{t}}_{k+1}=c\Delta {\dot{t}}_k+w_{cdk}\end{array}$$

(11)

where $\otimes$ denotes the quaternion product. $p_{n,k}$ and $v_{n,k}$ denote the position and velocity in the navigation frame at epoch k. $g_n$ represents gravity expressed in navigation fame, which is assumed to be constant for local navigation. $c\Delta {\dot{t}}_k$ denotes the receiver clock drift error, which is modeled as a random walk process. $c\Delta t_k$ represents the range equivalent of the receiver clock bias, which is the integration of clock drift error. $T$ is the system propagation time interval. $w_f$, $w_{\omega }$, $w_{cbk}$ and $w_{cdk}$ are Gaussian nose terms. $R_b^n\left(q\right)$ denotes the rotational transformation matrix from the body frame to the navigation frame, which can be formulated using the parameters of quaternion as:

$$R_b^n=\left[\begin{array}{ccc}1-2\left(q_2^2+q_3^2\right)& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_1{q}_3+q_0{q}_2\right)\\ 2\left(q_1{q}_2+q_0{q}_3\right)& 1-2\left(q_1^2+q_3^2\right)& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_2{q}_3+q_0{q}_1\right)& 1-2\left(q_1^2+q_2^2\right)\end{array}\right]$$

(12)

$\Delta q_k$ represents the quaternion rotation of body frame during the time interval:

$$\Delta q_k=\left[\begin{array}{c}\cos \left(0.5\Vert {\theta }_k\Vert \right)\\ \sin \left(0.5\Vert {\theta }_k\Vert \right)\frac{{\theta }_k}{\Vert {\theta }_k\Vert }\end{array}\right]$$

(13)

where ${\theta }_k$ denotes the integral of the body frame angular rate measurements.

Equation (11) forms the propagation model of the system. In the tightly-coupled integration, the system observation model is formulated as Equation (14). Where $j$ denotes the number of satellites in view, $I_j$ represents the estimated line-of-sight unit vector pointing from the initial estimate of the user position to the j-th satellite:

$$\left[\begin{array}{c}{\widehat{\rho }}_{1,k}-{\tilde{\rho }}_{1,k}\\ ⋮\\ {\widehat{\rho }}_{j,k}-{\tilde{\rho }}_{j,k}\\ {\widehat{\dot{\rho }}}_{1,k}-{\tilde{\dot{\rho }}}_{1,k}\\ ⋮\\ {\widehat{\dot{\rho }}}_{j,k}-{\tilde{\dot{\rho }}}_{j,k}\end{array}\right]={\left[\begin{array}{ccccc}{\left(-I_{1,k}^T\right)}_{1\times 3}& 0_{1\times 3}& 0_{1\times 9}& 1& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮\\ {\left(-I_{j,k}^T\right)}_{1\times 3}& 0_{1\times 3}& 0_{1\times 9}& 1& 0\\ 0_{1\times 3}& {\left(-I_{1,k}^T\right)}_{1\times 3}& 0_{1\times 9}& 0& 1\\ ⋮& ⋮& ⋮& ⋮& ⋮\\ 0_{1\times 3}& {\left(-I_{j,k}^T\right)}_{1\times 3}& 0_{1\times 9}& 0& 1\end{array}\right]}_{2j\times 17}\cdot \left[\begin{array}{c}\delta p_k^n\\ \delta v_k^n\\ \delta {\Psi }_k\\ \delta f_{ib,k}^{b,error}\\ \delta {\omega }_{ib,k}^{b,error}\\ c\delta t_k\\ c\delta {\dot{t}}_k\end{array}\right]+{\epsilon }_k$$

(14)

where ${\widehat{\rho }}_{j,k}$ is the predicted pseudorange measurement from the j-th satellite, and:

$${\widehat{\rho }}_{j,k}=\sqrt{{\left(x_{j,k}^n-{\widehat{x}}_{u,k}^n\right)}^2+{\left(y_{j,k}^n-{\widehat{y}}_{u,k}^n\right)}^2+{\left(z_{j,k}^n-{\widehat{z}}_{u,k}^n\right)}^2}+c\Delta {\widehat{t}}_k$$

(15)

where ${\widehat{x}}_{u,k}^n$, ${\widehat{y}}_{u,k}^n$, ${\widehat{z}}_{u,k}^n$ are the USV position estimates expressed in NED frame. $x_{j,k}^n$, $y_{j,k}^n$, $z_{j,k}^n$ are the j-th satellite position coordinates expressed in NED frame.

The predicted pseudorange-rate measurements are calculated as:

$${\widehat{\dot{\rho }}}_{j,k}=I_{j,k}^{n,n}\left(x_{j,k}^n-{\widehat{x}}_{u,k}^n\right)+I_{j,k}^{n,e}\left(y_{j,k}^n-{\widehat{y}}_{u,k}^n\right)+I_{j,k}^{n,d}\left(z_{j,k}^n-{\widehat{z}}_{u,k}^n\right)+c\Delta {\widehat{\dot{t}}}_k$$

(16)

where

$$\begin{array}{l}{I}_{j,k}^{n,n}=\left(x_{j,k}^n-{\widehat{x}}_{u,k}^n\right)/{\widehat{d}}_{j,k},I_{j,k}^{n,e}=\left(y_{j,k}^n-{\widehat{y}}_{u,k}^n\right)/{\widehat{d}}_{j,k},I_{j,k}^{n,d}=\left(z_{j,k}^n-{\widehat{z}}_{u,k}^n\right)/{\widehat{d}}_{j,k}\\ {\widehat{d}}_{j,k}=\sqrt{{\left(x_{j,k}^n-{\widehat{x}}_{u,k}^n\right)}^2+{\left(y_{j,k}^n-{\widehat{y}}_{u,k}^n\right)}^2+{\left(z_{j,k}^n-{\widehat{z}}_{u,k}^n\right)}^2}\end{array}$$

where ${\widehat{\dot{\rho }}}_{j,k}$ is the predicted delta range measurement from the j-th satellite. ${\widehat{\dot{x}}}_{u,k}^n$, ${\widehat{\dot{y}}}_{u,k}^n$, ${\widehat{\dot{z}}}_{u,k}^n$ are the USV velocity estimates expressed in NED frame. ${\dot{x}}_{j,k}^n$, ${\dot{y}}_{j,k}^n$, ${\dot{z}}_{j,k}^n$ are the j-th satellite velocity coordinates expressed in NED frame.

Due to introducing the quaternion, there exists dimension mismatch among the state vector and the state error covariance matrix. The reason is that the degree of freedom of a quaternion vector is three rather than four. When the state vector includes quaternion vector elements, the dimension of the state vector is 18 × 1; however, the dimension of the state error covariance matrix is 17 × 17. In order to solve the problem of dimension mismatch, we introduce the rotation vector in the rotation space, which is transformed from the corresponding quaternion vector error.

3.2. Quaternion-Based Integration Using UPF

The idea of UKF comes from the fact that it is much easier to approximate a Gaussian distribution, rather than to simulate a nonlinear function. The particles for UPF are generated using the UKF a posterior estimates. The state vector and its associated errors are defined as:

$${\widehat{x}}_k^{+}={\left[\begin{array}{c}{\widehat{p}}_{n,k}^{+}\\ {\widehat{v}}_{n,k}^{+}\\ {\widehat{q}}_k^{+}\\ {\widehat{f}}_{b,k}^{bias+}\\ {\widehat{\omega }}_{b,k}^{bias+}\\ c\Delta {\widehat{t}}_k^{+}\\ c\Delta {\widehat{\dot{t}}}_k^{+}\end{array}\right]}_{18\times 1},\delta {\widehat{x}}_k^{+}=\left[\begin{array}{c}{\widehat{p}}_{n,k}^{+}-p_{n,k}\\ {\widehat{v}}_{n,k}^{+}-v_{n,k}\\ {\widehat{\phi }}_k^{+}\\ {\widehat{f}}_{b,k}^{bias+}-f_{b,k}^{bias}\\ {\widehat{\omega }}_{b,k}^{bias+}-{\omega }_{b,k}^{bias}\\ c\Delta {\widehat{t}}_k^{+}-c\Delta t_k\\ c\Delta {\widehat{\dot{t}}}_k^{+}-c\Delta {\dot{t}}_k\end{array}\right]={\left[\begin{array}{c}\delta {\widehat{p}}_{n,k}^{+}\\ \delta {\widehat{v}}_{n,k}^{+}\\ {\widehat{\phi }}_k^{+}\\ \delta {\widehat{f}}_{b,k}^{bias+}\\ \delta {\widehat{\omega }}_{b,k}^{bias+}\\ c\delta {\widehat{t}}_k^{+}\\ c\delta {\widehat{\dot{t}}}_k^{+}\end{array}\right]}_{17\times 1}$$

(17)

where ${\widehat{\phi }}_k^{+}$ is the rotation vector corresponding to $q_k^{+}\otimes {\left(q_k\right)}^{-1}$. The state error covariance $P_k^{+}$ can be formulated as:

$$P_k^{+}={\left[\begin{array}{ccccccc}{\sigma }_{\delta \widehat{p}}^2& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {\sigma }_{\delta \widehat{v}}^2& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& {\sigma }_{\delta \widehat{\phi }}^2& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\sigma }_{\delta \widehat{f}}^2& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\sigma }_{\delta \widehat{\omega }}^2& 0_{3\times 3}& 0_{3\times 3}\\ 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& {\sigma }_{c\delta \widehat{t}}^2& 0\\ 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0& {\sigma }_{c\delta \widehat{\dot{t}}}^2\end{array}\right]}_{17\times 17}$$

(18)

The sigma-points are generated according to Equation (19)

$$\begin{array}{l}{\widehat{x}}_{k-1,i}^{+}={\widehat{x}}_{k-1}^{+}+{\left(\sqrt{n{P}_{k-1}^{+}}\right)}_i^T,i=1,\dots ,n\\ {\widehat{x}}_{k-1,i+n}^{+}={\widehat{x}}_{k-1}^{+}-{\left(\sqrt{n{P}_{k-1}^{+}}\right)}_i^T,i=1,\dots ,n\end{array}$$

(19)

where $2n\left(n=17\right)$ equally weighted sigma-points are generated. The calculated sigma-points has the form of:

$${\widehat{x}}_{k-1,i}^{+}=\left[\begin{array}{c}{\widehat{p}}_{n,k-1}^{+}+\Delta {\widehat{p}}_{n,k-1,i}^{+}\\ {\widehat{v}}_{n,k-1}^{+}+\Delta {\widehat{v}}_{n,k-1,i}^{+}\\ \delta q\left({\widehat{\phi }}_{k-1,i}^{+}\right)\otimes {\widehat{q}}_{k-1}^{+}\\ {\widehat{f}}_{b,k-1}^{bias+}+\Delta {\widehat{f}}_{b,k-1,i}^{bias+}\\ {\widehat{\omega }}_{b,k-1}^{bias+}+\Delta {\widehat{\omega }}_{b,k-1,i}^{bias+}\\ c\Delta {\widehat{t}}_{k-1}^{+}+\Delta c\Delta {\widehat{t}}_{k-1,i}^{+}\\ c\Delta {\widehat{\dot{t}}}_{k-1}^{+}+\Delta c\Delta {\widehat{\dot{t}}}_{k-1,i}^{+}\end{array}\right],{\widehat{x}}_{k-1,i+n}^{+}=\left[\begin{array}{c}{\widehat{p}}_{n,k-1}^{+}-\Delta {\widehat{p}}_{n,k-1,i}^{+}\\ {\widehat{v}}_{n,k-1}^{+}-\Delta {\widehat{v}}_{n,k-1,i}^{+}\\ \delta q\left(-{\widehat{\phi }}_{k-1,i}^{+}\right)\otimes {\widehat{q}}_{k-1}^{+}\\ {\widehat{f}}_{b,k-1}^{bias+}-\Delta {\widehat{f}}_{b,k-1,i}^{bias+}\\ {\widehat{\omega }}_{b,k-1}^{bias+}-\Delta {\widehat{\omega }}_{b,k-1,i}^{bias+}\\ c\Delta {\widehat{t}}_{k-1}^{+}-\Delta c\Delta {\widehat{t}}_{k-1,i}^{+}\\ c\Delta {\widehat{\dot{t}}}_{k-1}^{+}-\Delta c\Delta {\widehat{\dot{t}}}_{k-1,i}^{+}\end{array}\right]$$

(20)

Define the delta rotation angle rate in the quaternion form as:

$$\delta q\left({\widehat{\phi }}_{k-1,i}^{+}\right)=\left[\begin{array}{c}\cos \left(0.5\Vert {\widehat{\phi }}_{k-1,i}^{+}\Vert \right)\\ \sin \left(0.5\Vert {\widehat{\phi }}_{k-1,i}^{+}\Vert \right)\frac{{\widehat{\phi }}_{k-1,i}^{+}}{\Vert {\widehat{\phi }}_{k-1,i}^{+}\Vert }\end{array}\right]$$

(21)

The a priori mean and the state error covariance matrix are computed through Equations (22) and (23):

$${\widehat{x}}_k^{-}=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}{\mathrm{f}}_{k-1}\left({\widehat{x}}_{k-1,i}^{+}\right)}$$

(22)

$$P_k^{-}=\frac{1}{2n}{{\displaystyle \sum _{i=1}^{2n}\left[{\mathrm{f}}_{k-1}\left({\widehat{x}}_{k-1,i}^{+}\right)-{\widehat{x}}_k^{-}\right]\left[{\mathrm{f}}_{k-1}\left({\widehat{x}}_{k-1,i}^{+}\right)-{\widehat{x}}_k^{-}\right]}}^T+Q_{k-1}$$

(23)

The same as in Equation (19), we generate the sigma-points for measurements update by replacing $P_{k-1}^{+}$ with $P_k^{-}$. The sigma-points ${\widehat{x}}_{k,i}^{-}$ are calculated according to Equation (20). With the sigma-points propagation, the predicted measurement and covariance matrix are calculated according to Equations (24)–(26):

$${\widehat{y}}_k=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}{\mathrm{h}}_k\left({\widehat{x}}_{k,i}^{-}\right)}$$

(24)

$$P_k^{yy}=\frac{1}{2n}{{\displaystyle \sum _{i=1}^{2n}\left[{\mathrm{h}}_k\left({\widehat{x}}_{k,i}^{-}\right)-{\widehat{y}}_k\right]\left[{\mathrm{h}}_k\left({\widehat{x}}_{k,i}^{-}\right)-{\widehat{y}}_k\right]}}^T+R_k$$

(25)

$$P_k^{xy}=\frac{1}{2n}{{\displaystyle \sum _{i=1}^{2n}\left[{\widehat{x}}_{k,i}^{-}-{\widehat{x}}_k^{-}\right]\left[{\mathrm{h}}_k\left({\widehat{x}}_{k,i}^{-}\right)-{\widehat{y}}_k\right]}}^T$$

(26)

At each measurement epoch of GPS, we update the state estimate and the covariance as KF frame:

$$\begin{array}{l}{K}_k=P_k^{xy}{\left(P_k^{yy}\right)}^{-1}\\ {\widehat{x}}_k^{+}={\widehat{x}}_k^{-}+K_k\left({\tilde{y}}_k-{\widehat{y}}_k\right)\\ P_k^{+}=P_k^{-}-K_k{P}_k^{yy}{K}_k^T\end{array}$$

(27)

For the UPF, the a posterior estimates of ${\widehat{x}}_k^{+}$ and $P_k^{+}$ from Equation (27) are used to form the importance density distribution for generating particles:

$${\chi }_{k,i}^{+}={\widehat{x}}_k^{+}+\Delta {\widehat{x}}_{k,i}^{+}=\left[\begin{array}{c}{\widehat{p}}_{n,k}^{+}+\Delta {\widehat{p}}_{n,k,i}^{+}\\ {\widehat{v}}_{n,k}^{+}+\Delta {\widehat{v}}_{n,k,i}^{+}\\ \delta q\left({\widehat{\phi }}_{k,i}^{+}\right)\otimes {\widehat{q}}_k^{+}\\ {\widehat{f}}_{b,k}^{bias+}+\Delta {\widehat{f}}_{b,k,i}^{bias+}\\ {\widehat{\omega }}_{b,k}^{bias+}+\Delta {\widehat{\omega }}_{b,k,i}^{bias+}\\ c\Delta {\widehat{t}}_k^{+}+\Delta c\Delta {\widehat{t}}_{k,i}^{+}\\ c\Delta {\widehat{\dot{t}}}_k^{+}+\Delta c\Delta {\widehat{\dot{t}}}_{k,i}^{+}\end{array}\right],\Delta {\widehat{x}}_{k,i}^{+}\sim N\left(0,P_k^{+}\right)$$

(28)

where $i=1,\cdots ,N$, $N$ is the number of particles. The normalized importance weights are computed as:

$$w\left({\chi }_{k,i}^{+}\right)=\frac{p\left({\tilde{y}}_k|{\chi }_{k,i}^{+}\right)N\left({\chi }_{k,i}^{+};{\widehat{x}}_k^{-},P_k^{-}\right)}{N\left({\chi }_{k,i}^{+};{\widehat{x}}_k^{+},P_k^{+}\right)}$$

(29)

$$\overline{w}\left({\chi }_{k,i}^{+}\right)=\frac{w\left({\chi }_{k,i}^{+}\right)}{{\displaystyle \sum _{i=1}^{N}w\left({\chi }_{k,i}^{+}\right)}},\text{with} {\displaystyle \sum _{i=1}^N\overline{w}\left({\chi }_{k,i}^{+}\right)}=1$$

(30)

where

$$p\left({\tilde{y}}_k|{\chi }_{k,i}^{+}\right)=\frac{1}{\sqrt{{\left(2\pi \right)}^m\Vert R_k\Vert }}\text{exp}\left\{-\frac{{\left[{\tilde{y}}_k-h_k\left({\chi }_{k,i}^{+}\right)\right]}^T{R}^{-1}\left[{\tilde{y}}_k-h_k\left({\chi }_{k,i}^{+}\right)\right]}{2}\right\}$$

$$N\left({\chi }_{k,i}^{+};{\widehat{x}}_k^{-},P_k^{-}\right)=\frac{1}{\sqrt{{\left(2\pi \right)}^n\Vert P_k^{-}\Vert }}\text{exp}\left\{-\frac{{\left[{\chi }_{k,i}^{+}-{\widehat{x}}_k^{-}\right]}^T{\left(P_k^{-}\right)}^{-1}\left[{\chi }_{k,i}^{+}-{\widehat{x}}_k^{-}\right]}{2}\right\}$$

$$N\left({\chi }_{k,i}^{+};{\widehat{x}}_k^{+},P_k^{+}\right)=\frac{1}{\sqrt{{\left(2\pi \right)}^n\Vert P_k^{+}\Vert }}\text{exp}\left\{-\frac{{\left[{\chi }_{k,i}^{+}-{\widehat{x}}_k^{+}\right]}^T{\left(P_k^{+}\right)}^{-1}\left[{\chi }_{k,i}^{+}-{\widehat{x}}_k^{+}\right]}{2}\right\}$$

where $m$ denotes the observer dimension which varies during the time and is based on the number of satellites being tracked by the receiver.

The a posteriori mean and covariance estimates are computed as:

$$\begin{array}{l}{\widehat{x}}_k^{+}={\displaystyle \sum _{i=1}^p\overline{w}\left({\chi }_{k,i}^{+}\right)}{\chi }_{k,i}^{+}\\ P_k^{+}={\displaystyle \sum _{i=1}^N\overline{w}\left({\chi }_{k,i}^{+}\right)\left[{\chi }_{k,i}^{+}-{\widehat{x}}_k^{+}\right]{\left[{\chi }_{k,i}^{+}-{\widehat{x}}_k^{+}\right]}^T}\end{array}$$

(31)

where ${\chi }_{k,i}^{+}-{\widehat{x}}_k^{+}$ can be calculated as:

$${\left[{\left({\widehat{p}}_{n,k,i}^{+}-{\widehat{p}}_{n,k}^{+}\right)}^T,{\left({\widehat{v}}_{n,k,i}^{+}-{\widehat{v}}_{n,k}^{+}\right)}^T,{\left({\widehat{\phi }}_{k,i}^{+}\right)}^T,{\left({\widehat{f}}_{b,k,i}^{bias+}-{\widehat{f}}_{b,k}^{bias+}\right)}^T,{\left({\widehat{\omega }}_{b,k,i}^{bias+}-{\widehat{\omega }}_{b,k}^{bias+}\right)}^T,{\left(c\Delta {\widehat{t}}_{k,i}^{+}-c\Delta {\widehat{t}}_k^{+}\right)}^T,{\left(c\Delta {\widehat{\dot{t}}}_{k,i}^{+}-c\Delta {\widehat{\dot{t}}}_k^{+}\right)}^T\right]}^T.$$

${\widehat{\phi }}_{k,i}^{+}$ is the rotation vector corresponding to ${\widehat{q}}_{k,i}^{+}\otimes {\left({\widehat{q}}_k^{+}\right)}^{-1}$. Due to the fact that the unit quaternion is not mathematically closed for addition and scalar multiplication, a normalization procedure is necessary to ensure the constraint $q^{T}q=1$ is satisfied in the presence of measurement noise and numerical round-off errors. The normalization can be conducted by replacing ${\widehat{q}}_k^{+}$ with ${\widehat{q}}_k^{+}/\Vert {\widehat{q}}_k^{+}\Vert$.

3.3. Residual-Based AUPF

For the a posterior state estimations, a reliable resolution is proposed which greatly depends on prior statistics of the system process and measurement noise. However, these statistics are hardly known exactly in practice because they are based on the types of applications and process dynamics [13]. In addition, the estimation environment regarding INS/GNSS kinematic application is not always fixed but subject to change [14]. In order to maintain the estimation accuracy, a residual-based covariance matching technique is used to adaptively adjust the algorithm parameters and to track the changes in the noise source. For the INS/GNSS integration, the IMU measurements are used in system model, and the product manual specifies the sensor turn-on bias, temperature related variations in biases and noises. In this paper, we assume that the process noise error covariance $Q$ is approximately known, and we focus on the adaptive estimation of sensor measurement error covariance $\widehat{R}$.

Different from [2], which proposed an adaptive EKF algorithm, for the UKF, modified measurement noise error covariance matrices are adaptively updated as:

$${\widehat{R}}_k={\widehat{C}}_{v_k}+P_k^{yy+}, \text{where} {\widehat{C}}_{v_k}=\frac{1}{N}{\displaystyle \sum _{j=j_0}^k{z}_j{z}_j^T}$$

(32)

where $P_k^{yy+}$ is calculated as:

$$P_k^{yy+}=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}\left[h_k\left({\widehat{x}}_{k,i}^{+}\right)-{\widehat{y}}_k^{+}\right]{\left[h_k\left({\widehat{x}}_{k,i}^{+}\right)-{\widehat{y}}_k^{+}\right]}^T}$$

(33)

where ${\widehat{y}}_k^{+}=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}{h}_k\left({\widehat{x}}_{k,i}^{+}\right)}$. The ${\widehat{x}}_{k,i}^{+}$ denotes the sigma-points drawn from the UKF a posterior estimates. The sigma-points are generated as:

$$\begin{array}{l}{\widehat{x}}_{k,i}^{+}={\widehat{x}}_k^{+}+{\left(\sqrt{n{P}_k^{+}}\right)}_i^T,i=1,\dots ,n\\ {\widehat{x}}_{k,i+n}^{+}={\widehat{x}}_k^{+}-{\left(\sqrt{n{P}_k^{+}}\right)}_i^T,i=1,\dots ,n\end{array}$$

(34)

$${\widehat{x}}_{k,i}^{+}=\left[\begin{array}{c}{\widehat{p}}_{n,k}^{+}+\Delta {\widehat{p}}_{n,k,i}^{+}\\ {\widehat{v}}_{n,k}^{+}+\Delta {\widehat{v}}_{n,k,i}^{+}\\ \delta q\left({\widehat{\phi }}_{k,i}^{+}\right)\otimes {\widehat{q}}_k^{+}\\ {\widehat{f}}_{b,k}^{bias+}+\Delta {\widehat{f}}_{b,k,i}^{bias+}\\ {\widehat{\omega }}_{b,k}^{bias+}+\Delta {\widehat{\omega }}_{b,k,i}^{bias+}\\ c\Delta {\widehat{t}}_k^{+}+\Delta c\Delta {\widehat{t}}_{k,i}^{+}\\ c\Delta {\widehat{\dot{t}}}_k^{+}+\Delta c\Delta {\widehat{\dot{t}}}_{k,i}^{+}\end{array}\right],{\widehat{x}}_{k,i+n}^{+}=\left[\begin{array}{c}{\widehat{p}}_{n,k}^{+}-\Delta {\widehat{p}}_{n,k,i}^{+}\\ {\widehat{v}}_{n,k}^{+}-\Delta {\widehat{v}}_{n,k,i}^{+}\\ \delta q\left(-{\widehat{\phi }}_{k,i}^{+}\right)\otimes {\widehat{q}}_k^{+}\\ {\widehat{f}}_{b,k}^{bias+}-\Delta {\widehat{f}}_{b,k,i}^{bias+}\\ {\widehat{\omega }}_{b,k}^{bias+}-\Delta {\widehat{\omega }}_{b,k,i}^{bias+}\\ c\Delta {\widehat{t}}_k^{+}-\Delta c\Delta {\widehat{t}}_{k,i}^{+}\\ c\Delta {\widehat{\dot{t}}}_k^{+}-\Delta c\Delta {\widehat{\dot{t}}}_{k,i}^{+}\end{array}\right]$$

(35)

The calculated sigma-points have the form of Equation (35). The delta rotation angle rate in the quaternion form can be formulated as:

$$\delta q\left({\widehat{\phi }}_{k,i}^{+}\right)=\left[\begin{array}{c}\cos \left(0.5\Vert {\widehat{\phi }}_{k,i}^{+}\Vert \right)\\ \sin \left(0.5\Vert {\widehat{\phi }}_{k,i}^{+}\Vert \right)\frac{{\widehat{\phi }}_{k,i}^{+}}{\Vert {\widehat{\phi }}_{k,i}^{+}\Vert }\end{array}\right]$$

(36)

The flowchart of the proposed AUPF is illustrated in Figure 2.

4. Simulation Results

A simulator with the module of USV trajectory simulation and the raw measurements simulation for IMU and GPS was developed. The trajectory used in this simulation scenario is illustrated in Figure 3. We assume that the USV is operating at sea level. The sliding mode controller in [15] drives the USV fallowing the designed trajectory. To represent a real MEMS-IMU as closely as possible, different error types including the Gaussian model and random bias were induced in the simulated IMU. Raw GPS pseudorange and Doppler measurements were simulated at a 1 Hz rate, while the IMU raw data were simulated at a 100 Hz rate. The sensor error characteristics of MEMS-IMU are given in

Table 1. Error characteristics of the simulated MEMS-IMU.

Gyroscope (Angular Rates)		Accelerometer (Specific Forces)
Bias in-run Stability	≦±13 [°/h] (1σ)	Bias in-run Stability	≦±1300 [μg] (1σ)
Noise (ARW)	0.028 [°/s/√Hz] (1σ)	Noise (VRW)	70 [μg/√Hz] (1σ)
Scale Factor Error	<1000 [ppm]	Scale Factor Error	<1000 [ppm]

Micro-electromechanical system inertial measurement unit (MEMS-IMU).

. The program is implemented in MATLAB R2013a using Intel Xeon E31270 CPU and 8 GB RAM.

Figure 3 shows the estimation results of trajectory of USV in horizontal plane. Although the low cost MEMS-IMU is used, the agreement with the actual trajectory is generally good. The estimated position and velocity errors can be found in Figure 4. After the initial transient, the estimation error in the north direction remains less than 3 m approximately, 0.5 m in the east direction and −4.2 m in the down direction; the errors of velocity remain within a region of approximately ±0.05 m/s. In Figure 5, the attitudes estimated by the AUPF algorithm (using quaternion) are plotted. The quaternion are converted to their corresponding Euler angles. Except the initial transient, the roll and pitch errors remain mostly in the region ±0.1 deg, but the yaw error tends to be a little larger. Accelerometer bias estimation results are illustrated in Figure 6. Gyroscope bias estimation results are plotted in Figure 7. Due to the fact that the yaw angle is the least observable state in practical vehicle motion, we can see form Figure 7 that the bias error on the z-axis takes more time to be stable.

In order to illustrate the performance of the proposed AUPF algorithm, several groups of tests adopting the AEKF, AUKF, PF and AUPF are conducted, respectively, in this part. For comparing the navigation accuracy among the algorithms mentioned above, we choose the norm of the position ($\Vert \Delta \mathbf{p}\Vert$) and velocity ($\Vert \Delta \mathbf{v}\Vert$) estimation errors for analysis, which are calculated as $\Vert \Delta \mathbf{x}\Vert =\sqrt{\Delta x_N^2+\Delta x_E^2+\Delta x_D^2}$. As shown in Figure 8, AUPF presents best estimation accuracy compared with others.

Furthermore, with the same group of GPS and IMU data, the estimation accuracy and computation time of the algorithms are illustrated in

Table 2. Performance comparison between state-of-the-art algorithms.

Algorithm	Runs	M(‖Δp‖) [m]	V(‖Δp‖) [m]	M(‖Δv‖) [m/s]	V(‖Δv‖) [m/s]	Time [s]
AEKF	1	5.2451	0.0426	0.0327	0.0042	3.3774
AUKF	1	5.1962	0.0397	0.0286	0.0037	17.3859
PF(300)	100	4.5173	0.0315	0.0207	0.0021	283.4482
AUPF(100)	100	4.2358	0.0273	0.0183	0.0014	62.1538

Adaptive extended Kalman filter (AEKF); Adaptive unscented Kalman filter (AUKF); Particle filter (PF); Adaptive unscented particle filter (AUPF).

. M($\Vert \Delta \mathbf{p}\Vert$) and M($\Vert \Delta \mathbf{v}\Vert$) denote the mean of the norm of the position and velocity estimation errors. V($\Vert \Delta \mathbf{p}\Vert$) and V($\Vert \Delta \mathbf{v}\Vert$) denote the variance of the norm of the position and velocity estimation errors. The time represents the algorithm processing time, which depends on the computation power of the computer and only for reference. We conduct one simulation run for AEKF and AUKF because the same prior statistical parameters and the same group of sensor data result in unchanged performance. For the PF based algorithm, the particles are generated randomly, so the estimation results have slight differences after each simulation. Table 2 shows the average values of 100 runs of the algorithms. We can see that AUPF (100 particles) can significantly improve the accuracy compared with PF (300 particles), and with computational efficiency at the same time.

Although the validation of the proposed AUPF algorithm is conducted from simulation in this paper, it has a theoretical foundation with a stringent mathematical representation. However, a real data experiment is an intuitively clear way to validate the performance of the presented AUPF algorithm, and it is of great practical interest to validate and compare it with other filters in experiments.

5. Conclusions

In this paper, we have studied the problem of navigation system design for low-cost USVs, in which we mainly focus on the filtering algorithm. A quaternion-based tightly-coupled integration approach is chosen to fuse the low-cost MEMS-INS measurements and GPS data. Due to the nonlinear and non-Gaussian properties of the INS/GNSS tightly-coupled integration problem, an APUF algorithm is proposed, which combines UKF with PF. A residual-base covariance matching technique is used to adaptively adjust the parameters and to track the changes in observation data in an adaptive manner. The resulting AUPF algorithm can reduce the computational burden without degrading the system estimation accuracy. A USV trajectory simulator is designed to generate the raw senor measurements of IMU and GNSS. The simulation results verify the robustness and accuracy of the AUPF algorithm in comparison to the AEKF and AUKF. In addition, the comparison with PF underlines the computational advantage of AUPF.