Programming Assignment 2: 3D Orientation Solution

Objective
The goal of this assignment is to have some hands-on experience in IMU 3D orientation tracking, which is a very important technique for IMU based motion tracking. Students are expected to implemented 3D orientation tracking algorithm by first identifying the initial orientation and then do gyro integration to enable continuous tracking.

Problem description
This MP is divided into two parts. Part 1: initial orientation calculation; Part 2: 3D orientation tracking

2.1 Initial orientation calculation

Assume the phone is static at the initial moment. We will provide you with the accelerometer reading at that moment (unit: ). Your goal is to identify the initial phone orientation from accelerometer data. We will not provide compass data here since all the data are collected indoor and compass won’t give an accurate north indoor. Instead, assume at the initial moment, the projection of phone’s local Y axis onto the horizontal plane is pointing towards the global Y axis. This will also give a fixed phone initial orientation.

We expect you to output global direction in which the phone’s local X axis is pointing at in this part.

Hint: find the global Y axis’s direction in the local frame and let this direction be a vector . Let the gravity in the local frame be another vector . Then essentially you need to solve the following equation:

 





2.1 3D orientation tracking

In this part, you need to use the initial orientation calculated in part 1, and then do gyro integration for each timestamp onward. We will provide you with traces of gyroscope data, in CSV files. There are three columns in the file, representing the gyroscope readings in three local axes (unit: ). The gyroscope is sampled at 100Hz. Your task is to track the phone’s 3D orientation and output the end direction in which the phone’s local X axis is pointing at in the global frame, after performing these rotations.

One way of solving this problem can be:

Assume the gyroscope’s sample interval is .
Get the phone’s instant rotation axis and rotation angle in the local frame for each time stamp , where and .
Project the instant rotation axis into the global frame using the phone’s 3D orientation matrix at time .
Convert the instant rotation axis and angle in global frame into the form of rotation matrix .
Find the total 3D rotation matrix for time : .
Students are expected to use common signal processing and linear algebra functions (e.g., high/low pass filtering, convolution, cross coloration, peak detection, fft etc.) and libraries (numpy, scipy etc.). If not sure about whether a specific function/library can be used, please contact TA.

Input/Output format
Please make your code into a single file mp2.m (if using matlab) or mp2.py (if using python). TA will use [python mp2.py] or [matlab -nodisplay -nosplash -r "mp2; exit"] to run your code.

The code should read the input from “data.csv” and write results to “result.txt”. The first line of “data.csv” is the initial accelerometer reading. From the second line, it shows the gyroscope reading for each time stamp. The output should have two line (one line for part 1 and one line for part 2), with three numbers separated by space for each line, representing the phone’s X direction in the global frame. (Normalize or not are both fine.)

How to submit
Please submit code on compass, and fill in this Google Form about team information.

Grading
We will grade on 10 traces of data (including the example data we provided), each worth 10 points. Among them we have included the simple case where the phone’s initial local frame is aligned with the global frame, and phone will only rotate along Z axis onwards. (In case you find the MP too difficult, only doing 1D integration on Z axis should at least also give you some points.)