INS/GPS for High-Dynamic UAV-Based Applications

Abstract

The carrier-phase-derived delta pseudorange measurements are often used for velocity determination. However, it is a type of integrated measurements with errors strongly related to pseudorange errors at the start and end of the integration interval. Conventional methods circumvent these errors with approximations, which may lead to large velocity estimation errors in high-dynamic applications. In this paper, we employ the extra states to “remember” the pseudorange errors at the start point of the integration interval. Sequential processing is employed for reducing the processing load. Simulations are performed based on a field-collected UAV trajectory. Numerical results show that the correct handling of errors involved in the delta pseudorange measurements is critical for high-dynamic applications. Besides, sequential processing can update different types of measurements without degrading the system estimation accuracy, if certain conditions are met.

1. Introduction

GPS receivers are widely used in navigation. However, the system performance largely depends on the signal environment, and the measurement update rate is low. This raises the need to integrate GPS with the inertial navigation system (INS) to have a robust continuous navigation solution regardless of the environment. Among several integration architectures, the tightly coupled INS/GPS integration is one of the most promising methods to fuse the GPS and INS data, where the code-derived pseudorange and carrier-phase-derived delta pseudorange measurements are often exploited. However, the delta pseudorange is an integrated measurement with errors strictly related to the pseudorange errors at the endpoints of the integration interval. In practice, various approximations are made to handle these errors. The weakest but often used approach is to simply consider the integral of velocity divided by time (an average value) as the instantaneous velocity measurement at the endpoint of the integration interval [1]. It may fulfill the accuracy requirements in static or low dynamic applications. Nevertheless, if the vehicle is maneuvering under high dynamics with low GPS data update rate, large velocity estimation errors will appear. That is, the velocity errors will be strongly correlated to the accelerations and jerks involved in the trajectory [2]. In order to tackle this problem, we use delay states to “remember” the pseudorange errors at the start of the integration interval. Besides, for reducing the processing load, sequential processing is utilized to avoid the time consuming computation of matrix inversion when deriving the Kalman gain. Different types of measurements (e.g., integrated and nonintegrated measurements) are sequentially processed with no compromise made on their errors.

In the remainder of this paper, the content is organized as follows. In Section 2, the often used velocity determination method based on the delta pseudorange measurements is overviewed, and its problem is indicated. In Section 3, the approach based on the augmented state vector is proposed for tackling this problem. Moreover, sequential processing is utilized and the computational burden is analyzed. In Section 4, the INS/GPS tightly coupled system state space models are introduced. In Section 5, simulation tests are conducted. Numerical results are compared and analyzed. In the Appendix, the number of numerical operations involved in the calculation of the matrix inversion using the Gauss-Jordan elimination method is counted.

2. Velocity Determination

The delta pseudorange is formed by integrating the pseudorange rate over the filter measurement update interval. It is equal to the difference of pseudoranges measured at the endpoints of the filter update interval. Accordingly, the errors involved in delta pseudorange are strictly related to the corresponding pseudorange errors at the start and end of the integration interval, which are given in (1):Δ𝝆_𝑘=^𝑘_𝑘−1𝛿𝝆̇𝑑𝑡=𝛿𝝆_𝑘−𝛿𝝆_𝑘−1, (1) where Δ𝝆_𝑘 represents the delta pseudorange error measured at time instance 𝑘; 𝛿𝝆̇ denotes the pseudorange rate error; 𝛿𝝆_𝑘 and 𝛿𝝆_𝑘−1 are the pseudorange errors at the two endpoints of the integration interval, which can be related to error states (i.e., position errors and receiver clock bias error) as𝛿𝝆_𝑘=𝝆_𝑘−𝝆_𝑘=𝐇_𝑘𝛿𝐱_𝑘+𝐯_𝑘,𝛿𝝆_𝑘−1=𝝆_𝑘−1−𝝆_𝑘−1=𝐇_𝑘−1𝛿𝐱_𝑘−1+𝐯_𝑘−1, (2) where the error states are defined as 𝛿𝐱=𝐱_estimated−𝐱_true; the 𝐇_𝑘 and 𝐇_𝑘−1 are the Jacobian matrices from the first-order linearization of the nonlinear pseudorange observation model at time instants of the start and end of the integration interval; 𝐯_𝑘 and 𝐯_𝑘−1 represent the zero-mean Gaussian white noises.

Substituting (2) into (1) yields𝛿𝝆_𝑘−𝛿𝝆_𝑘−1=𝐇_𝑘𝛿𝐱_𝑘−𝐇_𝑘−1𝛿𝐱_𝑘−1+𝐯_{𝑘,𝑘−1}. (3)

For clarity purpose, we will denote 𝐯^𝜙_𝑘 to represent the cumulative effect of the carrier-phase-derived delta pseudorange measurement noises 𝐯_{𝑘,𝑘−1} from time instants 𝑘−1 to 𝑘. We divide (3) by the time interval Δ𝑡=𝑡_𝑘−𝑡_𝑘−1 and assume that this time interval is small. Thus, the changes in the Jacobian matrix 𝐇 are negligible (i.e., 𝐇_𝑘−1≈𝐇_𝑘), and we arrive at𝛿𝝆_𝑘−𝛿𝝆_𝑘−1Δ𝑡=𝐇_𝑘𝛿𝐱_𝑘−𝛿𝐱_𝑘−1Δ𝑡+𝐯^𝜙_𝑘⟹𝛿𝝆̇_𝑘=𝐇_𝑘𝛿𝐱̇_𝑘+𝐯^𝜙_𝑘, (4) where 𝛿𝐱̇_𝑘 contains the system velocity errors and receiver clock drift error.

This equation is often employed for velocity determination using delta pseudorange measurements. However, it relates the average information (i.e.,(𝛿𝝆_𝑘−𝛿𝝆_𝑘−1)/Δ𝑡) to the instantaneous velocity estimation errors at time instant 𝑘 (i.e., 𝛿𝐱̇_𝑘). If the vehicle is maneuvering under high dynamics, this approximation will be very poor.

