A posteriori estimation of stochastic model for multi-sensor integrated inertial kinematic positioning and navigation on basis of variance component estimation

Abstract

Improving a priori stochastic models of the process and measurement noise vectors in Kalman Filer (KF) has always been a challenge. As one preferable technique to address this challenge, the variance component estimation (VCE) applied on the Kalman Filter’s process and measurement noise covariance matrix (Q & R) has been proved in plenty of applications. Unsurprisingly, VCE was expected to re-establish the stochastic model about the random errors in the IMU’s measurements in a multisensor integrated positioning and navigation system applying Kalman Filter. However, in the conventional error states-based GPS aided inertial navigation system (GPS/INS), the stochastic model tuning is difficult for the IMU’s measurements due to the amalgamation of the observables from inertial sensor and other aiding sensors. This paper proposes a generic method for the stochastic model tuning about the random errors in IMU measurements together with other sensors. The core of this novel approach is based on an innovative multisensor integration strategy which deploys upon the vehicle’s generic kinematic model and takes the IMU’s output as raw measurements in Kalman Filter (IMU/GNSS Kalman Filter). As a result, the statistical orthogonality between random error vectors of any two sensors enables the separate but parallel statistics collection of each individual random error source. Given these independent statistics corresponding to each error source, the VCE technique iteratively tunes all stochastic model of the process and measurement noise vectors. The success of the VCE algorithm is shown through a real dataset involving GPS and inertial sensors.

Introduction

The solution optimality of the Kalman filter (KF) relies on the appropriate stochastic model, which is commonly, also here specifically, about the variance-covariance (VC) matrices Q and R associated with the process and measurement noise vectors. The determination of these two covariance matrices in Kalman filter has been actively pursued by plenty of researchers since the advent of Kalman filter. Despite of the variation in numerous application-based algorithms, their methodologies can be roughly classified into two categories: VC matrix estimation (VCME) and variance component (scale factor) estimation (VCE). Mehra (1970, 1972) published his pioneering VCME work on the estimation of Q and R based on the system innovations in steady-state KF. Four relevant methods briefed in his work are: Bayesian, maximum likelihood, correlation and covariance matching. Mehra’s work has still been of important directive significance to various recent researches on VCME (Dunık and Šimandl 2008; Bavdekar, et al. 2011; Bulut et al. 2011; Matisko and Havlena 2013; etc.).

Alternatively, scale factor based estimation strategy is more attractive because of its computation effectiveness and reliable accuracy in the case where the matrix skeletons for Q and R are known. This type of the VCE methods in Kalman filter (VCE-KF) originated from the variance and covariance estimation in Least Squares (VCE-LS) (Helmert 1907). Along with the essential theoretical development, for instance, in (Förstner 1979; Grafarend, et al. 1980; Koch 1986; Ou 1989; Xu, et al. 2006; Amiri-Simkooei 2007; Teunissen and Amiri-Simkooei 2008; etc.), VCE-LS has been extensively used in practice (Sieg and Hirsch 2000; Wang and Rizos 2002; Tiberius and Kenselaar 2003; Rietdorf 2004; Tesmer 2004; Zhou, et al. 2006; Bähr et al. 2007; Gopaul, et al. 2010; Xiao and Wujiao 2014; etc.). In this category, a practical VCE algorithm was proposed based on following two discoveries (Wang 1997):

• Kalman filter is constructed epoch wise by applying the least squares principle, which utilizes all of the random information as three groups of statistically independent measurements: the predicted state vector as a group of pseudo-measurements, the zero mean process noise vector also as a group of pseudo-measurements, and the raw measurement vector, whose residual or correction vectors can directly be calculated as the projection of the system innovation vector.

• The redundancy distribution associated with the above mentioned three groups of measurements, and the redundant index for each of the individual independent measurements can be calculated epoch wise after the reliability theory transplanted from Least Squares into Kalman filter.

Given the redundancy contribution indexes and residuals for the individual variance components, the simplified VCE algorithm has accordingly been developed (Förstner 1979; Wang 1997; Wang 2009; etc.). For more details on its practical applications, one can refer to (Wang, et al. 2009; Gopaul, et al. 2010; Wang, et al. 2010; etc.).

However, in a traditional multisensor-aided inertial navigation system based on the error-state Kalman filter (so-called indirect Kalman filter), the variance component estimation technique depending on the measurement residuals is not applicable for individual sensor’s stochastic model tuning because the error measurements involve two sensors and their measurement residuals cannot be used to construct the stochastic model for individual sensor. This paper addresses the above-mentioned problem in the error-state-based INS system by taking the advantages of the novel multisensor integration strategy developed in (Qian et al. 2013, 2015; Wang et al. 2014) and succeeds in the stochastic model tuning for noise vectors, especially the posteriori stochastic model for the measurements from an IMU. The new integration strategy is designed to take a 3D kinematic trajectory model as the system model and therefore, allow applying the raw measurements from all of the sensors, inclusive of the IMUs, directly in measurement updates.

This paper is divided into six sections. This introduction is followed by Section VCE using redundancy contribution, which reviews variance component estimation using redundancy contribution in Least Square and Kalman Filter. Then, Section Multi-sensor integration for kinematic navigation and positioning using IMU/GNSS KF describes the direct Kalman filter used in full tightly-coupled multisensor integrated kinematic positioning and navigation. Section VCE realization under the novel multisensor integration strategy details the implementation of the VCE algorithm with certain discussions. The results from real road test data are provided to show the success of the proposed VCE algorithm in Section Road test results. Section Conclusion and future work concludes the paper.

Methods

The rigorous variance-covariance component estimation in Least Squares proposed by Helmert (1907) was simplified to a VCE algorithm based on measurement redundant contribution (Förstner 1979), which becomes popular in real applications (Cui et al. 2001; Bähr et al. 2007; etc.). Furthermore, Wang (1997) transplanted it into Kalman filter through an alternative derivation of the Kalman filtering algorithm by applying three groups of the independent random information: the purely-predicted state vector, the process noise vector and the real measurement vector. This derivation can project the system innovation vector into the residuals associated with these above used three groups of the measurements and also makes possible the calculation of their redundancy contribution. So, the VCE process as in (Förstner 1979) can be realized in Kalman filter. For the needs of further development, the relevant details will briefly be reviewed below.

