In my implementation, I take the logistic from the original project. However, I focused on only orientation tracking to get a sense of how unscented kalman filter works, which helps me to implement a UKF for trakcing 3D position, velocity, and orientation by fusing IMU and GPS data.

• Tested on Mac with Python 3.6.5
• Used a library called quat_helper.py for quaternion operations
• Employed the framework of the filterpy.kalman UKF
• Used an easy way to get the mean of a set of quaternions, see quaternion_mean in quat_helper.py

One thing to note is that there are two conventions on quaternion product/multiplication/composition: Hamilton and JPL (see 'Why and How to Avoid the Flipped Quaternion Multiplication'). In my implementation, I used Hamiton convention, which is commonly used (qb = qmult(dq, qa)).

In this project, I have implemented Unscented Kalman filter to track three dimensional orientation. This means to estimate the underlying 3D orientation by learning the appropriate model parameters from ground truth data given by a Vicon motion capture system, given IMU sensor readings from gyroscopes and accelerometers. Then be able to generate a real-time panoramic image from camera images using the 3D orientation filter.

• Tested on: Kubuntu 16.04.3, Intel i5-4200U (4) @ 2.600GHz 4GB
• Python 2.7, OpenCV 3.2

• The first part of the problem was to calculate bias and scale parameters for the accelerometer and gyroscope readings.
• Convert IMU readings to quaternions.
• Implement UKF.
• Perform Image Stitching.

• The UKF implementation was done using only orientation(gyroscope) in the state vector as the control input: q = [q₀, q₁, q₂, q₃]^T .
• Initialize P (Covariance matrix) as size of 3x3. Similarly, R and Q. R is measurement noise and Q is process noise.
• After Kalman filter predict step, new P and state vector q are obtained, which are the used for update step.
• Then Sigma Points are obtained by Cholesky decomposition of (P+Q).

• This step deals with updating P and getting new mean state q. Which then leads to obtaining new Sigma Points. This new sigma points are used to calculate multiple covariances, like Pzz, Pxz, and Pvv.
• The next step involves computing K (Kalman Gain) = Pxz Pvv-1 and I (Innovation term) = Accelerometer reading – Mean of Sigma Points
• These are used to calculate the P and q for the next stage.

Roll-Pitch-Yaw	Vicon Stitch	Estimated Orientation Stitch

Test Dataset Roll-Pitch-Yaw	Estimated Orientation Stitch

Vicon Stitch	Estimated Orientation Stitch

Download:

Training Dataset

Test Dataset

Place the cam data in "cam" folder, vicon data in "vicon" folder and imu data in "imu" folder.

• Run ukf.py to calculate the predicted values for q state vectors.

  • Outputs of Roll, Pitch and Yaw for predicted vs vicon data will be in folder --> 'Results' with RPY prefix.
  • Outputs of stitching will be in folder --> 'Results' with 'Pano' prefix.

  NOTE - All variables to toggle stitching, change dataset and change stitching medium(vicon/imu) are in 'ukf.py' at beginning.

• quat_helper.py contains neccessarry functions for quaternion state vector manipulation.

• A quaternion-based unscented Kalman filter for orientation tracking
• The Kalman Filter