Therefore, we lay (4) aside. And from (2), we have𝛿𝝆_𝑘−𝛿𝝆_𝑘−1=𝝆_𝑘−𝝆_𝑘−𝝆_𝑘−1−𝝆_𝑘−1=𝝆_𝑘−𝝆_𝑘−1−𝝆_𝑘−𝝆_𝑘−1. (5)

We denote 𝐲^𝜙_𝑘=𝝆_𝑘−𝝆_𝑘−1,𝐲^𝜙_𝑘=𝝆_𝑘−𝝆_𝑘−1 and 𝛿𝐲^𝜙_𝑘=𝐲^𝜙_𝑘−𝐲^𝜙_𝑘. Thus, (3) can be reformulated as𝛿𝐲^𝜙_𝑘=𝐇_𝑘𝛿𝐱_𝑘−𝐇_𝑘−1𝛿𝐱_𝑘−1+𝐯^𝜙_𝑘. (6)

Equation (6) relates the system current and delay error states to the delta pseudorange innovation vector. It can be used as the system observation model. However, this equation is not in the general format for applying a Kalman filter (KF). That is, the delay state term 𝐇_𝑘−1𝛿𝐱_𝑘−1 appears on the right side of the equation. In order to solve this problem, modifications are required. In this contribution, we augment the state vector to include the delay states. Details will be given in the next section.

3. Augmentation of the System State Vector

We augment the system state vector to include the delay states to “remember” the system estimation errors at the start of the integration interval. These delay states are explicitly estimated from the previous measurement update and do not evolve over time (e.g., [3]). Therefore, we should model them as random constants in the current measurement update. The augmented system model is𝛿𝐱_𝑘𝛿𝐱_𝑑,𝑘=𝚽_{𝑘,𝑘−1}𝐎𝐎𝐈𝛿𝐱_𝑘−1𝛿𝐱_{𝑑,𝑘−1}+𝐰_𝑘−1𝟎, (7) where 𝛿𝐱_𝑑,𝑘 denotes the delay error state vector; Φ_{𝑘,𝑘−1} is the transition matrix for current states; 𝐈 is an identity matrix; 𝐰_𝑘−1 represents the zero-mean Gaussian white noises.

The state estimation error covariance and system process noise error covariance matrices are given in their partitioned forms as:𝐏^′_𝑘=𝐏_𝑘𝐏_{𝑐𝑑,𝑘}𝐏_{𝑑𝑐,𝑘}𝐏_𝑑,𝑘,𝐐^′_𝑘=𝐐_𝑘𝐎𝐎𝐎, (8) where 𝐏_𝑘 is the error covariance of 𝛿𝐱_𝑘; 𝐏_𝑑,𝑘 is the error covariance of 𝛿𝐱_𝑑,𝑘, which is explicitly estimated from the previous measurement update; 𝐏_{𝑐𝑑,𝑘} and 𝐏_{𝑑𝑐,𝑘} are the cross-error covariance matrices between the current error state vector 𝛿𝐱_𝑘 and the delay error state vector 𝛿𝐱_𝑑,𝑘; 𝐐_𝑘 is the process noise error covariance of 𝐰_𝑘; 𝐎 is a zero matrix.

For the delta pseudorange observation model, it is given as𝛿𝐲^𝜙_𝑘=𝐇_𝑘−𝐇_𝑑,𝑘𝛿𝐱_𝑘𝛿𝐱_𝑑,𝑘+𝐯^𝜙_𝑘, (9) where 𝐇_𝑘 and 𝐇_𝑑,𝑘 are the Jacobian matrices related to the time instants of the start and end of the integration interval; 𝛿𝐲^𝜙_𝑘 is the delta pseudorange innovation vector; 𝐯^𝜙_𝑘 represents the measurement noise.

As compared with (6), in this arrangement, we actually consider the “delay states” as a part of the current states. Thus, all the terms are assumed to be related with the current time instant 𝑘.

Besides the delta pseudorange, the code-derived pseudorange measurements are also considered in the filter. For the pseudorange measurements, the observation model is𝝆_𝑘−𝝆_𝑘=𝐇_𝑘𝛿𝐱_𝑘+𝐯^𝜌_𝑘. (10)

Substituting 𝛿𝐲^𝜌_𝑘=𝝆_𝑘−𝝆_𝑘 into (10), the overall system observation model can be derived in its partitioned form as𝛿𝐲^𝜌_𝑘𝛿𝐲^𝜙_𝑘=𝐇_𝑘𝐎𝐇_𝑘−𝐇_𝑑,𝑘𝛿𝐱_𝑘𝛿𝐱_𝑑,𝑘+𝐯^𝜌_𝑘𝐯^𝜙_𝑘,with𝐑^′_𝑘=𝐑^𝜌_𝑘𝐎𝐎𝐑^𝜙_𝑘, (11) where 𝐑^𝜌_𝑘 and 𝐑^𝜙_𝑘 are the pseudorange and delta pseudorange measurement error covariance matrices, which are assumed to be uncorrelated with each other.

Thus, (7) and (11) comprise the new system propagation and observation models, which will be used in the INS/GPS integration system. In the following subsections, we will firstly apply the sequential processing to update different types of measurements based on the new developed system models. And then, its computational burden is analyzed with respect to the conventional batch processing.

3.1. Measurement Updates

In this contribution, we process the pseudorange and delta pseudorange measurements as two batches of data. In this way, the dimension of observation vector will be half of the case, if we process the data in one batch.

For sequential processing, if the measurement errors are uncorrelated, they can be processed one after another with zero-width time interval until all measurements are sequentially updated. Otherwise, decoupling of the correlated measurement errors must be made, and linear combinations of measurements should be conducted to yield a new set of measurements whose errors are uncorrelated. After sequentially updating the measurements, the final state estimates and the associated error covariance matrix will be the same as if the measurements are processed in one batch. In this way, the time-consuming calculation of the matrix inversion can be prevented (e.g., [4, 5]). The exact number of numerical operations involved in the matrix inversion is calculated in the Appendix using the Gauss-Jordan Elimination method (e.g., [6]).