VCE using redundancy contribution in LS

Let least square system is represented by

$$ L+v=B\delta \widehat{x}+F\left({x}^{(0)}\right) $$

(2.1)

where are

L :

the measurement vector

v :

the measurement residual vector

B :

the design matrix

F :

the nonlinear observation equations

x ⁽⁰⁾ :

the approximate of the parameter vector x

$\delta \widehat{x}$ :

the correction vector for x ⁽⁰⁾.

Assume that L consists of m statistically independent measurement types, (2.1) can be partitioned into

$$ \left[\begin{array}{c}\hfill {L}_1\hfill \\ {}\hfill \vdots \hfill \\ {}\hfill {L}_i\hfill \\ {}\hfill \vdots \hfill \\ {}\hfill {L}_m\hfill \end{array}\right]+\left[\begin{array}{c}\hfill {v}_1\hfill \\ {}\hfill \vdots \hfill \\ {}\hfill {v}_i\hfill \\ {}\hfill \vdots \hfill \\ {}\hfill {v}_m\hfill \end{array}\right]=\left[\begin{array}{c}\hfill {B}_1\hfill \\ {}\hfill \vdots \hfill \\ {}\hfill {B}_i\hfill \\ {}\hfill \vdots \hfill \\ {}\hfill {B}_m\hfill \end{array}\right]\delta \widehat{x}-\left[\begin{array}{c}\hfill {f}_1\left({x}^{(0)}\right)\hfill \\ {}\hfill \vdots \hfill \\ {}\hfill {f}_i\left({x}^{(0)}\right)\hfill \\ {}\hfill \vdots \hfill \\ {}\hfill {f}_m\left({x}^{(0)}\right)\hfill \end{array}\right] $$

(2.2)

where are

L _i :

n _i × 1 vector of the i-th type of the measurements

v _i :

the residual vector of L _i

B _i :

the design matrix associated with L _i

The measurement weight matrix is also grouped into

$$ P= diag\left(\begin{array}{ccccc}\hfill {P}_1\hfill & \hfill \cdots \hfill & \hfill {P}_i\hfill & \hfill \cdots \hfill & \hfill {P}_m\hfill \end{array}\right) $$

(2.3)

with its corresponding covariance matrix

$$ \begin{array}{l}D= diag\left(\begin{array}{ccccc}\hfill {D}_1\hfill & \hfill \cdots \hfill & \hfill {D}_i\hfill & \hfill \cdots \hfill & \hfill {D}_m\hfill \end{array}\right)\\ {}\kern1em = diag\left(\begin{array}{ccccc}\hfill {\sigma}_{01}^2{P}_1^{-1}\hfill & \hfill \cdots \hfill & \hfill {\sigma}_{0i}^2{P}_i^{-1}\hfill & \hfill \cdots \hfill & \hfill {\sigma}_{0m}^2{P}_m^{-1}\hfill \end{array}\right)\end{array} $$

(2.4)

where ${\sigma}_{0i}^2\left(1, \dots,\ m\right)$ is the i-th variance component (variance factor) of the unit weight to be estimated for the i-th group of the measurements (Cui, et al. 2001).

Under the assumption of ${v}_i\sim N\left(0,\ {D}_{v_i}\right)$, the expectation of the weighted residual sum of squares is (Cui, et al. 2001):

$$ \begin{array}{l}E\left({v}_i^T{P}_i{v}_i\right)=\left({n}_i-2tr\left({N}^{-1}{N}_i\right)+tr{\left({N}^{-1}{N}_i\right)}^2\right){\sigma}_{0i}^2\\ {}\kern5.25em +{\displaystyle \sum_{j=1,j\ne i}^m\left\{tr\left({N}^{-1}{N}_i{N}^{-1}{N}_j\right){\sigma}_{0j}^2\right\}}\end{array} $$

(2.5)

with N = B ^T PB, and ${N}_i={B}_i^T{P}_i{B}_i$ (i = 1, 2, …, m). The rigorous solution for ${\sigma}_{0i}^2\left(1, \dots,\ m\right)$ can be delivered by solving the m dimensional equation system. There have been multiple simplified algorithms, of which one is

$$ E\left({v}_i^T{P}_i{v}_i\right)={\sigma}_{0i}^2\left({n}_i-tr\left({N}^{-1}{N}_i\right)\right) $$

(2.6)

by assuming that ${\sigma}_{01}^2={\sigma}_{02}^2=\cdots ={\sigma}_{0m}^2={\sigma}_{0i}^2$ in (2.5). Furthermore, provided that n _i  − tr(N ^− 1 N _i ) = r _i , the practical estimation of ${\sigma}_{0i}^2\left(1, \dots,\ m\right)$ is reduced to (Förstner 1979)

$$ {\widehat{\sigma}}_{0i}^2={v}_i^T{P}_i{v}_i/{r}_i $$

(2.7)

where r _i is the total redundancy contribution that reflects the extent of the influence of L _i on the parameter estimation. The bigger r _i is, the less L _i affects the parameter estimation. The number of the measurements in a group can be one or more. With a group of independent measurements, the redundant index of each measurement satisfies 0 < r _i  < 1. When r _i  = 1, the measurement is completely redundant. It becomes a high leverage measurement in case r _i tends to zero.

The alternative derivation of Kalman filter

Let the linear or linearized system described by KF at time t _k be

$$ \begin{array}{cc}\hfill {x}_k={\varPhi}_k{x}_{k-1}+{\varLambda}_k{w}_k\hfill & \hfill \left(\mathrm{system}\ \mathrm{model}\right)\hfill \end{array} $$

(2.8)

$$ \begin{array}{cc}\hfill {z}_k={H}_k{x}_k+{\varepsilon}_k\hfill & \hfill \left(\mathrm{measurement}\ \mathrm{model}\right)\hfill \end{array} $$

