LC integration combines estimates from GPS and INS outputs, and such integration might be basically performed in two different ways. The first one, referred as open loop, estimates INS error by exploiting GPS information, and does not interfere with the operation of INS. The second approach, named closed loop, involves the use of a Kalman filter to mitigate INS errors.

In our work we have employed the second method where a Kalman filter calculates position and velocity error states to correct the INS solution. The block diagram of the closed-loop LC solution is shown in Figure 1.

Independent position and velocity estimates are calculated within a GPS receiver and are optionally filtered. Then, the output of this filter is used periodically as input to an INS filter. The second Kalman filter uses the difference between the GPS-derived positions, velocities and the ones computed by means of an INS device to get the error estimates.

The design of the Kalman filter for loosely coupled integration is described later in this Section. An INS filter generally consists of nine navigation error states, including three positions Δr̲ⁿ, three velocities Δv̲ⁿ and three attitude error states ε̲ⁿ, see [3,11–17]. For convenience, the symbols x̲ and x _ _ are used from this moment on to indicate a vector and matrix, respectively. Due to the presence of noise in the inertial sensor measurements, the system state vector needs to be enlarged depending on the inertial sensor's error characteristics.

The increased number of the error states could include bias error estimations both of the INS gyros and accelerometers (b̲_g and b̲_a) and/or the scale factor estimations (S̲_g and S̲_a). The output noise within accelerometer and gyro measurements may be represented as: (1) noise acc = ( 1 + S _ a ) ⋅ f _ + b _ a ( t ) + w _ a noise gyro = ( 1 + S _ g ) ⋅ f _ + b _ g ( t ) + w _ gwhere:

f̲ is the correct acceleration or angular velocity (in the body frame);

In the paper we have modeled the accelerometers and gyroscopes' noises as white noise components (i.e., w̲_a, w̲_g) and we did not consider the deterministic errors, such as the scale factor and the bias, since their contribution is negligible. Moreover, we have used both simulated and real data to model gyro and accelerometers errors. As far as the simulation test is concerned, we have developed a proper software in Matlab^® able to generate accelerometers and gyros raw measurements at different rates (e.g., sampling frequency at 500 or 100 Hz) and with different noise components. Furthermore, the simulator can provide information about the orientation angles (yaw, pitch and roll) of a vehicle that is moving along a trajectory. As far as the simulated path is concerned, we have implemented the same surveyed track of the real scenario in which the tests have performed, giving us the possibility to do accurate comparison between synthetic data and the real ones. In the case of real data we have utilized measurements from a 3DM-Gx2 MEMS-IMU. It has been argued in [11] that gyro and accelerometer errors of this device are dominated by white noise (see Figure 2). Therefore, a 9-state Kalman filter should be adequate for correcting inertial solutions.

The perturbation of the inertial navigation equations to obtain error states is detailed in [3]. A scheme that summarizes the overall n-frame INS processes is provided in Figure 3. A common orientation for Local Tangent Plane is the North-East-Down (NED) system defined as follows:

Xⁿ horizontal axis in the direction of increasing latitude;

In the following we will refer to the NED coordinate system as n-frame.

The position in the n-frame is expressed in geodetic coordinates, namely (2) r _ n = [ φ , λ , h ] Twhere φ, λ and h represent the latitude, longitude and altitude of the estimated user's position, expressed in radians and meters (for altitude) respectively.

The velocities in the n-frame are given by: (3) v _ n = [ v N , v E , v D ] Twhere v_N, v_E, v_D are the velocities along North, East and Down coordinates and computed in m/s.