3.1.1. Sequential Updates of Delta Pseudorange Measurements

The conventional batch measurement update of delta pseudorange measurements is performed as follows:𝐊_𝑘𝐊_𝑑,𝑘=𝐏⁻_𝑘𝐏⁻_{𝑐𝑑,𝑘}𝐏⁻_{𝑑𝑐,𝑘}𝐏⁻_𝑑,𝑘𝐇^𝑇_𝑘−𝐇^𝑇_𝑑,𝑘×𝐇_𝑘−𝐇_𝑑,𝑘𝐏⁻_𝑘𝐏⁻_{𝑐𝑑,𝑘}𝐏⁻_{𝑑𝑐,𝑘}𝐏⁻_𝑑,𝑘×𝐇^𝑇_𝑘−𝐇^𝑇_𝑑,𝑘+𝐑^𝜙_𝑘⁻¹,𝛿𝐱⁺_𝑘𝛿𝐱⁺_𝑑,𝑘=𝛿𝐱⁻_𝑘𝛿𝐱⁻_𝑑,𝑘+𝐊_𝑘𝐊_𝑑,𝑘×𝛿𝐲^𝜙_𝑘−𝐇_𝑘−𝐇_𝑑,𝑘𝛿𝐱⁻_𝑘𝛿𝐱⁻_𝑑,𝑘,𝐏⁺_𝑘𝐏⁺_{𝑐𝑑,𝑘}𝐏⁺_{𝑑𝑐,𝑘}𝐏⁺_𝑑,𝑘=𝐈𝐎𝐎𝐈−𝐊_𝑘𝐊_𝑑,𝑘𝐇_𝑘−𝐇_𝑑,𝑘×𝐏⁻_𝑘𝐏⁻_{𝑐𝑑,𝑘}𝐏⁻_{𝑑𝑐,𝑘}𝐏⁻_𝑑,𝑘, (12) where 𝐊_𝑘 is the Kalman gain for the current error state vector 𝛿𝐱_𝑘, while 𝐊_𝑑,𝑘 is the Kalman gain for the delay error state vector 𝛿𝐱_𝑑,𝑘.

As stated before, the delay error states will not be updated from current measurement update. Thus, we can set 𝐊_𝑑,𝑘 to be a zero matrix. Hereby, (12) can be simplified as𝐊_𝑘=𝐏⁻_𝑘𝐇^𝑇_𝑘−𝐏⁻_{𝑐𝑑,𝑘}𝐇^𝑇_𝑑,𝑘×𝐇_𝑘𝐏⁻_𝑘𝐇^𝑇_𝑘−𝐇_𝑑,𝑘𝐏⁻_{𝑑𝑐,𝑘}𝐇^𝑇_𝑘−𝐇_𝑘𝐏⁻_{𝑐𝑑,𝑘}𝐇^𝑇_𝑑,𝑘+𝐇_𝑑,𝑘𝐏⁻_𝑑,𝑘𝐇^𝑇_𝑑,𝑘+𝐑^𝜙_𝑘,⁻¹𝛿𝐱⁺_𝑘=𝛿𝐱⁻_𝑘+𝐊_𝑘𝛿𝐲^𝜙_𝑘−𝐇_𝑘𝛿𝐱⁻_𝑘+𝐇_𝑑,𝑘𝛿𝐱⁻_𝑑,𝑘,𝐏⁺_𝑘=𝐈−𝐊_𝑘𝐇_𝑘𝐏⁻_𝑘+𝐊_𝑘𝐇_𝑑,𝑘𝐏⁻_{𝑑𝑐,𝑘},𝐏⁺_{𝑐𝑑,𝑘}=𝐈−𝐊_𝑘𝐇_𝑘𝐏⁻_{𝑐𝑑,𝑘}+𝐊_𝑘𝐇_𝑑,𝑘𝐏⁻_𝑑,𝑘,𝐏⁺_{𝑑𝑐,𝑘}=𝐏⁺_{𝑐𝑑,𝑘}^𝑇. (13) It is worth mentioning that, in (13), we have not considered the measurement update of 𝛿𝐱_𝑑,𝑘, but the uncertainty of 𝛿𝐱_𝑑,𝑘 (i.e., 𝐏⁻_𝑑,𝑘, 𝐏⁻_{𝑑𝑐,𝑘}, 𝐏⁻_{𝑐𝑑,𝑘}) is accounted for in the derivation of the Kalman gain 𝐊_𝑘.

In sequential update of delta pseudorange measurements, the following parameters need to be firstly initialized. We denote 𝐇_𝑘(𝑖) and 𝐇_𝑑,𝑘(𝑖) as the 𝑖th row of 𝐇_𝑘 and 𝐇_𝑑,𝑘, respectively, and use 𝐑^𝜙_𝑘(𝑖,𝑖) to represent the 𝑖th main diagonal element of 𝐑^𝜙_𝑘.𝐏_𝑘(0)=𝐏⁻_𝑘,𝐏_{𝑐𝑑,𝑘}(0)=𝐏⁻_{𝑐𝑑,𝑘},𝐏_{𝑑𝑐,𝑘}(0)=𝐏⁻_{𝑑𝑐,𝑘},𝛿𝐱_𝑘(0)=𝛿𝐱⁻_𝑘,𝐇_𝑘=𝐇_𝑘(1)⋮𝐇_𝑘(𝑚),𝐇_𝑑,𝑘=𝐇_𝑑,𝑘(1)⋮𝐇_𝑑,𝑘(𝑚),𝐑^𝜙_𝑘=𝐑^𝜙_𝑘(1,1)⋯0⋮⋱⋮0⋯𝐑^𝜙_𝑘().𝑚,𝑚 (14)

