# INS/GPS for High-Dynamic UAV-Based Applications

Authors: Junchuan Zhou; Stefan Knedlik; Otmar Loffeld
Journal: International Journal of Navigation and Observation
 (2012)

---

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


---

## Body

## 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 Section2, 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)Δρk=∫k-1kδρ̇dt=δρk-δρk-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(2)δρk=ρ̂k-ρk=Hkδxk+vk,δρk-1=ρ̂k-1-ρk-1=Hk-1δxk-1+vk-1,
where the error states are defined as δx=x̂estimated-xtrue; the Hk and Hk-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; vk and vk-1 represent the zero-mean Gaussian white noises.Substituting (2) into (1) yields(3)δρk-δρk-1=Hkδxk-Hk-1δxk-1+vk,k-1.For clarity purpose, we will denotevkϕ to represent the cumulative effect of the carrier-phase-derived delta pseudorange measurement noises vk,k-1 from time instants k-1 to k. We divide (3) by the time interval Δt=tk-tk-1 and assume that this time interval is small. Thus, the changes in the Jacobian matrix H are negligible (i.e., Hk-1≈Hk), and we arrive at(4)δρk-δρk-1Δt=Hk(δxk-δxk-1)Δt+vkϕ⟹δρ̇k=Hkδẋk+vkϕ,
where δẋ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., δẋ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(5)δρk-δρk-1=(ρ̂k-ρk)-(ρ̂k-1-ρk-1)=(ρ̂k-ρ̂k-1)-(ρk-ρk-1).We denoteŷkϕ=ρ̂k-ρ̂k-1,ykϕ=ρk-ρk-1 and δykϕ=ŷkϕ-ykϕ. Thus, (3) can be reformulated as(6)δykϕ=Hkδxk-Hk-1δxk-1+vkϕ.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 Hk-1δxk-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(7)[δxkδxd,k]=[Φk,k-1OOI][δxk-1δxd,k-1]+[wk-10],
where δxd,k denotes the delay error state vector; Φk,k-1 is the transition matrix for current states; I is an identity matrix; wk-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)Pk′=[PkPcd,kPdc,kPd,k],Qk′=[QkOOO],
where Pk is the error covariance of δxk; Pd,k is the error covariance of δxd,k, which is explicitly estimated from the previous measurement update; Pcd,k and Pdc,k are the cross-error covariance matrices between the current error state vector δxk and the delay error state vector δxd,k; Qk is the process noise error covariance of wk; O is a zero matrix.For the delta pseudorange observation model, it is given as(9)δykϕ=[Hk-Hd,k][δxkδxd,k]+vkϕ,
where Hk and Hd,k are the Jacobian matrices related to the time instants of the start and end of the integration interval; δykϕ is the delta pseudorange innovation vector; vkϕ 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)ρ̂k-ρk=Hkδxk+vkρ.Substitutingδykρ=ρ̂k-ρk into (10), the overall system observation model can be derived in its partitioned form as(11)[δykρδykϕ]=[HkOHk-Hd,k][δxkδxd,k]+[vkρvkϕ],withRk'=[RkρOORkϕ],
where Rkρ and Rkϕ 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)[KkKd,k]=[Pk-Pcd,k-Pdc,k-Pd,k-][HkT-Hd,kT]×[[Hk-Hd,k][Pk-Pcd,k-Pdc,k-Pd,k-]×[HkT-Hd,kT]+Rkϕ]-1,[δx̂k+δx̂d,k+]=[δx̂k-δx̂d,k-]+[KkKd,k]×[δykϕ-[Hk-Hd,k][δx̂k-δx̂d,k-]],[Pk+Pcd,k+Pdc,k+Pd,k+]=[[IOOI]-[KkKd,k][Hk-Hd,k]]×[Pk-Pcd,k-Pdc,k-Pd,k-],
where Kk is the Kalman gain for the current error state vector δxk, while Kd,k is the Kalman gain for the delay error state vector δxd,k.As stated before, the delay error states will not be updated from current measurement update. Thus, we can setKd,k to be a zero matrix. Hereby, (12) can be simplified as(13)Kk=[Pk-HkT-Pcd,k-Hd,kT]×[HkPk-HkT-Hd,kPdc,k-HkT-HkPcd,k-Hd,kT+Hd,kPd,k-Hd,kT+Rkϕ],-1δx̂k+=δx̂k-+Kk[δykϕ-Hkδx̂k-+Hd,kδx̂d,k-],Pk+=(I-KkHk)Pk-+KkHd,kPdc,k-,Pcd,k+=(I-KkHk)Pcd,k-+KkHd,kPd,k-,Pdc,k+=(Pcd,k+)T.
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., Pd,k-, Pdc,k-, Pcd,k-) is accounted for in the derivation of the Kalman gain Kk.In sequential update of delta pseudorange measurements, the following parameters need to be firstly initialized. We denoteHk(i) and Hd,k(i) as the ith row of Hk and Hd,k, respectively, and use Rkϕ(i,i) to represent the ith main diagonal element of Rkϕ.(14)Pk(0)=Pk-,Pcd,k(0)=Pcd,k-,Pdc,k(0)=Pdc,k-,δx̂k(0)=δx̂k-,Hk=[Hk(1)⋮Hk(m)],Hd,k=[Hd,k(1)⋮Hd,k(m)],Rkϕ=[Rkϕ(1,1)⋯0⋮⋱⋮0⋯Rkϕ(m,m)].Fromi=1 to m, the measurements are updated sequentially as follows:(15)Kk(i)=[Pk(i-1)Hk(i)T-Pcd,k(i-1)Hd,k(i)T]×[Hk(i)Pk(i-1)Hk(i)T-Hd,k(i)Pdc,k(i-1)Hk(i)T-Hk(i)Pcd,k(i-1)Hd,k(i)T+Hd,k(i)Pd,kHd,k(i)T+Rkϕ(i,i)]-1,δx̂k(i)=δx̂k(i-1)+Kk(i)×[δykϕ(i)-Hk(i)δx̂k(i-1)+Hd,k(i)δx̂d,k],Pk(i)=[1-Kk(i)Hk(i)]Pk(i-1)+Kk(i)Hd,k(i)Pdc,k(i-1),Pcd,k(i)=[1-Kk(i)Hk(i)]Pcd,k(i-1)+Kk(i)Hd,k(i)Pd,k,Pdc,k(i)=Pcd,k(i)T.


