Rover Sensor Fusion & Localization

Overview

This project combined the complete embedded sensing chain: construction of the rover test hardware, direct acquisition and calibration of its navigation sensors, ROS integration, and implementation of an Unscented Kalman Filter for localization. Two suspended rover platforms were used so the estimator depended on general rigid-body kinematics rather than a vehicle-specific wheel or suspension model.

The mobile platform was built around a four-wheel RC chassis with independent suspension and an added two-level test deck. A Raspberry Pi 4 provided the Linux/ROS computer, while a Navio2 expansion board supplied the MPU9250 accelerometer, gyroscope, and magnetometer together with GNSS and barometric sensing. The open deck kept the boards, power distribution, external antennas, wiring, and camera accessible during field debugging and made it possible to move the same navigation stack between rover chassis.

Instead of using ArduPilot and MAVLink—which limited IMU delivery to roughly 40 Hz in this setup—I developed a custom C++ navio2_sensors_reader package. It acquired raw inertial data directly over SPI at 100 Hz, aligned each sensor with the selected body coordinate frame, and published the results through ROS. I also extended the low-level u-blox interface to request GNSS velocity in the NED frame through UBX-NAV-VELNED. Latitude, longitude, and altitude were converted from WGS84 through ECEF into a local North-East-Down frame whose origin was established by the first valid GNSS fix.

The 16-state SMU-UKF estimated three-dimensional position and velocity, quaternion orientation, accelerometer bias, and gyroscope bias. Accelerometer and gyroscope measurements propagated the state; GNSS position and velocity corrected inertial drift, while magnetometer data and the WMM2020 magnetic declination model supplied a geographic heading correction. Initial accelerometer and gyroscope offsets were measured online while the stationary rover collected a configurable sample window. A separate hard-iron calibration, implemented and cross-checked in C++ and MATLAB, corrected the magnetometer.

The software supported both real-time operation and faster-than-real-time replay from ROS bags. Testing progressed from stationary boards and short parking-lot paths to repeatable oval and circular trajectories on an athletics track. At the final revision, the filter ran at 75 Hz compared with the GNSS receiver’s 10 Hz updates and handled simulated GNSS interruptions lasting a few seconds. Its estimates were comparable to GNSS rather than consistently better: vertical motion, attitude, and inertial-bias convergence remained the main limitations. Those results identified the next steps clearly—fusing barometric altitude, improving magnetic filtering, and adding roll and pitch measurements to the correction vector.