From 𝑖=1 to 𝑚, the measurements are updated sequentially as follows:𝐊_𝑘(𝑖)=𝐏_𝑘(𝑖−1)𝐇_𝑘(𝑖)^𝑇−𝐏_{𝑐𝑑,𝑘}(𝑖−1)𝐇_𝑑,𝑘(𝑖)^𝑇×𝐇_𝑘(𝑖)𝐏_𝑘(𝑖−1)𝐇_𝑘(𝑖)^𝑇−𝐇_𝑑,𝑘(𝑖)𝐏_{𝑑𝑐,𝑘}(𝑖−1)𝐇_𝑘(𝑖)^𝑇−𝐇_𝑘(𝑖)𝐏_{𝑐𝑑,𝑘}(𝑖−1)𝐇_𝑑,𝑘(𝑖)^𝑇+𝐇_𝑑,𝑘(𝑖)𝐏_𝑑,𝑘𝐇_𝑑,𝑘(𝑖)^𝑇+𝐑^𝜙_𝑘(𝑖,𝑖)⁻¹,𝛿𝐱_𝑘(𝑖)=𝛿𝐱_𝑘(𝑖−1)+𝐊_𝑘(𝑖)×𝛿𝐲^𝜙_𝑘(𝑖)−𝐇_𝑘(𝑖)𝛿𝐱_𝑘(𝑖−1)+𝐇_𝑑,𝑘(𝑖)𝛿𝐱_𝑑,𝑘,𝐏_𝑘(𝑖)=1−𝐊_𝑘(𝑖)𝐇_𝑘(𝑖)𝐏_𝑘(𝑖−1)+𝐊_𝑘(𝑖)𝐇_𝑑,𝑘(𝑖)𝐏_{𝑑𝑐,𝑘}(),𝐏𝑖−1_{𝑐𝑑,𝑘}(𝑖)=1−𝐊_𝑘(𝑖)𝐇_𝑘(𝑖)𝐏_{𝑐𝑑,𝑘}(𝑖−1)+𝐊_𝑘(𝑖)𝐇_𝑑,𝑘(𝑖)𝐏_𝑑,𝑘,𝐏_{𝑑𝑐,𝑘}(𝑖)=𝐏_{𝑐𝑑,𝑘}(𝑖)^𝑇. (15)

3.1.2. Sequential Updates of Pseudorange Measurements

The batch processing of pseudorange measurements is handled after we process the delta pseudorange measurements. The a posteriori estimates from the delta pseudorange measurement update (i.e., 𝛿𝐱⁺_𝑘 and 𝐏⁺_𝑘) are used here as the a priori estimates (i.e., 𝛿𝐱⁻_𝑘 and 𝐏⁻_𝑘).𝐊_𝑘=𝐏⁻_𝑘𝐇^𝑇_𝑘𝐇_𝑘𝐏⁻_𝑘𝐇^𝑇_𝑘+𝐑^𝜌_𝑘⁻¹,𝛿𝐱⁺_𝑘=𝛿𝐱⁻_𝑘+𝐊_𝑘𝛿𝐲^𝜌_𝑘−𝐇_𝑘𝛿𝐱⁻_𝑘,𝐏⁺_𝑘=𝐈−𝐊_𝑘𝐇_𝑘𝐏⁻_𝑘. (16)

For sequential processing, we define𝐇_𝑘=𝐇_𝑘(1)⋮𝐇_𝑘(𝑚),𝐑^𝜌_𝑘=𝐑^𝜌_𝑘(1,1)⋯0⋮⋱⋮0⋯𝐑^𝜌_𝑘(𝑚,𝑚). (17)

From 𝑖=1 to 𝑚, the measurements are sequentially updated as𝐊_𝑘(𝑚+𝑖)=𝐏_𝑘(𝑚+𝑖−1)𝐇_𝑘(𝑖)^𝑇×𝐇_𝑘(𝑖)𝐏_𝑘(𝑚+𝑖−1)𝐇_𝑘(𝑖)^𝑇+𝐑^𝜌_𝑘(𝑖,𝑖)⁻¹,𝛿𝐱_𝑘(𝑚+𝑖)=𝛿𝐱_𝑘(𝑚+𝑖−1)+𝐊_𝑘()×𝑚+𝑖𝛿𝐲^𝜌_𝑘(𝑖)−𝐇_𝑘(𝑖)𝛿𝐱_𝑘(,𝐏𝑚+𝑖−1)_𝑘(𝑚+𝑖)=1−𝐊_𝑘(𝑚+𝑖)𝐇_𝑘(𝑖)𝐏_𝑘(𝑚+𝑖−1). (18)

After all the measurements are updated, the a posteriori estimates are calculated as𝛿𝐱⁺_𝑘=𝛿𝐱_𝑘(𝑚+𝑚),𝐏⁺_𝑘=𝐏_𝑘(𝑚+𝑚). (19)

They are used as the delay states and covariance parameters in the next recursion𝛿𝐱_𝑑,𝑘+1=𝛿𝐱_𝑘(𝑚+𝑚),𝐏_𝑑,𝑘+1=𝐏_𝑘(𝑚+𝑚). (20)

The whole process repeats until the end of the trajectory.

3.2. Computational Burden Comparisons

It is known that the sequential processing can save computational time. But how much can be exactly saved? In order to answer this question, the number of numerical operations involved in addition and multiplication should be counted separately. The method utilized in [7] has been employed in this paper, where subtraction is counted as addition and the division over scalar is considered as multiplication by the inverse of the scalar. The “n” represents the dimension of state vector and “m” is the dimension of the measurement vector. We denote the “nn” as an n-by-n square matrix, and “nn × nn” stands for an n-by-n matrix multiplies another n-by-n matrix. “n1” means an n-dimensional vector, “1” means a scalar value. The numbers in the column of “Num.” represent the amount of operations involved in the measurement update of both the pseudorange and delta pseudorange measurements, while the numbers in the parentheses denote the operations encountered in the delta pseudorange measurement update only. For batch measurement update (i.e., (13) and (16)), the involved numerical operations are given in the left side of Table 1. For measurement update in sequence (i.e., (15) and (18)), they are given in the right side of Table 1. Table 2 summaries the overall conducted numerical operations involved in sequential and batch measurement updates.

