INS / GPS for High-Dynamic UAV-Based Applications

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 highdynamic 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.


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 carrierphase-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.

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): where Δρ k represents the delta pseudorange error measured at time instance k; δ ρ denotes the pseudorange rate error; δρ k and δρ k−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 where the error states are defined as δx = x estimated − x true ; the H k and H k−1 are the Jacobian matrices from the firstorder linearization of the nonlinear pseudorange observation model at time instants of the start and end of the integration interval; v k and v k−1 represent the zero-mean Gaussian white noises.Substituting (2) into (1) yields For clarity purpose, we will denote v φ k to represent the cumulative effect of the carrier-phase-derived delta pseudorange measurement noises v k,k−1 from time instants k − 1 to k.We divide (3) by the time interval Δt = t k − t k−1 and assume that this time interval is small.Thus, the changes in the Jacobian matrix H are negligible (i.e., H k−1 ≈ H k ), and we arrive at where δ ẋk 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., (δρ k − δρ k−1 )/Δt) to the instantaneous velocity estimation errors at time instant k (i.e., δ ẋk ).If the vehicle is maneuvering under high dynamics, this approximation will be very poor.
Therefore, we lay (4) aside.And from (2), we have ( We denote Thus, (3) can be reformulated as 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 H k−1 δx k−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.

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 where δx d,k denotes the delay error state vector; Φ k,k−1 is the transition matrix for current states; I is an identity matrix; w k−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: where P k is the error covariance of δx k ; P d,k is the error covariance of δx d,k , which is explicitly estimated from the previous measurement update; P cd,k and P dc,k are the crosserror covariance matrices between the current error state vector δx k and the delay error state vector δx d,k ; Q k is the process noise error covariance of w k ; O is a zero matrix.
For the delta pseudorange observation model, it is given as where H k and H d,k are the Jacobian matrices related to the time instants of the start and end of the integration interval; δy φ k is the delta pseudorange innovation vector; v φ k 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 k.
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 δy ρ k = ρ k − ρ k into (10), the overall system observation model can be derived in its partitioned form as where R ρ k and R φ k 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.

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]).

Sequential Updates of Delta Pseudorange Measurements.
The conventional batch measurement update of delta pseudorange measurements is performed as follows: where K k is the Kalman gain for the current error state vector δx k , while K d,k is the Kalman gain for the delay error state vector δx d,k .
As stated before, the delay error states will not be updated from current measurement update.Thus, we can set K d,k to be a zero matrix.Hereby, (12) can be simplified as It is worth mentioning that, in (13), we have not considered the measurement update of δ x d,k , but the uncertainty of δ x d,k (i.e., P − d,k , P − dc,k , P − cd,k ) is accounted for in the derivation of the Kalman gain K k .
In sequential update of delta pseudorange measurements, the following parameters need to be firstly initialized.We denote H k (i) and H d,k (i) as the ith row of H k and H d,k , respectively, and use . . .
International Journal of Navigation and Observation From i = 1 to m, the measurements are updated sequentially as follows: (15)

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., δ x + k and P + k ) are used here as the a priori estimates (i.e., δ x − k and P − k ).
For sequential processing, we define From i = 1 to m, the measurements are sequentially updated as After all the measurements are updated, the a posteriori estimates are calculated as They are used as the delay states and covariance parameters in the next recursion The whole process repeats until the end of the trajectory.

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.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 (m = 1).In this case, there should be no difference between the sequential and batch processing.We can prove this by assigning m to be 1.The batch processing returns 18n 2 − 2n for addition, and 18n 2 + 12n + 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., n = 17).
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., m ≤ 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.

Tightly Coupled INS/GPS State Space Models
The INS propagation model we use is often seen for lowcost 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: where k is the time instant; f b is the measurement vector of the specific force; ω b represents the measurement vector of angular rate; g n is the local gravity vector; ψ is the Euler angles; f error b,k and ω error b,k are the estimated specific force and angular rate errors; w f and w ω are the remaining noise errors, which are assumed to be zero-mean Gaussian white noises; R b2n is the frame rotation matrix from body frame to NED navigation frame; E b2n is the rotation rate transformation matrix from body to navigation frame, where CX, SX, and TX represent the trigonometric operations of cosine, sine, and tangent of X, 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 International Journal of Navigation and Observation δx = x estimated − x true .The system transition model Φ k,k−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: In ( 23), the gyroscope and accelerometer sensor errors δf error b and δω error b are modeled as random-walk plus constant (e.g., [8]).The receiver clock drift error cδ ṫ is modeled as constant plus random walk process, while the receiver clock bias cδt is the integral of the clock drift error.The submatrices F 33 and F 23 are given as: where C 2 X, T 2 X represent the square of cosine and tangent trigonometric operations of X, and 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)), H d,k and H k 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: where l sv(i) n,k , l sv(i) e,k , l sv(i) d,k (i = 1 to m) denote the unit direction vectors pointing from the ith satellite position to the INS estimated user position in navigation frame at time instant k.The delay observation matrix H d,k is identical with H k , except that all the parameters involved are related to the time instant k−1 instead of k.In the recursive KF algorithm, we do not need to specify H d,k .Instead, we can simply use previous epoch observation matrix H k−1 as H d,k in the current recursion.

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.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.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.

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 Δx = Δx 2 n + Δx 2 e + Δx 2 d .The comparisons are made between conditions listed in Table 5, and the results are depicted in Figures 6 and 7.
For the "augmented" system (red curves), both codederived pseudorange and carrier-phase-derived delta pseudorange measurements are used in the position determination through the system observation model (i.e., (11)).Red "Augmented" system using pseudorange and delta pseudorange measurements Green "Augmented" system using delta pseudorange measurements alone Blue System using pseudorange for position and delta pseudorange for velocity Black System using pseudorange for position and Doppler shift for velocity 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.

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.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., P cd,k and P dc,k ) need to be carefully treated.In sequential processing, among the measurement scalar updates, these cross covariance matrices, that is, P cd,k and P dc,k , are updated from one measurement to the other with zero-width time interval, as shown in (15).

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

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 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 2n multiplications and 2n-1 additions: (1)  22 • • • a (1)  2n b (1)  21 b (1)  22  (1)  nn b (1)  n1 b (1) 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 2n-1 multiplications and 2n-2 additions.
In the n-th step, we introduce zeros in the n-th column except the n-th row, which involves n + 1 multiplications and n additions: 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.
Using the summation formula According to Table 6, the number of multiplications involved in Gauss-Jordan Elimination is  We can verify our calculation by inverting a scalar number as an extreme example for square matrix inversion (i.e., n = 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).

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

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

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.

Table 3 :
Parameters used for generating the receiver-related measurement errors (1σ).

Table 5 :
Conditions for comparison.

Table 6 :
Computational cost of matrix inverse calculation.