(2.9)

where is

x _k :

the n _x × 1 state vector

w _k :

the n _w × 1 process noise vector

Λ _k :

the coefficient matrix of w _k

z _k :

the n _z × 1 measurement vector

ε _k :

the measurement noise vector

Φ _k :

the state transition matrix

H _k :

the design matrix

with w _k  ~ N(0, Q _w ), ε _k  ~ N(0, R _k ), where w _k and ε _k are uncorrelated with each other and themselves from epoch to epoch.

By considering three independent groups of the measurements and pseudo-measurements at an arbitrary epoch k (Wang 1997):

• (1) the raw measurement vector l _z  = z _k with its variance matrix ${D}_{l_z}={R}_k$,

• (2) the zero mean process noise vector w _k as a pseudo- measurement vector l _w  = w _k with its variance matrix ${D}_{l_w}={Q}_k$,

• (3) the predicted state vector x _{k/k − 1} from the previous epoch as another pseudo-measurement vector l _w  = x _{k/k − 1} = Φ _k x _k − 1 with its variance matrix ${D}_{l_x}={\varPhi}_k{D}_{x_{x-1}}{\varPhi}_k^T$, where ${D}_{x_{x-1}}$ is the variance matrix of x _k − 1,

a measurement equation system can be constructed for Kalman filter algorithm at epoch k as follow:

$$ \left[\begin{array}{c}\hfill {v}_z\hfill \\ {}\hfill {v}_w\hfill \\ {}\hfill {v}_x\hfill \end{array}\right]=\left[\begin{array}{cc}\hfill {H}_k\hfill & \hfill O\hfill \\ {}\hfill O\hfill & \hfill I\hfill \\ {}\hfill I\hfill & \hfill -{\varLambda}_k\hfill \end{array}\right]\ \left[\begin{array}{c}\hfill \widehat{x}\hfill \\ {}\hfill \widehat{w}\hfill \end{array}\right]-\left[\begin{array}{c}\hfill {l}_z\hfill \\ {}\hfill {l}_w\hfill \\ {}\hfill {l}_x\hfill \end{array}\right] $$

(2.10)

to which the Least Squares Principle can be applied to derive the identical solution for Kalman filter (Wang 1997; Caspary and Wang 1998).

One of the significant contributions made by this alternate derivation of KF was about to handle the process noise vector separately, which has made possible the simultaneous estimation of the variance components associated with the process noise vector w _k and the measurement noise vector ε _k .

Global VCE for Q and R in KF using redundancy contribution

Because of the solution equivalence between LS and KF as summarized in 2.2, we are intuitively inspired to realize the most popular simplified VCE technique (Forstner’s method) in our novel multisensor integrated kinematic positioning and navigation (Qian, et al. 2013, 2015; Wang et al. 2014). Wang (1997) proved that the measurement residual vectors for three independent measurement groups at epoch k as in (2.10) can be computed based on the same innovation vector as follow:

$$ \begin{array}{c}\hfill {v}_z=\left(I-{H}_kK\right)\left({z}_k-{H}_k{x}_{k/k-1}\right)\hfill \\ {}\hfill {v}_w={Q}_k{\varLambda}_k^T{D}_{x_{k/k-1}}^{-1}K\left({z}_k-{H}_k{x}_{k/k-1}\right)\hfill \\ {}\hfill {v}_x={\varPhi}_k{D}_{k-1}{\varPhi}_k^T{D}_{x_{k/k-1}}^{-1}K\left({z}_k-{H}_k{x}_{k/k-1}\right)\hfill \end{array} $$

(2.11)

where K is the Kalman gain matrix at epoch k, the covariance matrix of the predicted state x _{k/k − 1} is ${D}_{x_{k/k-1}}={\varPhi}_k{D}_{x_{x-1}}{\varPhi}_k^T+{\varLambda}_k{Q}_k{\varLambda}_k^T$, and the innovation vector d = z _k  − H _k x _{k/k − 1}.

Consequently, three residual vectors (v _z , v _w , v _x ) for three measurement vectors (l _z , l _w , l _x ) are actually correlated with each other through the same innovation vector. In addition, the corresponding redundancy indices for each measurement group are (Wang 1997):

$$ \begin{array}{l}{r}_z=tr\left(I-{H}_kK\right)\hfill \\ {}{r}_w=tr\left({Q}_k{\varLambda}_k^T{H}_k^T{D}_d^{-1}{H}_k{\varLambda}_k\right)\hfill \\ {}{r}_x=tr\left({\varPhi}_k{D}_{x_{k-1}}{\varPhi}_k^T{H}_k^T{D}_d^{-1}{H}_K\right)\hfill \end{array} $$

(2.12)

wherein the total system redundancy n _z = r _z + r _w + r _x, and the covariance matrix of the innovation vector is denoted by ${D}_d={R}_k+{H}_k{D}_{x_{k/k-1}}{H}_k^T$. Thus, for an arbitrary epoch k, the variance factors of the three measurement groups defined in (2.10) can be estimated as follow:

$$ {\sigma}_{0j}^2(k)={v}_j^T{D}_{l_j}^{-1}{v}_j/{r}_j\kern0.75em \left(j=z,w,x\right) $$

(2.13)

As for a global variance component estimate over a specific or the whole time duration, a simple accumulation can obtain a reliable estimate due to the cross-epoch-orthogonal properties of the measurement residuals (Wang 1997). For example, the global variance component estimate up to epoch k is:

$$ {\sigma}_{0j}^2\left(k\Big|1\dots k\right)=\frac{{\displaystyle \sum_{i=1}^k{v}_{j_i}^T{D}_{l_j}^{-1}{v}_{j_i}}}{{\displaystyle \sum_{i=1}^k{r}_{j_i}}}\kern0.75em \left(j=z,w,x\right) $$

(2.14)

Commonly, the components in w _k and ε _k are modeled as uncorrelated so that Q _k and R _k become diagonal. As a result, the redundant index for each independent component in either w _k or ε _k is given by