Table 1 

Numerical operations involved in sequential and batch measurement updates.

Table 2 

The sum of additions and multiplications involved in batch and sequential processing.

The correctness of the calculations in Table 2 can be easily verified when we consider the case that there is only one satellite in view (𝑚=1). In this case, there should be no difference between the sequential and batch processing. We can prove this by assigning 𝑚 to be 1. The batch processing returns 18𝑛²−2𝑛 for addition, and 18𝑛²+12𝑛+2 for multiplication, which are obviously equal to the corresponding calculations in sequential processing.

Based on the calculations in Table 2, the relationships between the number of numerical operations and the dimension of the observation vector are illustrated in Figures 1 and 2, where the dimension of the state vector is 17 (i.e., 𝑛=17).

Figure 1 

Computational burden analysis (addition versus multiplication) with varying number of measurements.

Figure 2 

Computational burden analysis (pseudorange versus delta pseudorange) with varying number of measurements.

Figure 1 shows that if more measurements are processed, more computational time can be saved from sequential processing. However, if the number of measurements is smaller than 4, no big difference can be observed. In this paper, we update the delta pseudorange and pseudorange as two batches of data. And hence, in the calculation, the dimension of observation vector is always smaller than 11 (i.e.,𝑚≤11), which saves a large number of numerical operations.

Figure 2 illustrates that the measurement update of the delta pseudorange requires much more numerical operations than the pseudorange measurement update due to the increased computational complexity in (15) as compared with (18). However, when compared with the batch measurement update (i.e., red curves in Figure 2), they are both saved.

4. Tightly Coupled INS/GPS State Space Models

The INS propagation model we use is often seen for low-cost microelectromechanical- (MEMS-) based IMU, which is also used as the system propagation model in INS/GPS integrated system. In terms of a low-cost IMU, the effects from the earth rotation cannot be observed. Hence, they are not considered in the model. Moreover, the transport rate and Coriolis terms have been neglected for simplicity. The simplified propagation model in discrete time domain can be expressed in navigation frame as:𝐩_𝑛,𝑘+1=𝐩_𝑛,𝑘+𝐯_𝑛,𝑘𝐯⋅Δ𝑡,_𝑛,𝑘+1=𝐯_𝑛,𝑘+𝐑_{𝑏2𝑛,𝑘}⋅𝐟̃_𝑏,𝑘−𝐟̂^error_𝑏,𝑘−𝐰_𝑓+𝐠_𝑛𝝍⋅Δ𝑡,_𝑘+1=𝝍_𝑘+𝐄_{𝑏2𝑛,𝑘}⋅𝝎_𝑏,𝑘−𝝎^error_𝑏,𝑘−𝐰_𝜔⋅Δ𝑡, (21) where 𝑘 is the time instant; 𝐟_𝑏̃ is the measurement vector of the specific force; 𝝎_𝑏 represents the measurement vector of angular rate; 𝐠_𝑛 is the local gravity vector; 𝝍 is the Euler angles; 𝐟̂^error_𝑏,𝑘 and 𝝎^error_𝑏,𝑘 are the estimated specific force and angular rate errors; 𝐰_f and 𝐰_𝜔 are the remaining noise errors, which are assumed to be zero-mean Gaussian white noises; 𝐑_𝑏2𝑛 is the frame rotation matrix from body frame to NED navigation frame; 𝐄_𝑏2𝑛 is the rotation rate transformation matrix from body to navigation frame,R_𝑏2𝑛=,𝐄𝐶𝛾𝐶𝛽𝐶𝛾𝑆𝛽𝑆𝛼−𝑆𝛾𝐶𝛼𝐶𝛾𝑆𝛽𝐶𝛼+𝑆𝛾𝑆𝛼𝑆𝛾𝐶𝛽𝑆𝛾𝑆𝛽𝑆𝛼+𝐶𝛾𝐶𝛼𝑆𝛾𝑆𝛽𝐶𝛼−𝐶𝛾𝑆𝛼−𝑆𝛽𝐶𝛽𝑆𝛼𝐶𝛽𝐶𝛼_𝑏2𝑛=,1𝑆𝛼𝑇𝛽𝐶𝛼𝑇𝛽0𝐶𝛼−𝑆𝛼0𝑆𝛼/𝐶𝛽𝐶𝛼/𝐶𝛽 (22) where 𝐶𝑋,𝑆𝑋, and 𝑇𝑋 represent the trigonometric operations of cosine, sine, and tangent of 𝑋, respectively. The 𝛼,𝛽,𝛾 denote the roll, pitch, and yaw, respectively.

Regarding the INS/GPS tightly coupled integration, the extended Kalman filter (EKF) is considered as state of the art for fusing the INS and GPS data, and the error states are often used [8]. In this paper, the error states are defined as 𝛿𝐱=𝐱_estimated−𝐱_true. The system transition model Φ_{𝑘,𝑘−1} (i.e., in (7)) for current error state vector is shown in (23), where the first three rows come from the first order linearization of (21) with respect to the corresponding states:𝛿𝐩_𝑛,𝑘+1𝛿𝐯_𝑛,𝑘+1𝛿𝝍_𝑘+1𝛿𝐟^error_𝑏,𝑘+1𝛿𝝎^error_𝑏,𝑘+1𝑐𝛿𝑡_𝑘+1𝑐𝛿𝑡̇_𝑘+1=𝐈_3×3𝐈_3×3Δ𝑡𝐎_3×3𝐎_3×3𝐎_3×3𝟎_3×1𝟎_3×1𝐎_3×3𝐈_3×3𝐅₂₃−𝐑_𝑏2𝑛Δ𝑡𝐎_3×3𝟎_3×1𝟎_3×1𝐎_3×3𝐎_3×3𝐅₃₃𝐎_3×3−𝐄_𝑏2𝑛Δ𝑡𝟎_3×1𝟎_3×1𝐎_3×3𝐎_3×3𝐎_3×3𝐈_3×3𝐎_3×3𝟎_3×1𝟎_3×1𝐎_3×3𝐎_3×3𝐎_3×3𝐎_3×3𝐈_3×3𝟎_3×1𝟎_3×1𝟎^𝑇_3×1𝟎^𝑇_3×1𝟎^𝑇_3×1𝟎^𝑇_3×1𝟎^𝑇_3×1𝟎1Δ𝑡_𝑇3×1𝟎^𝑇_3×1𝟎^𝑇_3×1𝟎^𝑇_3×1𝟎^𝑇_3×1⋅01𝛿𝐩_𝑛,𝑘𝛿𝐯_𝑛,𝑘𝛿𝝍_𝑘𝛿𝐟^error_𝑏,𝑘𝛿𝝎^error_𝑏,𝑘𝑐𝛿𝑡_𝑘𝑐𝛿𝑡_𝑘̇+𝐰_𝑘. (23)

