Task 1. Estimate the orientation of the IMU

Ascertain automatically from a stationary and moving vehicle what the orientation of the IMU is with respect to the vehicle. The only sensors that you have are GPS, Accelerometer, and Gyroscope.

Figure 1. The different approaches when estimating the IMU frame with respect to car frame.

Problem Overview

The data IMU (Accelerometer + Gyroscope) we can use the Extended Kalman Filter to estimate the state of the car $x_n$.

$$x_n := (\boldsymbol{R}^{IMU}_n, \boldsymbol{v}^{IMU}_n, \boldsymbol{p}^{IMU}_n, \boldsymbol{b}^\omega_n, \boldsymbol{b}^a_n)$$

However, when we locate the IMU sensor in the car. In different set-up scenarios, the position of the IMU may by variances, the Invariant Extended KalmanFilter (IEKF) helps in this problem, this algorithm can estimate the pose between the car and IMU frame ($\boldsymbol{R}^c_n, \boldsymbol{p}^c_n)$by using EKF+ Neural Net. So the new form of car state is $x_n$

$$x_n := (\boldsymbol{R}^{IMU}_n, \boldsymbol{v}^{IMU}_n, \boldsymbol{p}^{IMU}_n, \boldsymbol{b}^\omega_n, \boldsymbol{b}^a_n, \boldsymbol{R}^c_n, \boldsymbol{p}^c_n)$$

Proposed Solution Task 1.

• Using only the IMU sensor is enough to solve this problem and does not need GPS for this task, because the result in the Brossard paper can use only IMU to estimate the car's state with acceptable error. But in real-life applications, we need to verify more data and testing reliable of the algorithms.

• Using the IEKF to estimate the velocity of the origin point of the car frame by using the equation. From that, we can know if the car is stationary $(v_n^c =0)$ or moving state $(v_n^c \ne0)$

$$v^c_n = R^{cT}_nR^{_{IMU}T}_nv^{_{IMU}}_n + (\omega_n)_\times p_n^c$$

• The IEKF is chosen for this task over classical EKF algorithms, but more testing and verifying reliable and real-time performance between 2 algorithms.

• The output is a plot velocity of the car and orientation of IMU pose with respect to the car frame.

• The programming language is Python because IEKF heavily relies on NumPy and PyTorch libraries.

Block diagram Task 1

Adding calculating block 🟧in the Update step to calculate Car velocity, orientation, and IMU velocity.

Figure 2. Block diagram in task 1 to estimate Car velocity, IMU velocity and Rotation

Code in Task 1.

• Create a new python program car_velocity.py to get a new car state, which includes: Rotation matrix of the car body with respect to the world frame, the velocity of the car, and the velocity of the IMU.

• Modifying the utils_numpy_filter.py to add a new code block in the update step of IEKF algorithms

• Modifying the utils_plot.py by adding a new plot function to plot the data.

Result in Task 1 (only IMU data)

The result includes 10 sequences but for a better overview, 3 of them are over 400s were chosen to plot the result below.

1.Sequence 2011_09_30_drive_0028_extract in 538.48 s

2. Sequence 2011_10_03_drive_0027_extract in 474.54i s

3. Sequence 2011_10_03_drive_0034_extract in 484.11 s

The IMU frame with respect to the car frame is estimated by using IKEF+Neural Net, so can help to reduce the drifts but not totally eliminated it. We need to test with different long-length trajectories and different shapes of trajectories to get a better conclusion.

I plotted 10 sequences and got the draft result:

• Roll angle is always the same at zero because the car is relatively stable in roll angle when its run at normal speed (10~15 m/s)

• Pitch angle was increasing over time, because of the drift of the Z- axis and the car moving up/down in some states ( e.g Car going up/down the hill)

• Yaw angle converges or not depends on the length of trajectories and the shape of the trajectories-closed loop or not)

Result Task 1 (IMU+GPS)

I use the algorithm same with task 2 (please see block diagram in task 2)

The result when adding GPS signal is more coverge than only use IMU data