$$ {r}_z^{i_z}=1-{\left({H}_kK\right)}_{i_z{i}_z} $$

(2.15)

$$ {r}_w^{i_w}={\left({Q}_k{\varLambda}_k^T{H}_k^T{D}_d^{-1}{H}_k{\varLambda}_k\right)}_{i_w{i}_w} $$

(2.16)

Accordingly, the individual variance component in vector w _k or ε _k can be estimated by analogy with (2.13) and (2.14).

Discussion

In traditional error-state based inertial navigation system (e.g., Rogers 2003; Bekir 2007), the variance component estimation for the IMU measurements is difficult because the error measurement vector blends the errors from an inertial sensor and other aiding sensors. However, this dilemma is addressed through a novel multisensor integration strategy (Wang 1997; Wang and Sternberg 2000; Qian, et al. 2013, 2015; Wang, et al. 2014), whose major features in comparison with the transnational aided inertial navigation are summarized below (Fig. 1):

• 1) Deployment of the vehicle’s kinematic model for the kinematic states, i.e., the velocity, acceleration, attitude and angular rate vectors as the core of the system model in Kalman filter,

• 2) Utilization of direct Kalman filter instead of indirect Kalman filter,

• 3) Application of all of the multisensor measurements through the measurement update in Kalman filter, especially the use of the IMU measurements instead of their use only through the free inertial navigation calculation between two aiding epochs in the traditional inertial navigation.

Fig. 1

Integration mechanism comparison between the traditional and novel integration strategies (Qian, et al. 2015)

Thus, the novel multisensor integration strategy allows for equal treatment of all the sensors no matter which sensor is used as an aiding sensor. The structure of Kalman filter, e.g., for measurement models of IMU and GNSS receivers, is briefly introduced below.

State vector

Here, the state vector x, specifically for IMU/GNSS KF, may consist of 27 components:

$$ {\left[\begin{array}{ccccccccc}\hfill {\mathrm{r}}^T\hfill & \hfill {\left({\mathrm{v}}_{nb}^b\right)}^T\hfill & \hfill {\left({\mathrm{a}}_{nb}^b\right)}^T\hfill & \hfill {\uptheta}^T\hfill & \hfill {\left({\upomega}_{nb}^b\right)}^T\hfill & \hfill {\mathrm{b}}_g^T\hfill & \hfill {\mathrm{b}}_a^T\hfill & \hfill {\mathrm{s}}_g^T\hfill & \hfill {\mathrm{s}}_a^T\hfill \end{array}\right]}^T $$

(3.1)

with

$$ \begin{array}{l}\begin{array}{ccccc}\hfill \mathbf{r}=\left(\begin{array}{c}\hfill X\hfill \\ {}\hfill Y\hfill \\ {}\hfill Z\hfill \end{array}\right),\hfill & \hfill {\mathbf{v}}_{nb}^b=\left(\begin{array}{c}\hfill {v}_{nbx}^b\hfill \\ {}\hfill {v}_{nby}^b\hfill \\ {}\hfill {v}_{nbz}^b\hfill \end{array}\right),\hfill & \hfill {\mathbf{a}}_{nb}^b = \left(\begin{array}{c}\hfill {a}_{nbx}^b\hfill \\ {}\hfill {a}_{nby}^b\hfill \\ {}\hfill {a}_{nbz}^b\hfill \end{array}\right),\hfill & \hfill \boldsymbol{\uptheta} =\left(\begin{array}{c}\hfill \gamma \hfill \\ {}\hfill p\hfill \\ {}\hfill \alpha \hfill \end{array}\right),\hfill & \hfill {\boldsymbol{\upomega}}_{nb}^b=\left(\begin{array}{c}\hfill {\omega}_{nbx}^b\hfill \\ {}\hfill {\omega}_{nby}^b\hfill \\ {}\hfill {\omega}_{nbz}^b\hfill \end{array}\right)\hfill \end{array}\\ {}\begin{array}{cccc}\hfill {\mathbf{b}}_g=\left(\begin{array}{c}\hfill {b}_{gx}\hfill \\ {}\hfill {b}_{gy}\hfill \\ {}\hfill {b}_{gz}\hfill \end{array}\right),\hfill & \hfill {\mathbf{b}}_a=\left(\begin{array}{c}\hfill {b}_{ax}\hfill \\ {}\hfill {b}_{ay}\hfill \\ {}\hfill {b}_{az}\hfill \end{array}\right),\hfill & \hfill {\mathbf{s}}_g=\left(\begin{array}{c}\hfill {s}_{gx}\hfill \\ {}\hfill {s}_{gy}\hfill \\ {}\hfill {s}_{gz}\hfill \end{array}\right),\hfill & \hfill {\mathbf{s}}_a=\left(\begin{array}{c}\hfill {s}_{ax}\hfill \\ {}\hfill {s}_{ay}\hfill \\ {}\hfill {s}_{az}\hfill \end{array}\right)\hfill \end{array}\end{array} $$

wherein are

r :

the position vector in the navigation frame

$\begin{array}{ccc}\hfill {\mathbf{v}}_{nb}^b,\hfill & \hfill {\mathbf{a}}_{nb}^b,\hfill & \hfill {\boldsymbol{\upomega}}_{nb}^b\hfill \end{array}$ :

the velocity, acceleration and angular rate vector in body frame

$\begin{array}{ccc}\hfill \uptheta =\Big[p\hfill & \hfill \gamma \hfill & \hfill \psi \hfill \end{array}\Big]{}^T$ :

Euler angle vector (pitch, roll, heading)

$\begin{array}{cc}\hfill {\mathbf{b}}_g,\hfill & \hfill {\mathbf{b}}_a\hfill \end{array}$ :

the gyro and accelerometer’s bias vector

$\begin{array}{cc}\hfill {\mathrm{s}}_g,\hfill & \hfill {\mathrm{s}}_a\hfill \end{array}$ :

the gyro and accelerometer’s scale factors.

Measurement equations for GNSS observables