In (23), the gyroscope and accelerometer sensor errors 𝛿𝐟^error_𝑏 and 𝛿𝝎^error_𝑏 are modeled as random-walk plus constant (e.g., [8]). The receiver clock drift error 𝑐𝛿𝑡̇ is modeled as constant plus random walk process, while the receiver clock bias 𝑐𝛿𝑡 is the integral of the clock drift error. The sub-matrices 𝐅₃₃ and 𝐅₂₃ are given as:𝐅₃₃=𝜔1+^𝑦+𝜔𝐶𝛼𝑇𝛽_𝑧𝑇𝛽𝑆𝛼Δ𝑡𝜔_𝑧𝐶𝛼𝑇²+𝜔𝛽+1_𝑦𝑆𝛼𝑇²𝛽+1Δ𝑡0−𝜔_𝑧+𝜔𝐶𝛼_𝑦𝑆𝛼Δ𝑡10𝜔_𝑦𝐶𝛼−𝜔_𝑧𝑆𝛼𝐶𝛽Δ𝑡𝐶𝛼𝑆𝛽𝜔_𝑧+𝑆𝛽𝑆𝛼𝜔_𝑦𝐶²𝛽Δ𝑡1,𝐅₂₃=𝑓_𝑦+𝑓𝑆𝛼𝑆𝛾+𝐶𝛼𝐶𝛾𝑆𝛽_𝑧𝐶𝛼𝑆𝛾+𝐶𝛾𝑆𝛽𝑆𝛼𝑓_𝑧−𝑓𝐶𝛽𝐶𝛼𝐶𝛾_𝑥+𝑓𝐶𝛾𝑆𝛽_𝑦𝐶𝛽𝐶𝛾𝑆𝛼𝑓_𝑧−𝑓𝐶𝛾𝑆𝛼+𝐶𝛼𝑆𝛽𝑆𝛾_𝑦−𝑓𝐶𝛼𝐶𝛾−𝑆𝛽𝑆𝛼𝑆𝛾_𝑥𝐶𝛽𝑆𝛾−𝑓_𝑧−𝑓𝑆𝛽𝑆𝛼𝑆𝛾+𝐶𝛼𝐶𝛾_𝑦𝐶𝛾𝑆𝛼−𝐶𝛼𝑆𝛽𝑆𝛾𝑓_𝑧−𝑓𝐶𝛽𝐶𝛼𝑆𝛾_𝑥+𝑓𝑆𝛽𝑆𝛾_𝑦𝐶𝛽𝑆𝛼𝑆𝛾𝑓_𝑧−𝑓𝑆𝛼𝑆𝛾+𝐶𝛼𝐶𝛾𝑆𝛽_𝑦+𝑓𝐶𝛼𝑆𝛾−𝐶𝛾𝑆𝛽𝑆𝛼_𝑥𝐶𝛽𝐶𝛾𝑓_𝑦𝐶𝛽𝐶𝛼−𝑓_𝑧𝐶𝛽𝑆𝛼−𝑓_𝑥−𝑓𝐶𝛽_𝑧−𝑓𝐶𝛼𝑆𝛽_𝑦𝑆𝛽𝑆𝛼0⋅Δ𝑡, (24)where 𝐶²𝑋,𝑇²𝑋 represent the square of cosine and tangent trigonometric operations of 𝑋, and 𝑓_𝑥=𝑓_𝑥−𝑓^error_𝑥, 𝑓_𝑦=𝑓_𝑦−𝑓^error_𝑦, 𝑓_𝑧=𝑓_𝑧−𝑓^error_𝑧, 𝜔_𝑦=𝜔_𝑦−𝜔^error_𝑦, 𝜔_𝑧=𝜔_𝑧−𝜔^error_𝑧 are the IMU raw data compensated with the estimated sensor errors expressed in body frame. The linearization can, for example, be accomplished using the “Jacobian” function in MATLAB.

The transition matrix for the delay state vector is an identity matrix. Thus, according to (7), we have already specified the system propagation model.

For the system observation model (i.e., in (11)), 𝐇_𝑑,𝑘 and 𝐇_𝑘 are the Jacobian matrices from the first order linearization of the pseudorange nonlinear observation equations around the INS-estimated position at the time instants of the start and end of the integration interval. The Jacobian matrix related with the current error state vector is shown as:𝐇_𝑘=𝑙^{𝑠𝑣(𝑖)}_𝑛,𝑘𝑙^{𝑠𝑣(𝑖)}_𝑒,𝑘𝑙^{𝑠𝑣(𝑖)}_𝑑,𝑘0⋯010_𝑖×17, (25) where 𝑙^{𝑠𝑣(𝑖)}_𝑛,𝑘,𝑙^{𝑠𝑣(𝑖)}_𝑒,𝑘,𝑙^{𝑠𝑣(𝑖)}_𝑑,𝑘(𝑖=1to𝑚) denote the unit direction vectors pointing from the 𝑖th satellite position to the INS estimated user position in navigation frame at time instant 𝑘. The delay observation matrix 𝐇_𝑑,𝑘 is identical with 𝐇_𝑘, except that all the parameters involved are related to the time instant 𝑘−1 instead of 𝑘. In the recursive KF algorithm, we do not need to specify 𝐇_𝑑,𝑘. Instead, we can simply use previous epoch observation matrix 𝐇_𝑘−1 as 𝐇_𝑑,𝑘 in the current recursion.

