Ben Amani

Anne-Christine Afia

Christophe Escher

Macabiau

Amani Ben

Dr Anne-Christine Escher

Dr Christophe Macabiau Graduated

A Low-cost GNSS/IMU/Visual monoSLAM/WSS Integration Based on Kalman Filtering for Navigation in Urban Environments

Car navigation performance improvement is a subject of great interest nowadays especially with the development of autonomous car navigation. In urban environments, it is often difficult to rely on standalone Global Navigation Satellite System (GNSS) to obtain continuously an accurate and reliable navigation solution. In fact, the presence of buildings and other structures hindering the reception of GNSS signals (blockage, multipath, NLOS, poor geometry, etc.) makes it difficult for GNSS to provide accurate, continuous and reliable navigation solution in such an environment. A possible solution for this problem is to fuse information from a limited number of GNSS measurements and other sensors in order to enhance the system performance in terms of accuracy and availability. In this paper, we propose an integrated navigation system that fuses different sensor information in order to improve the car navigation performance in urban environments. A Low-cost navigation solution is proposed since the intended application is cost-sensitive. The proposed solution integrates information from an Inertial Measurement Unit (IMU), a GNSS receiver, a Wheel Speed Sensor (WSS) and a vision module based on monocular Simultaneous Localization And Mapping (SLAM). Motion constraints related to the movement of a land vehicle on the ground are also taken into account.

INTRODUCTION

The requirements in terms of navigation accuracy, integrity, continuity and availability are increasing for land vehicles especially with the development of autonomous land vehicles. In this work, the targeted 95% accuracy is of 1m for the horizontal position and of 1° for the attitude. These requirements are set by a French project called MIMOSA aiming to develop a low-cost navigation equipment for land vehicles capable of providing continuously an accurate navigation solution. One of the targeted applications of this project is the positioning of the garbage trucks in urban environments.

In the last decades, GNSS has been the most widely used system for navigation especially with its decreasing cost over the years. However, despite of its capability to provide absolute navigation information with long time accuracy, this system suffers from the problems related to signal propagation especially in urban environments where buildings, trees and other structures hinder the reception of GNSS signals. In addition to nominal errors, GNSS measurements suffer in urban environments from blockage reducing the number of satellites in view and resulting either in poor geometry degrading the navigation performance, or even in the unavailability of GNSS if the number of satellites of view is less than 4. The problem of unavailability will be handled in the near future with the development of new constellations such as GALILEO, BEIDOU, etc. The most important issue in urban environments is signal reflections which result in significant positioning error. When the Line-Of-Sight (LOS) signal is reflected and received with the reflected signal, this is known as multipath. If the LOS signal is blocked and only its reflection arrives to the receiver antenna, this is known as Non-Line-Of-Sight (NLOS) signal. The latter error results in a positive pseudo-range measurement error equal to the additional path delay and its value may exceed in some cases a kilometer [START_REF] Jiang | NLOS GPS signal detection using a dual polarisation antenna[END_REF]. Many techniques are proposed in the literature in order to mitigate these problems and improve the GNSS accuracy [START_REF] Jiang | NLOS GPS signal detection using a dual polarisation antenna[END_REF]. Unfortunately, all these techniques have limitations as highlighted in [START_REF] Jiang | NLOS GPS signal detection using a dual polarisation antenna[END_REF].

A possible way to overcome these problems is to fuse the "good" GNSS measurements with other sensors having complementary advantages. In fact, by exploiting the complementarity of sensors, hybridization algorithms can improve the navigation solution compared to solutions provided by each stand-alone sensor.

Generally, the most widely implemented hybridization algorithms for land vehicles fuse GNSS measurements with inertial and/or odometric data [START_REF] Shin | Accuracy improvement of low cost INS/GPS for land applications[END_REF] [3]. Thereby, these Dead-Reckoning (DR) sensors ensure the system continuity when GNSS information is unavailable and improve the system performance when GNSS signals are corrupted, and in return the GNSS limits the drift of the DR solution if it is available. However the performance achieved by this hybridization depends thoroughly on the quality of the DR sensor used especially when GNSS signals are degraded or unavailable.

Since the targeted application is cost-sensitive and lowcost sensors should be used, the common solution of fusing GNSS with inertial and/or odometric data mentioned previously cannot provide the high performance required by the project. For this reason, the SLAM is proposed as an additional sensor having the potential to improve the navigation performance. In fact, cameras have become one of the most attractive positioning sensors in the last decades and it has been proven that vision techniques are capable of providing accurate navigation solution [START_REF] Nistér | Visual odometry for ground vehicle applications[END_REF] while having reasonable cost. In general, vision systems reach very accurate results when using stereovision. However, the main drawback of such a configuration compared to a single camera is that in large-scale environments such as streets, the images captured by the cameras might contain objects that are placed too far. Processing these images does not allow recovering the depth values unless the stereo camera baseline is of few meters [START_REF] Hong | Visual odometry for outdoor environment using a downward-tilting camera and self-shadow removal algorithm[END_REF]. In addition to this compactness issue, a calibration issue arises when a multi-camera system is used and the calibration of a single camera is much easier [START_REF] Nutzi | Fusion of IMU and vision for absolute scale estimation in monocular SLAM[END_REF]. Therefore, a single camera will be considered in this study. Nonetheless, in case of a monocular vision module, a classical issue due to the projective nature of a single camera arises: the depth information of a 3D world point projected onto the image plane cannot be recovered using a single camera since a single 2D image point is the projection of an infinite number of 3D world points. This depth ambiguity results in a scale factor affecting the position estimated by the visual module and decreases dramatically its accuracy. Therefore, the visual navigation solution accuracy depends thoroughly on the good estimation of this scale factor. However the attitude estimation using a monocular camera is free from scale. This latter visual information is very important especially with the use of low-cost inertial sensors with bad gyroscope quality. In order to estimate this information, a vision module based on monocular SLAM technique is used as a black box providing the attitude. A review of SLAM algorithm is performed in [START_REF] Ben-Afia | Review and classification of vision-based localisation techniques in unknown environment[END_REF]. Compared to simple visual odometry, this technique has the advantage of reducing the estimation error using the bundle adjustment process described in [START_REF] Engles | Bundle adjustment rules[END_REF]. In this study, only the visual heading provided by the SLAM is used since we focus on horizontal navigation performance.

In addition to the low-cost sensors mentioned above, motion constraints for land vehicles are costless solutions capable of improving navigation performance. These constraints consist of Non-Holonomic-Constraints (NHC) [START_REF] Dissanayake | The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications[END_REF], Zero Velocity Update (ZVU) [START_REF] Groves | Principles of GNSS, Inertial and Multisensor Integrated Navigation Systems[END_REF] and Zero Angular Rate Update (ZARU) [START_REF] Groves | Principles of GNSS, Inertial and Multisensor Integrated Navigation Systems[END_REF]. NHC assume that the vehicle does not move in the plane perpendicular to the forward direction. ZVU and ZARU are applied when the navigation system is stationary. They respectively constrain the vehicle velocity and angular rate to be equal to zero when a stationarity is detected. When GNSS is unavailable or corrupted, this information helps reducing the navigation solution drift.

The work proposed in this paper takes advantage of all the low-cost possible ways to improve the navigation solution mentioned above and uses their information in an integrated multi-sensor system. In the proposed architecture, the Inertial Navigation System (INS) is selected as the reference sensor since it provides a complete and continuous navigation solution including position, velocity and attitude with the highest rate. The INS estimation errors are corrected by GNSS, vision, WSS and motion constraints measurements using an error-state EKF based on a closed loop configuration. When GNSS measurements (pseudo-ranges and Doppler) are available, an innovation test [START_REF] Groves | Principles of GNSS, Inertial and Multisensor Integrated Navigation Systems[END_REF] checking the consistency of new measurements with previous information is applied in order to exclude measurements considered as corrupted. When GNSS is unavailable, the filter keeps on running with the other measurements. Since the GNSS measurements have the lowest rate and are not necessarily available, this architecture is such as a virtual DR sensor is running continuously using the inertial, visual, odometric and motion constraints measurements. When GNSS measurements arrive, they allow the calibration of the DR system, otherwise the DR system works continuously without having a large drift.

The paper is organized as follows. First the notations and coordinate frames used are described. Second, the integrated navigation system is detailed and the fusion strategy is proposed. The equations of the process and measurement models are presented and discussed. Then, the performance analysis of the proposed algorithm is performed. Finally, conclusions are given at the end the paper.

COORDINATE FRAMES AND NOTATIONS

Before tackling the sensor fusion strategy, it is important to remind the different coordinate frames associated to the sensors and the relationship between them.

Figure 1 illustrates the coordinate frames we use: we assume that the IMU platform and the vehicle body frame (b) are aligned. This frame is attached to the vehicle center of gravity. Its X-axis points towards the right side of the vehicle, its Y-axis points towards the forward direction and its Z-axis points upwards and completes the right handed frame. The local frame (l) is defined by the East North Up (ENU) local tangent plane. This frame is attached to a local point (the initial navigation point is chosen in our study), and its X, Y and Z axis point towards the east, north and up directions, respectively. The Earth-Centered-Earth-Fixed frame (E) is attached to the Earth center of mass. Its X-axis points towards the intersection between the prime meridian and the equator plane, its Z-axis extends through the Earth spin axis and its Y-axis completes the right handed coordinate system. The geodetic coordinates (latitude, longitude and altitude) are defined with respect to this coordinate system. The vision frame (v) is the frame in which the SLAM provides its outputs. This frame is attached to the initial pose of the vehicle [START_REF] Eudes | Localisation et cartographie simultanées par ajustement de faisceaux local : propagation d'erreurs et réduction de la dérive à l'aide d'un odomètre[END_REF] and is defined up to rotation with respect to the (l) frame. If we assume that the IMU and the camera are aligned, then this rotation corresponds to the initial (b) to (l) rotation. We assume that all the lever arms and orientation between the aiding sensors and the IMU are perfectly known and are measured before the navigation starts. Let us note: -∆𝒑 𝐺 𝑏 the lever arm between GPS antenna and IMU: -∆𝒑 𝑤 𝑏 the lever arm between the center of the rear axle of the vehicle and IMU:

Multi-sensor system design

The aim of this study is to develop a low-cost multisensor fusion system capable of providing accurate navigation information for land vehicles in urban environments. The proposed architecture consists of a single frequency GNSS receiver providing pseudo-range and Doppler measurements for each satellite in view, a low-cost IMU, a vision module processing the images of a single camera using the SLAM technique and providing heading information, a WSS providing the vehicle velocity and constraints describing the motion of the vehicle. The IMU is selected as the reference sensor since it is the only sensor continuously providing a complete navigation solution (position, velocity and attitude) at the highest rate. The mechanization providing these quantities is implemented in the (l) frame. As shown in Figure 2, the INS mechanization errors are corrected by the measurements of the other sensors. An error-state EKF is used in order to estimate the corrections that should be applied to the inertial navigation solution. As highlighted in [START_REF] Shin | Accuracy improvement of low cost INS/GPS for land applications[END_REF] and [START_REF] Angrisano | GNSS/INS Integration Methods[END_REF], it is preferred to express the position error in meters than in radians in order to avoid numeric instability inside the filter. Hence, the (l) frame is used as the reference system instead of geodetic coordinates. The estimated IMU measurement errors (biases and scale factors) are fed back to the mechanization since we have to deal with a low-cost IMU. Otherwise the mechanization can experience unbounded error growth, and the assumption of small errors used in the linearization process of the filter can be violated [START_REF] Shin | Accuracy improvement of low cost INS/GPS for land applications[END_REF]. WSS measurement is affected by an unknown scale factor due to the tire radius change. This scale factor should be estimated and the WSS velocity should be corrected in order to obtain a good velocity measurement used to update the Kalman filter. Since the WSS provides only the forward velocity (along 𝑌 𝑏 ), the NHC completes the three-dimensional velocity by assuming that for a ground vehicle, the lateral and vertical velocities (along 𝑋 𝑏 and 𝑍 𝑏 ) should be equal to zero if the vehicle does not slip or jump.