#### 3.1.2. Sequential Updates of Pseudorange Measurements

The batch processing of pseudorange measurements is handled after we process the delta pseudorange measurements. Thea posteriori estimates from the delta pseudorange measurement update (i.e., δx̂k+ and Pk+) are used here as the a priori estimates (i.e., δx̂k- and Pk-).(16)Kk=Pk-HkT(HkPk-HkT+Rkρ)-1,δx̂k+=δx̂k-+Kk[δykρ-Hkδx̂k-],Pk+=(I-KkHk)Pk-.For sequential processing, we define(17)Hk=[Hk(1)⋮Hk(m)],Rkρ=[Rkρ(1,1)⋯0⋮⋱⋮0⋯Rkρ(m,m)].Fromi=1 to m, the measurements are sequentially updated as(18)Kk(m+i)=Pk(m+i-1)Hk(i)T×[Hk(i)Pk(m+i-1)Hk(i)T+Rkρ(i,i)]-1,δx̂k(m+i)=δx̂k(m+i-1)+Kk(m+i)×[δykρ(i)-Hk(i)δx̂k(m+i-1)],Pk(m+i)=[1-Kk(m+i)Hk(i)]Pk(m+i-1).After all the measurements are updated, thea posteriori estimates are calculated as(19)δx̂k+=δx̂k(m+m),Pk+=Pk(m+m).They are used as the delay states and covariance parameters in the next recursion(20)δx̂d,k+1=δx̂k(m+m),Pd,k+1=Pk(m+m).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.
OperationNum.Add.Multi.OperationNum.Add.Multi.nn×nm3 (2)(n-1)nmmn2nn×n13(2)×mn(n-1)n2nm×mn5 (4)(m-1)n2mn2n1+n13(2)×mn—nm+nm1 (1)nm—1n×nn10(8)×mn(n-1)n2nm×mm2 (1)(m-1)mnnm21n×n18(6)×mn-1nmn×nn10 (8)mn(n-1)mn21+18(6)×m1—mn×nm5 (4)(n-1)m2nm21×12(1)×m—1mm+mm5 (4)m2—n1×14(2)×m—nnn+nn5 (4)n2—n1×1n5(4)×m—n2mn×n13 (2)m(n-1)mnnn+nn5(4)×mn2—m1+m13 (2)m—nm×m12 (1)(m-1)nnmn1+n12 (1)n—inv(mm)2 (1)(3/2)m3-2m2+(m/2)(3/2)m3-(1/2)mTable 2
The sum of additions and multiplications involved in batch and sequential processing.
OperationsAdditionsOperationsMultiplicationsBatch3m3+(7n-4)m2+(18n2-9n+1)mBatch3m3+7nm2+(18n2+5n-1)mSequential(18n2-2n)mSequential(18n2+12n+2)mThe correctness of the calculations in Table2 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 18n2-2n for addition, and 18n2+12n+2 for multiplication, which are obviously equal to the corresponding calculations in sequential processing.Based on the calculations in Table2, 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
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.Figure1 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.Figure2 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.