The motion of a vehicle can be described by equations that involve INS kinematics. The derivations of these equations can be broken up into three parts: position, velocity and attitude. A full derivation is reported in [12]. The position, velocity and attitude rates (from [3] and [13]) are given by: (4) [ r ˙ _ n v ˙ _ n Ω ˙ _ ] = [ D ═ − 1 ⋅ v _ n R ═ b n ⋅ f _ b − ( 2 ω _ ie n + ω _ en n ) × v _ n + g _ n R ═ ⋅ ( ω _ ib n ) ]D͇⁻¹ is a diagonal matrix defined as follows: (5) D ═ − 1 = [ 1 R M + h 0 0 0 1 ′ ( R N + h ) cos φ 0 0 0 − 1 ]where R_M is the radius of curvature in the meridian and R_N is the prime vertical at certain latitude expressed as: (6) R M = a ( 1 − e 2 ) ( 1 − e 2 ⋅ sin 2 φ ) 3 / 2 R N = a ( 1 − e 2 ⋅ sin 2 φ ) 1 / 2with a = 6378317.0 m and e = 0.0818 and where f̲^b is the acceleration information in the body-frame and R _ b n is the frame rotation matrix from body to n-frame. (7) R _ b n = [ cos ψ cos θ cos ψ sin θ sin ϕ − sin ψ cos φ cos ψ sin θ cos ϕ + sin ψ sin ϕ sin ψ cos θ sin ψ sin θ sin ϕ + cos ψ cos ϕ sin ψ sin θ cos ϕ − cos ψ sin ϕ − sin θ cos ψ sin ϕ cos θ cos ϕ ]

A more detailed explanation of the previous equations can be found in [3] and [17]. The vector Ω̲ = [φ, θ, ψ] consists of the Euler angles (Roll, Pitch and Yaw). The orientation angles are computed by exploiting the gyroscopes sensors. Other techniques are based on a blending of accelerometers and magnetometers to compute the attitude [18,19]. Although this last method is particularly suitable for low-cost MEMS IMUs whose gyroscopes are not sensitive to the Earth's rotation (for this reason the yaw can not be estimated properly through gyro-compassing techniques [20]), it requires, on the other hand, an additional Kalman filter to combine the measurements coming from the two sensors (i.e., accelerometers and magnetometers). Therefore, we prefer to keep the integration level as simple as possible and we will design a unique GPS/INS Kalman filter where the yaw information is provided by the GPS receiver itself [21].

ω _ ie n, ω _ en n are the rotation vectors from the e-frame to the n-frame and the rate of change of latitude and longitude, respectively [3]: (8) ω _ ie n = [ ω e cos φ 0 − ω e sin φ ] ω _ ie n = [ v E R N + h − v N R M + h − v E tan φ R N + h ]where (ω_e ≈ 7.2921155·10⁵ rad/s) is the magnitude of the Earth rotation rate.

g̲ⁿ is the local gravity vector and R͇ is the transformation matrix from the body-axes angular rates to the Euler angle angular rates and is given by [3] as: (9) R = = [ 1 sin ϕ tan θ cos ϕ tan θ 0 cos ϕ − sin ϕ 0 cos θ sin ϕ cos θ cos ϕ ]

ω _ i b b represents the raw measurement vector of the gyros sensors in the bodyframe [3].

Eventually, the equations describing the error dynamics are obtained by perturbing the kinematic Equation (4). These error equations are required in the construction of the INS/GPS Kalman filter. The perturbation of the position, velocity and Euler angles can be written as: (10) r ^ _ n = r _ n + δ r _ n . v ^ _ n = v _ n + δ v _ n . Ω ^ _ n = Ω _ n + δ Ω _ n .

The linearized position error is given by: (11) δ r ˙ _ n = F ═ rr δ r _ n + F ═ rv δ v _ n . F ═ rr = [ 0 0 − v N ( R M + h ) 2 v E sin ϕ ( R N + h ) cos 2 ϕ 0 − v E ( R N + h ) 2 cos ϕ 0 0 0 ] F ═ rv = D ═ − 1

The velocity error is given by: (12) δ v ˙ _ n = F ═ vr δ r _ n + F ═ vv δ v _ n + ( f _ n × ) δ Ω _ n + R ═ b n δ f _ b F ═ vr = [ − 2 v E ω e cos φ − v E 2 ( R N + h ) cos 2 φ 0 − v N v D ( R M + h ) 2 + v E 2 tan φ ( R N + h ) 2 2 ω e ( v N cos φ − v D sin φ ) + v E v N ( R N + h ) cos 2 φ 0 − v E ( v D + v N tan φ ) ( R N + h ) 2 2 v E ω e sin φ 0 v E 2 ( R N + h ) 2 + v N 2 ( R M + h ) 2 − 2 γ R + h ] γ = γ 0 ( R R + h ) 2 withR = R M R N F ═ vv = [ v D R M + h 2 ω e sin φ − 2 v E tan φ R N + h v N R M + h 2 ω e sin φ + v E tan φ R N + h v D + v N tan φ R N + h 2 ω e cos φ + v E R N + h − 2 v N R M + h − 2 ω e cos φ − 2 v E R N + h 0 ]