Figure 2: Filter architecture

The ZVU is used when a stationarity is detected. This detection is based on the comparison of the WSS velocity mean with a threshold over a suitable time window. Normally, the ZVU is useless in the presence of the WSS and NHC. However, the assumption of no side slip of the NHC could be violated in practical situations. To avoid NHC violations, NHC is modelled as:

𝑣 𝑥𝑏 = 𝜂 𝑥𝑏 (1)
𝑣 𝑧𝑏 = 𝜂 𝑧𝑏 (2)
where 𝜂 𝑥𝑏 and 𝜂 𝑧𝑏 are Gaussian white noise sources with zero mean and a relatively high standard deviation equal to 0.5 m/s [START_REF] Angrisano | GNSS/INS Integration Methods[END_REF]. When a stationarity is detected, the trust that we have in a zero velocity increases and the standard deviation associated to the velocity measurement can be decreased. We set this standard deviation to 0.01 m/s. The ZARU is also applied in vehicle stationarity. It is also applied when a linear motion is detected. This detection is performed by comparing the standard deviation of the yaw rate obtained from differential odometry with a threshold or by using the steering angle information if available.

The idea of using visual, odometric and motion constraints measurements to correct the inertial sensor is to form a DR system running without interruption. This system is capable of remarkably reduce the drift compared to a stand-alone running inertial system.

As discussed in the introduction, GNSS measurements, if available, are very likely to be corrupted by reflections and blockage. Therefore, measurements affected by large errors should be detected and removed to avoid the degradation of the navigation solution. This detection is performed by an innovation test [START_REF] Groves | Principles of GNSS, Inertial and Multisensor Integrated Navigation Systems[END_REF] that compares the normalized Kalman filter innovation to a threshold and removes measurements that are not consistent with previous information. The remaining measurements are used to update the filter with absolute information. If GNSS is not available, then the DR system keeps on running to ensure the navigation continuity.

Inertial Sensor Model

An IMU is at least composed of accelerometers and gyroscopes. The accelerometers measure the specific force and the gyroscopes measure the angular rate. The measurements are provided in the body frame. In this work, 3 accelerometers and 3 gyroscopes are used. These measurements are inevitably affected by errors. For a low-cost IMU, these errors mainly consist of biases, scale factors and noise [START_REF] Angrisano | GNSS/INS Integration Methods[END_REF]. In this study, the inertial measurements are modelled by the following equations:

𝒇 ̃𝑏 = (𝑰 𝟑 + 𝒌 𝑎 ) • 𝒇 𝑏 + 𝒃 𝑎 + 𝜼 𝑎 (3)
𝝎 ̃𝑏 = (𝑰 𝟑 + 𝒌 𝑔 )

GNSS measurement model

The code pseudo-range measurement is the distance between the GNSS satellite and the receiver antenna. This measurement is computed from the propagation time between the transmission of the signal by the satellite and its reception by the receiver, and the multiplication of this time by the speed of light c. The fact that the clocks of the satellites and the receiver are not perfectly synchronized to GPS time, introduces a clock bias that drifts within time. Therefore, the pseudo-range measurement can be modeled as:

𝜌 ̃𝐺 𝑖 = 𝑟 𝑖 + 𝑐𝛿𝑡 0 + 𝜂 𝜌𝐺 𝑖 ( 5
)
where 𝑟 𝑖 is the true range between the satellite i and the receiver antenna, 𝛿𝑡 0 is the clock bias and 𝜂 𝜌𝐺 𝑖 is a zeromean white Gaussian noise modeling the residual of all nominal errors after correction.

The Doppler measurement represents the rate of change of the carrier phase. The multiplication of this measurement by the signal wavelength 𝜆 0 provides the pseudo-range rate measurement:

𝜌̇𝐺 𝑖 = 𝑑 ̃𝐺 𝑖 . 𝜆 0 = 𝑟̇𝑖 + 𝑐𝛿 ̇𝑡0 + 𝜂 𝜌̇𝐺 𝑖 (6)
where 𝑑 ̃𝐺 𝑖 is the Doppler measurement of satellite i, 𝑟̇𝑖 is the true pseudo-range rate between the satellite and the receiver, 𝛿 ̇𝑡0 is the clock drift and 𝜂 𝜌̇𝐺 𝑖 is the range rate error modeled as a zero-mean Gaussian white noise.

WSS and NHC measurement model WSS provides velocity along the forward axis in the (𝑏)

frame. The measurement model of the WSS is given in by:

𝒗 ̃𝑤 𝑙 = 𝑪 𝑏2𝑙 (1 + 𝑘 𝑤 )𝒗 ̃𝑤 𝑏 + 𝜼 𝑤 (7)
with 𝒗 ̃𝑤 𝑏 = [0 𝑣 ̃𝑤 𝑏𝑦 0] 𝑇 , 𝑘 𝑤 is the WSS scale factor and 𝜼 𝑤 is the WSS noise modeled as zero-mean, white and Gaussian. The lateral and vertical velocities are set to zero by taking into account the NHC.

Vision heading measurement model

This heading measurement is provided in the vision frame. This means that the vision module assumes that the initial heading of the vehicle is equal to zero. In order to obtain the heading of the vehicle with respect to the local frame 𝜓 ̃𝑆𝐿𝐴𝑀 𝑙 , it should be corrected with the initial heading 𝜓 0 𝑙 :

𝜓 ̃𝑆𝐿𝐴𝑀 𝑙 = 𝜓 ̃𝑆𝐿𝐴𝑀 𝑣 + 𝜓 0 𝑙 + 𝜂 𝜓 (8)
where 𝜂 𝜓 is a Gaussian white noise. Note that this assumption only holds if the vehicle performs a loop closure (the end of its trajectory is the starting point). In fact, by detecting again the same features, the SLAM is capable of eliminating the drift using the bundle adjustment process. If no loop closure is done, this assumption holds if the SLAM is aided by a previously built georeferenced database. If none of these two conditions is fulfilled, then the assumption of Gaussian white noise is not realistic for the heading measurement but holds for the estimated vehicle heading change between two images. Therefore the heading should be modeled as a random walk. The latter case will be studied in future works.

Sate Vector

The INS estimates the navigation parameters i.e. the position, velocity and attitude of the vehicle, and the aiding sensors correct the INS estimation error through the Kalman filter. The errors of the navigation parameters are defined by:

𝛿𝒑 𝑏 𝑙 = 𝒑 ̂𝑏 𝑙 -𝒑 𝑏 𝑙 ( 9
)
𝛿𝒗 𝑏 𝑙 = 𝒗 ̂𝑏 𝑙 -𝒗 𝑏 𝑙 ( 10
)
𝑪 ̂𝑏2𝑙 = (𝑰 -𝑬 𝑏2𝑙 ) 𝑪 𝑏2𝑙 ( 11
)
where 𝑬 𝑏2𝑙 is the skew-symmetric matrix of the attitude error

𝜺 𝑏2𝑙 = [ 𝜀 𝐸 𝜀 𝑁 𝜀 𝑈]
Using the previous notations, a 21-element error state vector associated to INS is consequently used:

𝛿𝒙 𝐼𝑁𝑆 = [𝛿𝒑 𝑏 𝑙 𝛿𝒗 𝑏 𝑙 𝜺 𝑏2𝑙 𝛿𝒃 𝑎 𝛿𝒃 𝑔 𝛿𝒌 𝑎 𝛿𝒌 𝑔 ] 𝑇 (12)
Since WSS velocity is affected by an unknown scale factor, and GNSS clock bias and clock drift are also unknown, these quantities should be estimated by the filter. Therefore:

𝛿𝒙 𝐺 = [𝛿(𝑐𝛿𝑡 0 ) 𝛿(𝑐𝛿 ̇𝑡0 )] 𝑇 (13)
𝛿𝑥 𝑤 = 𝛿𝑘 𝑤 (14)
The full EKF state is the concatenation of the state vectors associated to each sensor. It is given by the 24-element state vector:

𝛿𝒙 = [ 𝛿𝒙 𝐼𝑁𝑆 𝑇 𝛿𝒙 𝐺 𝑇 𝛿𝑥 𝑤 ] 𝑇 (15)

State transition model

The INS mechanization equation is given by:

[ 𝒑̇𝑏 𝑙 𝒗̇𝑏 𝑙 𝑪 ̇𝑏2𝑙 = [ 𝒗 𝑏 𝑙 𝑪 𝑏2𝑙 𝒇 𝑏 -(2𝝎 𝑖𝑒 𝑙 + 𝝎 𝑒𝑙 𝑙 ) × 𝒗 𝑏 𝑙 + 𝒈 𝑙 𝑪 𝑏2𝑙 (𝛀 𝑏 -𝛀 𝑖𝑙 𝑏 ) ] ( 16
)
where 𝒈 𝑙 is the local gravity including the Gravitation term and the centripetal term related to Earth rotation.

To linearize the mechanization equation and derive the linear INS model, Eq.(3-4) and Eq.(9-11) are applied to the mechanization equation using the perturbation analysis described in [START_REF] Shin | Accuracy improvement of low cost INS/GPS for land applications[END_REF]. Based on this analysis, The INS state transition matrix 𝑭 𝐼𝑁𝑆 and the design matrix 𝑮 𝐼𝑁𝑆 are derived in the (l) frame. Their expressions are given in our previous paper [START_REF] Ben-Afia | A GNSS/IMU/WSS/VSLAM Hybridization Using an Extended Kalman Filter[END_REF].

We model the inertial measurement bias and scale factor errors as Gauss-Markov process [START_REF] Angrisano | GNSS/INS Integration Methods[END_REF]:

𝛿𝑒 𝑠𝑢 ̇= - 1 𝜏 𝑒 𝑠𝑢 • 𝛿𝑒 𝑠𝑢 + 𝜂 𝑒 𝑠𝑢 ( 17
)
where 𝑒 is the error (𝑒 = 𝑏 or 𝑒 = 𝑘), 𝑠 is the sensor (𝑠 = 𝑎 or 𝑠 = 𝑔) and 𝑢 is the (𝑏) frame axis (𝑢 = 𝑥, 𝑢 = 𝑦 or 𝑢 = 𝑧). 𝜏 𝑒 𝑠𝑢 is the correlation time of the error 𝑒 of the sensor 𝑠 along the 𝑢-axis and 𝜂 𝑒 𝑠𝑢 is a zero-mean Gaussian noise.

The WSS scale factor as well as the GNSS clock bias and clock drift are modeled as follows:

𝛿(𝑐𝛿 ̇𝑡0 ) = 𝛿(𝑐𝛿𝑡 0 ) + 𝜂 bias (18)
𝛿(𝑐𝛿 ̈𝑡0 ) = 𝜂 drift (19)
The clock error spectral densities can be computed as:

𝑞 bias = 𝑐 2 . ℎ 0 2 (20)
𝑞 drift = 𝑐 2 . 2𝜋 2 . ℎ -2 (21)
where ℎ 0 and ℎ -2 are allan variance parameters describing the clock errors, typical values for compensated crystal clock can be 2 . 10 -19 and 2 . 10 -20 respectively [START_REF] Brown | Introduction to Random Signals and Applied Kalman Filtering[END_REF].

The WSS scale factor error is modeled as a constant, thus:

𝛿𝑘 𝑤 ̇= 𝜂 𝑘𝑤 (22)
The models defined previously can be summarized in the following linearized continuous time sate transition model:

𝛿𝒙 ̇= 𝑭 • 𝛿𝒙 + 𝑮 • 𝒖 (23)
where

𝑭 = [ 𝑭 𝑰𝑵𝑺 𝟎 21𝑥1 𝟎 21𝑥1 𝟎 21𝑥1 𝟎 1𝑥21 0 1 0 𝟎 2𝑥21 𝟎 2𝑥1 𝟎 2𝑥1 𝟎 2𝑥1 ] (24) 𝑮 = [ 𝑮 𝐼𝑁𝑆 𝟎 21𝑥3 𝟎 3𝑥21 𝑰 3 ] (25)
The corresponding process noise covariance matrix is given by:

𝑸 = cov(𝒖) (27)
The discrete time sate transition model assuming that F and G are constant over the time step between two consecutive state propagations ∆𝑡 is given by:

𝛿𝒙 𝑘+1 = 𝚽 𝑘 • 𝛿𝒙 𝑘+1 + 𝒘 𝑘 (28)
with [START_REF] Shin | Accuracy improvement of low cost INS/GPS for land applications[END_REF] 𝚽 𝑘 = 𝑰 + 𝑭. ∆𝑡 (29) and

𝐐 𝑘 = cov(𝒘 𝑘 ) = 𝚽 𝑘 𝑮𝑸𝑮 𝑻 𝚽 𝑘 𝑇 ∆𝑡 (30)
With the discrete state transition model, we can propagate the state using the prediction equations of the EKF. Since we are in a closed-loop configuration where the errors estimated by the EKF are fed back after the measurement update, the state vector should be reset to zero. Nonetheless, the state covariance is predicted using:

𝑷 𝑘+1 - = 𝚽 𝑘 𝑷 𝑘 𝚽 𝑘 𝑇 + 𝐐 𝑘 (31)

Kalman filter update

The update of the Kalman filter is performed by comparing the INS predicted states with the aiding measurements quantities, by relating the difference of the measured and predicted quantities to the state vector components. This update is performed as soon as a sensor measurement is available. This implies that the observation model varies according to the availability of aiding sensors.

GNSS observation model

The GNSS observation model is given by the following equation:

𝛿𝒛 𝐺 = [ 𝜌 ̃𝐺 1 -𝜌 ̂𝐺 1 ⋮ 𝜌 ̃𝐺 𝑁 -𝜌 ̂𝐺 𝑁 𝜌̇𝐺 1 -𝜌̇𝐺 1 ⋮ 𝜌̇𝐺 𝑁 -𝜌̇𝐺 𝑁 ] = 𝑯 𝐺 • 𝛿𝒙 + [ 𝜂 𝜌𝐺 1 ⋮ 𝜂 𝜌𝐺 𝑁 𝜂 𝜌̇𝐺 1 ⋮ 𝜂 𝜌̇𝐺 𝑁 ] (32)
where where the notation (𝒂) × means the skew-symmetric matrix of the (3𝑥3) vector 𝒂.

𝑯 𝐺 = [ (𝒑 𝑠 1 -𝒑 ̂𝑏 𝑙 ) 𝑟 1 0 3𝑥3 0 3𝑥15 1 0 0 ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ (𝒑 𝑠 𝑁 -𝒑 ̂𝑏 𝑙 ) 𝑟 𝑁 0 3𝑥3 0 3𝑥15 1 0 0 (𝒗 𝑠 1 -𝒗 ̂𝑏 𝑙 ) 𝑟 1 + (𝒑 𝑠 1 -𝒑 ̂𝑏 𝑙 ). 𝑟̇1 (𝑟 1 )

ZVU observation model

The update based on ZVU is derived from the previous equations as follows:

𝛿𝒛 𝑍𝑉𝑈 = -𝒗 ̂𝑏 𝑙 = 𝑯 𝑍𝑉𝑈 𝛿𝒙 + 𝜼 𝑍𝑉𝑈 (36)
with

𝑯 𝑍𝑉𝑈 = [𝟎 3 -𝑰 3 𝟎 3𝑥18 ] (37)

Vision observation model

The heading observation is related to the state vector by the following equation:

𝛿𝑧 𝜓 =
𝑷 𝑘+1 + = 𝑷 𝑘+1 - -𝑲 𝑘+1 • 𝑯 𝑘+1 • 𝑷 𝑘+1 - ( 44
)
where R is the measurement covariance matrix.

EXPERIMENTS AND RESULTS

The algorithm is tested using GNSS and inertial data collected in Toulouse downtown during 30 minutes. GNSS data is given by the GPS L1 C/A stand-alone mode of a Ublox-6 receiver running at 1Hz. Inertial measurements are obtained using an Xsens Mti IMU running at 100 Hz. Figure 3 shows the reference trajectory determined using the NovAtel SPAN equipment [16].

Figure 3: Vehicle trajectory in Toulouse downtown

The WSS velocity and visual data are generated from the reference trajectory such that the WSS is affected by a scale factor of -0.01 and a zero-mean white Gaussian noise with a standard deviation equal to 0.05m/s. The SLAM heading is assumed to be a zero-mean white Gaussian noise with a standard deviation of 2°. The WSS and SLAM are assumed to run respectively at 10Hz and 20 Hz. As mentioned before, only the horizontal performance will be assessed in this study.

To evaluate the contribution of each aiding sensor, seven configurations are tested and compared in this study. A GPS outage of 60s is also simulated in order to assess the performance of the DR system during GPS outage:

-Config1: GNSS -Config2: GNSS+INS -Config3: GNSS+INS+Innovation test -Config4: Config3+NHC+ZVU+ZARU -Config5: Config4+WSS -Config6: Config5+heading -Config7: DR sensor

Config1 results

In this configuration we assume that the positioning is only based on GPS L1 C/A signals. As illustrated in Figure 4, the horizontal error is very important especially when the vehicle goes through dense streets (between 550s and 700s). In this environment, the horizontal error reaches 172m. In addition, when less than 4 satellites are in view, GPS is unable to provide a navigation solution. During GPS outage, the system is able to coast thanks to the continuity of the inertial sensor. As illustrated in Figure 5, the position accuracy is increased when GPS is available and there is no discontinuity anymore. For example, between 550s and 770s, the RMSE is decreased by 87.1%. Yet the horizontal error remains very high especially during GPS outage. An error of 805m is reached after 60s of outage.

Config3 results

This configuration adds the innovation test of GPS measurements before using them to update the Kalman state vector. This innovation test is very important in order to improve the navigation solution in urban environments when GPS measurements are available by eliminating the erroneous measurements. In fact, by removing the measurements inconsistent with previous information, we only keep the measurements that are not affected by large error. Figure 6 shows the number of excluded pseudo-ranges and pseudo-range rates at each epoch.

Figure 6: Number of removed SVs with innovation test

It is important to check that after exclusion, the number of remaining satellites keeps high enough to improve the navigation solution. A drawback of this technique is that in case of little number of remaining satellites, the performance may be degraded due to the exclusion. In our case, the remaining number of measurements is high enough to be able to exclude without degrading the performance. As explained before, the fact that after exclusion, the number of the remaining good measurements is high helps to improve the navigation solution. The RMSE is decreased by 75.24% after applying the innovation test.

Figure 1 :

 1 Figure 1: Reference frames In this paper, the notations with the hat, ^, are the estimated quantities and those with the tilde, ~, are the measured quantities. The notations without hat or tilde are the actual quantities.

Figure 4 :

 4 Figure 4: Stand-alone GPS horizontal position errorConfig2 resultsThis configuration consists of an INS based on the Xsens IMU measurements corrected by the pseudo-range and Doppler measurements from GPS in a closed-loop tight configuration. No aiding comes from any of the other sensors. During GPS outage, the system is able to coast thanks to the continuity of the inertial sensor. As illustrated in Figure5, the position accuracy is increased when GPS is available and there is no discontinuity anymore. For example, between 550s and 770s, the RMSE is decreased by 87.1%. Yet the horizontal error remains very high especially during GPS outage. An error of 805m is reached after 60s of outage.

Figure 5 :

 5 Figure 5: Comparison of horizontal position error

Figure 7 and

 7 Figure 7 and Figure 8 highlight the contribution of this technique in the improvement of the navigation solution when GPS measurements are available (without simulating the GPS outage of 60s).

Figure 7 :Figure 8 :

 78 Figure 7: Comparison of horizontal position error

Table 1

 1 specifies, for each sensor, the outputs and the associated coordinate system.

	Nav. module	Output		Frame	Frame Origin
		INS Position	𝒑 ̂𝑏 𝑙		Initial
	INS	INS Velocity INS Attitude	𝒗 ̂𝑏 𝑙 𝑪 ̂𝑏2𝑙	(𝑙)	vehicle position
	GNSS	i th Pseudo-range i th Doppler	𝜌 ̃𝐺 𝑖 𝑑 ̃𝐺 𝑖	--	--
					Initial
	VSLAM	Camera heading	𝜓 ̃𝑆𝐿𝐴𝑀	(𝑣)	vehicle position
	WSS	Velocity	𝑣 ̃𝑤 𝑏𝑦	(𝑏)	Vehicle center of
					mass

Table 1 : Sensor outputs NAVIGATION FILTER

 1

  𝒇 𝑏 and 𝝎 𝑏 are the actual specific force and angular rate. 𝒌 𝑎 and 𝒌 𝑔 are the scale factors. 𝒃 𝑎 and 𝒃 𝑔 are the biases, and 𝜼 𝑎 and 𝜼 𝑔 are zero-mean white Gaussian noises.

• 𝝎 𝑏 + 𝒃 𝑔 + 𝜼 𝑔 (4)

where 𝒇 ̃𝑏 and 𝝎 ̃𝑏 are respectively the measured specific force and angular rate in the body frame.

  𝒖 = [ 𝜼 𝑎 𝜼 𝑔 𝜼 𝑏𝑎 𝜼 𝑏𝑔 𝜼 𝑘𝑎 𝜼 𝑘𝑔 𝜂 bias 𝜼 drift 𝜂 𝑘𝑤] 𝑇

  2

  Kalman filter updateOnce 𝛿𝒛 and 𝑯 are determined, the update is performed using the Kalman filter equations:

	𝑲 𝑘+1 = 𝑷 𝑘+1 -𝑯 𝑘+1	𝑇 • [𝑯 𝑘+1 𝑷 𝑘+1 -𝑯 𝑘+1	𝑇 + 𝑹 𝑘+1 ]	-1	(42)
	𝛿𝒙 ̂𝑘+1 +	= 𝑲 𝑘+1 𝛿𝒛 𝑘			(43)
							𝜓 ̃𝑉𝑆𝐿𝐴𝑀 𝑙	-𝜓 ̂𝐼𝑁𝑆 𝑙	= 𝑯 𝜓 𝛿𝒙 + 𝜂 𝜓	(38)
						with
						𝑯 𝜓
						= [ 0 1𝑥6	-𝐶 ̂𝑏2𝑙 12 𝐶 ̂𝑏2𝑙 32 [𝐶 ̂𝑏2𝑙 12 ] 2 + [𝐶 ̂𝑏2𝑙 22 ]	2	-𝐶 ̂𝑏2𝑙 22 𝐶 ̂𝑏2𝑙 32 [𝐶 ̂𝑏2𝑙 12 ] 2 + [𝐶 ̂𝑏2𝑙 22 ]	2 1 0 1𝑥17]	(39)
						ZARU observation model
						Based on Eq. 4, when the vehicle is stationary or moves
						in linear motion, the actual angular rate is equal to zero.
						This means that the measurements given by the IMU
						gyroscopes are equal to the gyro biases:
						𝛿𝒛 𝑍𝐴𝑅𝑈 = 𝝎 ̃𝑏 = 𝒃 𝑔 + 𝜼 𝑔 = 𝑯 𝑍𝐴𝑅𝑈 𝛿𝒙 + 𝜼 𝑔	(40)
						with
							𝑯 𝑍𝐴𝑅𝑈 = [0 3𝑥12 𝑰 3 0 3𝑥9 ]	(41)

Table 2

 2 gives the availability of satellites during the measurement campaign (The simulated GPS outage is not considered):

	Number of SVs	Pseudo-range test	Pseudo-range rate test
	0 < N < 2	0 %	0.2 %
	2 ≤ N < 4	0.3 %	2.6 %
	4 ≤ N < 6	12.7 %	20.5 %
	6 ≤ N < 8	39 %	40.4 %
	≥ 8	48%	36.4 %

Table 2 : Satellite availability after exclusion

 2

Config4 results

This configuration adds constraints related to the vehicle motion. The advantage of adding these constraints is that it does not have an additional cost compared to the previous configuration. The comparison of the horizontal error made using this configuration and that using the previous configuration shows that the estimation is clearly improved by introducing these constraints as shown in Figure 9 and Figure 11. However, in GPS outage, the drift of the navigation solution remains unacceptable since it reaches 309m after 60s of outage.

Figure 9: Comparison of horizontal position error

When GPS is available, the use of motion constraints improves slightly the performance of the navigation solution. This is highlighted in Figure 10.

GPS is available

In Figure 11, the heading estimation is clearly improved by using the motion constraints. This is due to 2 main reasons: first, the ZARU allows the improvement of attitude estimation by helping estimating the gyro biases.

In addition, the ZVU and NHC improve the velocity estimation. This is due to the correlation that exists between the vehicle velocity and its attitude.

Figure 11: Comparison of heading error

Config5 results

In this configuration, the WSS is added. This sensor provides additional information to the filter about its longitudinal velocity. The comparison of the navigation performance with the previous configuration shows that the horizontal position error is improved as shown in Figure 12. The drift during GPS outage is reduced from 309m to about 19.5m which is an improvement of 93.7%.

When GPS is available and the innovation test on GPS measurements is performed, we have almost the same navigation performance as the previous configuration.

For the heading, no improvement is noticed compared to the previous configuration as shown in Figure 13.

Config6 results

In this configuration, the SLAM heading is added. As mentioned before, this heading is assumed as a zero-mean white Gaussian noise. This assumption could be realistic if the vehicle finishes its trajectory at its starting point, or if we have a previously built georeferenced database. The results introducing this information show an important improvement of the heading estimation as shown in Figure 14.

Figure 14: Comparison of heading error

For the horizontal error, this information is quite important since it reduces the drift of the DR system when GPS is unavailable and reduces the horizontal error when GPS is available, as illustrated in Figure 15. During the 60s of GPS outage, the error does not exceed 1.2m when we introduce the heading information.

Figure 15: Comparison of horizontal position error

Config7 results

This configuration is tested in order to analyze the possible drift of the DR system without the use of GPS aid during the full trajectory. Figure 16 shows that the DR system is able to provide a navigation solution without drift. It also shows that the performance of the DR system with GPS aid is slightly better.

CONCLUSION

In this paper, we proposed an algorithm integrating different low-cost sensors in order to improve the navigation performance in terms of accuracy and continuity in urban environments. This algorithm fuses information from an IMU considered as the reference sensor which is aided by GNSS raw measurements, WSS velocity, SLAM heading and motion constraints. Results show that the use of a GNSS/INS system is not sufficient especially in urban environments where the GNSS measurements could be corrupted or unavailable. GNSS outage was simulated to evaluate the horizontal positioning and heading drift by comparing 7 configurations. This comparison aimed to highlight the contribution of each additional sensor to the decrease of the drift when GNSS is unavailable and to the improvement of the navigation solution when GNSS measurements are corrupted. A horizontal error RMS of 3.34m on a trajectory of 30 minutes is achieved. This was possible thanks to the good SLAM heading considered in this study. However, this assumption is not realistic if the SLAM algorithm relies only on the DR principle. Future works should focus on the improvement of the SLAM measurement model and on the accuracy improvement of the navigation solution since the performance required by the project is still not achieved.

[3] M. Spangenberg, V. Calmettes, D. Kubrak, and O.


Ben Amani

Anne-Christine Afia

Christophe Escher

Sébastien Macabiau

Roche

Amani Ben

Dr Anne-Christine Escher

Dr Christophe Macabiau Graduated

Sébastien Roche

A GNSS/IMU/WSS/VSLAM Hybridization Using an Extended Kalman Filter

Since 2013, she has been a PhD student in the Signal Processing and Navigation Research Group (SIGNAV) of ENAC. Her research focuses on developing multi-sensor fusion algorithms for the navigation in constrained environments.

INTRODUCTION

Vehicles moving on airport surfaces require a stringent positioning accuracy in order to guarantee their safety. The surveillance requirements in terms of accuracy, integrity, continuity and availability on airport surfaces are detailed in [START_REF] Icao | Manual on advanced surface movement guidance and control systems (A-SMGCS) -DOC9830 AN/452[END_REF]. The position accuracy during manoeuvers on airport surfaces should not exceed few meters. In our work, this accuracy is targeted for airport ground vehicles. Stand-alone GNSS cannot always provide this required level of accuracy because of the possible signal degradation mainly due to multipath resulting from its diffraction on airport surfaces, or due to its blockage [START_REF] Park | A Study of Severe Multipath Errors for the Proposed GBAS Airport Surface Movement Application[END_REF].

As an alternative to stand-alone GNSS, it has been shown that fusing information from different sensors results in an improved position robustness and accuracy if they have complementary advantages. In fact, by exploiting the complementarity of sensors, hybridization algorithms can improve the navigation solution with respect to that provided by each stand-alone sensor. The most widely implemented hybridization algorithms for land vehicles fuse GNSS measurements with inertial and/or odometric data [3] [4]. This way, these dead-reckoning (DR) sensors ensure the system continuity when GNSS information is unavailable and improve the system performance when GNSS signals are corrupted by multipath; and in return the GNSS limits the drift of DR solution if it is available. However the performance achieved by this hybridization depends thoroughly on the quality of the DR sensor used especially when GNSS signals are degraded. Since the targeted application is cost-sensitive, low-cost sensors will be used in this study. Therefore, the challenge of this paper is to achieve a high level of accuracy with a low-cost solution, and some other complementary low-cost sensors should be employed to achieve the performance required.

In the last decades, cameras have become one of the most attractive positioning sensors. In fact it has been proven that vision techniques are capable of providing accurate navigation solution [START_REF] Nistér | Visual odometry for ground vehicle applications[END_REF] while having reasonable cost. In general, vision systems reach very accurate results when using stereovision. However, the main drawback of such a configuration compared to a single camera is that in largescale environments, the images captured by the cameras might contain objects placed too far. Processing these images does not allow recovering the depth values unless the stereo camera baseline is of few metres [START_REF] Hong | Visual odometry for outdoor environment using a downwardtilting camera and self-shadow removal algorithm[END_REF]. In addition to this compactness issue, a calibration issue arises when a multi-camera system is used and the calibration of a single camera is much easier [START_REF] Strasdat | Scale drift-aware large scale monocular SLAM[END_REF]. Therefore, a single camera will be considered in this study. In case of a monocular vision module, a classical issue due to the projective nature of a single camera arises: the depth information of a 3D world point projected onto the image plane cannot be recovered using a single camera since a single 2D image point is the projection of an infinite number of 3D world points. This depth ambiguity results in a scale factor affecting the position estimated by the visual module and decreases dramatically its accuracy. Therefore, the visual navigation solution accuracy depends thoroughly on the good estimation of the scale factor.

Most of prior work focused on eliminating this scale problem in order to improve the visual solution either by using other sensors such as inertial [START_REF] Nutzi | Fusion of IMU and vision for absolute scale estimation in monocular SLAM[END_REF], GNSS [START_REF] Dusha | Attitude observability of a loosely-coupled GPS/Visual Odometry Integrated Navigation Filter[END_REF], and odometric measurements [START_REF] Eudes | Weighted Local Bundle Adjustment and Application to Odometry and Visual SLAM Fusion[END_REF], or by using previouslymapped markers [START_REF] Lamberti | Mixed markerbased/marker-less visual odometry system for mobile robots[END_REF]. However, little work has considered the monocular vision module as a black box providing aiding measurements (position in undefined scale and scale free attitude) in a multi-sensor fusion framework. The proposed work in this paper focuses on this idea and uses visual information as aiding measurements in an integrated multi-sensor system aiming at providing a cost-effective and a robust navigation solution. In the proposed architecture, the INS is selected as the reference sensor since it provides a complete and continuous navigation solution including position, velocity and attitude. The INS estimation errors are corrected by GNSS, vision and WSS measurements using an error-state EKF based on a closed loop configuration and a loosely-coupled architecture. The visual scale factor is estimated in the filter state vector and is used after in the update step of the Kalman filter in order to make homogeneous the inertial and visual position. This way, vision provides accurate information to the fusion algorithm and in return, the other sensors help the visual module to overcome the scale factor issue. This paper is organized as follows. First, an overview of the use of camera as a navigation sensor is performed. Then, the integrated navigation system is detailed and the fusion strategy is proposed. The equations of the process and measurement models are presented and discussed. Finally, the performance analysis of the proposed algorithm is performed. In particular, the results of the implemented GNSS/IMU/WSS/VSLAM are compared to the ones of the classical GNSS/IMU/WSS, GNSS/IMU in order to highlight the contribution of each sensor and particularly the camera, especially when the quality of the GNSS solution is degraded or when GNSS solution is unavailable.

II. CAMERA AS A NAVIGATION SENSOR

There are different ways to use the camera as a navigation sensor. In fact, mounted on board a vehicle, cameras can simply provide geometric information, such as distance or angle to a target detected in the image sequence, or process the information supplied by the images in order to provide an estimation of the position and orientation of the camera, referred to as the camera pose. This pose is expressed in a frame called vision frame described in section (III.A).

We focus in this study on the latter case where the camera is used as a pose estimation sensor. In this case, the vehicle motion is estimated based on the changes that occur in the images captured when the camera moves. In fact, motion is estimated based on the comparison of some specific object locations in the images. This comparison can be performed either with respect to a database previously built which means that the environment is already known, or between the images captured by the camera using the dead-reckoning principle for unknown environments. Since the development of a database is too heavy and too complex, this study focuses on the case where the environment is unknown.

A review and a classification of visual pose estimation techniques are performed in a previous work [START_REF] Ben-Afia | Review and classification of vision-based localisation techniques in unknown environment[END_REF]. Generally, these techniques are based on the detection and tracking of salient zones of the images called Features. These techniques are classified into two main categories: Visual Odometry (VO) and Visual Simultaneous Localization And Mapping (VSLAM). Both techniques incrementally estimate the pose in the vision frame by examining the changes that motion induces on images taken by the camera(s). The only difference is that VO aims at estimating only the camera pose according to the displacement measured from the comparison of the image sequence. This comparison is performed by establishing matches between the features observed in the sequence of images. On the other hand, VSLAM not only provides an estimation of the camera pose, but also builds at the same time the map of the environment in which the camera is moving. That is, after detecting features in an image, VSLAM determines the 3D feature positions and uses these reconstructed features in order to estimate the camera pose.

In this study, a keyframe-based VSLAM is used. This technique reconstructs features detected in some selected images called Keyframes and computes the camera pose using these features. Then a refinement process aiming at minimizing the re-projection errors of the reconstructed features is performed in order to improve jointly the estimation of the map and the camera pose. This process is called Bundle Adjustment [START_REF] Engles | Bundle adjustment rules[END_REF]. More details on the use of camera as a navigation sensor are in [START_REF] Ben-Afia | Review and classification of vision-based localisation techniques in unknown environment[END_REF]. Thanks to this refinement step, VSLAM has better performance than VO. Nonetheless its navigation solution not only drifts because of the DR principle, but also suffers from an unknown scale factor affecting the position when a monocular camera is used. These problems are handled in the proposed multisensor fusion architecture and the corrected visual information is, in turn, exploited to improve the system accuracy.

III. NAVIGATION FILTER

A.

Muti-sensor System Design

The aim of this study is to develop a low-cost multi-sensor fusion system capable of providing accurate navigation information for ground airport vehicles.

The proposed architecture consists of a single frequency GNSS receiver, a low-cost Inertial Measurement Unit (IMU), a vision module processing the images of a single camera with the keyframe-based VSLAM technique and a WSS. The IMU is selected as the reference sensor since it is the only sensor continuously providing a complete navigation solution (position, velocity and attitude). As shown in Figure 1, the INS mechanization errors are corrected by the measurements of the other sensors. An error-state EKF is used in order to estimate the corrections that should be applied to the inertial navigation solution.

The estimated IMU measurement errors are fed back to the mechanization since we have to deal with a low-cost IMU.

Otherwise, as highlighted in [START_REF] Shin | Accuracy improvement of low cost INS/GPS for land applications[END_REF], the mechanization can experience unbounded error growth, and the assumption of small errors used in the linearization process of the filter can be violated.

As mentioned previously, the proposed architecture uses GNSS, VSLAM and WSS as aiding sensors. It also exploits constraints reflecting the behaviour of the vehicle during navigation. These constraints consist mainly in considering that the vehicle moves only in the forward direction [START_REF] Sukkarieh | Low Cost, High Integrity, Aided Inertial Navigation Systems for Autonomous Land Vehicles[END_REF]. GNSS, if available, provides the vehicle position and velocity. The choice of processed GNSS data (position and velocity) instead of raw GNSS measurements (pseudoranges and pseudo-range rates) is justified by the fact that the airport can be assimilated to a semi-constrained environment. In such environments, we often meet the condition of having at least 4 satellites in view. In addition, loose coupling of GNSS with INS is much simpler, more robust and has faster processing time compared to tight coupling [START_REF] Angrisano | GNSS/INS Integration Methods[END_REF]. VSLAM provides the vehicle attitude in the vision frame, as well as its position given with an undefined scale 𝑘 𝑣 .

In order to be able to exploit the VSLAM attitude information, the orientation of the vision frame with respect to the local frame should be estimated. The VSLAM position information is used if the scale is also estimated. These two quantities are added to the state vector in order to be estimated.

Finally the WSS provides the forward vehicle velocity.

Figure 1: Multi-sensor system design

Due to the integration of such heterogeneous sensors, a special attention should be paid to the different coordinate systems associated to the sensors and the relationship between them. Table 1 specifies, for each sensor, the outputs and the associated coordinate system.

As highlighted in [START_REF] Shin | Accuracy improvement of low cost INS/GPS for land applications[END_REF] and [START_REF] Sukkarieh | Low Cost, High Integrity, Aided Inertial Navigation Systems for Autonomous Land Vehicles[END_REF] , it is preferred to express the position error in meters than in radians in order to avoid numeric instability inside the filter. Hence, the Local Tangent Plane is used as the reference system instead of the Latitude, Longitude and Height frame. As shown in Figure 2, the origin of this frame defined locally is chosen to be the initial position of the vehicle and its axes point towards the geographic East, North and Up directions (ENU). The velocity and the orientation are also expressed in this frame.

In this study, we represent the orientation of a frame (𝛼) w.r.t. a frame (𝛽) by a (3𝑥3) rotation matrix denoted 𝑪 𝛼2𝛽 .

The WSS output is defined along the body frame axes as shown in Figure 2. This frame is rigidly attached to the vehicle. The y-axis points to the forward direction, the zaxis points upwards and the x-axis completes the righthanded orthogonal frame.

Figure 2: Reference frames

The VSLAM outputs are given with respect to the vision frame. The VSLAM is considered in this study as a black box providing the camera pose in the vision frame. Therefore, the vision frame orientation with respect to the local frame is unknown. However, its origin is assumed to be the initial position of the vehicle [START_REF] Angrisano | GNSS/INS Integration Methods[END_REF]. The difference between the sensors lies also in their running rates. Normally, the INS has the highest rate followed by the camera. The WSS and the GNSS have the lowest rates. Regardless of the frame rate values, the filter integrates each aiding measurement as it comes and updates the state vector. The GNSS measurements considered of bad quality are removed. This selection is based on the Dilution Of Precision (DOP) as well as the C/𝑁 0 values of the satellites.

Nav

If the aiding sensors do not provide any measurement or if a GNSS measurement is removed based on the previously mentioned criteria, then the IMU measurements are used for propagating the state. If a sensor (WSS or camera) provides a measurement at the same time as GNSS and if the GNSS measurements were not rejected, then the filter selects the GNSS measurements since they are the most reliable measurement in good GNSS conditions.

B. Navigation Filter

Notations and assumptions

In this study, the notations with the hat ^ are estimated quantities and those with the tilde ~ are measured quantities.

The notations without hat or tilde are actual quantities.

We assume that all the lever arms and orientation between the aiding sensors and the IMU are perfectly known and are measured before the navigation starts. where 𝒇 ̃𝑏 and 𝝎 ̃𝑏 are respectively the measured specific force and angular rate in the body frame. 𝒌 𝑎 and 𝒌 𝑔 are the scale factors associated to the accelerometers. 𝒃 𝑎 and 𝒃 𝑔 are the biases, and 𝜼 𝑎 and 𝜼 𝑔 are the noises.

Vision measurement model

The VSLAM outputs are given in the vision frame. In order to build the vison module measurement model, these outputs should be expressed as a function of the INS mechanization outputs. This can be done using the following equations. We remind that (𝒗) and (𝒍) have the same origin, that the position provided by VSLAM has an undefined scale and that the attitude is scale free.

Position:

𝒌 ̂𝑣 . 𝒑 ̃𝑐𝑎𝑚 𝑣 = 𝑪 ̂𝑙2𝑣 • (𝑪 ̂𝑏2𝑙 • ∆𝒑 𝑐𝑎𝑚 𝑏 + 𝒑 ̂𝑏 𝑙 ) + 𝜼 𝑉 (3)
with 𝜼 𝑉 is modeled as zero-mean, white and Gaussian noise.

(𝑋 𝑙 )

Attitude:

𝑪 ̃𝑐𝑎𝑚2𝑣 = 𝑪 ̂𝑙2𝑣 • 𝑪 ̂𝑏2𝑙 • 𝑪 𝑐𝑎𝑚2𝑏 (4)
GNSS measurement model GNSS provides position and velocity in (𝒍) frame. The GNSS measurement model taking into account the leverarm is [START_REF] Shin | Accuracy improvement of low cost INS/GPS for land applications[END_REF]:

Position 𝒑 ̃𝐺 𝑙 = 𝒑 ̂𝑏 𝑙 + 𝑪 ̂𝑏2𝑙 • ∆𝒑 𝐺𝑁𝑆𝑆 𝑏 + 𝜼 𝐺,𝑝 (5)
Velocity

𝒗 ̃𝐺 𝑙 = 𝒗 ̂𝑏 𝑙 + 𝑪 ̂𝑏2𝑙 𝛀 𝑙𝑏 𝑏 ∆𝒑 𝐺 𝑏 + 𝜼 𝐺,𝑣 (6)

WSS measurement model

WSS provides velocity along the forward axis of (𝒃) frame. The measurement model of the WSS is given in by: 𝒗 ̃𝑤 𝑙 = 𝑪 ̂𝑏2𝑙 (1 + 𝑘 ̂𝑤)𝒗 ̃𝑤 𝑏 + 𝑪 ̂𝑏2𝑙 𝜴 𝑙𝑏 𝑏 ∆𝒑 𝑤 𝑏 + 𝜼 𝑤 [START_REF] Strasdat | Scale drift-aware large scale monocular SLAM[END_REF] with 𝒗 ̃𝑤 𝑏 = [0 𝑣 ̃𝑤 𝑦 0] 𝑇 , 𝑘 𝑤 is the WSS scale factor and 𝜼 𝑤 is the WSS noise modeled as zero-mean, white and Gaussian. The lateral and vertical velocities are set to zero. This is due to the motion constraints of a ground vehicle, called Non-Holonomic Constraints [START_REF] Sukkarieh | Low Cost, High Integrity, Aided Inertial Navigation Systems for Autonomous Land Vehicles[END_REF].

Sate Vector

The INS estimates the navigation parameters i.e. the position, velocity and attitude of the vehicle, and the aiding sensors allow the filter to estimate the errors of INS parameters as well as the errors of the inertial raw measurements. The errors of the navigation parameters are defined by:

𝛿𝒑 𝑏 𝑙 = 𝒑 ̂𝑏 𝑙 -𝒑 𝑏 𝑙 ( 8
) 𝛿𝒗 𝑏 𝑙 = 𝒗 ̂𝑏 𝑙 -𝒗 𝑏 𝑙 (9)
𝑪 ̂𝑏2𝑙 = (𝑰 -𝑬 𝑏2𝑙 ) 𝑪 𝑏2𝑙 [START_REF] Eudes | Weighted Local Bundle Adjustment and Application to Odometry and Visual SLAM Fusion[END_REF] where 𝑬 𝑏2𝑙 is the skew-symmetric matrix of the attitude error 𝜺 𝑏2𝑙 = [ 𝜀 𝐸 𝜀 𝑁 𝜀 𝑈]

Using the previous notations, a 21-element error state vector associated to INS is consequently used:

𝛿𝒙 𝐼𝑁𝑆 = [𝛿𝒑 𝑏 𝑙 𝛿𝒗 𝑏 𝑙 𝜺 𝑏2𝑙 𝛿𝒃 𝑎 𝛿𝒃 𝑔 𝛿𝒌 𝑎 𝛿𝒌 𝑔 ] 𝑇 (11)
Since we use processed measurements from GNSS, no state is associated to GNSS.

Vision position and WSS velocity are affected by the scale factor ambiguities. Vision attitude is exploited only if the orientation of (𝒗) w.r.t (𝒍) is known. These parameters should be estimated. The associated state vectors are therefore given by:

𝛿𝒙 𝑉𝑖𝑠𝑖𝑜𝑛 = [𝛿𝑘 𝑣 𝜺 𝑙2𝑣 ] 𝑇 (12)
𝛿𝑥 𝑤𝑠𝑠 = 𝛿𝑘 𝑤 [START_REF] Engles | Bundle adjustment rules[END_REF] The full EKF state is the concatenation of the state vectors associated to each sensor. It is given by:

𝛿𝒙 = [ 𝛿𝒙 𝐼𝑁𝑆 𝑇 𝛿𝑥 𝑉𝑖𝑠𝑖𝑜𝑛 𝑇 𝛿𝑥 𝑤𝑠𝑠 ] 𝑇 (14)

State transition model

The behavior of the navigation parameter errors can be obtained by perturbing the following INS mechanization equation using Eq. (1-2) and Eq. (8-10) and keeping only the first order terms:

[ 𝒑̇𝑏 𝑙 𝒗̇𝑏 𝑙 𝑪 ̇𝑏2𝑙 ] = [ 𝒗 𝑏 𝑙 𝑪 𝑏2𝑙 𝒇 𝑏 -(2𝝎 𝑖𝑒 𝑙 + 𝝎 𝑒𝑙 𝑙 ) × 𝒗 𝑏 𝑙 + 𝒈 𝑙 𝑪 𝑏2𝑙 (𝛀 𝑏 -𝛀 𝑖𝑙 𝑏 ) ] ( 15
)
where 𝒈 𝑙 is local gravity including the Gravitation term and the centripetal term related to Earth rotation.

We model the inertial measurement bias and scale factor errors as Gauss-Markov process [START_REF] Angrisano | GNSS/INS Integration Methods[END_REF]:

𝛿𝑒 𝑠𝑢 ̇= - 1 𝜏 𝑒 𝑠𝑢 • 𝛿𝑒 𝑠𝑢 + 𝜂 𝑒 𝑠𝑢 ( 16
)
where 𝑒 is the error (𝑒 = 𝑏 or 𝑒 = 𝑘), 𝑠 is the sensor (𝑠 = 𝑎 or 𝑠 = 𝑔) and 𝑢 is the (𝒃) frame axis (𝑢 = 𝑥, 𝑢 = 𝑦 or 𝑢 = 𝑧). 𝜏 𝑒 𝑠𝑢 is the correlation time of the error 𝑒 of the sensor 𝑠 along the 𝑢-axis and 𝜂 𝑒 𝑠𝑢 is a zero-mean Gaussian noise.

The vision and WSS scale factor errors, as well as the orientation error of the vision frame with respect to the local frame, are modeled as constant states:

𝛿𝑘 𝑣 ̇= 𝜂 𝑘𝑣 (17)
𝜺 𝑙2𝑣 ̇= 𝜼 𝜖 (18)

𝛿𝑘 𝑤 = ̇𝜂𝑘𝑤 (19)
The models defined previously can be summarized to the following linearized continuous time sate transition model:

𝛿𝒙 ̇= 𝑭 • 𝛿𝒙 + 𝑮 • 𝒖 (20)
The expressions of 𝐹 and 𝐺 are given in the Appendix.

𝒖 = [ 𝜼 𝑎 𝜼 𝑔 𝜼 𝑏𝑎 𝜼 𝑏𝑔 𝜼 𝑘𝑎 𝜼 𝑘𝑔 𝜂 𝑘𝑣 𝜼 𝜖 𝜂 𝑘𝑤] 𝑇 (21)

The corresponding process noise covariance matrix is given by:

𝑸 = cov(𝒖) (22)
The discrete time sate transition model assuming that F and G are constant over the time step between two consecutive state propagations ∆𝑡 is given by:

𝛿𝒙 𝑘+1 = 𝚽 𝑘 • 𝛿𝒙 𝑘+1 + 𝒘 𝑘 (23) with [3] 𝚽 𝑘 = 𝑰 + 𝑭. ∆𝑡 (24)
and 𝐐 𝑘 = cov(𝒘 𝑘 ) = 𝚽 𝑘 𝑮𝑸𝑮 𝑻 𝚽 𝑘 𝑇 ∆𝑡 (25)

With the discrete state transition model, we can propagate the state using the prediction equations of the EKF. Since we are in a closed-loop configuration where the errors estimated by the EKF are fed back after the measurement update, the state vector should be reset to zero. Nonetheless, the state covariance is predicted using the equation: However, the lever-arm between the GNSS antenna and the IMU, denoted ∆𝒑 𝐺 𝑏 , should be accounted for. Using Eq.(5-6), the measurement vector is given by: 𝛿𝒛 𝐺,𝑝 = 𝒑 ̃𝐺 𝑙 -𝑪 ̂𝑏2𝑙 ∆𝒑 𝐺 𝑏 -𝒑 ̂𝑏 𝑙 (29) 𝛿𝒛 𝐺,𝑣 = 𝒗 ̃𝐺 𝑙 -𝑪 ̂𝑏2𝑙 𝛀 𝑙𝑏 𝑏 ∆𝒑 𝐺 𝑏 -𝒗 ̂𝑏 𝑙 (30)

Since we use an error-state EKF, the relationship between the measurement vector and the state can be written as:

𝛿𝒛 𝐺,𝑝 = 𝑯 𝐺,𝑝 • 𝛿𝒙 + 𝜼 𝐺,𝑝 (31)
where the notation (𝒂) × means the skew-symmetric matrix of the (3𝑥3) vector 𝒂.

WSS measurement model:

As for GNSS, the WSS velocity measurement is defined from Eq.7, taking into account the WSS scale factor [START_REF] Seo | Lever Arm Compensation for GPS/INS/Odometer Integrated System[END_REF]: where 𝜑 𝑉𝑆𝐿𝐴𝑀 , 𝜃 𝑉𝑆𝐿𝐴𝑀 and 𝜓 𝑉𝑆𝐿𝐴𝑀 describe the attitude of the vehicle using the VSLAM attitude output. In fact, using Eq.4, the vehicle attitude can be deduced from the camera attitude:

𝛿𝒛
(𝑪 ̃𝑏2𝑙 ) 𝑉𝑆𝐿𝐴𝑀 = 𝑪 ̂𝑙2𝑣 𝑇 • 𝑪 ̃𝑐𝑎𝑚2𝑣 • 𝑪 𝑐𝑎𝑚2𝑏 𝑇 (40)
Using the expression of 𝐶 𝑏2𝑙 defined in the Appendix: Let 𝛿𝑧, 𝐻 and 𝑅 be respectively the full measurement vector, measurement matrix and measurement covariance matrix. Then, the update is performed using the Kalman filter equations:

𝜑 ̃𝑉𝑆𝐿𝐴𝑀 = -
𝐾 𝑘+1 = 𝑃 𝑘+1 -𝐻 𝑘+1 𝑇 • [𝐻 𝑘+1 𝑃 𝑘+1 -𝐻 𝑘+1 𝑇 + 𝑅 𝑘+1 ] -1 (53) 𝛿𝑥 ̂𝑘+1 + = 𝐾 𝑘+1 𝛿𝑧 𝑘 (54) 𝑃 𝑘+1 + = 𝑃 𝑘+1 --𝐾 𝑘+1 • 𝐻 𝑘+1 • 𝑃 𝑘+1 - ( 55
)
IV.

EXPERIMENTS AND RESULTS

The algorithm is tested using GNSS and inertial data collected in an environment similar to airport. GNSS data is given by the GPS stand-alone mode of a Ublox-6 running at 1Hz. Inertial measurements are obtained using an Xsens Mti IMU running at 100 Hz. In a first step, VSLAM and WSS are simulated from the reference data using Eq.(3-4) and Eq.7 with respective rates of 50Hz for the VSLAM and 10Hz for the WSS. In a second step, a 1280x1024 forward looking camera was mounted on the roof of the car. The video is processed using the VSLAM algorithm. The reference trajectory is determined using the NovAtel SPAN equipment [START_REF] Seo | Lever Arm Compensation for GPS/INS/Odometer Integrated System[END_REF]. Figure 3 shows the equipment used for the measurement campaign. For simplicity reasons and since we target a vehicular application, only the horizontal performance will be analyzed. We remind that VSLAM outputs are given in the (𝒗) frame defined up to a rotation and a scale factor with respect to the (𝒍) frame. Finally a comparison between the headings outputted by each fusion algorithm is illustrated in Figure 8. It is clear that the heading aiding of the VSLAM improves dramatically the heading estimation. However, a simulation of zero-mean Gaussian error on the attitude outputted by the SLAM is a very optimistic assumption. Real data should be used in order to validate the model.

On the other hand, the use of WSS improves the estimation of the heading. This improvement is due to the correlation that exists between the attitude and the velocity. This correlation is expressed into the 𝐹 matrix of the Kalman filter process model.

In the case of INS/GPS/WSS/VSLAM, the vision scale factor is estimated and is reinserted to the filter in order to be used in the update step. Figure 9 shows the estimation of this scale factor by the developed EKF. If the estimation of this parameter is wrong, then the VSLAM position measurement will be wrong and will introduce a high error in the filter. That is why, the covariance matrix of the VSLAM position measurement depends thoroughly on the variance of the scale factor outputted by the EKF. In order to take into account this dependency, the measurement covariance matrix is multiplied by the scale variance. This improves the performance of the filter. In the first 205s, the vehicle is at rest. Since the VSLAM algorithm starts when the vehicles starts moving, then the use of the algorithm, and therefore the estimation of the scale factor starts after the vehicle moves. The convergence of the scale is achieved in 55s.

B. Real VSLAM data

The video taken by the camera is processed using the VSLAM algorithm. The comparison of the trajectory given by the VSLAM with the reference trajectory is performed in Figure 10. It shows clearly that the SLAM trajectory is affected by a scale factor. A zoom on the VSLAM trajectory in Figure 11 shows that in addition to the scale problem, this trajectory is turned with respect to the reference trajectory. As for simulated data, a comparison between the accuracy of the different navigation algorithms is illustrated in Figure 12 and Figure 13. It shows that, even if the VSLAM trajectory is affected by a scale factor and a rotation, the use of this information improves the performance of the navigation system. This is highlighted in Figures 14 and Figure 15 where the scale factor and the rotation angle (2D case) are well estimated by the Kalman filter. The improvement of the navigation performance using the VSLAM is shown by Table 3 in which a comparison of the West-East error statistics is performed. Finally, a GNSS outage is simulated on the entire trajectory in order to test the performance of our algorithm with only dead-reckoning sensors. Figure 16 and Figure 17 show that after 250s, the drift is about 20m with the use of visual information.

CONCLUSION

The current paper introduces a navigation system fusing low-cost sensors in order to improve the navigation parameter estimation for vehicles moving on airport surfaces. Results show that the use of a GPS/INS system is not sufficient to reach a high level of accuracy. The integration of complementary sensors such as the VSLAM providing position and attitude and the WSS providing velocity improves dramatically the navigation system performances. This was shown using simulated and real data. The possibility of having more than a measurement makes the system able to select the measurements according to their quality which improves the estimation of the navigation solution. This is the primary results of a study focusing on sensor integration. The next step will be to improve the fusion algorithm by using, for example, the filter innovation and the redundancy of measurements in order to exclude measurements having a large error. This can be done especially when GNSS raw measurements are used. This allows us to exclude the faulty GNSS measurements which may improve the filter performance.

AKNOWLEDGMENT

We would like to thank the French DGE for funding this project as well as M3Systems and the Architecture & IC Design, Embedded Software Department of CEA Saclay.

Appendix State transition Model in the ENU frame

State transition model:

The state transition equation is given by: 𝛿𝒙 ̇= 𝑭 The full state transition matrix is given by:

𝐹 = [ 𝐹 𝐼𝑁𝑆 0 21𝑥5 0 5𝑥21 0 5𝑥5 ]
And the G matrix is given by: 𝐺 = [ 0 3𝑥3 0 3𝑥3 0 3𝑥17 𝐶 𝑏2𝑙 0 3𝑥3 0 3𝑥17 0 3𝑥3 -𝐶 𝑏2𝑙 0 3𝑥17 0 17𝑥3 0 17𝑥3 𝐼 17𝑥17 ]

  The following notations are used: -Lever arm between GPS antenna and IMU: ∆𝒑 𝐺 𝑏 -Lever arm between camera and IMU: ∆𝒑 𝑐𝑎𝑚 𝑏 -Lever arm between WSS and IMU: ∆𝒑 𝑤 𝑏 -Orientation of the camera w.r.t. (𝒃): 𝑪 𝑐𝑎𝑚2𝑏Inertial Sensor ModelAn IMU is at least composed of accelerometers and gyroscopes. The accelerometers measure the specific force and the gyroscopes measure the angular rate. The measurements are provided in the body frame. In this work, 3 accelerometers and 3 gyroscopes are used. These measurements are inevitably affected by errors. For a lowcost IMU, these errors mainly consist of biases, scale factors and noise. In this study, the inertial measurements are modelled by the following equations:𝒇 ̃𝑏 = (𝑰 𝟑 + 𝒌 𝑎 ) • 𝒇 𝑏 + 𝒃 𝑎 + 𝜼 𝑎[START_REF] Icao | Manual on advanced surface movement guidance and control systems (A-SMGCS) -DOC9830 AN/452[END_REF] 𝝎 ̃𝑏 = (𝑰 𝟑 + 𝒌 𝑔 ) • 𝝎 𝑏 + 𝒃 𝑔 + 𝜼 𝑔[START_REF] Park | A Study of Severe Multipath Errors for the Proposed GBAS Airport Surface Movement Application[END_REF]

  GNSS measurement model:The position and velocity measurement vectors consist respectively in the difference between the GNSS position and the INS position denoted 𝛿𝒛 𝐺,𝑝 , and the GNSS velocity and the INS velocity denoted 𝛿𝒛 𝐺,𝑣 . In the case where the GNSS antenna and the IMU are placed in the same place, these measurement vectors are normally given by:

  𝑤,𝑣 = 𝑪 ̂𝑏2𝑙 (1 + 𝑘 ̂𝑤)𝒗 ̃𝑤 𝑏 -𝑪 ̂𝑏2𝑙 Ω 𝑙𝑏 𝑏 ∆𝒑 𝑤 𝑏 -𝑣 ̂𝑏 𝑙 (35) 𝑯 𝑤,𝑣 = [𝟎 3 | -𝑰 3 | ( 𝒗 ̂𝑏 𝑙 ) ×| 𝟎 3 | 𝑪 ̂𝑏2𝑙 (∆𝒑 𝑤 𝑏 ) ×| (36) 𝟎 3𝑥10 | ( 𝒗 ̂𝑏 𝑙 + 𝑪 ̂𝑏2𝑙 𝜴 𝑙𝑏 𝑏 ∆𝒑 𝑤 𝑏 )] Visual measurement model: The visual module provides position and attitude measurements. These measurements are expression by Eq.(3-4). Therefore 𝛿𝒛 𝑐𝑎𝑚,𝑝 = 𝑘 ̂𝑣 . 𝒑 ̃𝑐𝑎𝑚 𝑣 -𝑪 ̂𝑙2𝑣 • (𝑪 ̂𝑏2𝑙 • ∆𝒑 𝑐𝑎𝑚 𝑏 + 𝒑 ̂𝑏 𝑙 ) (37) 𝑯 𝑐𝑎𝑚,𝑝 = [𝑪 ̂𝑙2𝑣 | 𝟎 3 | 𝑪 ̂𝑙2𝑣 (𝑪 ̂𝑏2𝑙 ∆𝒑 𝑐𝑎𝑚,𝑎𝑡𝑡 𝛿𝒙 + 𝜼 𝑣,𝑎𝑡𝑡 (39)

Figure 3 :

 3 Figure 3: measurement campaign equipment A. Simulated VSLAM data The vision scale factor is modeled as drifting linearly over time as shown in Figure 4. This model is not necessarily good since the visual scale factor has only spatial drift [18].In fact, the vision scale only changes if new or different features are observed, and it keeps constant if the same features are observed. But since we are in outdoor environment, the observed features are always different and the time drift model can therefore be appropriate. We assume that the noise on the visual position is zero-mean, white and Gaussian with a standard deviation of 2m. The noise on the attitude angles provided by the SLAM is assumed to have a standard deviation of 1°.

Figure 4 :Figure 5

 45 Figure 4: Vision scale factor drift Figure 5 illustrates a comparison between the reference, the GPS and the VSLAM trajectories in the horizontal plane.We remind that VSLAM outputs are given in the (𝒗) frame defined up to a rotation and a scale factor with respect to the (𝒍) frame.

Figure 5 :Figure 6 and

 56 Figure 5: Horizontal trajectoriesA comparison between the navigation performances of each of the following fusion algorithms is done in this section in order to highlight the contribution of each sensor to the improvement of the navigation performance:-GPS -GPS/IMU -GPS/IMU/WSS -GPS/IMU/WSS/VSLAM Only the horizontal parameters will be analyzed since they are the most important parameters for land vehicle navigation. Figure6and Figure7illustrate a comparison of the horizontal position errors in the West-East and South-North directions given by the different systems.

Figure 6 :Figure 7 :

 67 Figure 6: Horizontal West-East error

Figure 8 :Figure 9 :

 89 Figure 8: Heading error

Figure 10 :Figure 11 :

 1011 Figure 10: Reference and VSLAM trajectories

Figure 12 :Figure 13 :

 1213 Figure 12: Horizontal West-East error

Figure 14 :Figure 15 :

 1415 Figure 14: Vision scale factor estimation

  Definitions and NotationsIn order to build the state transition model, some parameters should first be defined: -The vehicle latitude, longitude and height are respectively denoted 𝜑, 𝜆 and ℎ -The vehicle velocity in the (𝑙) frame is represented by the vector 𝒗 𝑏 𝑙 = [ 𝑣 𝐸 𝑣 𝑁 𝑣 𝑈 ] 𝑇 -The vehicle attitude is defined by the roll, pitch and heading respectively denoted 𝜙 , 𝜃 and 𝜓. -The meridian and transverse radii of curvature in the earth ellipsoid respectively denoted 𝑅 𝑚 and 𝑅 𝑛 . -The relationship between 𝜙 , 𝜃 and 𝜓, and 𝐶 𝑏2𝑙 is given by :𝑪 𝑏2𝑙 = [ 𝑐𝜙𝑐𝜓 + s 𝜙 s 𝜃 s 𝜓 c 𝜃 s 𝜓 s 𝜙 c 𝜓 -c 𝜙 s 𝜃 s 𝜓 -c 𝜙 s 𝜓 + s 𝜙 s 𝜃 c 𝜓 c 𝜃 c 𝜓 -s 𝜙 s 𝜓 -c 𝜙 s 𝜃 c 𝜓 -s 𝜙 c 𝜃 s 𝜃 c 𝜙 c 𝜃 ]with 𝑐 and s are respectively cosine and sine.

Table 1 :

 1 Sensor outputs

	. module	Output		Frame	Frame Origin
		INS Position	𝒑 ̂𝑏 𝑙	Local	Initial
	INS	INS Velocity	𝒗 ̂𝑏 𝑙	ENU	vehicle
		INS Attitude	𝑪 ̂𝑏2𝑙	(𝑙)	position
	GNSS	Antenna Position Antenna Velocity	𝒑 ̃𝐺 𝑙 𝒗 ̃𝐺 𝑙	Local ENU (𝑙)	Initial vehicle position
	VSLA	Camera Position	𝑣 𝒑 ̃𝑐𝑎𝑚	Vision	Initial vehicle
	M	Camera Attitude	𝑪 ̃𝑐𝑎𝑚2𝑣	(𝑣)	position
	WSS	Rear Wheels Velocity	𝑣 ̃𝑤 𝑦	Body (𝑏)	Vehicle center of mass

  𝝎 𝑒𝑙𝑙 is the transport rate resolved in (𝒍) frame. -𝜼 𝐺,𝑝 and 𝜼 𝐺,𝑣 are additive zero-mean, white and Gaussian noises whose variances are provided by the GNSS navigation module.

	with
	-𝛀 𝑙𝑏 𝑏 = (𝛀 ̃b -𝛀 𝑖𝑙 𝑏 ), 𝛀 b and 𝛀 𝑖𝑙 𝑏 are respectively the skew-
	𝑏 . symmetric matrix of the gyro measurement 𝜔 ̃𝑏and of 𝜔 𝑖𝑙 -𝝎 𝑖𝑙 𝑏 = 𝑪 𝑏2𝑙 𝑇 (𝝎 𝑖𝑒 𝑙 + 𝝎 𝑒𝑙 𝑙 ), with 𝝎 𝑖𝑒 𝑙 is the earth rate and

  The same angle definition is applied for 𝜑 ̂𝐼𝑁𝑆 , 𝜃 ̂𝐼𝑁𝑆 and 𝜓 ̂𝐼𝑁𝑆 deduced from the rotation matrix 𝑪 ̂𝑏2𝑙 outputted by the INS mechanization. By applying Eq.10, we find:

						𝐻 38 =	[𝐶 ̂𝑏2𝑙 12 ] -𝐶 ̂𝑏2𝑙 2 + [𝐶 ̂𝑏2𝑙 22 ] 22 𝐶 ̂𝑏2𝑙 32	2	(52)
		arctan (	(𝑪 ̃𝑏2𝑙 31 ) 𝑉𝑆𝐿𝐴𝑀 (𝑪 ̃𝑏2𝑙 33 ) 𝑉𝑆𝐿𝐴𝑀	)	(41)	Filter modes and update The previous section defines the measurement models for each aiding sensor. Since the processing rate of each sensor
	𝜃 ̃𝑉𝑆𝐿𝐴𝑀 = arcsin ((𝑪 ̃𝑏2𝑙 32 ) 𝑉𝑆𝐿𝐴𝑀 )	(42)	is different from the other and since the different rates are not necessarily multiples of each other, then we can identify
						7 running modes:
	𝜓 ̃𝑉𝑆𝐿𝐴𝑀 = arctan (	(𝑪 ̃𝑏2𝑙 12 ) 𝑉𝑆𝐿𝐴𝑀 (𝐶 ̃𝑏2𝑙 22 ) 𝑉𝑆𝐿𝐴𝑀	)	(43)	-INS only mode (for state covariance propagation since we are in closed-loop) -INS/VSLAM
						-INS/WSS
						-INS/GNSS
						-INS/VSLAM/WSS
						-INS/VSLAM/GNSS
						-INS/WSS/GNSS
	𝜃 ̂𝐼𝑁𝑆 = arcsin(𝜀 𝑁 𝐶 ̂𝑏2𝑙 12 -𝜀 𝐸 𝐶 ̂𝑏2𝑙 22 + 𝐶 ̂𝑏2𝑙 32 )	(44)	-INS/VSLAM/WSS/GNSS
	𝜓 ̂𝐼𝑁𝑆 = arctan (	𝐶 ̂𝑏2𝑙 12 + 𝜀 𝑈 𝐶 ̂𝑏2𝑙 22 -𝜀 𝑁 𝐶 ̂𝑏2𝑙 32 -𝜀 𝑈 𝐶 ̂𝑏2𝑙 12 + 𝐶 ̂𝑏2𝑙 32 ) 22 + 𝜀 𝐸 𝐶 ̂𝑏2𝑙	(45)	For each mode, the full measurement vector/matrix is the concatenation of the available measurement vectors/matrices. The measurement covariance is the
	where 𝐶 ̂𝑏2𝑙					diagonal matrix whose elements are the variances of the available measurements.
	0 1𝑥6 𝐻 17 𝐻 18 0 0 1𝑥17	(46)
	𝑯 𝑐𝑎𝑚,𝑎𝑡𝑡 = [ 0 1𝑥6 𝐻 27 𝐻 28 0 0 1𝑥17	]
	0 1𝑥6 𝐻 37 𝐻 38 1 0 1𝑥17
	with:
	𝐻 17 =	[𝐶 ̂𝑏2𝑙 31 ] 𝐶 ̂𝑏2𝑙 21 𝐶 ̂𝑏2𝑙 2 + [𝐶 ̂𝑏2𝑙 33 ] 33 -𝐶 ̂𝑏2𝑙 23 𝐶 ̂𝑏2𝑙 2 31	(47)
	𝐻 18 =	[𝐶 ̂𝑏2𝑙 31 ] 𝐶 ̂𝑏2𝑙 13 𝐶 ̂𝑏2𝑙 2 + [𝐶 ̂𝑏2𝑙 33 ] 31 -𝐶 ̂𝑏2𝑙 11 𝐶 ̂𝑏2𝑙 2 33	(48)
	𝐻 27 =	√ 1 -[𝐶 ̂𝑏2𝑙 32 ] -𝐶 ̂𝑏2𝑙 22	2	(49)
	𝐻 28 =	√ 1 -[𝐶 ̂𝑏2𝑙 32 ] 𝐶 ̂𝑏2𝑙 12	2	(50)
	𝐻 37 =	[𝐶 ̂𝑏2𝑙 12 ] -𝐶 ̂𝑏2𝑙 2 + [𝐶 ̂𝑏2𝑙 22 ] 12 𝐶 ̂𝑏2𝑙 32	2	(51)

𝑖𝑗

is the (i,j) component of matrix 𝑪 ̂𝑏2𝑙 .

The measurement matrix 𝑯 𝑐𝑎𝑚,𝑎𝑡𝑡 is computed by differentiating the expressions of the three angles with respect to 𝜀 𝐸 , 𝜀 𝑁 and 𝜀 𝑈 :

Table 2 : West-East error statistics

 2 Each time a sensor is added, the navigation performance is improved. The errors in both West-East and South-North directions decrease dramatically when the sensors are added. A comparison of the West-East error statistics is performed in Table2.

	Algorithm	Mean (m) STD (m) Max (m)
	GPS	13.01	31.56	115.3
	GPS/INS	-11.09	22.33	83.59
	GPS/INS/WSS	-6.34	11.65	55.88
	GPS/INS/WSS/VSLAM	-1.84	3.61	12.17

Table 3 : West-East error statistics

 3

	.18	8.72	46.03

Table 4

 4 gives the West-East error statistics in the case of GNSS removal and shows that it is possible to get a good accuracy with low-cost sensors.

	Algorithm	Mean (m) STD (m) Max (m)
	INS	3520	3051	8943
	INS/WSS	351	241.8	669.7
	INS/WSS/VSLAM	9.6	9.7	20.7

Table 4 : West-East error statistics Figure 16: Horizontal West-East error Figure 17: Horizontal South-North error

 4

  • 𝛿𝒙 + 𝑮 • 𝒖 The computation of the state transition matrix for INS states gives

	𝑭 𝑒𝑣 =	[	0 1 𝑅 𝑛 + ℎ tan(𝜑) 𝑅 𝑛 + ℎ	-1 𝑅 𝑚 + ℎ 0 0	0 0 0 ]		𝐹 𝑣𝑝 13 = 𝐹 𝑣𝑝 22 =	𝑣 𝐸 𝑣 𝑈 (𝑅 𝑛 + ℎ) 2 --2𝜔 𝑒 𝑣 𝐸 𝑐𝑜𝑠(𝜑) 𝑣 𝐸 𝑣 𝑁 𝑡𝑎𝑛(𝜑) (𝑅 𝑛 + ℎ) 2 𝑅 𝑚 + ℎ -(𝑅 𝑛 + ℎ)(𝑅 𝑚 + ℎ) 𝑐𝑜𝑠 2 (𝜑) 𝑣 𝐸 2
												𝐹 𝑣𝑝 23 =	𝑣 𝑁 𝑣 𝑈 (𝑅 𝑚 + ℎ) 2 +	𝑣 𝐸 2 tan(𝜑) (𝑅 𝑛 + ℎ) 2
												𝐹 𝑣𝑝 32 = -	2𝜔 𝑒 𝑣 𝐸 𝑠𝑖𝑛(𝜑) 𝑅 𝑚 + ℎ
												𝐹 𝑣𝑝 33 =	-𝑣 𝐸 2 (𝑅 𝑛 + ℎ) 2 -	𝑣 𝑁 2 (𝑅 𝑚 + ℎ) 2 +	2𝑔 𝑙 𝑅 + ℎ
												𝐹 𝑣𝑣 11 𝐹 𝑣𝑣 12 𝐹 𝑣𝑣 13
												𝐹 𝑣𝑣 = [ 𝐹 𝑣𝑣 21 𝐹 𝑣𝑣 22 𝐹 𝑣𝑣 23	], with
												𝐹 𝑣𝑣 31 𝐹 𝑣𝑣 32	0
												𝐹 𝑣𝑣 11 =	𝑣 𝑁 𝑡𝑎𝑛(𝜑)-𝑣 𝑈 𝑅 𝑛 + ℎ
												𝐹 𝑣𝑣 12 =	𝑣 𝐸 𝑡𝑎𝑛(𝜑) 𝑅 𝑛 + ℎ	+ 2𝜔 𝑒 𝑠𝑖𝑛(𝜑)
												𝐹 𝑣𝑣 13 = -2𝜔 𝑒 𝑐𝑜𝑠(𝜑) -	𝑣 𝐸 𝑅 𝑛 + ℎ
												𝐹 𝑣𝑣 21 =	-2𝑣 𝐸 𝑡𝑎𝑛(𝜑) 𝑅 𝑛 + ℎ	-2𝜔 𝑒 𝑠𝑖𝑛(𝜑)
				𝐹 𝑝𝑝 𝐹 𝑝𝑣	0 3	0 3	0 3	0 3	0 3	𝐹 𝑣𝑣 22 =	-𝑣 𝑈 𝑅 𝑚 + ℎ
					𝐹 𝑣𝑝 𝐹 𝑣𝑣 𝐹 𝑣𝑒 𝐶 𝑏2𝑙 𝐹 𝑒𝑝 𝐹 𝑒𝑣 -Ω 𝑖𝑙 𝑙 0 3 -𝐶 𝑏2𝑙 0 3	𝐶 𝑏2𝑙 • 𝐹 𝑏 0 3	0 3 -𝐶 𝑏2𝑙 • 𝑊 𝑏	𝐹 𝑣𝑣 23 =	-𝑣 𝑁 𝑅 𝑚 + ℎ
	𝑭 𝐼𝑁𝑆 =			0 3 0 3 0 3 0 3 0 3 0 3	0 3 0 3 0 3	𝛽 𝑏𝑎 0 3 0 3	0 3 𝛽 𝑏𝑔 0 3	0 3 0 3 𝛽 𝑠𝑎	0 3 0 3 0 3	𝐹 𝑣𝑣 31 =	2𝑣 𝐸 𝑅 𝑛 + ℎ	+ 2𝜔 𝑒 𝑐𝑜𝑠(𝜑)
	With	[	0 3 0 3	0 3	0 3	0 3	0 3	𝛽 𝑠𝑔	]	𝐹 𝑣𝑣 32 =	2𝑣 𝑁 𝑅 𝑚 + ℎ
		𝑉 𝑈 𝑅 𝑛 + ℎ	-	𝑉 𝑁 𝑡𝑎𝑛(𝜑) 𝑅 𝑚 + ℎ	𝑉 𝐸 𝑡𝑎𝑛(𝜑) 𝑅 𝑚 + ℎ	-𝑉 𝐸 𝑅 𝑛 + ℎ	𝐹 𝑣𝑒 = (𝐶 𝑏2𝑙 𝑓 𝑏 ) ×
	𝑭 𝑝𝑝 =	[				0 0			𝑉 𝑈 𝑅 𝑚 + ℎ 0	-𝑉 𝑁 𝑅 𝑚 + ℎ 0 ]	𝑭 𝑒𝑝 0	0	𝑣 𝑁 (𝑅 𝑚 + ℎ) 2
	𝑭 𝑝𝑣 = 𝑰 𝟑							=	0	-	𝜔 𝑒 sin(𝜑) 𝑅 𝑚 + ℎ	-𝑣 𝐸 (𝑅 𝑛 + ℎ) 2
	𝑭 𝑣𝑝 = [	0 𝐹 𝑣𝑝 12 𝐹 𝑣𝑝 13 0 𝐹 𝑣𝑝 22 𝐹 𝑣𝑝 23	], with			[ 0	𝜔 𝑒 cos(𝜑) 𝑅 𝑚 + ℎ	+	𝑣 𝐸 (𝑅 𝑛 + ℎ)(𝑅 𝑚 + ℎ) cos 2 (𝜑)	-𝑣 𝐸 tan(𝜑) (𝑅 𝑛 + ℎ) 2 ]
			0 𝐹 𝑣𝑝 32 𝐹 𝑣𝑝 33
	𝐹 𝑣𝑝 12 =	2𝜔 𝑒 (𝑣 𝑁 𝑐𝑜𝑠(𝜑) + 𝑣 𝑈 𝑠𝑖𝑛(𝜑)) 𝑅 𝑚 + ℎ	+	𝑣 𝐸 𝑣 𝑁 (𝑅 𝑛 + ℎ)(𝑅 𝑚 + ℎ) 𝑐𝑜𝑠 2 (𝜑)