## 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)[KkKd,k]=[Pk-Pcd,k-Pdc,k-Pd,k-][HkT-Hd,kT]×[[Hk-Hd,k][Pk-Pcd,k-Pdc,k-Pd,k-]×[HkT-Hd,kT]+Rkϕ]-1,[δx̂k+δx̂d,k+]=[δx̂k-δx̂d,k-]+[KkKd,k]×[δykϕ-[Hk-Hd,k][δx̂k-δx̂d,k-]],[Pk+Pcd,k+Pdc,k+Pd,k+]=[[IOOI]-[KkKd,k][Hk-Hd,k]]×[Pk-Pcd,k-Pdc,k-Pd,k-],
where Kk is the Kalman gain for the current error state vector δxk, while Kd,k is the Kalman gain for the delay error state vector δxd,k.As stated before, the delay error states will not be updated from current measurement update. Thus, we can setKd,k to be a zero matrix. Hereby, (12) can be simplified as(13)Kk=[Pk-HkT-Pcd,k-Hd,kT]×[HkPk-HkT-Hd,kPdc,k-HkT-HkPcd,k-Hd,kT+Hd,kPd,k-Hd,kT+Rkϕ],-1δx̂k+=δx̂k-+Kk[δykϕ-Hkδx̂k-+Hd,kδx̂d,k-],Pk+=(I-KkHk)Pk-+KkHd,kPdc,k-,Pcd,k+=(I-KkHk)Pcd,k-+KkHd,kPd,k-,Pdc,k+=(Pcd,k+)T.
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., Pd,k-, Pdc,k-, Pcd,k-) is accounted for in the derivation of the Kalman gain Kk.In sequential update of delta pseudorange measurements, the following parameters need to be firstly initialized. We denoteHk(i) and Hd,k(i) as the ith row of Hk and Hd,k, respectively, and use Rkϕ(i,i) to represent the ith main diagonal element of Rkϕ.(14)Pk(0)=Pk-,Pcd,k(0)=Pcd,k-,Pdc,k(0)=Pdc,k-,δx̂k(0)=δx̂k-,Hk=[Hk(1)⋮Hk(m)],Hd,k=[Hd,k(1)⋮Hd,k(m)],Rkϕ=[Rkϕ(1,1)⋯0⋮⋱⋮0⋯Rkϕ(m,m)].Fromi=1 to m, the measurements are updated sequentially as follows:(15)Kk(i)=[Pk(i-1)Hk(i)T-Pcd,k(i-1)Hd,k(i)T]×[Hk(i)Pk(i-1)Hk(i)T-Hd,k(i)Pdc,k(i-1)Hk(i)T-Hk(i)Pcd,k(i-1)Hd,k(i)T+Hd,k(i)Pd,kHd,k(i)T+Rkϕ(i,i)]-1,δx̂k(i)=δx̂k(i-1)+Kk(i)×[δykϕ(i)-Hk(i)δx̂k(i-1)+Hd,k(i)δx̂d,k],Pk(i)=[1-Kk(i)Hk(i)]Pk(i-1)+Kk(i)Hd,k(i)Pdc,k(i-1),Pcd,k(i)=[1-Kk(i)Hk(i)]Pcd,k(i-1)+Kk(i)Hd,k(i)Pd,k,Pdc,k(i)=Pcd,k(i)T.


