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 $G$ with GPS signal, adding GPS signal to Update step by adding Measurement covariance $R_{GPS}$, Normalize measurement GPS position, Add normalize measurement GPS value to the Measurement covariances Matrix $H$.

• 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 $\mathcal{N} = (\mu, \sigma^2)$, where $\mu = 0$ is the mean and $\sigma = 2 m$ 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 experiments

• Create 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 name gps_normalize_factors.p for GPS data, and normalize_factors.p for IMU data

• Create programs utils_numpy_gps_filter.py and utils_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 in ultils_plot.py to plot the data

Preprocess Step

1. Reading the KITTI dataset and adding Gaussian noise to GPS data

Noised GPS position signal

Step 2. Normalize GPS data

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.

Brossard Algorithm (IMU)

Tri Bien Algorithm (IMU+GPS)