The attitude error can be written as: (13) δ Ω ˙ _ n = = F ═ er δ r _ n + F ═ ev δ v _ n − ( ω in n × ) Ω _ n − R ═ b n δ ω _ i b b . F ═ er = [ − ω e sin φ 0 − v E ( R N + h ) 2 0 0 − v N ( R M + h ) 2 − ω e cos φ − − v E ( R N + h ) cos 2 φ 0 v E tan φ ( R N + h ) 2 ] F ═ ev = [ 0 1 R N + h 0 − 1 R M + h 0 0 0 tan φ R N + h 0 ]Details about Equations (10)–(13) can be found in [3,11–17].

In this subsection we recall the traditional design of an error state Kalman filter for a loosely coupled GPS/INS application. An n-frame error state model [3] is given by: (14) [ δ r ˙ _ n δ v ˙ _ n δ Ω ˙ _ n ] = [ F ═ rr F ═ rv 0 3 × 3 F ═ vr F ═ vv ( f _ n × ) F ═ er F ═ ev − ( ω _ in n × ) ︸ F ] ⋅ [ δ r _ n δ v _ n δ Ω _ n ] + w _ .where all the sub-matrices F͇_xx are the ones as stated in Equations (10)–(13). f̲ⁿ is the raw measurements of accelerometers expressed in n-frame whereas ω _ in n is the raw information of the gyros sensors in the n-frame too. Eventually, parameters w̲ indicate the noise components of gyros and accelerometers, respectively. As previously explained in Section 2.1, only white noise has been considered. The discrete-time analogue of (14) is expressed as: (15) Φ _ K = e F _ ( t k ) Δ t = I ═ + F ═ ( t k ) Δ t + ( F ═ ( t k ) Δ t ) 2 2 + h . o . twhere h.o.t. means higher order terms that can be neglected for the computation.

The covariance matrix Q_K associated to the discrete-time noise vector w̲ can be determined by the approximate expression [14]: (16) Q ═ K ≈ 1 2 [ Φ _ K G ═ ( t K ) Q ═ ( t K ) G ═ T ( t K ) + G ═ ( t K ) Q ═ ( t K ) G ═ T ( t K ) Φ _ K T ] ⋅ Δ twhere Δt is the sampling time that we set equal to 1 s and G͇ is a matrix equal to: (17) G ═ = [ 0 3 × 3 0 3 × 3 R b n 0 3 × 3 0 3 × 3 − R b n ] .Q͇ is a diagonal matrix representing the white noise on the accelerometers q̲_a and q̲_g gyros that can be stated as: (18) Q ═ = [ diag ( q _ a ) 0 3 × 3 0 3 × 3 diag ( q _ g ) ] ·

In a loosely-coupled integration approach, the filter measurement is the difference between the INS and the GPS navigation solutions. The measurement vector is given by: (19) z _ = [ δ R _ n δ V _ n ] = [ r _ INS n − r _ GPS n v _ INS n − v GPS n ] = [ φ INS − φ GPS λ INS − λ GPS h INS − h GPS v _ INS n − v _ GPS n ] .

Following the approach suggested in [3], since φ and λ are in radians and their values are very small, we multiply the first two rows of Equation (19) by (R_M + h) and (R_N + h)cos φ to obtain: (20) z _ = [ ( R M + h ) ( φ INS − φ GPS ) ( R N + h ) cos φ ( λ INS − λ GPS ) h INS − h GPS v _ INS n − v _ GPS n ] .

The design matrix becomes: (21) H ═ = [ R M + h 0 0 0 0 0 0 0 0 0 ( R N + h ) cos φ 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 ] .

Finally, the measurement noise covariance matrix is calculated as: (22) R ═ = [ ( R M + h ) 2 ⋅ σ φ 2 0 0 0 0 0 0 ( R N + h ) 2 ⋅ cos 2 φ ⋅ σ λ 2 0 0 0 0 0 0 σ h 2 0 0 0 0 0 0 σ V N 2 0 0 0 0 0 0 σ V E 2 0 0 0 0 0 0 σ V D 2 ] .