### 3.1.2. Sequential Updates of Pseudorange Measurements

The batch processing of pseudorange measurements is handled after we process the delta pseudorange measurements. Thea posteriori estimates from the delta pseudorange measurement update (i.e., δx̂k+ and Pk+) are used here as the a priori estimates (i.e., δx̂k- and Pk-).(16)Kk=Pk-HkT(HkPk-HkT+Rkρ)-1,δx̂k+=δx̂k-+Kk[δykρ-Hkδx̂k-],Pk+=(I-KkHk)Pk-.For sequential processing, we define(17)Hk=[Hk(1)⋮Hk(m)],Rkρ=[Rkρ(1,1)⋯0⋮⋱⋮0⋯Rkρ(m,m)].Fromi=1 to m, the measurements are sequentially updated as(18)Kk(m+i)=Pk(m+i-1)Hk(i)T×[Hk(i)Pk(m+i-1)Hk(i)T+Rkρ(i,i)]-1,δx̂k(m+i)=δx̂k(m+i-1)+Kk(m+i)×[δykρ(i)-Hk(i)δx̂k(m+i-1)],Pk(m+i)=[1-Kk(m+i)Hk(i)]Pk(m+i-1).After all the measurements are updated, thea posteriori estimates are calculated as(19)δx̂k+=δx̂k(m+m),Pk+=Pk(m+m).They are used as the delay states and covariance parameters in the next recursion(20)δx̂d,k+1=δx̂k(m+m),Pd,k+1=Pk(m+m).The whole process repeats until the end of the trajectory.


## 3.1.1. Sequential Updates of Delta Pseudorange Measurements

