1. INTRODUCTION
The last few decades have seen much research interest in Inertial Navigation System (INS)/Global Positioning System (GPS) integration (Groves, Reference Groves2008). Generally, this research can be partitioned into two main aspects: one is research on the architectures of integration, such as the loosely-coupled, tightly-coupled and ultra-tightly-coupled INS/GPS integration and the other is research on integrated filtering algorithm design (Chang, Reference Chang2014; Miller and Campbell, Reference Miller and Campbell2012; Sun et al., Reference Sun, Petovello and Cannon2013; Crassidis, Reference Crassidis2006; Zhang et al., Reference Zhang, Meng, Zhang and Wang2015; Crassidis et al., Reference Crassidis, Markley and Cheng2007). This paper focuses on the second aspect and takes the loosely-coupled INS/GPS integration system as the representative application object. Before filtering algorithm design, the INS/GPS integrated process model should be firmly established. Various approaches have been studied, most of which are based on the inertial navigation error equations. Generally, the difference between these approaches is in terms of attitude error model as there are numerous attitude parameterisation techniques (Markley, Reference Markley2013). Among these techniques, the quaternion method is most celebrated due to its low computation, better accuracy and singularity avoidance. However, these error equation-based models suffer more or less from model error due to external disturbances and artificial approximation. In contrast, the Standard Inertial Navigation Equations (SINE)-based process model with no model simplification has drawn incremental research interest in recent years (Crassidis, Reference Crassidis2006; Zhou et al., Reference Zhou, Knedlik and Loffeld2010; Zhou et al., Reference Zhou, Yang, Zhang, Edwan and Loffeld2011). Any vehicle's manoeuvre and external disturbances can be captured by the SINE-based process model using an Inertial Measurement Unit (IMU). Therefore, compared with the error equation-based process model, the SINE-based process model will generally be better in terms of accuracy and robustness.
Since the SINE are naturally nonlinear, some nonlinear filtering algorithms should be implemented. In this paper, the Unscented Transformation (UT)-based Unscented Kalman Filter (UKF) is investigated due to its easy implementation, appropriate performance and computational feasibility (Julier et al., Reference Julier, Uhlmann and Durrant-Whyte2000; Chang et al., Reference Chang, Hu, Li and Qin2013). In the UT, a set of deterministically selected samples called sigma points are used to make probabilistic inference. Eliminating cumbersome derivation and evaluation of the Jacobian matrices, the UKF has proved to be a promising substitute for the widely used Extended Kalman Filter (EKF). However, when applied to the quaternion-based model, the UKF may destroy the unit-norm constraint of the quaternion due to the averaging operation on sigma points. With this consideration, a quaternion form of the traditional UKF called UnScented QUaternion Estimator (USQUE) is developed to cope with the quaternion normalization constraint for the quaternion-based attitude kinematics (Crassidis, Reference Crassidis2006; Crassidis and Markley, Reference Crassidis and Markley2003). In the USQUE, a hierarchy considering the dimensional mismatch is implemented, where the four-dimensional quaternion is used for global non-singular attitude representation and a set of three-dimensional unconstrained parameters, such as the Generalised Rodrigues Parameter (GRP), for local attitude representation and filtering.
Unfortunately, making use of the SINE as the filtering process model will result in a much greater computational burden. This is because in the USQUE framework the inertial navigation computation has to be implemented N times simultaneously in each filtering recursion with N denoting the number of sigma points. Moreover, sigma points construction for the high dimensional problem is also very disagreeable in terms of computational burden. Although some reduced-order UKF with fewer sigma points can be used to reduce the computational burden, the filtering performance is degraded simultaneously. In many applications, the nonlinear models are conditionally linear functions of the state vector, which means that the nonlinear transformations are applied to only a subset of the elements of the state vector while linear transformations are applied to the remaining part. For the nonlinear problem with such a special structure, a refined form of the traditional UT called Marginalised UT (MUT) could be implemented with non-degraded performance but decreasing computational expense (Morelande and Moran, Reference Morelande and Moran2007; Morelande and Ristic, Reference Morelande and Ristic2008). The decrease of the computational burden is achieved by applying the UT to make probabilistic inference for only the nonlinear subset of the state. In this respect, the smaller the nonlinear subset of the state is, the more adventageous the MUT will be. The idea of reducing computational complexity in MUT is similar to that in the marginalized or Rao-Blackwellized particle filter (Nordlund and Gustafsson, Reference Nordlund and Gustafsson2004; Semper et al., Reference Semper, Crassidis, Jemin George, Mukherjee and Singla2015). However, as will be shown in the next section, the SINE-based process model is nonlinear in all the state elements (attitude quaternion, velocity and position). Even if the process model can be reconstructed as conditionally linear, the MUT can still not be applied in the USQUE directly due to the dimensional mismatching hierarchy of the USQUE. More specifically, in the MUT the covariance and estimate error of the nonlinear subset of the state are necessary for determining the estimate of the linear subset. Unfortunately, in our reconstructed SINE-based process model, the nonlinear subset of the state is the attitude quaternion. As has been discussed above, in the USQUE the attitude quaternion and its corresponding covariance are dimensionally mismatched. Therefore, the MUT cannot be used directly in the USQUE framework. This represents the main motivation of this paper, which aims to embed the MUT into the USQUE structure for the SINE-based INS/GPS integration to reduce the computational expense. In this paper, the SINE-based process model is firstly reconstructed to be nonlinear only in the attitude quaternion and linear in velocity and position by determining the current angular rate of the navigation frame with respect to the earth frame and the local curvature matrix using the state estimate of the previous epoch. Then an ingenious filtering algorithm called Marginalized USQUE (MUSQUE) is derived by embedding the MUT into the USQUE. Enlightened by the fact that the quaternion and GRP-based attitude estimate errors have the same physical meaning, the attitude error based parts in the MUT structure are all parameterised in terms of GRP. Therefore, in the MUSQUE, the quaternion and GRP-based sigma points are propagated simultaneously in the MUT structure to deal with the dimensional mismatching hierarchy of the USQUE.
This paper is organised as follows. Section 2 mathematically formulates the SINE-based INS/GPS integration process model in discrete-time form. The ingenious MUSQUE is derived based on the reconstructed conditionally linear process model for the INS/GPS integration in Section 3. The performances of the proposed algorithms are analysed experimentally in Section 4. Conclusions are drawn in Section 5.
2. PROBLEM FORMULATION
Denote by n the local level navigation frame (East-North-Up, ENU), by b the Strapdown Inertial Navigation System (SINS) body frame (Right-Forth-Up), by e the Earth frame, by i some chosen inertial frame.
Since the SINE are used as the filtering process model, they are presented in their discrete-time forms directly in this section.
The quaternion kinematics equation is given by
$$q_{b,k}^n = \Omega \left( {\omega _{nb,k - 1}^b} \right)q_{b,k - 1}^n$$
where
$$\Omega \left( {\omega _{nb,k - 1}^b} \right) = \left[ {\matrix{ {Z_{k - 1}} & {\alpha _{k - 1}} \cr { - \alpha _{k - 1}^T} & {\cos \left( {0\!\cdot\!5\Vert {\omega _{nb,k - 1}^b} \Vert \Delta t} \right)} }} \right]$$
with
$$Z_{k - 1} = \cos \left( {0\!\cdot\!5\Vert {\omega _{nb,k - 1}^b} \Vert \Delta t} \right)I_{3 \times 3} - \left( {\alpha _{k - 1} \times} \right)$$
$$\alpha _{k - 1} = {{\sin \left( {0\!\cdot\!5\Vert {\omega _{nb,k - 1}^b} \Vert \Delta t} \right)\omega _{nb,k - 1}^b} / {\Vert {\omega _{nb,k - 1}^b} \Vert}} $$
where Δt is the updated interval, k and k − 1 are the time instant and I
3×3 is the identity matrix of indicated dimension. The 3 × 3 skew symmetric matrix (· ×) is defined so that the cross product satisfies a × b = (a × )b for arbitrary two vectors. Meanwhile,
$\omega _{nb,k - 1}^b $
is the angular rate of the body frame with respect to the navigation frame, expressed in the body frame at time instant k − 1, which is given by
$$\omega _{nb,k - 1}^b = \omega _{ib,k - 1}^b - C\left( {q_{b,k - 1}^n}\right)^T \left( {\omega _{ie}^n + \omega _{en,k - 1}^n} \right)$$
where
$\omega _{ib,k - 1}^b $
is the body angular rate measured by gyroscopes in the body frame. The Earth rotation rate in the navigation frame
$\omega _{ie}^n $
is given by
$\omega _{ie}^n = \left[ {0{\rm,} \omega _{ie}^{} \cos \varphi {\rm,} \omega _{ie}^{} \cos \varphi} \right]^T $
with ϕ being latitude and
$\omega _{ie}^{} $
being the earth rotation constant. Meanwhile,
$\omega _{en,k - 1}^n $
is the angular rate of the navigation frame with respect to the Earth frame, expressed in the navigation frame and is given by
$${\omega} _{en,k - 1}^n = \left[ {\matrix{ {{{ - v_{N,k - 1}^n} / {\left( {R_M + h_{k - 1}} \right)}}} \cr {{{v_{E,k - 1}^n} / {\left( {R_N + h_{k - 1}} \right)}}} \cr {{{v_{E,k - 1}^n \tan \varphi _{k - 1}} / {\left( {R_N + h_{k - 1}} \right)}}} }} \right]$$
where
$v^n = \left[ {v_E^n, v_N^n, v_U^n} \right]^T $
is the ground velocity. The parameters R
M
and R
N
are the main curvature radii of the prime meridian and the equator, respectively, and are assumed to be constant. The parameter h is the altitude of the body.
The matrix
$C\lpar {q_{b,k - 1}^n} \rpar$
denotes the attitude matrix from the body frame to the navigation frame in terms of
$q_{b,k - 1}^n $
.
The discrete-time velocity updated equation is given by
$${v}_k^n = {v}_{k - 1}^n + \left[ {C\left( {q_{b,k - 1}^n} \right){\,f}_{k - 1}^b - \left( {2{\omega} _{ie}^n + {\omega} _{en,k - 1}^n} \right) \times {v}_{k - 1}^n + {g}^n} \right]\Delta t$$
where
${f}_{k - 1}^b $
is the specific force measured by accelerometers in the body frame,
${g}^n $
is the gravity vector in the navigation frame.
The position vector is denoted by p = [λ, ϕ, h] T with λ being longitude. The discrete-time position updated equation is given by
$${p}_{k} = {p}_{k - 1} + R_{c,k - 1} v_{k - 1}^n \Delta t$$
where
$$R_{c,k - 1} = \left[ {\matrix{ {{1 / {\left( {\left( {R_N + h_{k - 1}} \right)\cos \varphi _{k - 1}} \right)}}} & 0 & 0 \cr 0 & {{1 / {\left( {R_M + h_{k - 1}} \right)}}} & 0 \cr 0 & 0 & 1 \cr}} \right]$$
is the local curvature matrix.
It should be noted that only the discrete-time SINE-based on the Euler method are presented. There are also many other fruitful algorithms for the SINE on-computer computation. Since the main focus of this paper is on the filtering algorithm design for the SINE-based INS/GPS integration, they have not been investigated here.
Select the state as
$x_k = \left[ {q_{b,k}^n, v_k^n, p_k} \right]^T $
, the SINE-based process model can be written in a compact form as
$$x_k = g\left( {x_{k - 1}} \right) + w_{k - 1} $$
where g(·) is the nonlinear function defined by Equations (1), (7) and (8) and w k−1 is the process noise, which is assumed to be zero-mean Gaussian white noise. It should be noted that the process noise and the following measurement noise are always non-additive or/and white in practice due to the external disturbance and the unexpected error sources in the inertial sensors. Since this paper is focussed on the computational filtering algorithm design, such a general case is not considered.
For the loosely coupled INS/GPS integration, the positions of the GPS are always used as the measurement, that is
$$z_k = p_{GPS,k} + \eta _k $$
where η k is the measurement noise which is assumed to be Gaussian with zero means and known covariance matrix R k .
3. MUSQUE FOR INS/GPS INTEGRATION
In this section the MUT for the conditionally linear model is firstly reviewed. Then the SINE-based INS/GPS integration process model is reconstructed to be nonlinear only in the attitude quaternion while linear in velocity and position. Finally the MUSQUE for the INS/GPS integration is designed with main focus on the treatment of the dimensional mismatching hierarchy inherent in the USQUE.
3.1. MUT for conditionally linear model
A nonlinear function is said to be conditionally linear if it has the following structure:
$${y} = g\left( {x} \right) = g_1 \left( {a} \right) + g_2 \left( {a} \right) \cdot {b}$$
where
$x = \left[ {{a};{b}} \right] \in R^n $
is the state vector which is assumed to be Gaussian in this paper,
${a} \in R^{n_a} $
,
${b} \in R^{n_b} $
and n
a
+ n
b
= n. The function g
1( · ) is the nonlinear function of a and g
2( · ) is the nonlinear or linear function of a. For such a structure, the nonlinear function is nonlinear in a and linear in b. With its partitioned form, the mean and covariance of the state can be written as
$${\hat x} = E\left[ {x} \right] = E\left[ {\matrix{ {a} \cr {b} \cr}} \right] = \left[ {\matrix{ {{\hat a}} \cr {{\hat b}} \cr}} \right]$$
$${P}_{x} = \left[ {\matrix{ {{\mathop{\rm cov}} \left( {{a,a}} \right)} & {{\mathop{\rm cov}} \left( {{a,b}} \right)} \cr {{\mathop{\rm cov}} \left( {{b,a}} \right)} & {{\mathop{\rm cov}} \left( {{b,b}} \right)} \cr}} \right] = \left[ {\matrix{ {{P}_{a}} & {{P}_{{ab}}} \cr {{P}_{{ba}}} & {{P}_{b}} \cr}} \right]$$
According to Anderson and Moore (Reference Anderson and Moore1979), the random variable b is conditionally Gaussian on a based on the above partitioned structure, that is
$$p\left( {\left. {b} \right \vert {a}} \right) = N\left( {{b};\zeta _{b} \left( {a} \right),{\it \Xi}} \right)$$
where
$$\zeta _{b} \left( {a} \right) = {\hat b} + {P}_{{ba}} {P}_{a}^{{- 1}} \left( {{a - \hat a}} \right)$$
$${\it \Xi} = {P}_{b} {- P}_{{ba}} {P}_{a}^{{- 1}} {P}_{{ab}} $$
Based on the statistical theory, the mean of the transformed variable y can be calculated as
$$\eqalign{& {\hat y} =\, \int {g\left( {x} \right)} \cdot p\left( {x} \right)d{x} \cr &{\hskip5pt} =\,\, \int\!\!\!\int {\left[ {g_1 \left( {a} \right) + g_2 \left( {a} \right) \cdot {b}} \right] \cdot p\left( {\left. {b} \right \vert {a}} \right) \cdot p\left( {a} \right)d{b}d{a}}} $$
Substituting Equation (15) into Equation (18) gives
$${\hat y} = \ \int {\left[ {g_1 \left( {a} \right) + g_2 \left( {a} \right) \cdot \zeta _{b} \left( {a} \right)} \right]} \cdot p\left( {a} \right)d{a}$$
Denote
$$\psi \left( {a} \right) = g_1 \left( {a} \right) + g_2 \left( {a} \right) \cdot \zeta _{b} \left( {a} \right)$$
The covariance of the transformed variable y can be calculated as
$$\eqalign{& {P}_{y} =\, \int {\left( {{y} - {\hat y}} \right)\left( {{y} - {\hat y}} \right)^T \cdot p\left( {x} \right)d{x}} \cr & {\rm} =\,\, \int\!\!\!\int {\left( {g\left( {x} \right) - {\hat y}} \right)\left( {g\left( {x} \right) - {\hat y}} \right)^T \cdot p\left( {\left. {b} \right \vert {a}} \right) \cdot p\left( {a} \right)d{b}d{a}}} $$
Substituting Equations (15) and (20) into Equation (21) gives
$${P}_{y} = \,\,\int {\left[ {\left( {\psi \left( {a} \right) - {\hat y}} \right)\left( {\psi \left( {a} \right) - {\hat y}} \right)^T} \right.} \left. { + g_2 \left( {a} \right){\it \Xi} g_2 \left( {a} \right)^T} \right] \cdot p\left( {a} \right)d{a}$$
Since the state vector x is assumed to be Gaussian, the random variable a is also Gaussian, i.e.
$$p\left( {a} \right) = N\left( {{a};{\hat a},{P}_{a}} \right)$$
In this respect, the integrals in Equations (19) and (22) are both of the form nonlinear function × Gaussian density. Therefore, they can be approximated by many numerical rules, such as the Gauss-Hermite Quadrature rule, the UT, or the cubature rule (Särkkä, Reference Särkkä2013). With the UT as the special case, Equations (19) and (22) can be approximated as follows.
Firstly, the sigma points are generated to parameterize the mean and covariance of the state variable. As can be seen from Equations (19) and (22), the nonlinear function is only in a, thus the sigma points are generated only for a. The sigma points and corresponding weights are given as
$$\eqalign{& {\chi} \left( 0 \right) = {\hat a} \cr & {\chi} \left( i \right) = {\hat a} + \left[ {\sqrt {\left( {n_a + \kappa} \right){P}_{a}}} \right]_i \cr & {\chi} \left( {i + n_a} \right) = {\hat a} - \left[ {\sqrt {\left( {n_a + \kappa} \right){P}_{a}}} \right]_i} $$
$$\eqalign{& {W}\left( 0 \right) = {\kappa / {\left( {n_a + \kappa} \right)}} \cr & {W}\left( i \right) = {1 / {2\left( {n_a + \kappa} \right)}} \cr & {W}\left( {i + n_a} \right) = {1 / {2\left( {n_a + \kappa} \right)}}} $$
where κ is a tuneable parameter of which the suggested optimal value for Gaussian distributions is κ = 3 − n
a
and
$\left[ {\sqrt {P}} \right]_i $
is the i-th column or row of the matrix
$\sqrt {P} $
which is the square root of the matrix
${P}$
and can be found through matrix decompositions, such as the efficient Cholesky decomposition. There are also many other sigma-point construction strategies besides Equation (24a) and (24b), and the reader is referred to (Julier and Uhlmann, Reference Julier and Uhlmann2004) for more details.
Propagate each of these sigma points through the nonlinear Equation (20)
$$\matrix{ {{\xi} \left( i \right) = \psi \left( {{\chi} \left( i \right)} \right)} & {i = 0,1,2, \cdots 2n_a} \cr} $$
Then the mean and covariance of the transformed variable can be approximated as
$${\hat y} = \sum\limits_{i = 0}^{2n_a} {{W}\left( i \right){\xi} \left( i \right)} $$
$${P}_{y} = \sum\limits_{i = 0}^{2n_a} {{W}\left( i \right)} \left\{ {\left( {{\xi} \left( i \right) - {\hat y}} \right)\left( {{\xi} \left( i \right) - {\hat y}} \right)^T} \right.\left. { + g_2 \left( {{\chi} \left( i \right)} \right){\it \Xi} g_2 \left( {{\chi} \left( i \right)} \right)^T} \right\}$$
The cross covariance between x and y can also be approximated using the MUT procedure. However, as will be shown, the MUT is investigated for the time update of the USQUE, so the calculation of the cross covariance between x and y has not been presented for simplicity.
Although a reduced number of sigma points are generated in the MUT, the computational expense in the sigma points generation procedure may not be reduced since some matrix inversion and multiplications should be performed. However, when the linear subset of the state is large or/and the expense of transforming these sigma points through the nonlinear function is large, the MUT will be more adventageous than the traditional UT. This is just the case for the reconstructed SINE-based process model that will be shown in the following section.
3.2. Reconstructed SINE based model with conditionally linear structure
As can be seen from Equations (1), (7) and (8), the SINE-based model is nonlinear in all the state elements. So the MUT cannot be applied for the SINE-based INS/GPS integration with the current form. Based on the definition of the conditional linear function in Equation (12), it can be found that the SINE-based model is nonlinear in velocity and position due to
$\omega _{en}^n $
and R
c
. It is well known that the INS is capable of providing position and velocity information at much higher data rates and the differences for
$\omega _{en}^n $
and R
c
between one INS sampling interval are much smaller. In this respect, it is valid to approximate the
$\omega _{en}^n $
and R
c
using the previous state estimate. We rewrite Equation (6) as
$${\hat \omega} _{en,k - 1}^n = \left[ {\matrix{ {{{ - \hat v_{N,k - 1}^n} / {\left( {R_M + \hat h_{k - 1}} \right)}}} \cr {{{\hat v_{E,k - 1}^n} / {\left( {R_N + \hat h_{k - 1}} \right)}}} \cr {{{\hat v_{E,k - 1}^n \tan \hat \varphi _{k - 1}} / {\left( {R_N + \hat h_{k - 1}} \right)}}} \cr}} \right]$$
Correspondingly
$$\hat \omega _{nb,k - 1}^b = \omega _{ib,k - 1}^b - C\left( {q_{b,k - 1}^n} \right)^T \left( {\omega _{ie}^n + \hat \omega _{en,k - 1}^n} \right)$$
Meanwhile, R c can also be approximated in a similar manner as
$$\hat R_{c,k - 1} = \left[ {\matrix{ {{1 / {\left( {\left( {R_N + \hat h_{k - 1}} \right)\cos \hat \varphi _{k - 1}} \right)}}} & 0 & 0 \cr 0 & {{1 / {\left( {R_M + \hat h_{k - 1}} \right)}}} & 0 \cr 0 & 0 & 1 \cr}} \right]$$
With the above approximation, the SINE-based model is now only nonlinear in the attitude quaternion while linear in velocity and position. Partition the state variable as
$x_k = \left[ {x_k^q ;x_k^e} \right]$
where
$x_k^q $
denotes the quaternion subset of the state and
$x_k^e $
denotes the remaining subset, i.e. velocity and position. The SINE-based process model Equation (10) can be rewritten as
$$x_k = g_1 \left( {x_{k - 1}^q} \right) + g_2 \left( {x_{k - 1}^q} \right)x_{k - 1}^e + w_{k - 1} $$
where
$$g_1 \left( {x_{k - 1}^q} \right) = \left[ {\matrix{ {\Omega \left( {\hat \omega _{nb,k - 1}^b} \right)q_{b,k - 1}^n} \cr {\left( {C\left( {q_{b,k - 1}^n} \right){\,f}_{k - 1}^b + {g}^n} \right)\Delta t} \cr {0_{3 \times 1}} \cr}} \right]$$
$$g_2 \left( {x_{k - 1}^q} \right) = \left[ {\matrix{ {0_{4 \times 3}} & {0_{4 \times 3}} \cr {I_{3 \times 3} - \left[ {\Delta t\left( {2{\omega} _{ie}^n + {\hat \omega} _{en,k - 1}^n} \right) \times} \right]} & {0_{3 \times 3}} \cr {\hat R_{c,k - 1} \Delta t} & {I_{3 \times 3}} \cr}} \right]$$
As can be seen from Equation (33),
$g_2 \left( {x_{k - 1}^q} \right)$
is actually independent on
$x_{k - 1}^q $
and Equation (31) can be rewritten as
$$x_k = g_1 \left( {x_{k - 1}^q} \right) + g_2 \cdot x_{k - 1}^e + w_{k - 1} $$
After the SINE-based model has been reconstructed as a conditional linear function, the MUT has potential to be applicable in the USQUE, which is the subject of the next section.
3.3. MUSQUE
As for the SINE-based INS/GPS integration, the quaternion is used to parameterise the attitude, whose unit-norm constraint is often violated by the averaging operation in the UT framework. Although some researchers have conducted renormalisation on the averaging quaternion, it is still not adequate. This is because the degree of freedom of a quaternion is three and thus the dimension of the covariance matrix for the quaternion should be 3 × 3. Therefore, using the 4 × 4 covariance matrix for the quaternion will cause a numerical instability problem. To overcome this problem, Crassidis (Reference Crassidis2006) and Crassidis and Markely (Reference Crassidis and Markley2003) developed the so-called USQUE with the consideration of dimensional mismatch for spacecraft attitude estimation and integrated navigation. In the USQUE, the GRP is used for local attitude representation and filtering and the quaternion is used for global non-singular attitude propagation. Here, the “local attitude” is actually referred to as attitude error. Denote the attitude error quaternion by δq = [δq 0; δρ], where δρ is the vector part of the quaternion δq and δq 0 is the scalar part. The GRP corresponding to the error quaternion is defined as
$$\delta \Re = \ell \displaystyle{{\delta \rho} \over {a + \delta q_0}} $$
where a is a parameter from 0 to 1, and ℓ is a scale factor. According to Crassidis (Reference Crassidis2006) and Crassidis and Markley (Reference Crassidis and Markley2003), when a = ℓ = 1, Equation (35) gives the standard vector of Modified Rodrigues Parameters (MRP), and when a = 0 and ℓ = 1, Equation (35) gives the Gibbs vector. In Crassidis (Reference Crassidis2006) and Crassidis and Markley (Reference Crassidis and Markley2003) ℓ = 2(a + 1) is chosen and in this case the norm of
$\delta \Re $
is equal to the angle of rotation.
Since the GRP is used in the filtering recursion, the filtering state is redefined as
$$x_k = \left[ {\delta \Re _k ;v_k^n ;p_k} \right] = \left[ {\delta \Re _k ;x_k^e} \right]$$
And the corresponding covariance at time instant k − 1 is given in the partitioned form as
$${P}_{k - 1} = \left[ {\matrix{ {{P}_{\delta \Re, k - 1}} & {{P}_{\delta \Re e,k - 1}} \cr {{P}_{e\delta \Re, k - 1}} & {{P}_{e,k - 1}} \cr}} \right]$$
where
${P}_{\delta \Re, k - 1} $
is a 3 × 3 matrix corresponding to
$\delta \Re _{k{\rm - 1}} $
.
Next, we will present how the MUT is embedded into USQUE with the main focus on the treatment of the dimensional mismatching hierarchy. The resulting computationally efficient algorithm is called MUSQUE. According to Equation (34), the reconstructed SINE-based process model is only nonlinear in the attitude quaternion, thus the sigma points are only needed to be generated for the attitude. Therefore, in the MUSQUE the sigma points are firstly generated as
$$\eqalign{& {\chi} _{k - 1}^{\delta \Re} \left( 0 \right) = 0_{3 \times 1} \cr & {\chi} _{k - 1}^{\delta \Re} \left( i \right) = \left[ {\sqrt {\left( {n_{\delta \Re} + \kappa} \right){P}_{\delta \Re, k - 1}}} \right]_i \cr & {\chi} _{k - 1}^{\delta \Re} \left( {i + n_{\delta \Re}} \right) = - \left[ {\sqrt {\left( {n_{\delta \Re} + \kappa} \right){P}_{\delta \Re, k - 1}}} \right]_i} $$
$$\eqalign{& {W}\left( 0 \right) = {\kappa / {\left( {n_{\delta \Re} + \kappa} \right)}} \cr & {W}\left( i \right) = {1 / {2\left( {n_{\delta \Re} + \kappa} \right)}} \cr & {W}\left( {i + n_{\delta \Re}} \right) = {1 / {2\left( {n_{\delta \Re} + \kappa} \right)}}} $$
where
$n_{\delta \Re} = 3$
is the dimension of
$\delta \Re $
and i = 1, 2, 3. In Equation (38a) and (38b) the estimate of
$\delta \Re $
after the last filtering recursion being 03×1 is used. According to Crassidis (Reference Crassidis2006) and Crassidis and Markley (Reference Crassidis and Markley2003), it is known that the estimate of
$\delta \Re $
is set to zero after attitude propagation at the end of each filtering recursion, which is used to move information from one part of the estimate to another.
Since the quaternion is used for attitude propagation in the process model, each of
${\chi} _{k - 1}^{\delta \Re} \left( i \right)$
should be firstly transformed into the error quaternion
$\chi _{k - 1}^{\delta q} \left( i \right)$
, which is carried out according to the inverse form of Equation (35) as
$$\delta q_{k - 1,0} \left( i \right) = \displaystyle{{ - a\Vert {\chi _{k - 1}^{\delta \Re} \left( i \right)} \Vert ^2 + \ell \sqrt {\ell ^2 + \left( {1 - a^2} \right)\Vert {\chi _{k - 1}^{\delta \Re} \left( i \right)} \Vert ^2}} \over {\ell ^2 + \Vert {\chi _{k - 1}^{\delta \Re} \left( i \right)} \Vert ^2}} $$
$$\delta \rho _{k - 1} \left( i \right) = \ell ^{ - 1} \left[ {a + \delta q_{k - 1,0} \left( i \right)} \right]\chi _{k - 1}^{\delta \Re} \left( i \right)$$
where
$\chi _{k - 1}^{\delta q} \left( i \right) = \left[ {\delta q_{k - 1,0} \left( i \right),\delta \rho _{k - 1} \left( i \right)^T} \right]^T $
is the i-th component of
$\chi _{k - 1}^{\delta q} $
.
The corresponding quaternions are given by
$$\chi _{k - 1}^q = \chi _{k - 1}^{\delta q} \otimes \hat q_{k - 1} $$
where
$\hat q_{k - 1} $
is the estimate of the attitude in the last filtering recursion.
According to Equations (20) and (25), the predicted sigma points are calculated as
$$\chi _{\left. k \right \vert k - 1}^{} \left( i \right) = g_1 \left( {\chi _{k - 1}^q \left( i \right)} \right) + g_2 \cdot \zeta _{x_{k - 1}^e} \left( {{\chi} _{k - 1}^{\delta \Re} \left( i \right)} \right)$$
where
$$\zeta _{x_{k - 1}^e} \left( {{\chi} _{k - 1}^{\delta \Re} \left( i \right)} \right) = \hat x_{k - 1}^e + {P}_{e\delta \Re, k - 1} \left( {{P}_{\delta \Re, k - 1}} \right)^{ - 1} \left( {{\chi} _{k - 1}^{\delta \Re} \left( i \right) - 0_{3 \times 1}} \right)$$
It should be noticed that there is a little difference between Equations (41) and (25). As can be seen from Equations (20) and (25), only one set of sigma points are propagated in the traditional MUT framework while in the MUSQUE, both the sigma points corresponding to the quaternion and GRP are propagated simultaneously. This is used to treat the dimensional mismatching hierarchy inherent in the USQUE. The validity of doing so can be explained heuristically as that according to Equation (16) when calculating the conditional mean of the linear subset of the state, the estimate error of the nonlinear subset should be determined. For the SINE-based process model, the nonlinear subset is the attitude and the corresponding estimate error can be represented by both quaternion and GRP. According to Equation (40), the estimate error in terms of quaternion is given by
$$\delta \chi _{k - 1}^q \left( i \right) = \chi _{k - 1}^{\delta q} \left( i \right) \otimes \hat q_{k - 1} \otimes \hat q_{k - 1}^{ - 1} = \chi _{k - 1}^{\delta q} \left( i \right)$$
Since
$\chi _{k - 1}^{\delta q} \left( i \right)$
and
${\chi} _{k - 1}^{\delta \Re} \left( i \right)$
represent the same variable, it is valid to propagate
${\chi} _{k - 1}^{\delta \Re} \left( i \right)$
in Equation (41).
Since the state vector corresponding to the attitude is defined by GRP, the predicted sigma points corresponding to the quaternion should be transformed back into GRP in order to make use of the UKF algorithm. Firstly, partition the predicted sigma points into two parts
$$\chi _{k\left \vert {k - 1} \right.}^{} = \left[ {\chi _{k\left \vert {k - 1} \right.}^q ;\chi _{k\left \vert {k - 1} \right.}^e} \right]$$
where
$\chi _{k\left\vert {k - 1} \right.}^q $
denotes the predicted sigma points corresponding to the quaternion and
$\chi _{k\left\vert {k - 1} \right.}^e $
to the velocity and position. Then the error quaternions can be calculated as
$$\chi _{k\left \vert {k - 1} \right.}^{\delta q} = \chi _{k\left \vert {k - 1} \right.}^q \otimes \left( {\hat q_{k\left \vert {k - 1} \right.}} \right)^{ - 1} $$
where
$\hat q_{k\left\vert {k - 1} \right.} $
is the prediction of the attitude quaternion and is given by (Markley et al., Reference Markley, Cheng, Crassidis and Oshman2007)
$$\hat q_{k\left \vert {k - 1} \right.} = \arg \mathop {\max} \limits_{q \in {S}^3} q^T Aq$$
with
$$A = \sum\limits_{i = 0}^{2n_{\delta \Re}} {{W}\left( i \right)\chi _{k\left \vert k \right. - 1}^q \left( i \right) \cdot \chi _{k\left \vert {k - 1} \right.}^q \left( i \right)^T} $$
In the traditional USQUE, the first predicted quaternion sigma point, i.e.
$\chi _{k\left\vert k \right. - 1}^q \left( 0 \right)$
is chosen as the reference quaternion to calculate the error quaternions in Equation (45). Based on the fact that within the UT framework the mean of these sigma points is more accurate than any of these points, the norm-preserving quaternion averaging algorithm in Equations (46) and (47) is investigated into the USQUE, resulting in a more accurate and robust algorithm (Chang et al., Reference Chang, Hu and Chang2014).
Next, transform the error quaternion sigma points into their GRP form. Denote
$\chi _{k\left\vert {k - 1} \right.}^{\delta q} \left( i \right) = \left[ {\delta q_{k\left\vert {k - 1} \right.,0} \left( i \right),\delta \rho _{k\left\vert {k - 1} \right.} \left( i \right)^T} \right]^T $
, the corresponding GRP is given by
$$\chi _{k\left \vert {k - 1} \right.}^{\delta \Re} \left( i \right) = \ell \displaystyle{{\delta \rho _{k\left \vert {k - 1} \right.} \left( i \right)} \over {a + \delta q_{k\left \vert {k - 1,0} \right.} \left( i \right)}}$$
The predicted sigma points of the filtering state can now be obtained as
$$\chi _{k\left \vert {k - 1} \right.}^{\ast} = \left[ {\chi _{k\left \vert {k - 1} \right.}^{\delta \Re}\!; \chi _{k\left \vert {k - 1} \right.}^e} \right]$$
The predicted mean can be calculated as
$$\hat x_{k\left \vert {k - 1} \right.}^{} = \sum\limits_{i = 0}^{2n_{\delta \Re}} {{W}\left( i \right)\chi _{k\left \vert {k - 1} \right.}^{\ast} \left( i \right)} $$
According to Equation (27), when we calculate the predicted covariance, the linear transformed conditional variance of the linear subset of the state is needed. Due to the dimensional mismatching hierarchy inherent in the USQUE, the dimension of the linear transformed matrix is 10 × 6 while the dimension of the predicted covariance should be 9 × 9. Therefore, the linear transformed conditional variance of the linear subset of the state cannot be calculated directly as in the traditional MUT. As can be seen from Equation (33), in the reconstructed SINE-based model the subset matrix g 2 (1:4, 1:6) is a zero matrix, which means that the attitude kinematics equation is independent of the linear subset of the state, i.e. velocity and position. In this respect, the following reduced-order linear transformed matrix can be applied to address the dimensional mismatching problem.
$$\tilde g_2 = \left[ {\matrix{ {0_{3 \times 3}} & {0_{3 \times 3}} \cr {I_{3 \times 3} - \left[ {\Delta t\left( {2{\omega} _{ie}^n + {\hat \omega} _{en,k - 1}^n} \right) \times} \right]} & {0_{3 \times 3}} \cr {R_c \Delta t} & {I_{3 \times 3}} \cr}} \right]$$
Then the predicted covariance can be calculated as
$$P_{k\left \vert {k - 1} \right.}^{} = \sum\limits_{i = 0}^{2n_{\delta \Re}} {{W}\left( i \right)} \left( {\chi _{k\left \vert {k - 1} \right.}^{\ast} \left( i \right) - \hat x_{k\left \vert {k - 1} \right.}^{}} \right)\left( {\chi _{k\left \vert {k - 1} \right.}^{\ast} \left( i \right) - \hat x_{k\left \vert {k - 1} \right.}^{}} \right)^T + \tilde g_2 {\it \Xi} _{k - 1} \tilde g_2 ^T + Q_{k - 1} $$
where
$${\it \Xi} _{k - 1} = {P}_{e,k - 1} {- P}_{e\delta \Re, k - 1} {P}_{\delta \Re, k - 1}^{{-} 1} {P}_{\delta \Re e,k - 1} $$
This completes the prediction step of the MUSQUE.
According to Equation (11), the measurement function is linear. Based on the filtering state definition Equation (36), the measurement function can be written as
$$z_k = H_k x_k + \eta _k $$
where
$H_k = \left[ {\matrix{ {0_{3 \times 6}} & {I_{3 \times 3}} \cr}} \right]$
. Since the measurement function is linear, the measurement update of the MUSQUE is the same as that of the standard KF.
$$K_k = {P}_{k\left \vert {k - 1} \right.} H_k^T \left[ {H_k {P}_{k\left \vert {k - 1} \right.} H_k^T + {R}_k} \right]^{ - 1} $$
$$\hat x_k = \hat x_{k\left \vert {k - 1} \right.}^{} + K_k \left( {z_k - H_k \hat x_{k\left \vert {k - 1} \right.}^{}} \right)$$
$$P_k = \left[ {I_{9 \times 9} - K_k H_k} \right]{P}_{k\left \vert {k - 1} \right.} $$
Denote the first three elements of
$\hat x_k $
as
$\delta \hat \Re _k $
. The attitude quaternion can be updated based on the quaternion prediction
$\hat q_{k\left\vert {k - 1} \right.} $
as
$$\hat q_k = \delta \hat q_k \otimes \hat q_{k\left \vert {k - 1} \right.} $$
where
$$\delta \hat q_k = \left[ {\delta q_{k,0}, \delta \rho _k ^T} \right]^T $$
with
$$\delta q_{k,0} = \displaystyle{{ - a\Vert {\delta \hat \Re _k} \Vert ^2 + \ell \sqrt {\ell ^2 + \left( {1 - a^2} \right)\Vert {\delta \hat \Re _k} \Vert ^2}} \over {\ell ^2 + \Vert {\delta \hat \Re _k} \Vert ^2}} $$
$$\delta \rho _k = \ell ^{ - 1} \left[ {a + \delta q_{k,0}} \right]\delta \hat \Re _k $$
Reset the first three elements (GRP) of
$\hat x_k $
to zeros (Crassidis, Reference Crassidis2006; Crassidis and Markley, Reference Crassidis and Markley2003). This completes this filtering cycle and then goes to the prediction step for the next filtering cycle. As can be seen from Equations (40) and (58), the attitude updates are performed using quaternion multiplication, leading to a natural way of maintaining the normalisation constraint.
4. RESULTS AND ANALYSIS
This section is devoted to experimentally examining the MUSQUE proposed in this paper for INS/GPS integration, with the main focus on comparison with the traditional USQUE. A car-mounted experiment is carried out. The experimental platform is composed of a single-antenna GPS receiver outside the vehicle and the navigation-grade ring laser INS inside the vehicle. The specifications of the IMU are listed in Table 1. The GPS receiver provides position measurements at the rate of 1 Hz. Moreover, in the designed integrated system, the Position Dilution Of the Precision (PDOP) that can reflect the position precision of the GPS according to the geometry of the satellites has been used. Therefore, when the PDOP indicates that the position output of the GPS is not reliable (too few satellites visible), the tested filtering algorithm only performs time update. Three chosen 30 minute test segments are used to evaluate the two filtering algorithms.
Specifications of the navigation-grade inertial sensors.
For both the USQUE and MUSQUE, the initial state estimate is assumed to be the true value which is derived through the initial alignment and calibration. The initial covariance is also the same for USQUE and MUSQUE. In the initial covariance, the initial covariance of the attitude is assumed to be (1 · π/180)2 I 3×3. The initial covariance of the velocity is assumed to be 0·12 I 3×3 and the initial covariance of the position is assumed to be diag([1/R M ;1/R N ;1]2). For both the USQUE and MUSQUE, the covariance of the process noise used in Equation (52) is assumed to be
$${Q} = diag\left( {\left[ {{{{{0\cdot01^0} / h}} / {\sqrt {Hz}}} \cdot 1_{3 \times 1} ;{{100\mu g} / {\sqrt {Hz} \cdot 1_{3 \times 1} ;0_{3 \times 1}}}} \right]^2} \right)$$
and the covariance of the measurement noise is assumed to be R = 0·12 I 3×3 (m/s)2.
The maximum and average position, velocity and attitude differences between USQUE and MUSQUE for the three test segments are summarised in Table 2. The differences between USQUE and MUSQUE for the first test segment are also plotted in Figures 1–3. It is shown that the differences between USQUE and MUSQUE are very small, even for the maximum values. In this respect, we can conclude that MUSQUE has a comparable performance with USQUE. Moreover, the computational burden of MUSQUE is much smaller than that of USQUE. The computational cost time of the two algorithms for the three test segments is summarised in Table 3. It is shown that the computational cost of the MUSQUE is about 54% of that of the USQUE for the first test segment, 49% for the second segments and 51% for the last segments. The decreasing computational burden is mainly caused by the decreasing inertial navigation computation. There is no doubt that the computational cost of inertial navigation computation is very expensive. In each filtering recursion, N times inertial navigation computation should be performed with N denoting the number of sigma points. In each filtering recursion, 19 sigma points are generated in USQUE while only seven sigma points are generated in MUSQUE. Therefore MUSQUE is much more computationally efficient compared with USQUE. Compared with applying USQUE we can decrease the number of sigma points substantially, thereby making the applied MUSQUE computationally tenable. The aforementioned discussion suggests that if both accuracy and computational expense are considered the proposed MUSQUE will be preferrable.
Attitude estimate difference between USQUE and MUSQUE.
Velocity estimate difference between the USQUE and MUSQUE.
Position estimate difference between USQUE and MUSQUE.
The maximum and average of the estimate differences between USQUE and MUSQUE.
Computation time of USQUE and MUSQUE.
It should be noted that the differences of the yaw angles in time interval 130–150 s are a little larger than these at other time instant for the first test segment. This is mainly due to the severe changes in the yaw angles as the vehicle made a sharp turn at 130 s. The reconstructed SINE may not capture the severe changes due to the artificial approximation in Equations (28) and (30). However, the largest difference in the yaw angles is still very small. In this respect, we can still conclude that MUSQUE has a comparable performance to USQUE. Conservatively, one can turn to USQUE from MUSQUE when there is a sharp change in the yaw angles. In this case, some rate limits which flag the USQUE algorithm for use can be set experientially.
In order to further examine the performance of the MUSQUE, the initial alignment with large misalignment angles is performed using both USQUE and MUSQUE. The initial alignment is actually a special case of the integration with no initial attitude information. The data segment between 300–600 s of the first test segment is used to test the performance of the USQUE and MUSQUE for initial alignment, as shown in Figure 4. The attitude estimate of the USQUE is used as the attitude reference. The true initial attitude added misalignment [10°;10°;30°] is chosen as the initial attitude estimate for both USQUE and MUSQUE. The other initial state estimate is assumed to be the true value for both USQUE and MUSQUE. The initial covariance of the attitude is assumed to be diag(([10;10;30]/3 · π/180)2). Other parts of the initial covariance are the same as those defined above. The attitude estimate errors and their corresponding 3σ bounds are shown in Figures 5 and 6 for USQUE and MUSQUE, respectively. It is shown that the USQUE and MUSQUE have a comparable performance in terms of convergent speed and accuracy. Moreover, the estimate errors are all inside their 3σ bounds for both the USQUE and MUSQUE, which means that the performance of the USQUE and MUSQUE is consistent and credible.
Test segment for initial alignment.
Attitude estimate errors and their 3σ envelopes of USQUE.
Attitude estimate errors and their
$3\sigma$
envelopes of MUSQUE.
5. CONCLUSION
The expensive computational burden is the significant disadvantage of the USQUE for SINE-based INS/GPS integration. The computational burden is mainly caused by the inertial navigation computation, which is proportional to the number of sigma points generated in each filtering recursion. In order to improve the computational efficiency of the USQUE, the MUT that is applicable to nonlinear systems with a linear substructure is investigated. By embedding the MUT into USQUE, the so-called MUSQUE is derived. In the MUSQUE, the SINE are firstly reconstructed to be only nonlinear in the attitude quaternion while linear in the velocity and position, then the quaternion and GRP-based sigma points are propagated simultaneously to deal with the dimensional mismatching hierarchy of the USQUE. Since the sigma points generated in the MUT are only proportional to the nonlinear subset of the state, only seven sigma points are generated and propagated in each filtering recursion of the MUSQUE while 19 for the USQUE. Therefore, MUSQUE is much more computationally efficient than USQUE. The experimental results show that MUSQUE has a comparable performance with USQUE for both INS/GPS integration and initial alignment at a significantly lower expense. The filtering structure developed in this paper can also be applicable to the quaternion-based nonlinear systems that are or can be reconstructed as conditionally linear.
ACKNOWLEDGMENT
This work was supported in part by the National Natural Science Foundation of China (61304241, 61374206). The authors would like to thank Professor Gongmin Yan for providing the car-mounted experimental data.