5. Simulation Experiments

In the following simulation tests, a field-collected UAV trajectory is used. The position and velocity dynamic profiles are depicted in Figures 3 and 4. On the purpose of verifying the correctness of the proposed algorithm in the processing of integrated measurements sequentially, we assume that the GPS receiver is operating at the DGPS mode, in which majority of GPS measurement errors are corrected or minimized to small values, that is, the ionospheric and tropospheric delays. Hereby, they are neglected in this simulation.

Figure 3 

UAV trajectory.

Figure 4 

Trajectory dynamics.

For the GPS receiver, we assume that we use a NovAtel DL-4plus GPS receiver. The parameters shown in Table 3 are used to simulate the receiver-related measurement errors. The multipath errors are generated using SATNAV toolbox from GPSoft LLC. The basic idea is to form the zero-elevation angle equivalent pseudorange multipath errors using a linear autoregressive model (i.e., “filter” function in MATLAB) and scale these errors by the cosine of their elevation angles before they are applied to the range measurements. Regarding the IMU, an automotive level LandMark 20 eXT MEMS-IMU from Gladiator Technologies Inc. is simulated. The main sensor errors are generated according to its specification as shown in Table 4. The IMU raw data (simulated errors plus the field collected high quality IMU raw data) are plotted in Figure 5. For the integrated KF, the system time-update happens at the IMU measurements update rate, which is 50 Hz, while the measurement-update happens at the GPS measurement update rate, which is 5 Hz. The simulation is conducted with 8 satellites in view.

Table 3 

Parameters used for generating the receiver-related measurement errors (1𝜎).

Table 4 

Landmark 20 eXT MEMS-based IMU performance specification.

(a)

(b)

Figure 5 

Simulated LandMark 20 eXT MEMS IMU raw data.

With this simulation setup, two tests are made. In the first test, we verify the advantage of correctly handling the errors in the delta pseudorange measurements. And in the second one, we prove that the sequential measurement update of different types of measurements presents identical system estimation accuracy with respect to the batch processing, if certain conditions are met.

5.1. System Estimation Accuracy Using “Augmented” System Model

We initialize the filter to be 1 m away to the north, east and down directions from their true values. The errors analyzed here, represent the norm of position, velocity and attitude errors, respectively. For instance, the norm of position errors will be calculated as ‖Δ𝐱‖=Δ𝑥²_𝑛+Δ𝑥²_𝑒+Δ𝑥²_𝑑. The comparisons are made between conditions listed in Table 5, and the results are depicted in Figures 6 and 7.

Table 5 

Conditions for comparison.

(a)

(b)

(c)

Figure 6 

Comparison of position and velocity estimates (the 3rd subplot is the zoom-in of the 2nd subplot).

(a)

(b)

Figure 7 

Comparison of attitude estimates (the 2nd subplot is the zoom-in of the 1st subplot).

For the “augmented” system (red curves), both code-derived pseudorange and carrier-phase-derived delta pseudorange measurements are used in the position determination through the system observation model (i.e., (11)). The pseudorange provides the absolute positioning information, while the delta pseudorange provides the relative displacement of the system position from one time instant to the next. In such case, when the filter is tuned to give more confidence on the pseudorange measurements, the position estimation errors will converge faster, but the system performance will be noisier. On the other hand, when the filter gives more confidence on the delta pseudorange measurements, the estimation errors will converge slower, but system presents much smoothed estimation results. By giving the weights on the pseudorange and delta pseudorange measurements based on the errors introduced onto the GPS raw data, the red curves are obtained. In Figure 6, the third subplot is the “zoom-in” version of the second subplot, where the red curve shows the velocity estimation errors using the “augmented” system model. It demonstrates that with correctly handling the errors involved in the delta pseudorange measurement, the filter can correctly track the accelerations and jerks involved in the trajectory.

The green curves in Figure 6 show an extreme case. That is, if there are no pseudorange measurements (absolute positioning information) available in the filter, with only the delta pseudorange measurements (relative positioning information), the system presents the best velocity estimation accuracy. However, for position, the errors from the incorrect initialization remain through the whole trajectory, as shown in the first subplot of Figure 6.

The blue curves in Figure 6 represent the estimation results using the conventional approach. That is, to relate the pseudorange for position estimation, and the delta pseudorange for velocity estimation. In this test, the dynamics encountered in the trajectory is large. Therefore, large velocity estimation errors appear. The same problem can be observed from the attitude estimation results (i.e., Figure 7).

In this paper, we integrate the measurements obtained from a single-GPS receiver antenna with measurements from an INS without redundant attitude information (e.g., from magnetometers or a multiantenna GPS system). Therefore, no attitude fixes are available. In such case, the attitude information is contained in the position and velocity information, and its errors are related to the position and velocity estimation errors through the off-diagonal parameters in the error covariance matrices. Hence, the greater the accuracy of the position and velocity estimates, the greater the dampening on the attitude errors.

As an alternate approach, the Doppler shift measurements are often used for velocity determination (black curves in Figures 6 and 7). However, they are the raw measurements from the receiver frequency lock loop (FLL), which is known to be noisier than the carrier-phase-derived measurements.

5.2. Comparison between Sequential and Batch Measurement Updates

In this test, the simulation setup remains unchanged as the aforementioned one. Figure 8 plots the position, velocity, and attitude estimation errors from 50 Monte Carlo runs (with 50 simulated GPS and IMU data sets based on parameters in Tables 3 and 4) using the “augmented” system with sequential and batch measurement updates.

(a)

(b)

(c)

Figure 8 

System performance comparisons.