The conventional batch measurement update of delta pseudorange measurements is performed as follows:(12)[KkKd,k]=[Pk-Pcd,k-Pdc,k-Pd,k-][HkT-Hd,kT]×[[Hk-Hd,k][Pk-Pcd,k-Pdc,k-Pd,k-]×[HkT-Hd,kT]+Rkϕ]-1,[δx̂k+δx̂d,k+]=[δx̂k-δx̂d,k-]+[KkKd,k]×[δykϕ-[Hk-Hd,k][δx̂k-δx̂d,k-]],[Pk+Pcd,k+Pdc,k+Pd,k+]=[[IOOI]-[KkKd,k][Hk-Hd,k]]×[Pk-Pcd,k-Pdc,k-Pd,k-],
where Kk is the Kalman gain for the current error state vector δxk, while Kd,k is the Kalman gain for the delay error state vector δxd,k.As stated before, the delay error states will not be updated from current measurement update. Thus, we can setKd,k to be a zero matrix. Hereby, (12) can be simplified as(13)Kk=[Pk-HkT-Pcd,k-Hd,kT]×[HkPk-HkT-Hd,kPdc,k-HkT-HkPcd,k-Hd,kT+Hd,kPd,k-Hd,kT+Rkϕ],-1δx̂k+=δx̂k-+Kk[δykϕ-Hkδx̂k-+Hd,kδx̂d,k-],Pk+=(I-KkHk)Pk-+KkHd,kPdc,k-,Pcd,k+=(I-KkHk)Pcd,k-+KkHd,kPd,k-,Pdc,k+=(Pcd,k+)T.
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., Pd,k-, Pdc,k-, Pcd,k-) is accounted for in the derivation of the Kalman gain Kk.In sequential update of delta pseudorange measurements, the following parameters need to be firstly initialized. We denoteHk(i) and Hd,k(i) as the ith row of Hk and Hd,k, respectively, and use Rkϕ(i,i) to represent the ith main diagonal element of Rkϕ.(14)Pk(0)=Pk-,Pcd,k(0)=Pcd,k-,Pdc,k(0)=Pdc,k-,δx̂k(0)=δx̂k-,Hk=[Hk(1)⋮Hk(m)],Hd,k=[Hd,k(1)⋮Hd,k(m)],Rkϕ=[Rkϕ(1,1)⋯0⋮⋱⋮0⋯Rkϕ(m,m)].Fromi=1 to m, the measurements are updated sequentially as follows:(15)Kk(i)=[Pk(i-1)Hk(i)T-Pcd,k(i-1)Hd,k(i)T]×[Hk(i)Pk(i-1)Hk(i)T-Hd,k(i)Pdc,k(i-1)Hk(i)T-Hk(i)Pcd,k(i-1)Hd,k(i)T+Hd,k(i)Pd,kHd,k(i)T+Rkϕ(i,i)]-1,δx̂k(i)=δx̂k(i-1)+Kk(i)×[δykϕ(i)-Hk(i)δx̂k(i-1)+Hd,k(i)δx̂d,k],Pk(i)=[1-Kk(i)Hk(i)]Pk(i-1)+Kk(i)Hd,k(i)Pdc,k(i-1),Pcd,k(i)=[1-Kk(i)Hk(i)]Pcd,k(i-1)+Kk(i)Hd,k(i)Pd,k,Pdc,k(i)=Pcd,k(i)T.


## 3.1.2. Sequential Updates of Pseudorange Measurements