Three types of GPS measurements, the double differenced (DD) pseudo-ranges and carrier phases (L1 and L2), and range rates (Doppler velocity) are used in IMU/GNSS KF:

$$ \nabla \varDelta P{R}_{AB}^{jk}(t)\kern0.5em =\nabla \varDelta {\rho}_{AB}^{jk}(t)+{\varepsilon}_{\nabla \varDelta {P}_{AB}}^{jk}(t) $$

(3.2)

$$ \nabla \varDelta {\phi}_{AB}^{jk}(t)=\nabla \varDelta {\rho}_{AB}^{jk}(t)+{\lambda}_1\nabla \varDelta {N}_{AB}^{jk}+{\varepsilon}_{AB}^{jk}(t) $$

(3.3)

$$ \nabla \varDelta Do{p}_{AB}^{jk}(t)\kern0.5em =\nabla \varDelta \overset{\bullet }{\rho_{AB}^{jk}}(t)+{\varepsilon}_{\nabla \varDelta Do{p}_{AB}}^{jk}(t) $$

(3.4)

where ∇Δ is the double differencing operator on GNSS observables between station A and B for satellite i and j. For instance, one has

$$ \nabla \varDelta {\rho}_{AB}^{jk}=\left[{\rho}_B^k(t)-{\rho}_A^k(t)\right]-\left[{\rho}_B^j(t)-{\rho}_A^j(t)\right]. $$

Furthermore, the measurement noises of the DD pseudo-ranges, carrier phases and range rates are denoted by ${\varepsilon}_{\nabla \varDelta {P}_{AB}}^{jk}$, ${\varepsilon}_{\nabla \varDelta {\phi}_{AB}}^{jk}$ and ${\varepsilon}_{\nabla \varDelta Do{p}_{AB}}^{jk}$. Certainly, the ambiguity parameters $\nabla \varDelta {N}_{AB}^{jk}$ will also be added to the state vector in (3.1) before they can be fixed as integer numbers.

Measurement equation for GPS heading measurement

With the use of low-cost IMUs in multisensor integrated kinematic positioning and navigation, the extra heading measurement is always needed in order to supress the fast heading drift. In the IMU/GNSS KF implemented in this paper, the vehicle heading measurement is made available through the short baseline solution between two GPS rovers on the vehicle’s roof in navigation frame. The short baseline is achieved through N (N ≥ 5) DD L1 phase measurements with their fixed integer ambiguities. The DGPS heading measurement is simply modeled as:

$$ {l}_h=\alpha +{v}_H $$

(3.5)

where are

l _h :

the DGPS heading measurement

α :

the heading state

ε _h :

the heading measurement noise.

Measurement equations for IMU measurements

Without loss of the generality, three cross-orthogonal gyros in an IMU unit measure the angular rate vector ${\boldsymbol{\upomega}}_{ib}^b$ while three accelerometers simultaneously measure the specific force vector ${\mathbf{f}}_{ib}^b$. A generic assumption about inertial sensors is that both of ${\boldsymbol{\upomega}}_{ib}^b$ and ${\mathbf{f}}_{ib}^b$ mainly suffer from the following errors: constant start-off biases, drifting bias residuals, linear scale factors, misalignments of sensor axes and random withe noise. Accordingly, the IMU measurements in the being constructed KF are modelled as follow (Qian, et al. 2013):

$$ {\boldsymbol{\upomega}}_{ib-imu}^b=\left(I+{\mathbf{S}}_g\right){\boldsymbol{\upomega}}_{nb}^b+{\mathbf{C}}_n^b\left({\boldsymbol{\upomega}}_{ie}^n+{\boldsymbol{\upomega}}_{en}^n\right)+{\mathbf{b}}_g+{\boldsymbol{\upvarepsilon}}_g $$

(3.6)

$$ \begin{array}{l}{\mathbf{f}}_{ib-imu}^b=\left(I+{\mathbf{S}}_a\right){\mathbf{a}}_{nb}^b+\left(2{\boldsymbol{\upomega}}_{ie}^b+{\boldsymbol{\upomega}}_{en}^b\right)\times {\mathbf{v}}_{nb}^b\\ {}\kern3.25em -{\mathbf{C}}_n^b{\mathbf{g}}^n+{\mathbf{b}}_a+{\boldsymbol{\upvarepsilon}}_a\end{array} $$

(3.7)

wherein are

${\upomega}_{ie}^n$ :

the Earth’s rotation rate in the local ENU navigation frame

${\upomega}_{en}^n$ :

the craft rate in the local ENU navigation frame

${\upomega}_{nb}^n$ :

the rotation rate of IMU body frame with respect to the local ENU frame

${\mathrm{a}}_{nb}^n$ :

the acceleration of IMU body frame with respect to the local ENU frame

b _g :

the bias vector including start off biases and bias residuals for gyroscopes

b _a :

the bias vector including start off biases and bias residuals for accelerometers

S _g :

the scalar and misalignment error matrix of size 3 × 3 for gyroscopes

S _a :

the scalar and misalignment error matrix of size 3 × 3 for accelerometers

ε _g :

the Gaussian white noise vector for gyroscope measurements, and

ε _a :

the Gaussian white noise vector for accelerometer measurements.

${\mathrm{C}}_n^b$ :

Direction cosine matrix that transforms the vector from navigation frame to IMU body frame.

In (3.6) and (3.7), the mechanical installation errors (misalignment of sensor axes) are ignored due to their considerably small influence on the state vector in comparison with other major errors in the low-cost IMU used in our land vehicle system.

VCE realization under the novel multisensor integration strategy

Variance components for the process noise vector in IMU/GNSS KF

After the omission of the high order terms in the trajectory model, the discrete system equations are as follows:

$$ \begin{array}{c}\hfill {\boldsymbol{r}}_{k+1}={\boldsymbol{r}}_k+{C}_{n(k)}^n\left[\boldsymbol{I}+\left(\left[{\boldsymbol{\omega}}_{nb}^n\times \right]+\left[{\overset{.}{\boldsymbol{\omega}}}_{nb}^b\times \right]\varDelta t\right)\varDelta t+\left[{\boldsymbol{\omega}}_{nb}^b\times \right]\left[{\boldsymbol{\omega}}_{nb}^b\times \right]\frac{\varDelta {t}^{\mathtt{2}}}{\mathtt{2}}\right]\hfill \\ {}\hfill \left({\boldsymbol{v}}_{nb(k)}^b\varDelta t+\frac{\varDelta {t}^{\mathtt{2}}}{\mathtt{2}}{\boldsymbol{a}}_{nb(k)}^b+\frac{\varDelta {t}^{\mathtt{3}}}{\mathtt{6}}{\boldsymbol{j}}_{nb}^b\right)\hfill \end{array} $$

$$ \begin{array}{c}\hfill {\mathbf{v}}_{nb\left(k+1\right)}^b=\left[{I}_{3\times 3}-\varDelta t\left[{\boldsymbol{\upomega}}_{nb(k)}^b\times \right]+\frac{\varDelta {t}^{\mathtt{2}}}{\mathtt{2}}{\left[{\boldsymbol{\upomega}}_{nb(k)}^b\times \right]}^{\mathtt{2}}\right]{\mathbf{v}}_{nb(k)}^b\hfill \\ {}\hfill +\left[\varDelta t{\mathbf{I}}_{3\times 3}-\varDelta {t}^{\mathtt{2}}\left[{\boldsymbol{\upomega}}_{nb(k)}^b\times \right]\right]{\mathbf{a}}_{nb(k)}^b\hfill \\ {}\hfill +\frac{\varDelta {t}^{\mathtt{2}}}{\mathtt{2}}\left[{\mathbf{v}}_{nb(k)}^b\times \right]\overset{\bullet }{{\boldsymbol{\upomega}}_{nb}^b}+\frac{\varDelta {t}^{\mathtt{2}}}{\mathtt{2}}{\mathrm{j}}_{nb}^b\hfill \end{array} $$

$$ \begin{array}{l}{{\mathbf{a}}_{nb}^b}_{\left(k+1\right)}=\left[{I}_{3x3}-\varDelta t\ \left[{\boldsymbol{\upomega}}_{nb(k)}^b\times \right]+\frac{\varDelta {t}^2}{2}{\left[{\boldsymbol{\upomega}}_{nb(k)}^b\times \right]}^2\right]{\mathbf{a}}_{nb(k)}^b\\ {}\kern4.25em +\frac{\varDelta {t}^2}{2}\left[{\mathbf{a}}_{nb(k)}^b\times \right]{\overset{.}{\boldsymbol{\upomega}}}_{nb}^b+\varDelta t\ {\mathbf{j}}_{nb}^b\end{array} $$

$$ \begin{array}{c}\hfill {\boldsymbol{\uptheta}}_{k+1}={\boldsymbol{\uptheta}}_k+\varDelta t{\mathbf{C}}_{3\times 3}{\boldsymbol{\upomega}}_{nb(k)}^b+{\scriptscriptstyle \frac{\varDelta {t}^2}{2}}{\mathbf{C}}_{3\times 3}{\overset{.}{\boldsymbol{\upomega}}}_{nb}^b\hfill \\ {}\hfill {\boldsymbol{\upomega}}_{nb\left(k+1\right)}^b={\boldsymbol{\upomega}}_{nb(k)}^b+{\overset{.}{\boldsymbol{\upomega}}}_{nb}^b\hfill \\ {}\hfill {\mathbf{b}}_{g\left(k+1\right)}={\mathbf{b}}_{g(k)}+{\mathbf{w}}_{b_g}\hfill \\ {}\hfill {\mathbf{b}}_{a\left(k+1\right)}={\mathbf{b}}_{a(k)}+{\mathbf{w}}_{b_a}\hfill \\ {}\hfill {\mathbf{s}}_{g\left(k+1\right)}=\kern0.5em {\mathbf{s}}_{g(k)}+{\mathbf{w}}_{s_g}\hfill \\ {}\hfill {\mathbf{s}}_{a\left(k+1\right)}=\kern0.5em {\mathbf{s}}_{a(k)}+{\mathbf{w}}_{s_a}\hfill \end{array} $$

wherein all of ${\mathbf{j}}_{nb}^b$, ${\overset{.}{\boldsymbol{\upomega}}}_{nb}^b$, ${\mathbf{w}}_{b_g}$, ${\mathbf{w}}_{b_a}$, ${\mathbf{w}}_{s_g}$ and ${\mathbf{w}}_{s_a}$ are 3 × 1 process noise vectors. Correspondingly, ${\mathbf{j}}_{nb}^b$ and ${\overset{.}{\boldsymbol{\upomega}}}_{nb}^b$ are jerk and angular acceleration vector in the vehicle’s body frame. Herein, all components in the process noise vector are considered as uncorrelated, i.e., Q is diagonal. So, there are 18 variance factors associated with the process noise vector in total.

Variance components for the measurement vector in IMU/GNSS KF

Measurement noise model for GNSS observables

Measurement noise model for the double differenced GNSS measurements has been studied for years (Collins and Langley 1999; Wieser et al. 2005; Luo et al. 2009; Wang et al. 2013; Takasu 2013). The commonly applied approach is that the measurement variance changes along with the elevation angle of the satellite.

In this paper, the variance of a single GNSS observation is modelled as the function of the elevation angle ${\beta}_A^j$ of the line of sight from station A to satellite j and the a priori receiver noise ${\sigma}_{90^{\circ}}^2$ at station’s zenith (Takasu 2013):

$$ {\sigma}_{A,j}^2={\sigma}_{90^{\circ}}^2\left[1.0+\frac{1.0}{ \sin {\left({\beta}_A^j\right)}^2}\right] $$

In addition, the double differenced GNSS observables with short distance between rover and base station (<10 Km) are reasonably considered to be atmospherically error free and have no significant multipath effect. Correspondingly, the covariance of n double-differenced GNSS observation in the case of short baseline shall be

