Task 2. Dead-reckoning based on IMU and GPS data using IEKF
Augment Martins Brossard algorithm to take into account GPS as well. Use the KITTI data set as validation. Please superimpose your estimated trajectories with the map and the raw GPS coordinate.
Problem overview
The Brossard algorithm uses only IMU data. In some cases, the IMU lost data such as 2011_10_03_drive_0034_extract
sequence the result was getting worse. To get a more robust result, I propose the updated version using the GPS data and combine with MeshNet to calculate the GPS measurement covariance and adding some functions in the Update Step, Propagate Step, as shown in Fig 1 below.
Proposed Solution Task 2.
Create a program to read and convert the raw KITTI dataset (https://www.cvlibs.net/datasets/kitti/index.php) to get GPS data, because the Brossard dataset supplement was not including the GPS data (only IMU and ground truth car's angular, velocity, position)
Create a code block to convert the GPS coordinate to the world frame coordinate.
Create an update code block in Predict + Update step with the new car state (adding GPS signal)
Create a code block to create Measurement Covariance by using MeshNet in Pytorch.
Update the Measurement Jacobian matrix with GPS signal, adding GPS signal to Update step by adding Measurement covariance , Normalize measurement GPS position, Add normalize measurement GPS value to the Measurement covariances Matrix .
Plot the result in Map, and compare it with the Brossard algorithm.
Code solution Task 2
Login/Download the raw GPS signal from the website(https://www.cvlibs.net/datasets/kitti/index.php), create a folder
../KITTI/raw
and put all dataset files into the folder.Create a program
read_KITTI.py
to convert raw GPS signal from the KITTI dataset to Pytorch Pickle format *.p. The new dataset has the same value as the old dataset but it added the latitude, longitude, and altitude of GPS coordinates.Convert from latitude, longitude, and altitude GPS coordinates to North, East, and Up (NEU) coordinates. Then add simulated Gaussian noise , where is the mean and is the standard deviation. The result is shown in the below figure GPS vs Ground truth position.
Create a main code
iekf_gps.py
to run all experimentsCreate a code block
forward_nets_gps
to create Measurement Covariance by using MeshNet in Pytorch.Create a program
set_normalize_data.py
to normalize the GPS and IMU data, result will save in a folder../temp
with namegps_normalize_factors.p
for GPS data, and normalize_factors.p
for IMU dataCreate programs
utils_numpy_gps_filter.py
andutils_torch_gps_filter.py
to add argument GPS signal in Jacobian Matrix G, Measurement covariance R, Measurement covariances matrix H.Code a function
gps_results_filter
inultils_plot.py
to plot the data
Preprocess Step
1. Reading the KITTI dataset and adding Gaussian noise to GPS data
Step 2. Normalize GPS data
Result in different sequences in KITTI dataset
Sequence drive_0033
Sequence drive_027
Sequences drive_028
Sequence drive_079
The Sequence 2011_10_03_drive_0034_extract
is the most challenging sequence, which is missing both IMU and GPS data in the near place, as shown in the figure below.
Last updated