The batch processing of pseudorange measurements is handled after we process the delta pseudorange measurements. Thea posteriori estimates from the delta pseudorange measurement update (i.e., δx̂k+ and Pk+) are used here as the a priori estimates (i.e., δx̂k- and Pk-).(16)Kk=Pk-HkT(HkPk-HkT+Rkρ)-1,δx̂k+=δx̂k-+Kk[δykρ-Hkδx̂k-],Pk+=(I-KkHk)Pk-.For sequential processing, we define(17)Hk=[Hk(1)⋮Hk(m)],Rkρ=[Rkρ(1,1)⋯0⋮⋱⋮0⋯Rkρ(m,m)].Fromi=1 to m, the measurements are sequentially updated as(18)Kk(m+i)=Pk(m+i-1)Hk(i)T×[Hk(i)Pk(m+i-1)Hk(i)T+Rkρ(i,i)]-1,δx̂k(m+i)=δx̂k(m+i-1)+Kk(m+i)×[δykρ(i)-Hk(i)δx̂k(m+i-1)],Pk(m+i)=[1-Kk(m+i)Hk(i)]Pk(m+i-1).After all the measurements are updated, thea posteriori estimates are calculated as(19)δx̂k+=δx̂k(m+m),Pk+=Pk(m+m).They are used as the delay states and covariance parameters in the next recursion(20)δx̂d,k+1=δx̂k(m+m),Pd,k+1=Pk(m+m).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.
OperationNum.Add.Multi.OperationNum.Add.Multi.nn×nm3 (2)(n-1)nmmn2nn×n13(2)×mn(n-1)n2nm×mn5 (4)(m-1)n2mn2n1+n13(2)×mn—nm+nm1 (1)nm—1n×nn10(8)×mn(n-1)n2nm×mm2 (1)(m-1)mnnm21n×n18(6)×mn-1nmn×nn10 (8)mn(n-1)mn21+18(6)×m1—mn×nm5 (4)(n-1)m2nm21×12(1)×m—1mm+mm5 (4)m2—n1×14(2)×m—nnn+nn5 (4)n2—n1×1n5(4)×m—n2mn×n13 (2)m(n-1)mnnn+nn5(4)×mn2—m1+m13 (2)m—nm×m12 (1)(m-1)nnmn1+n12 (1)n—inv(mm)2 (1)(3/2)m3-2m2+(m/2)(3/2)m3-(1/2)mTable 2
The sum of additions and multiplications involved in batch and sequential processing.
OperationsAdditionsOperationsMultiplicationsBatch3m3+(7n-4)m2+(18n2-9n+1)mBatch3m3+7nm2+(18n2+5n-1)mSequential(18n2-2n)mSequential(18n2+12n+2)mThe correctness of the calculations in Table2 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 18n2-2n for addition, and 18n2+12n+2 for multiplication, which are obviously equal to the corresponding calculations in sequential processing.Based on the calculations in Table2, 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
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.Figure1 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.Figure2 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:(21)pn,k+1=pn,k+vn,k⋅Δt,vn,k+1=vn,k+[Rb2n,k⋅(f̃b,k-f̂b,kerror-wf)+gn]⋅Δt,ψk+1=ψk+Eb2n,k⋅(ω̃b,k-ω̂b,kerror-wω)⋅Δt,
where k is the time instant; f̃b is the measurement vector of the specific force; ω̃b represents the measurement vector of angular rate; gn is the local gravity vector; ψ is the Euler angles; f̂b,kerror and ω̂b,kerror are the estimated specific force and angular rate errors; wf and wω are the remaining noise errors, which are assumed to be zero-mean Gaussian white noises; Rb2n is the frame rotation matrix from body frame to NED navigation frame; Eb2n is the rotation rate transformation matrix from body to navigation frame,(22)Rb2n=(CγCβCγSβSα-SγCαCγSβCα+SγSαSγCβSγSβSα+CγCαSγSβCα-CγSα-SβCβSαCβCα),Eb2n=(1SαTβCαTβ0Cα-Sα0Sα/CβCα/Cβ),
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 δx=x̂estimated-xtrue. 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:(23)[δpn,k+1δvn,k+1δψk+1δfb,k+1errorδωb,k+1errorcδtk+1cδṫk+1]=[I3×3I3×3ΔtO3×3O3×3O3×303×103×1O3×3I3×3F23-Rb2nΔtO3×303×103×1O3×3O3×3F33O3×3-Eb2nΔt03×103×1O3×3O3×3O3×3I3×3O3×303×103×1O3×3O3×3O3×3O3×3I3×303×103×103×1T03×1T03×1T03×1T03×1T1Δt03×1T03×1T03×1T03×1T03×1T01]⋅[δpn,kδvn,kδψkδfb,kerrorδωb,kerrorcδtkcδṫk]+wk.In (23), the gyroscope and accelerometer sensor errors δfberror and δωberror are modeled as random-walk plus constant (e.g., [8]). The receiver clock drift error cδṫ is modeled as constant plus random walk process, while the receiver clock bias cδt is the integral of the clock drift error. The sub-matrices F33 and F23 are given as:(24)F33=[1+(ω̂yCαTβ+ω̂zTβSα)Δt[ω̂zCα(T2β+1)+ω̂ySα(T2β+1)]Δt0-[ω̂zCα+ω̂ySα]Δt10[ω̂yCα-ω̂zSαCβ]Δt[CαSβω̂z+SβSαω̂yC2β]Δt1],F23=[f̂y(SαSγ+CαCγSβ)+f̂z(CαSγ+CγSβSα)f̂zCβCαCγ-f̂xCγSβ+f̂yCβCγSαf̂z(CγSα+CαSβSγ)-f̂y(CαCγ-SβSαSγ)-f̂xCβSγ-f̂z(SβSαSγ+CαCγ)-f̂y(CγSα-CαSβSγ)f̂zCβCαSγ-f̂xSβSγ+f̂yCβSαSγf̂z(SαSγ+CαCγSβ)-f̂y(CαSγ-CγSβSα)+f̂xCβCγf̂yCβCα-f̂zCβSα-f̂xCβ-f̂zCαSβ-f̂ySβSα0]⋅Δt,where C2X,T2X represent the square of cosine and tangent trigonometric operations of X, and f̂x=f̃x-f̂xerror, f̂y=f̃y-f̂yerror, f̂z=f̃z-f̂zerror, ω̂y=ω̃y-ω̂yerror, ω̂z=ω̃z-ω̂zerror 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)), Hd,k and Hk 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:(25)Hk=[ln,ksv(i)le,ksv(i)ld,ksv(i)0⋯010]i×17,
where ln,ksv(i),le,ksv(i),ld,ksv(i)(i=1tom) 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 Hd,k is identical with Hk, 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 Hd,k. Instead, we can simply use previous epoch observation matrix Hk-1 as Hd,k 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 Figures3 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 Table3 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σ).
Receiver thermal noiseMultipath errorsPseudorange (DLL)0.5 m0.4 mDoppler (FLL)0.05 m/s0.04 m/sCarrier phase (PLL)0.001 m0.004 mTable 4
Landmark 20 eXT MEMS-based IMU performance specification.
Gyroscope (angular rates)Bias in-run stabilityNoise (ARW)Scale factor error20 [°/h] (1σ)0.035 [°/s/√Hz] (1σ)≤1000 [ppm]Accelerometer (specific forces)Bias in-run stabilityNoise (VRW)Scale factor error20 [μg] (1σ)40 [μg/√Hz] (1σ)≤1000 [ppm]Simulated LandMark 20 eXT MEMS IMU raw data.
(a)(b)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∥Δx∥=Δxn2+Δxe2+Δxd2. 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.
Red“Augmented” system using pseudorange and delta pseudorange measurementsGreen“Augmented” system using delta pseudorange measurements aloneBlueSystem using pseudorange for position and delta pseudorange for velocityBlackSystem using pseudorange for position and Doppler shift for velocityComparison of position and velocity estimates (the 3rd subplot is the zoom-in of the 2nd subplot).
(a)(b)(c)Comparison of attitude estimates (the 2nd subplot is the zoom-in of the 1st subplot).
(a)(b)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 Figure6 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 Figure6 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 Figures6 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. Figure8 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.System performance comparisons.
(a)(b)(c)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.,Pcd,k and Pdc,k) need to be carefully treated. In sequential processing, among the measurement scalar updates, these cross covariance matrices, that is, Pcd,k and Pdc,k, are updated from one measurement to the other with zero-width time interval, as shown in (15).


## 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∥Δx∥=Δxn2+Δxe2+Δxd2. 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.
Red“Augmented” system using pseudorange and delta pseudorange measurementsGreen“Augmented” system using delta pseudorange measurements aloneBlueSystem using pseudorange for position and delta pseudorange for velocityBlackSystem using pseudorange for position and Doppler shift for velocityComparison of position and velocity estimates (the 3rd subplot is the zoom-in of the 2nd subplot).
(a)(b)(c)Comparison of attitude estimates (the 2nd subplot is the zoom-in of the 1st subplot).
(a)(b)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 Figure6 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 Figure6 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 Figures6 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. Figure8 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.System performance comparisons.
(a)(b)(c)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.,Pcd,k and Pdc,k) need to be carefully treated. In sequential processing, among the measurement scalar updates, these cross covariance matrices, that is, Pcd,k and Pdc,k, 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.



---

*Source: 678596-2012-02-27.xml*