$$ {\sigma}_{\varDelta \nabla}^2=\left[\begin{array}{cc}\hfill {\sigma}_{A,j}^2+{\sigma}_{B,j}^2+{\sigma}_{A,1}^2+{\sigma}_{B,1}^2\hfill & \hfill {\sigma}_{A,j}^2+{\sigma}_{B,j}^2\hfill \\ {}\hfill {\sigma}_{A,j}^2+{\sigma}_{B,j}^2\hfill & \hfill {\sigma}_{A,j}^2+{\sigma}_{B,j}^2+{\sigma}_{A,2}^2+{\sigma}_{B,2}^2\hfill \\ {}\hfill \vdots \hfill & \hfill \vdots \hfill \\ {}\hfill {\sigma}_{A,j}^2+{\sigma}_{B,j}^2\hfill & \hfill {\sigma}_{A,j}^2+{\sigma}_{B,j}^2\hfill \end{array}\right. $$

$$ \left.\begin{array}{cc}\hfill \cdots \hfill & \hfill {\sigma}_{A,j}^2+{\sigma}_{B,j}^2\hfill \\ {}\hfill \cdots \hfill & \hfill {\sigma}_{A,j}^2+{\sigma}_{B,j}^2\hfill \\ {}\hfill \vdots \hfill & \hfill \vdots \hfill \\ {}\hfill \cdots \hfill & \hfill {\sigma}_{A,j}^2+{\sigma}_{B,j}^2+{\sigma}_{A,n}^2+{\sigma}_{B,n}^2\hfill \end{array}\right] $$

where A and B are the reference station and the rover station, respectively, satellite j is taken as the reference satellite, ${\sigma}_{A,j}^2$ and ${\sigma}_{B,j}^2$ are the variances for the measurements to the reference satellite j while ${\sigma}_{A,k}^2$ and ${\sigma}_{B,k}^2$ (k = 1, …, n; k ≠ j) are the variances for the measurements to the individually locked satellites.

As a result, five variance components for three independent types of the double differenced GNSS observations and the DGPS heading observation will be estimated:

$$ {\upsigma}_{GNSS}^2(k)=\left[\begin{array}{ccccc}\hfill {\upsigma}_P^2\hfill & \hfill {\upsigma}_{Dop}^2\hfill & \hfill {\upsigma}_{L1}^2\hfill & \hfill {\upsigma}_{L2}^2\hfill & \hfill {\upsigma}_{Heading}^2\hfill \end{array}\ \right] $$

where is

${\sigma}_P^2$ :

variance component for pseudo-range measurement at zenith

${\sigma}_{Dop}^2$ :

variance component for Doppler velocity measurement at zenith

${\sigma}_{L1}^2$ :

variance component for L1 carrier phase measurement at zenith.

${\sigma}_{L2}^2$ :

variance component for L2 carrier phase measurement at zenith. and

${\sigma}_h^2$ :

variance component for DGPS heading measurement.

Measurement noise model for IMU measurements

As introduced in Multi-sensor integration for kinematic navigation and positioning using IMU/GNSS KF section, the novel IMU/GNSS integration strategy allows to apply the IMU measurements directly through KF measurement update. This significant change on multisensor integration strategy makes possible estimate the variance components for individual IMU measurements about their random noises for the very first time.

Practically, the random errors on the measurements from three gyros and three accelerometers can be considered statistically independent after ignoring the small timing error and tiny installation error compared to the other error sources. Correspondingly, the covariance matrix for these six independent measurements will be a 6 × 6 diagonal matrix. The variance component vector for the IMU measurements is estimated as:

$$ {\upsigma}_{IMU}^2(k)={\left[\begin{array}{cccccc}\hfill {\upsigma}_{g(x)}^2\hfill & \hfill {\upsigma}_{g(y)}^2\hfill & \hfill {\upsigma}_{g(z)}^2\hfill & \hfill {\upsigma}_{a(x)}^2\hfill & \hfill {\upsigma}_{a(y)}^2\hfill & \hfill {\upsigma}_{a(z)}^2\hfill \end{array}\right]}^T $$

Implementation discussion

Weakly observable process noise components

It has been found that small redundancy indices may result in the divergence of the estimates of the corresponding variance components. In IMU/GNSS KF, the redundancy contribution indices for the IMU bias drifts and scale factor drifts are quite small with the test data acquired by our land vehicle. A similar situation was given by Wang et al. (2010). In order to avoid a potential divergence, twelve variance components for four weakly estimable process noise sub vectors: w _bg, w _ba, w _sg and w _sa and are fixed in the VCE process.

Iterative VCE process

The proposed iterative VCE process starts from the initial variance matrices of Q ⁽⁰⁾ and R ⁽⁰⁾ associated with the process and measurement noise vector, respectively. The successively obtained redundant contribution and the weighted residual sum of squares for each variance component are accumulated epoch wise while the KF runs forward. In order to achieve a convergent solution, the variance components estimates after i-th round are cherry-picked as the initial value for the (i + 1)-th iteration. In this paper’s implementation, the sifting rule is simply based on the redundancy contribution index (r). Given small r (<0.1) for the i -th group of the measurements, the corresponding variance component ${\sigma}_i^2$ will not be applied for the next round. For example, if X axis gyro’s redundancy contribution index is 0.029 in the current iteration while its standard deviation is 0.5 deg/s, the standard deviation will remain to be 0.5 deg/s in the next iteration (Fig. 2).

Fig. 2

Flow chart of VCE in IMU/GNSS KF

Results

Overview

The road test presented in this section, was performed on April 1, 2012 using the York University Multisensor Integrated kinematic positioning and navigation System (YUMIS) developed at the Earth Observation Laboratory of York University (Qian et al. 2012).

YUMIS system consists of Crossbow IMU440CA (MEMS) IMU, two NovAtel GPS receivers and a Linux/RTAI based desktop. The IMU data were collected at 100 Hz and the measurements from two GPS rovers were acquired at 4Hz. Besides, the third GPS receiver was set up as a base station near the start point. Figure 3 gives the top view of the land vehicle’s trajectory and the velocity profile.