As depicted in the figure, the sequential measurement update presents equivalent performance as compared with batch measurement update. Nevertheless, two rules must be obeyed.(1)If we process a group of measurements, which involve both integrated and nonintegrated measurements, we must always process the integrated measurements (delta pseudorange) before the nonintegrated measurements (pseudorange) to guarantee that there is no measurement update before we process the integrated measurements.(2)The cross-covariance matrices (i.e., 𝐏_{𝑐𝑑,𝑘} and 𝐏_{𝑑𝑐,𝑘}) need to be carefully treated. In sequential processing, among the measurement scalar updates, these cross covariance matrices, that is, 𝐏_{𝑐𝑑,𝑘} and 𝐏_{𝑑𝑐,𝑘}, are updated from one measurement to the other with zero-width time interval, as shown in (15).

6. Conclusion

The errors involved in delta pseudorange measurements are related to the pseudorange errors at the start and end of the integration interval. The correct handling of these errors is essential in cases that the vehicle is maneuvering under high dynamics with low GPS update rate. In this contribution, the method with the “augmented” system state vector is considered, and is employed in the INS/GPS tightly coupled integration. Simulation is conducted using a field-collected UAV trajectory. Numerical results show that the filter can correctly track the dynamics in the trajectory. Moreover, sequential processing is used for measurement update in the KF to prevent the matrix inverse calculation. Monte Carlo runs are conducted and verify that the sequential processing can update different types of measurements (i.e., integrated and nonintegrated) without degrading the system estimation accuracy, if certain rules are met.

Appendix

Calculation of the Matrix Inversion Using Gauss-Jordan Elimination Method

The Gauss-Jordan Elimination method is one of the methods which are widely used to calculate the inverse of a square matrix (e.g., [6]). It is done by augmenting the square matrix with an identity matrix of the same dimensions, and passing through the following matrix operations𝑎multiply¹¹𝑎₁₂⋯𝑎_1𝑛𝑎10⋯0₂₁𝑎₂₂⋯𝑎_2𝑛01⋯0⋮⋮⋱⋮𝑎⋮⋮⋱⋮_𝑛1𝑎_𝑛2⋯𝑎_𝑛𝑛,00⋯1by𝐀⁻¹tohave10⋯0𝑏₁₁𝑏₁₂⋯𝑏_1𝑛01⋯0𝑏₂₁𝑏₂₂⋯𝑏_2𝑛⋮⋮⋱⋮⋮⋮⋱⋮00⋯1𝑏_𝑛1𝑏_𝑛2⋯𝑏_𝑛𝑛. (A.1)

After the Gauss-Jordan elimination, the right side of the augmented matrix is the inverse of the original square matrix. In this contribution, we firstly transform the left side of the augmented matrix (original square matrix) to be a diagonal matrix and divide the augmented matrix by its diagonal elements on each row to have an identity matrix on the left side and the inverse of original matrix on the right side. The computation is shown as follows.

In the first step, we use the elementary row operations to introduce zeros in the first column beginning from the second row, which involves 2𝑛 multiplications and 2𝑛-1 additions:𝑎₁₁𝑎₁₂⋯𝑎_1𝑛𝑏₁₁𝑏₁₂⋯𝑏_1𝑛0𝑎⁽¹⁾₂₂⋯𝑎⁽¹⁾_2𝑛𝑏⁽¹⁾₂₁𝑏⁽¹⁾₂₂⋯𝑏⁽¹⁾_2𝑛⋮⋮⋱⋮⋮⋮⋱⋮0𝑎⁽¹⁾_𝑛2⋯𝑎⁽¹⁾_𝑛𝑛𝑏⁽¹⁾_𝑛1𝑏⁽¹⁾_𝑛2⋯𝑏⁽¹⁾_{𝑛𝑛𝑛×2𝑛}. (A.2)

After the first step, the elements in the rows below the first will change. We indicate this by denoting the elements with a superscript, which stands for the number of arithmetic operations involved.

In the second step, we introduce zeros in the second column except the second row, which involves 2𝑛-1 multiplications and 2𝑛-2 additions.

In the 𝑛-th step, we introduce zeros in the 𝑛-th column except the 𝑛-th row, which involves 𝑛+1 multiplications and 𝑛 additions:𝑎₁₁0⋯0𝑏^(𝑛−1)₁₁𝑏^(𝑛−1)₁₂⋯𝑏^(𝑛−1)_1𝑛0𝑎⁽¹⁾₂₂⋯0𝑏^(𝑛−1)₂₁𝑏^(𝑛−1)₂₂⋯𝑏^(𝑛−1)_2𝑛⋮⋮⋱⋮⋮⋮⋱⋮00⋯𝑎^(𝑛−1)_𝑛𝑛𝑏^(𝑛−1)_𝑛1𝑏^(𝑛−1)_𝑛2⋯𝑏^(𝑛−1)_{𝑛𝑛𝑛×2𝑛}. (A.3)

In the last step, we divide the augmented matrix by its diagonal elements on each row to have the matrix as shown in (A.1). The involved numerical operations are given in Table 6.

Table 6 

Computational cost of matrix inverse calculation.

Using the summation formula^𝑛_𝑖=1𝑛()𝑖=𝑛+12. (A.4)

According to Table 6, the number of multiplications involved in Gauss-Jordan Elimination is(𝑛−1)^𝑛_𝑖=1𝑖+𝑛²(𝑛−1)+𝑛²=()𝑛()𝑛−1𝑛+12+𝑛²(𝑛−1)+𝑛²=32𝑛³−12𝑛. (A.5)

And the required number of additions is computed as(𝑛−1)^𝑛−1_𝑖=1𝑖+𝑛²()=3𝑛−12𝑛³−2𝑛²+𝑛2. (A.6)

We can verify our calculation by inverting a scalar number as an extreme example for square matrix inversion (i.e., 𝑛=1). The arithmetic operations involved contain obviously only one multiplication (division), which is the same as the results computed from (A.5) and (A.6).

Acknowledgment

Part of the work reported herein has been funded by the German Research Foundation (DFG), Grant no. KN 876/1-2, which is gratefully acknowledged.