The value of each element in the diagonal matrix R depends on the accuracy of the GPS estimates. A detailed description of Kalman filtering combining models with measurements is reported in [15]. However, since we use a feedback loosely coupled approach, the error state vector is set to zero after every measurement updates [16,17,22].

Tests were carried out along a surveyed track located within CSIRO's site in Pullenvale, QLD, Australia. A 3-D plot of the test track and is shown in Figure 4.

We investigated the algorithm's performance using both simulated data and real data. In the first case we have generated synthetic gyros and accelerometers values along a path that perfectly matches the realistic scenario. This simulation will be used as a term of comparison with the results we obtained by working with real GPS and INS measurements on the field.

We have also corrupted the simulated data, representing the inertial accelerometer and gyro, with noise sources resembling the typical characteristics of a low-cost MEMS IMU, as in Table 1.

Simulated GPS positions and velocities' estimates were also generated, and the variances of those measurements were set equal to the values reported in Table 2.

The variances of Table 2 refer to the case of unfiltered GPS measurements. By utilizing the synthetic GPS and INS data we were able to run the LC algorithm and the results can be seen in Figure 5.

We have repeated the test, by using this time, data directly read from the GPS and the IMU sensors:

raw gyro and accelerometer outputs from a 3DM-Gx2 INS after an initial calibration,

In our LC GPS/INS implementation, the process noise covariance, Q, was estimated through the Allan Variance technique which is detailed in [11]. The components of the measurement noise covariance, R, were selected as summarized in Table 3.

The results of such a GPS/INS hybridization technique are plotted in Figure 6.

It is clear from Figure 5 that the performance of the LC integration solution (red line) significantly improves the performance of the INS-only solution (black line). In fact, the LC approach trusts the GPS estimates (when they are available) and does not allow the INS navigation solution to drift. The main difference between the synthesized measurements and the real ones is that the positions obtained by the simulated GPS are centred on the reference track and the deviations are modelled as white noise. On the other hand, in a real scenario, the user's position estimations at consecutive time instants are correlated because they are processed within the GPS receiver by a proper Kalman filter. Such measurements can have a bias offset with respect to the surveyed path (as it can be noted in Figure 6). This fact can be explained by considering that a GPS system has an accuracy that depends on several factors: quality of the GPS receiver, the algorithm used to compute the Position-Velocity-Time (PVT) with or without carrier phase information, the effect of ionosphere compensation in the PVT estimation, the number of visible satellites when the test is performed. Another aspect that is clear by observing Figures 5 and 6 is the different time required to the vehicle to complete the path when simulated or real data have been used. This fact should not be surprising if we consider the velocity profile of the two figures. By comparing the velocity along the eastern and northern directions, we can notice how the estimated speed is higher when real GPS data are applied with respect to the case of simulated data. As a consequence, the time necessary to run the trajectory will be shorter. Although the solution provided by GPS is sufficiently accurate (e.g. notice in Figure 6 the improvement in the Euler angles' estimation when the INS is aided by the GPS), it is still unable to fulfil the requirements of continuity and reliability in many situations. In order to reduce the error of the GPS we have designed a LC integration scheme that uses a Kalman filter with some constraints. This aspect will be described in details in the next Section and details on constrained Kalman filters can be found in [23–26].

It is well known that GPS estimation is affected by a certain error that is strongly dependent on the number of satellites available for the PVT estimation. Thus, as a consequence, the LC solution trusts the GPS position and velocity estimates in a way proportional to number of satellites in view. Therefore, the overall performance of the LC solution will leverage on the availability of the GPS updates. In order to improve the performance with respect to the results shown in Figures 5 and 6 we have designed a Kalman filter that exploits additional aiding information. In particular, we have considered an external source that provides some additional geographical data. For instance, such pieces of information can be obtained by the Google Maps service, as shown in the block diagram depicted in Figure 7. Such an implementation requires that the user is equipped with a smartphone designed to embed a GPS receiver, accelerometers and gyros sensors as well as a communication transceiver able to establish a wireless internet connection. In this way the user is able to receive information about the road being travelled, as provided by the Google Maps (GM) service.

