1. INTRODUCTION
SINS employs inertial sensors to provide continuous navigation information with excellent short-term accuracy using high frequency navigation information measurements, but some challenges still exist due to sensor unavailability and random drifts. The odometer is widely used in land vehicles to provide speed and distance information. These techniques are completely autonomous since they do not depend on external sources of information that can be disturbed or blocked by military interference. However, the odometer signal may suffer from signal blockage due to electronic component failure causing interruption of information flow. The odometer can also suffer from other problems, such as vehicle sideslip in complex road environments that reduces the positioning accuracy (Yang, Reference Yang2011). Thus, the odometer has to be integrated with the complementary characteristics of SINS to provide a more accurate and uninterrupted navigation solution. If SINS/odometer systems use low cost inertial sensors with stochastic bias nonlinearity or the vehicle has a large misalignment due to inaccurate initial alignment, the inherent biases of the SINS inertial sensors and the relatively high noise levels associated with their measurements mean that the Kalman filter will not be competent to provide accurate positioning (Georgy et al., Reference Georgy, Noureldin, Korenberg and Mohamed2010). The Sparse-Grid Quadrature Kalman Filter (SGQKF) has been recently designed as an online filtering technique (Jia et al., Reference Jia, Xin and Cheng2010, Reference Jia, Xin and Cheng2012; Kwangyul and Hyochoong, Reference Kwangyul and Hyochoong2013) to accommodate arbitrary inertial sensor characteristics, motion dynamics and noise distributions. The SGQKF employs non-linear SINS bias dynamic models and the odometer measurements for prediction and update estimation. If the odometer observations are continuously available, the effects of the models are minimized because the SINS biases do not increase because of the continuous odometer measurement updates. However, during odometer measurement outages, the SINS bias computations are based only on the model without external observation updates from the odometer leading to incorrect estimates of the SINS biases. Consequently, in even short observation outages or degradation due to signal blockage or loss, the performance degrades drastically (Atia et al., Reference Atia, Noureldin and Korenberg2012).
Many methods have been proposed to bridge uncertain observations, which can be divided into Artificial Intelligence (AI) methods and Nonlinear System Identification methods. Artificial Neural Networks (Semeniuk and Noureldin, Reference Semeniuk and Noureldin2006) can be utilized as a SINS bias predictor in a training network applied to a large amount of continuously recorded data collected during reliable observation availability. However the problem with these schemes is that the resulting models depend on over-learning from the training data records. Thus, it may be serviceable in short outages but the performance may degrade significantly for long outages beyond several minutes (Semeniuk and Noureldin, Reference Semeniuk and Noureldin2006; Atia et al., Reference Atia, Noureldin and Korenberg2012). The most significant issues occur when depending on trained models for long periods and the Kalman filter can not account for the uncertainties. Furthermore, models with such complex parameters must be trained when reliable observations are available which adds complexity and computational burden to the navigation system. For better performance, artificial fuzzy systems (Abdel-Hamid et al., Reference Abdel-Hamid, Noureldin and El-Sheimy2007; Loebis et al., Reference Loebis, Sutton, Chudley and Naeem2004) have been used to deal with stochastic errors and uncertain sensor observations during odometer outages. The fuzzy rules mainly depend on expert experience, but the fuzzy membership function is difficult to determine.
Another method to deal with uncertain observations is to assume that the stochastic process is defined by Bernoulli random variables describing the presence or absence of the observations with an approximation to a least squares filter used to augment the state vector and the Bernoulli random variables with a probability for the unscented transformation (Li and Xia, Reference Li and Xia2012; Hermoso-Carazo and Linares-Pérez, Reference Hermoso-Carazo and Linares-Pérez2011). However, accurate Bernoulli random variables with probability are hard to obtain in practical navigation systems. In this study, the SGQKF is used with the Gaussian process regression (GPR) technique to bridge odometer outages due to unreliable data transmission loss or delay. This paper describes a nonlinear modelling technique that is capable of coping with signal outages and maintaining good SINS bias estimates during odometer failures on the basis of training data. Integration of SGQKF with the GPR technique benefits from the SGQKF estimation capability instead of totally relying on trained models to bridge odometer outages.
This paper is organized as follows: Section 2 describes the Gaussian process regression and the Sparse-grid Quadrature Kalman filter. Section 3 introduces the GPR/SGQKF framework and the SINS/OD integrated navigation system for bridging odometer outages utilizing GPR. Section 4 provides test results followed by a discussion in Section 5.
2. RELATED STUDIES USING SGQKF/GPR
2.1. Sparse-grid Quadrature Kalman Filter
State estimates of dynamic systems are difficult and nonlinear state estimates have been extensively used in navigation. The most successful techniques for state estimation are Bayesian filters, which recursively estimate posterior probability distributions (PDF) over the state of a system. In general, analytical solutions for the PDF are thus impossible to obtain in nonlinear systems. Optimal Bayesian estimates are difficult to obtain for real-world problems. The quadrature points collocation method combined with the Sparse-grid technique has recently been introduced for nonlinear filtering (Jia et al., Reference Jia, Xin and Cheng2010; Kwangyul and Hyochoong, Reference Kwangyul and Hyochoong2013). Cheng et al. (Reference Cheng, Ran and Wang2013) described the SGQKF technique that can also use the Kronrod-Patterson rule. The univariate quadrature points in the Kronrod-Patterson rule are of high precision and are nested to achieve a higher computational accuracy with fewer quadrature points. In addition, its computational accuracy can be flexibly controlled. In contrast with the Quadrature Kalman Filter, the SGQKF effectively alleviates the curse of dimensionality for high dimensional problems to give even higher estimation accuracy.
The crucial components of the SGQKF are the prediction and observation models, which probabilistically describe the instantaneous evolution of the process and the measurements returned by the sensors. These models use parameters to describe the process with the parametric models being very efficient, but accurate parametric models are difficult to obtain, so their predictive capabilities are restricted because they often ignore hard to model aspects of the process (Ko and Fox, Reference Ko and Fox2009). Many types of measures have been induced in correcting models containing installation errors and scale factor errors in navigation systems. One of the main problems is often that although the basic physical mechanism is easy to establish analytically, these systems have sophisticated relationships between the various elements that are difficult to model.
2.2. Gaussian Process Regression
To overcome the limitations of parametric models, many studies have used nonparametric Gaussian process regression models (GPs) (Lawrence, Reference Lawrence2005; Rasmussen et al., Reference Rasmussen, Christopher and Williams2006; Ko et al., Reference Ko, Klein, Fox and Hähnel2007; Asheri et al., Reference Asheri, Rabiee, Pourdamghani and Mohammad2010; Ko and Fox, Reference Ko and Fox2011; Snelson and Ghahramani, Reference Snelson and Ghahramani2012) to learn prediction and observation models for dynamic systems. Such models combine modelling flexibility with consistent uncertainty estimates, which makes them ideal learning probabilistic estimation models in navigation systems. The fact that GPR models provide Gaussian uncertainty estimates for their predictions allows them to be seamlessly incorporated into probabilistic filtering techniques. GP-Bayes filters are a general framework for integrating Gaussian process prediction and observation models into Bayesian filtering techniques, including particle filters and unscented Kalman filters (Ko et al., Reference Ko, Klein, Fox and Hähnel2007). GP-Bayes filters learn GP filter models from training data containing sequences of control inputs, observations, and ground truth states (Asheri et al., Reference Asheri, Rabiee, Pourdamghani and Mohammad2010; Snelson and Ghahramani, Reference Snelson and Ghahramani2012; Ranganathan et al., Reference Ranganathan, Yang and Ho2011). In the context of the SINS/odometer combination, GP-Bayes filters have shown excellent performance, significantly outperforming their parametric Bayes-filter counterparts. Furthermore, GP-Bayes filters can be combined with parametric models to improve data efficiency and thereby reduce computational complexity (Ko and Fox, Reference Ko and Fox2011).
A Gaussian process represents distributions over functions based on training data. Assume a set training data, D=<X, z>, where X=[x 1, x 2, … , x N]∈d×N, xi is a matrix containing d-dimensional input samples i=1,2, … ,N, where N is total number of set samples, and z=[z 1,z 2, … , z N ] is a vector containing the scalar training output z i. Assume a noisy process
$$z_i = f\,(x_i ) + \varepsilon $$where ε is additive Gaussian noise with zero mean and variance δ n2. Conditioning on training data D=<X, z> and a test input x*, a GP results in a Gaussian predictive distribution over the corresponding output z*
$${\rm p}(z^*{\rm |}\; x^*,{\rm D}) = {\rm N}(z^*;\,{\rm GP}_\mu (x^*,{\rm D}),\quad {\rm GP}_{\rm \Xi} (x^*,D))$$with its mean
$${\rm GP}_\mu {\rm (}x^*,{\rm D)} = k^{\rm* T} [K + \delta _n ^{\rm 2} {\rm I}_N ]^{ - {\rm 1}} z$$and its variance
$${\rm GP}_{\rm \Xi} (x^*,{\rm D}) = {\rm} k(x^*,{\rm} x^*) - k^*^{\rm T} \left[ {K + \delta _n ^{\rm 2} {\rm I}_N} \right]^{ - {\rm 1}} {\rm} k^*$$Here, k is the kernel function of the GP, k* is a vector of kernel values between x* and the training inputs X, K is the N×N kernel matrix of the training input values, k*[i]=k(x*, x i) and K[i, j]=k(x i, xj). Note that the prediction uncertainty, captured by the variance GPΞ, depends on both the process noise and the correlation between the given input and the training data. The most widely used kernel function is the squared exponential Gaussian kernel with additive noise (Ko et al., Reference Ko, Klein, Fox and Hähnel2007):
$$k(x,x') = \delta _f^2 \exp \left( { - \displaystyle{1 \over 2}(x - x'){\rm \Lambda} ^{ - 1} (x - x')} \right)$$where δ f2 is the signal variable. The signal variable regulates the uncertainty of predictions in areas of training data density. The diagonal matrix Λ contains the length scales of the process, Λ=diag([1/l 12, 1/ l 22, … ,1/l d2]). The length scales reflect the relative smoothness of the process along the different input dimension. The δ n2 controls the global noise of the process. The parameters θ=[Λ, δ f, δn] are referred to as the hyper-parameters of the GPR. They can be learned by maximizing the log marginal likelihood of the given training output and input:
$$\hat \theta = \mathop {\arg \max \log} \limits_{\theta |} (\,p(z|X,\theta ) = \mathop {\mathop {\arg \max} \limits_{\theta |} \left[ { - \displaystyle{1 \over 2}z^T (K(X,X) + \delta _{^n} ^2 I_n )^{ - 1} z - \displaystyle{1 \over 2}\log |K(X,X) + \delta _{^n} ^2 I_n | - \displaystyle{n \over 2}\log 2\pi} \right]}} $$Equation (6) gives the objective function J(θ k) with the partial derivatives of the objective function as:
$$\nabla J(\theta _k ) = \displaystyle{\partial \over {\partial \theta _k}} \log (\,p(z|X,\theta ) = \displaystyle{1 \over 2}tr\left\{ {\left[ {(K(X,X)^{ - 1} z)(K(X,X)^{ - 1} z)^T - K(X,X)^{ - 1}} \right]\displaystyle{{\partial K(X,X)} \over {\partial \theta _k}}} \right\}$$This optimization can be performed using scaled conjugate gradient descent with the steps organized as follows:
(1) Give a good initialization θ 1 and a margin of error eps >0, and set the maximum number of iterations n to avoid infinite iterations;
(2) for n iterations and ||∇J(θ k)||⩾eps do
Compute the gradient of the log marginal likelihood ∇J(θ k).
Reserve the result as the direction of the steepest ascent p k=∇J(θ k);
Golden point search is used to determine the optimal step length, λ k, for the one dimensional search then update Jθ k+1=θ k+λ kpk
$$J(\theta _{k + 1} ) = J(\theta _{k + 1} = \theta _k + \lambda _k p_k ) = \mathop {\max} \limits_{\lambda _k} \quad \log (\,p(z|X,\theta _{k + 1} )|_{\theta _{k + 1} = \theta _k + \lambda _k p_k} $$End for.
Output θ k
In practice, the optimization problem is non-convex, so there is no guarantee of finding a global optimum. Thus, the approach requires good initialization to avoid local maxima. Here
$$\delta _n^2 = r\displaystyle{1 \over n}\sum\limits_{\,j = 1}^n {z_{^j} ^2} \quad \quad \delta _f^2 = (1 - r)\displaystyle{1 \over n}\sum\limits_{\,j = 1}^n {z_{^j} ^2} $$r∈[0, 1], where r is an empirical value. The length scales have good initializations as
$$l_i = \sqrt {\displaystyle{1 \over {2n^2 \log 2}}\sum\limits_{\,p = 1}^n {\sum\limits_{q = 1}^n {|x_p - x_q |^2}}} \quad i = 1,2, \ldots, n$$2.3. GPR combined with the SGQKF for bridging uncertain observation outages
The evolution of the dynamic system and the observation predictions are represented by Gaussian processes alone, which have some shortcomings. Since the GP is assumed to have a zero mean, the output from test states far away from the training state will quickly decay to zero. Combining GPR models with the SGQKF gives higher prediction accuracy because a parameterised model is used to represent the phenomenon with physical formulae. The GPR/SGQKF algorithm flow chart for bridging uncertain observation outages is described here.
Preparation before filtering while Np=2d 2+6d+1.
First, when observations of the corrected vehicle velocity state are available, the gyroscope and accelerometer readings are collected as input X while the SINS deviations from the odometer velocity reading or increments are collected as output z, so the training data set is Dh=<X, z>, and the Gaussian process learns from this data set to get the mapping for predictions of the data points with the GPR hyper-parameters optimized together using numerical conjugate gradient descent techniques. The x k* is a vector containing the most recent gyroscope and accelerometer measurements and the corrected velocity before the odometer is no longer available and the x k* is used as the new test input. The objective of the Gaussian process is to generate nonlinear predictions of the evolution of the SINS biases, z k, from the new input based on all the observed data as a system measurement of the training data z k ∼GP(x k*, Dh).
The second step computes the quadrature point weights and quadrature point allocations ξ i. i=1:N p
$$\eqalign{\omega _i = & \left\{ {\matrix{ {\left\{ \matrix{\displaystyle{{(1 + (\omega _2 )^2 )(d^2 - 3d + 2)} \over 2} \cr - (d^2 - d)\omega _2 + d\omega _4 } \right\}} & {i = 1} \cr { - (1 - \omega _2 )\omega _3 (n - 1) + \omega _5 } & {i = 2, \ldots, 2d + 1} \cr {\omega _6} & {i = 2d + 2, \ldots, 4d + 1} \cr {\omega _7} & {i = 4d + 2, \ldots, 6d + 1} \cr {\omega _8} & {i = 6d + 2, \ldots, 8d + 1} \cr {\omega _3 \cdot \omega _3} & {i = 8d + 2, \ldots, 2d^2 + 6d + 1 } \cr}} \right. \cr \cr\xi _i = &\left\{ {\matrix{ {[0,0, \ldots, 0]^{\rm T} ;} & {i = 1} \cr {[0, \ldots, \pm \sqrt 3, \ldots, 0]^{\rm T} ;} & {i = 2, \ldots, 2d + 1} \cr {[0, \ldots, \pm 4 {\cdot} 185, \ldots, 0]^{\rm T} ;} & {i = 2d + 2, \ldots, 4d + 1} \cr {[0, \ldots, \pm 0 {\cdot} 741, \ldots, 0]^{\rm T} ;} & {i = 4d + 2, \ldots, 6d + 1} \cr {[0, \ldots, \pm 2 {\cdot} 861, \ldots, 0]^{\rm T} ;} & {i = 6d + 2, \ldots, 8d + 1} \cr {[0, \ldots, \pm \sqrt 3, \ldots, \pm \sqrt 3, \ldots, 0]^{\rm T} ;} & {i = 8d + 2, \ldots, 2d^2 + 6d + 1} \cr}} \right.} $$During the observation outages, the GPR is applied to this data set to predict the SINS deviations from the odometer with the predicted SINS deviations sent to the SGQKF as a real update to estimate all the SINS biases:
• GPR/SGQKF Algorithm: Time-Update Step
Assume that the initialization function is known
$$\hat x_0 = E(x_0 )\quad{\rm and}\quad\hat P_0 = E[(\hat x_0 - x_0 )(\hat x_0 - x_0 )^T ]$$For k=1 to ∞, evaluate the Singular Value Decomposition and feature point covariance matrix
$$P_{xx,k - 1/k - 1} = U_{k - 1/k - 1} \sum _{k - 1/k - 11} V_{k - 1/k - 1} ^T \quad S_{xx,k - 1/k - 1} = U_{k - 1/k - 1} \sqrt {E_{k - 1/k - 1}} ;$$Evaluate the quadrature points
$$\gamma _{i,k - 1/k - 1} = S_{i,k - 1/k - 1} \,\xi _i + \hat X_{k - 1/k - 1} ;\quad i = 1,2 \ldots N_p $$Evaluate the propagated quadrature points
$$X^*_{i,k/k - 1} = f\,(\gamma _{i,k/k - 1} )\quad i = 1,2, \ldots N_p $$Estimate the predicted state
$$X^*_{i,k/k - 1} = f\,(\gamma _{i,k/k - 1} )\quad i = 1,2, \ldots N_p $$Estimate the predicted error covariance
$$P_{xx,k/k - 1} = \sum\limits_{i = 1}^{N_p} {\omega _i (X_{k/k - 1}^* - \hat X_{k/k - 1} )} (X_{k/k - 1}^* - \hat X_{k/k - 1} )^T + Q_k $$• GPR/SGQKF Algorithm: Measurement-Update StepFactor
$$P_{xx,k/k - 1} = U_{k/k - 1} \sum _{k/k - 1} V_{k/k - 1} ^T \quad S_{xx,k/k - 1} = U_{k/k - 1} \sqrt {\sum _{k/k - 1}} P_{xx,k/k - 1} = S_{k/k - 1} S_{k/k - 1} ^T $$Evaluate the quadrature points
$$\gamma _{i,k/k - 1} = S_{k/k - 1} \,\xi _i + \hat X_{k/k - 1} ;\quad i = 1,2 \ldots N_p $$Evaluate the propagated quadrature points
$$\hat Z^*_{i,k/k - 1} = h(\tilde \gamma _{i,k/k - 1} )\quad i = 1,2 \ldots N_p $$Estimate the predicted measurement
$$\hat Z_{i,k/k - 1} = \sum\limits_{i = 1}^{N_p} {\omega _i h(\tilde \gamma _{i,k/k - 1} ) =} \sum\limits_{i = 1}^{N_p} {\omega _i \hat Z^*_{i,k/k - 1}} $$Estimate the innovation covariance matrix
$$P_{zz,k/k - 1} = \sum\limits_{i = 1}^{N_p} {\omega _i (h(\tilde \gamma _{i,k/k - 1} ) - \hat Z_{i,k/k - 1}} )(h(\tilde \gamma _{i,k/k - 1} ) - \hat Z_{i,k/k - 1} )^T + R_{v\,temp} $$where Rv temp is relate to the GPR as given in Section 3.
Estimate the cross-covariance matrix
$$P_{xz,k/k - 1} = \sum\limits_{i = 1}^{N_p} {\omega _i (\tilde \gamma _{i,k/k - 1} - \hat X_{k/k - 1}} )(h(\tilde \gamma _{i,k/k - 1} ) - \hat Z_{k/k - 1} )^T $$Estimate the Kalman gain
$$K_k = P_{xz,k/k - 1} P_{zz,k/k - 1}^{ - 1} $$Estimate the updated State
$$\hat X_{k|k} = \hat X_{k|k - 1} + K_k (Z_{k\,temp} - \hat Z_{k|k - 1} )$$where Zk temp is relate to the GPR given in Section 3.
Estimate the corresponding error covariance
$$P_{k|k} = P_{k|k - 1} - K_k P_{zz,k|k - 1} K_k^T $$3. SGQKF COMBINED WITH GPR IN SINS/OD
A Sparse-grid Quadrature Kalman filter in the Kronrod-Patterson rule modified by the Gaussian process regression technique is used in the integrated navigation system.
3.1. SINS/odometer integrated navigation system
The odometer outage bridging method was applied to a 2D land-vehicle navigation system using an inertial sensors system integrated with the odometer based on the GPR/SGQKF. The platform has three gyroscopes, three accelerometers and the vehicle odometer shown in Figure 1. The SINS error state vector is x k=[δV eδVn ϕe ϕn ϕu ∇x ∇y εx εy εz]T. The following passage demonstrates that the GPs combine extreme modelling flexibility dealing with consistent uncertainty estimates making them an ideal tool for learning by probabilistic estimation models in navigation systems.
Experimental vehicle system.
The nonlinear SINS bias state dynamic model is indicated by the general form
$$\eqalign{x_k = & f\,(x_{k -1 } ) + w_{k -1} \cr z_k = & h(x_k ) + v_k} $$where w k−1 is the system noise and v k is the process noise contaminating the sensor measurements. The attitude error nonlinear equation is given by Yan et al. (Reference Yan, Yan and Xu2008)
$$\dot \phi ^n = C_w^{ - 1} [(1 - C_n^c )\hat \omega _{in}^n + C_n^c \delta \omega _{in}^n - C_b^c \delta \omega _{ib}^b ]$$The nonlinear speed error equation is:
$$\eqalign{\delta \dot V^n = & [1 - (C_n^c )^T ]C_b^c \hat f^b + \delta V^n \times (2\hat \omega _{ie}^n + \hat \omega _{en}^n ) + (\hat V^n - \delta V^n ) \cr & \times (2\delta \omega _{ie}^n + \delta \omega _{en}^n) + (C_n^c )^T C_b^c \delta f^b + \delta g^n} $$The SINS distance increment is
$$z_{SINS} (t_k ) = \Delta R_{INS} (t_k ) = \int_{t_{k - 1}} ^{t_k} {\hat V_{INS}} (t)dt$$The odometer distance increment is
$$z_{OD} (t_k ) = {\rm \Delta} R_{OD} (t_k ) = \hat C_b^n (t_k ){\rm \Delta} S_{OD} (t_k )$$where ∆S OD(t k) is the odometer distance increment in the body coordinate frame. The measurement z obs,k can be directly obtained from the deviations between the SINS and the odometer measurements.
$$z_{obs,k} = z_{SINS} (t_k ) - z_{OD} (t_k )$$In the SINS/ odometer integrated system, assuming the expected value of the noise is zero, the SGQKF performs the prediction step to predict the SINS bias state according to the SINS dynamic error models in Equations (28) and (29). When the odometer observations are available, the deviations of the SINS output from the odometer measurements are utilized as observations input to the SGQKF to derive the system to rectify the SINS bias state vector. With the observations z obs,k, the SINS bias state vector x k can be partially revised as follows:
$$\hat x_{k|k} = \hat x_{k|k - 1} + K_k (Z_{obs,k} - \hat Z_{k|k - 1} )$$The SGQKF then performs the update step to estimate the complete SINS bias states vector x k from which the SINS navigation output is corrected by subtracting the error states.
3.2. Bridging odometer outages using GPR
During observation outages with invalid odometer data, the inaccurate SINS corrections cause the conventional filter to quickly deviate from true ground trajectory if the GPR model is not used to predict the SINS velocity and attitude deviations. A SGQKF with the GPR technique is hence used to bridge uncertain observation outages in navigation systems. When the odometer is available, the system has valid data. The most accurate vehicle state estimate is based on all the current available knowledge. The current sensor readings from the three gyroscopes and the three accelerometers and the odometer measurements are used to calculate the SINS output deviations. These data points represent the error characteristics of the SINS solution. Thus, the GPR training input set is selected from the current sensor readings and the vehicle velocity (SINS output corrected by the SGQKF) and the GPR training output set is selected from the SINS output deviations from the odometer increment. The collected data points do not need to be sequential as shown in Table 1.
Input/output Data for the GPR Model Training for bridging odometer outages.
The hyper-parameters are then learned from the previously collected large data set D=<X, z>. During observation outages, GPR is applied with this data set D=<X, z > to compute the predictive distribution at the new test input x k* containing the most recent gyroscope and accelerometer measurements and the corrected velocity. The Gaussian predictive distribution p(z*|x*, D)=N(z*;GPμh (x*, D), GPΞh (x*, D)) is fed to the SGQKF as virtual measurements updates
$$\breve {z}_k = {\rm GP}_\mu ^h (x^*, {\rm D)}$$and the noise variance
$$\breve {R} v = {\rm GP}_\Xi ^h (x^*, {\rm D)}$$The SGQKF then uses a virtual update step to estimate the SINS bias evolution and correct the SINS output.
4. TEST RESULTS AND ANALYSIS
The SGQKF combined with the GPR technique is implemented in simulations of a navigation system and in a real vehicle to verify the effectiveness of the method for bridging uncertain observation outages. The GPR/SGQKF and the filters are initialized with the same conditions for each run. With X=[δV eδVn ϕe ϕn ϕu ∇x ∇yεx εy εz]T as the state variables and ∆RSINS(t k)−∆ROD(t k) as the observation variables, the instrument biases were a repeatability of gyroscope constant drift of 0·05°/h, a gyroscope random measurement noise of 0·05°/h, an accelerator biases of 5·0 mg and accelerator measurement noise of 5·0 mg. The initial true state value is assumed to be X 0=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0]T, P 0=diag[(0·1 m/s)2, (0·1 m/s)2, (10°)2, (1°)2, (1°)2, (5·0 mg)2, (5·0 mg)2, (0·05°/h)2, (0·05°/h)2, (0·05°/h)2]T. The process and measurement noise covariance are assumed to be Q k=diag [(50 μg) 2, (50 μg)2, (0·05°/h)2, (0·05°/h)2, (0·05°/h)2, 0,0,0,0,0]T, R v=diag[(0·1 m/s)2, (0·1 m/s)2]T. The system was evaluated using simulations and real tests with signal outages and predicted bias estimates. Figure 2 illustrates the estimation error comparisons for the reference filter in Li and Xia (Reference Li and Xia2012). The estimation errors are likely to diverge due to nonlinearities and the outage observations. Li and Xia gave an upper bound on the expected estimation error covariance and the conditions on the observation arrival probability for which the upper bound converges to a fixed point. For this reason, the critical arrival probability of the odometer observations update was set less than 10%, with the stability analysis of the reference filter showing the estimation error divergance. During signal outages, the GPR/SGQKF dealing with the outages measurements “GPR SGQKF” in Figure 2 was used to predict the signal changes for comparison with the “Li and Xia filter” (Li and Xia, Reference Li and Xia2012) method. Meanwhile the GPR/SGQKF bridging uncertain observation outages was compared with the SGQKF for the actual complete measurements (“actual SGQKF”). The RMSE of ∆v and ∆ϕ are shown in Figure 2 where
$$\Delta \delta V(k) = \sqrt {(V_{e,k} - \hat V_{e,k} )^2 + (V_{n,k} - \hat V_{n,k} )^2} $$
$${\rm and}\quad {\rm \Delta} \phi (k) = \sqrt {(\phi _{e,k} - \hat \phi _{e,k} )^2 + (\phi _{n,k} - \hat \phi _{n,k} )^2 + (\phi _{u,k} - \hat \phi _{u,k} )^2.} $$V k and ϕ k are the theory values, and 
$\hat V_k $ and 
$\hat \phi _k $ are the corresponding SINS/odometer values. The signal outages are 300 seconds long with 600 seconds simulation time in (a) and (b), 400 seconds long with 700 seconds simulation time in (c) and (d) respectively. The simulation results show that the reference filter diverged during the observation outages with the RMSE badly degraded, while the SGQKF combined with the GPR successfully resolved the filter divergence problem with the trajectory closest to the actual absolute observations.
Velocity and attitude angle errors in simulations to bridge observation outages. The signal outages are 300 seconds long with 600 seconds simulation time in (a) and (b). The signal outages are 400 seconds long with 700 seconds simulation time in (c) and (d).
The prediction method was also tested in a post processing offline road test model in a land vehicle. The sample time was 0·0152 s and the filter period was 1 s. The gyroscope repeatability of the drift was 0·1°/h and the accelerator repeatability bias was 1·0 mg. The odometer outages were from 320 to the 520 s during the 1450 s test. The results shown in Figure 3 include the trajectory and velocity errors, where
$$\delta V(k) = \sqrt {(V_{actual\,e,k} - \hat V_{e,k} )^2 + (V_{actual\,n,k} - \hat V_{n,k} )^2} $$Real road test vehicle velocity and trajectory to bridge uncertain observation outages.
V actual is the actual absolute measurement value, and 
$\hat V_k $ is the corresponding SINS/odometer value for measurement outages. The conclusions were consistent with the simulations, that the GPR/SGQKF method for coping with signal outages is close to the actual absolute measurements, thus the GPR models provide Gaussian uncertainty estimates for the predictions that allow them to be seamlessly incorporated into the SGQKF probabilistic filtering technique. This technique successfully alleviates trajectory and velocity divergence in estimate and maintains an accurate estimate of the SINS bias evolution during signal outages.
5. CONCLUSIONS
The GPR/SGQKF technique was developed to bridge uncertain observation outages in integrated navigation systems. The SGQKF employs non-linear dynamic models to estimate SINS/odometer evolution that has complex stochastic nonlinearities and uncertain noise estimates which degrade system performance during observation outages. The GPR learns the residual output using parametric models to generate predictive uncertainty SINS/odometer observations and noise covariance. During observation availability, the correct vehicle state, sensor measurements, and the SINS output deviations from the odometer are collected, while during observation outages the GPR is used with this data set to predict the SINS deviations from the odometer which enable the SGQKF to update the estimates of all the SINS biases. The techniques were tested in simulations and real road tests with comparison with actual complete measurements. The GPR combined with the SGQKF can successfully solve the filter divergence problem with uncertain observation outages. This approach can also be applied to other sensors having intermittent measurements in integrated navigation systems.
The system is quite effective, but needs additional developments. First, the GPR/ SGQKF technique does not depend on the measurements arriving at consistent intervals, but requires knowing when the observations are received. Only when observations are available for the most accurate state estimate, can the reliable measurements give a training data set for the GPR hyper-parameters learning. Secondly, this data set assembly process imposes some restrictions on the application due to computational challenges of learning the GPR hyper-parameters for large data sets. In recent years there have been many Sparse Pseudo-input Gaussian models developed to make sparse approximations to the full GP data set which are computationally tractable for large datasets but still preserve the desirable properties of the full GP. In addition, the GPR/SGQKF non-parametric models learned from the training data contain sequences of noise, observations, and vehicle truth states. The need for vehicle truth states limits the applicability of GPR/SGQKF to systems for which the vehicle truth can be estimated without excessive dependence. Many attempts have been introduced to overcome this limitation, such as Gaussian Process Latent Variable Models, GPBF-LEARN and online Sparse Gaussian Process Regression which are frameworks for training GP-Bayes filters without ground truth states. The future work will include implementing these into the model.
ACKNOWLEDGEMENT
This work was supported by the National Science Foundation of China (No. 61374215) and the Key Laboratory of Micro-Inertial Instrument and Advanced Navigation Technology of the Ministry of Education, China.