Fig. 3

Top view of the trajectory and the speed profile

VCE results and analysis

The empirical initial variances and their iterative estimates for the measurements and the process noise factors are plotted in Figs 4, 5, 6, 7, 8, 9 and 10.

Fig. 4

Iterative VCE results for the jerks

Fig. 5

Iterative VCE results for the angular accelerations

Fig. 6

Iterative VCE results for gyro measurements

Fig. 7

Iterative VCE results for the accelerometer measurements

Fig. 8

Iterative VCE results for the DD carrier phase measurements

Fig. 9

Iterative VCE results for the DD pseudo-ranges

Fig. 10

Iterative VCE results for DGPS heading measurement noise

The variances with the process noise factors

Figures 4 and 5 show the global VCE estimates of kinematic process noise component: jerk vector (${\mathbf{j}}_{nb}^b$) and angular acceleration vector (${\overset{.}{\boldsymbol{\upomega}}}_{nb}^b$) in the vehicle’s body frame. As can be seen, the estimates are convergent at the 4^th iteration.

However, it is noted that the global VCE results for jerks and angular accelerations achieved from this dataset around 10 min are definitely not universal to describe the generic land vehicle’s dynamics because this specific trajectory was mainly in straight line motion on the flat road and only experienced three sharp turns. Therefore, the results based on this specific short trajectory were used only to demonstrate the success of the proposed VCE algorithm in the multisensor integrated kinematic positioning and navigation. And further detailed research will be performed for practical utilization of the proposed VCE technique.

The variances with the IMU measurements

The initial variances for gyro measurements at the 2^nd iteration remained at the 1^st iteration (Fig. 6) as their redundant indices were relatively small (<0.1) due to the relative large initial angular acceleration process noise due to the large initial angular acceleration process noise (300 ⁰/s²).

Similarly, due to the weak redundancy contribution (<0.1), the measurement noise estimate for the accelerometer measurement at Y axis remained unchanged after the 2^nd iteration as well (Fig. 7). Therefore, its estimates will be maintained after the 2^nd iteration.

The variances with the GPS measurements

Figures 8 and 9 give the iterative VCE estimates for the GPS measurements.

In order to avoid the unstable quality of the raw Doppler velocity measurements, the time differenced L1 carrier phases were substituted for the double differenced Doppler measurements in IMU/GNSS KF. Accordingly, the accuracy of the Doppler velocity measurements is directly related to L1 carrier phase accuracy. As a result, the variance component estimation of the Doppler velocity measurements is skipped because its variance is easily achieved through error propagation.

The variance DGPS heading measurement noise

The DGPS heading measurements were derived from two GPS rovers on the vehicle’s roof using their L1 carrier phases. In general, the variance estimation for this heading measurement should be completed in more robust way directly involving the raw L1 carrier phase observables. However, in our implementation, the DGPS heading measurements were only valid when at least 5 L1 carrier phases reached the fixed ambiguities in order to guarantee a stable heading accuracy. Thus, the variance of the DGPS heading could be simplified through single numerical value. Correspondingly, the single variance factor is applicable to the DGPS heading measurement in IMU/GNSS KF.

Performance comparison and analysis

Navigation solution comparison

Figures 11 and 12 show the position and attitude differences between the 1^st and 7^th VCE iteration as well as the individual position solution’s ± 3σ bounds (1^st in green and 7^th in red).

Fig. 11

Position solution comparison (ENU frame)

Fig. 12

Pitch and roll solution comparison

First, one can conclude that there was no statistical inconsistency between two solutions because the differenced navigation state vectors were always within their 3σ envelops.

In addition, the consistent smaller (finer) navigation solution standard deviation in the 7^th iteration compared to the initial navigation solution verifies the positive influences of VCE effort on IMU/GNSS KF, which is due to the scaled-down Q and R from the conservatively large initial values.

Analysis of the IMU Measurement residuals

Due to fact that the variance component estimation of the IMU measurements have been realized practically in the multisensor integrated navigation system, more discussions will be performed here. Because the land vehicle’s dynamics is usually exposed in vertical (Z) axial gyro and the forward (Y) axial accelerometer, the variance estimates for their observables theoretically cast more significant influences on their measurement residuals than the ones in the other axes. Figure 12 shows the normalized histograms of their measurement residuals.

In Fig. 13, the a posteriori residual histograms follow the normal distribution curve much better than the ones before the VCE. The abnormal shape of the a priori residual histogram is resulted from the small redundant indices corresponding to IMU measurements due to the large process noises. In return, the corresponding measurement residuals could be very small so that their histograms became unrealistic.

Fig. 13

Residual histograms for Z gyro and Y accelerometer

Analysis of the GNSS measurement residuals

Figure 14 shows the histograms of the standardized measurement residuals for SV PRN 9, 15, 18 and 22 with and without the variance component estimation against the corresponding standardized normal distribution curves. The measurement variances used in KF computation were used for the standardization of the measurement residuals. In the optimal filter, these two variances shall statistically match each other reasonably well.

Fig. 14

Normalized residual distributions of GNSS DD L1 phase measurements

The a posteriori histograms fit the standardized normal distribution curves much better than that of a priori ones individually, which tells that the a posteriori estimated variances for the GNSS measurements were realistic.

Conclusion and future work

This paper proposed a simplified VCE algorithm for multisensor integrated kinematic positioning and navigation. First, the sensor measurements were processed through a fully tightly-coupled integration strategy (IMU/GNSS KF). In doing so, the random errors in IMU measurements could be statistically separated from the other error sources, which has made possible conducting the variance component estimation for the six IMU measurements along with the process noise components including jerks and changes of angular rates. Second, the simplified VCE using the redundancy contribution of individual independent measurements or measurement groups was applied and formulated for the individual components in the process noise vector and the measurement vector including the IMU and the double differenced GNSS measurements. The results showed that the stochastic models of the process and measurement noise in IMU/GNSS KF is improved through VC estimation.