As far as the accuracy of GM is concerned, the last improvements in terms of position precision developed by Google on GE and GM can be found in [27]. It has been shown in [28] that comparing GM and Google Earth (GE), the difference between the two mapping systems is of 2.5 m. On the other hand, when he repeated the test by plotting the point on high-accuracy map he noticed a difference of 10 m with respect to the GM.

This means the GM can not be used for systems that require a high level of accuracy but it could be fine for mass-market applications. Another concern of using a Google Maps service is the process of receiving the map information should be quick enough to be applied in real-time and the internet connection should always be available for all the duration of the test. If this happens we can build up a more complex LC scheme that also integrates street constraints to estimate the user's position and velocity.

In Figure 8 an example of results obtained using as constraints the boundaries of the road is shown. The blue line represents a simulated GPS position estimation over time whereas the red line indicates the boundary of the path the user is driving along. Both the GPS and constraint information are expressed in latitude, longitude and altitude coordinate respectively. In this example we have supposed the street is 10meters wide in both latitude and longitude. As for the altitude we have considered that the GPS estimation can be acceptable only when it falls within an interval of 4 meters with respect to the altitude information received by Google Maps. The boundaries of the hypothetical road have been computed setting the offsets Δn = 10 [m], Δe = [m] and then obtaining the Coordinates offsets (in radians) Δ φ = Δ n R and Δ λ = Δ e R cos ( π φ 180 ), where R = 6378137 [m] is the Earth radius and where φ, λ are the latitude and longitude coordinates as stated in Equation (2).

The offset in the position in decimal degrees is obtained as φ off = φ + Δ φ 180 π and λ off = λ + Δ λ 180 π.

Constraint information can be added by increasing the dimensions of the output mapping matrix H͇ of Equation (21) and the measurements z̲ of Equation (20) as: (23) z _ = [ ( R M + h ) ( φ INS − φ GPS ) ( R N + h ) cos φ ( λ INS − λ GPS ) h INS − h GPS v _ INS n − v _ GPS n ( R M + h ) ( φ INS − φ CONSTRAINT ) ( R N + h ) cos φ ( λ INS − λ CONSTRAINT ) h INS − h CONSTRAINT ] . (24) H ═ = [ R M + h 0 0 0 0 0 0 0 0 0 ( R N + h ) cos φ 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 R M + h 0 0 0 0 0 0 0 0 0 ( R N + h ) cos φ 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 ] (25) R = [ ( R M + h ) 2 ⋅ σ ϕ | GPS 2 0 0 0 0 0 0 0 0 0 ( R N + h ) 2 ⋅ cos 2 ϕ ⋅ σ λ | GPS 2 0 0 0 0 0 0 0 0 0 σ h | GPS 2 0 0 0 0 0 0 0 0 0 σ V N | GPS 2 0 0 0 0 0 0 0 0 0 0 σ V | GPS E 2 0 0 0 0 0 0 0 0 0 σ V | GPS E 2 0 0 0 0 0 0 0 0 ( R M + h ) 2 ⋅ σ ϕ | CONS 2 0 0 0 0 0 0 0 0 0 ( R N + h ) 2 ⋅ cos 2 ϕ ⋅ σ λ | CONS 2 0 0 0 0 0 0 0 0 0 σ h | CONS 2 ]

As previously explained also in this case constraint information are computed in LLH coordinate system. The variances of the map measurements need to be well-selected so that the resulting position estimates fall within the constraint boundaries.

The position estimates calculated from simulated data are shown in Figure 9, comparing the LC solution using both synthetic and real measurements with and without Map constraints.

It can be seen from Figure 9(b) that the use of additional constraint information can improve the performance. Without constraints the absolute value of the average error is 8.036 m, while with constraints the error is reduced to only 3.056, with an improvement of almost 5 m.

The same approach has been used with Google-Maps-sourced altitude information. In this case we have chosen a noise variance of 4 m² for the height constrained filter. The calculated altitude position errors using simulated GPS and INS measurements are shown in Figure 10.

The mean error without additional altitude information is 3.69 m, whereas with constraints the mean error is 1.05 m, thus yielding an improvement of about 2.5 m.