Sensor fusion for land vehicle slope estimation

Multi-level road junctions are becoming increasingly popular in several major cities. In order to cope with such scenarios, car navigation systems require positioning units to supply accurate slope information. This knowledge allows correct matching to one of the options available from 3D maps. For cost reasons, slope needs to be obtained using consumer-grade MEMS IMUs. One method to assess the vehicle pitch angle, is using accelerometer, although the contributions of gravity and motion need to be separated from its output. This compensation procedure, in land vehicle applications, is possible when unit is connected to vehicle (e.g. via CAN bus); in such a way motion information can be obtained and removed from the raw accelerometer measurements. These measurements, even when calibrated and motion-compensated, are affected by issues such as noise (vibrations, quantization and differentiation), biases due to lever arm between wheels and sensor, IMU cross axis effects, etc. These issues prevent accelerometer-only slope estimation to reach acceptable performance in multilevel navigation scenarios. In order to overcome them, this research work proposes to use gyroscope observables to complement the accelerometer in the pitch estimation algorithm. The gyroscope provides relative angle measurements, with the advantage of being very accurate on high frequencies, while drifting with time due to integration of biases. The accelerometer, instead, is typically subject to large errors at high frequencies. The two sensors observables have been merged into an innovative sensor fusion algorithm, based on two cascaded Kalman filters, in addition to the GNSS receiver data. The first one exploits GNSS PVT Outputs (e.g. altitude and vertical velocity) and car travelled path information, in order to calibrate and motion-compensate the accelerometer output. It provides also a smoothed, GNSS independent altitude estimation. The second one takes as input the raw slopes output from the first stage and performs fusion with the gyroscope signals. This stage also estimates the gyroscope calibration parameters. Algorithms have been designed and modelled in MATLAB™, validated on real field data acquired through a sensor logging equipment featuring a commercial GNSS receiver, a consumer-grade 6 axis IMU and a pressure sensor. The barometer measurements are not yet used in the sensor fusion algorithm, instead are fed to an alternative post processing method (also described in this work) providing a robust reference for validation. Simulation results confirms that the estimated slope is sufficiently smooth and accurate for multilevel junction navigation. This new algorithm overcomes the limitations of accelerometer-only architectures and constitutes an important building block for performance improvements of attitude and heading reference systems in automotive contexts. It could also be the basis for the development of a fully inertial navigation system. Future developments include the integration of the barometric information in the real time sensor fusion algorithm and extension to the other attitude angles.