Orientation Estimation¶

Introduction¶

You have learned that robot navigation is the problem of guiding a robot towards a goal in Chapter 5. In order to achieve navigation tasks, you need to localize your robot using some sensor systems (mentioned in Chapter 6). If you are doing outdoor applications with your robot, then you may use GPS signals from the satalites. However, it is not reliably applicable for intoor applications.

In order to get the information about the position/orientation of your robot, you may use some camera systems. After some image processing, you can determine your robot’s position/orientation. What about your robot is a mobile robot, let say? What if your taks requires going from one room to another? Would you equip all the rooms with expensive cameras on each corners of each room? That would not be the most practical and wisely designed solution.

For instance, how do we get the orientation information from our mobile phones? They are mobile at the end, right? Mainly for this purpose, there is a sensor type called IMU (i.e. Inertial Measurement Unit) which can measures the angular velocity and the linear acceleration of the body - in this case your mobile phone.

There are different techniques to obtain position/orientation of the phone using angular velocity and the linear acceleration. In this tutorial, you will learn how to implement four different orientation calculation techniques using MATLAB and IMU. Those techniques are:

Integration of angular velocity (only gyroscope)
Inclination sensing (only accelerometer)
Complementary filter (accelerometer + gyroscope)
Kalman filter (accelerometer + gyroscope)

Note

You will find the explanations of all those techniques in the following sections. The values which has hat on top means that they are estimated values.

Integration of Angular Velocity¶

Integration of angular velocity principle leans on the the basic idea: the integral of the velocity is the position. Since an IMU reads the angular velocity of the body, then the integration of the angular velocity is the orientation. In literature the orientation angles with respect to their rotation axes are defined as follows:

By this definition, the orientation angles are calculated as:

\[\begin{split}roll \rightarrow \phi &= \int_{t_0}^{t} G_x \Delta t\\ pitch \rightarrow \theta &= \int_{t_0}^{t} G_y \Delta t\\ yaw \rightarrow \psi &= \int_{t_0}^{t} G_z \Delta t\\\end{split}\]

where \(G_x\), \(G_y\) and \(G_z\) are the angular velocities read by the IMU about x,y and z axes respectively in body fixed frame. To find the orientations with respect to world frame, you need this rotation matrix:

\[\begin{split}\begin{bmatrix} \dot\phi \\ \dot\theta \\ \dot\psi \end{bmatrix} = \begin{bmatrix} 1 &sin(\phi) tan(\theta) &cos(\phi) tan(\theta) \\ 0 &cos(\phi) &-sin(\phi)\\ 0 &sin(\phi) sec(\theta) &cos(\phi) sec(\theta) \end{bmatrix} \cdot \begin{bmatrix} G_x \\ G_y \\ G_z \end{bmatrix}\end{split}\]

and,

\[\begin{split}\hat\phi = \phi_{prev} + \dot\phi \Delta t\\ \hat\theta = \theta_{prev} + \dot\theta \Delta t\end{split}\]

Warning

In theory, this method of orientation calculation works perfect. However the computers on which we are working are doing discrete calculations. On the other hand, we measure the angular velocity in real world, moving the phone in our hands. When you integrate a continuous signal in a discrete environment, you have an accumulation problem (i.e. drift error).

Although Integration of angular velocity looks a clean-cut for short period of time, it is not the best solution for longer periods.

Inclination Sensing¶

Accelerometers are sensitive to both linear acceleration and the local gravitational field. If the linear acceleration on the body is negligble, then assume that the only acceleration exerting on the body is the gravity. In our case, as the body stays still, the gravitational acceleration is measured only by the z-axis of the IMU.

If we rotate the IMU, let say \(\theta\) angles around y-axis, then the gravity vector is expressed by x and z components on the IMU readings.

Note

There is still no component of gravity vector along the y-axis of the IMU since the rotation was made around y-axis.

In more general form, the rotation is not only around one axis. If the IMU is rotated around x and y axes, by calling the rotation \(\phi\) around x-axis, the vectoral representation would be like that:

Here the \(\theta\) and \(\phi\) angles is found by the following formula:

\[\begin{split}tan(\phi) = - a_y / \tilde a_z = - a_y / \sqrt (a_x^2 + a_z^2)\\ tan(\theta) = a_x / \tilde a_z = a_x / \sqrt (a_y^2 + a_z^2)\end{split}\]

Therefore,

\[\begin{split}\hat\phi = arctan(\frac{-A_y}{\sqrt{A_x^2 + A_z^2}})\\ \hat\theta = arctan(\frac{A_x}{\sqrt{A_y^2 + A_z^2}})\end{split}\]

Complementary Filter¶

Idea behind complementary filter is to take slow moving signals from accelerometer and fast moving signals from a gyroscope and combine them. Accelerometer gives a good indicator of orientation in static conditions. Gyroscope gives a good indicator of tilt in dynamic conditions. So the idea is to pass the accelerometer signals through a low-pass filter and the gyroscope signals through a high-pass filter and combine them to give the final rate.

To implement Complementary filter, first a constant \(\alpha\) angle is chosen as a cut-off value for the filters. The larger \(\alpha\), the more the accelerometer measurements are ‘trusted’. As \(\alpha\) goes to zero, we base our estimate mainly on the gyroscope measurements. A good starting point is \(\alpha\) = 0.1.

\[\begin{split}\hat{\phi} = \alpha \cdot \hat\phi_{Acc} + (1-\alpha) \cdot (\hat\phi_{prev}+ \dot\phi_{Gyro} \cdot \Delta t)\\ \hat{\theta} = \alpha \cdot \hat\theta_{Acc} + (1-\alpha) \cdot (\hat\theta_{prev}+ \dot\theta_{Gyro} \cdot \Delta t)\\\end{split}\]

Kalman Filter¶

Kalman filter is one of the most common estimation algorithms. It produces estimates of imponderable states of a system based on the past estimations and current measurements. In another words, it is an estimator (and observer). Using the system model, it reduces the estimation error in every iteration. In our case, we can measure angular velocity and linear acceleration but we cannot measure orientation. In this case orientation is an imponderable state. Though, it is possible to calculate the orientation using the systems mathematical model.

Note

As you know, every linear time-invariant (LTI) system can be modelled as:

\[\begin{split}\begin{split} \vec{\hat{x}}_{t} &= \textbf{A} \cdot \vec{\hat x}_{t-1} + \textbf{B} \cdot \vec{u}_t + \vec{w}_t \label{eqn:state1}\\ \vec{\hat{y}}_{t} &= \textbf{C} \cdot \vec{\hat x}_{t} + \vec{v}_{t} \label{eqn:state2}\\ \vec{\tilde{y}}_{t} &= \vec{y}_{t} - \vec{\hat{y}}_{t} \label{eqn:state3} \end{split}\end{split}\]

Where \(\vec{\hat x}_{t}\) is the estimated textbf{system state vector}, \(\vec{u}_t\) is the textbf{input vector} and \(\vec{y}_t\) is the textbf{measurement vector} at time t.

\(\textbf{A}\) : system matrix (relates the current states to the next states)

\(\textbf{B}\) : input matrix (relates inputs to the next states)

\(\textbf{C}\) : output matrix (system states to the measured states)

\(\vec{w}_t\) : process noise

\(\vec{v}_t\) : measurement noise

\(\vec{\hat{x}}_{t}\) : estimated state vector.

\(\vec{\hat{y}}_{t}\) : estimated measurement vector (observation vector)

\(\vec{\tilde{y}}_{t}\) : error between the actual measurement and the estimated

We choose our system states as:

\[\begin{split}\vec{\hat x}_t = \begin{bmatrix} \hat{\phi}_t \\ \hat{\dot{\phi}}_{b_t} \\ \hat{\theta}_t \\ \hat{\dot{\theta}}_{b_t} \end{bmatrix}\end{split}\]

Where \(\hat{\dot{\phi}}_{b_t}\) is the gyro bias at time t associated with our estimate \(\hat{\phi}\) and \(\hat{\dot{\theta}}_{b_t}\) is the gyro bias at time t associated with our estimate \(\hat{\theta}\).

Our inputs:

\[\begin{split}\vec{u}_t = \begin{bmatrix} \dot{\phi}_{G_t} \\ \dot{\theta}_{G_t} \end{bmatrix}\\\end{split}\]

Where \(\dot{\phi}_{t}\) and \(\dot{\theta}_{t}\) are the gyroscope values for roll and pitch respectively.

Our measurements:

\[\begin{split}\vec{y}_t = \begin{bmatrix} \hat{\phi}_{Acc_t} \\ \hat{\theta}_{Acc_t} \end{bmatrix}\end{split}\]

Implementation of Kalman filter¶

Kalman Filter is based on modelling the process noise. As well, the Kalman filter provides a prediction of the future system state by prediction. Therefore Kalman filter consists of two parts; Prediction and Correction.

After defining all the parameters, now we can start building up the Kalman filter. In prediction step, the system model is used in calculation of error covariance matrix P.

Prediction

\[\begin{split}\vec{\hat x}_{t} = \textbf{A} \cdot \vec{\hat x}_{t-1} + \textbf{B} \cdot \vec{u}_t\\ \textbf{P} = \textbf{A} \cdot \textbf{P} \cdot \textbf{A}^T + \textbf{Q}\end{split}\]

Then, this error covariance matrix is used in updating the Kalman gain K. (In some resources, you can see this step is named as Update for this reason.)

Correction

\[\begin{split}\widetilde{y}_{t} = \vec{y}_{t} - \textbf{C} \cdot \vec{\hat x}_{t+1}\\ \textbf{S} = \textbf{C} \cdot \textbf{P} \cdot \textbf{C}^T + \textbf{R}\\ \textbf{K} = \textbf{P} \cdot \textbf{C}^T \cdot \textbf{S}^{-1}\\ \vec{\hat x}_{t} = \vec{\hat x}_{t} + \textbf{K} \cdot \widetilde{y}_{t}\\ \textbf{P} = (\textbf{I} - \textbf{K} \cdot \textbf{C}) \cdot \textbf{P}\end{split}\]

Where,

K is the Kalman gain,

P is the error covariance,

Q is covariance matrix of the process noise,

R is covariance matrix of the measurement noise,

Note

Lower variance in measurement noise (R -> 0) makes the Kalman gain K closer to 1 and our estimates will be more based on the measurements.

\[\lim_{\textbf{R} \to 0} \textbf{K} = \frac{\textbf{P}^- \cdot \textbf{C}^T}{\textbf{C} \cdot \textbf{P}^- \cdot \textbf{C}^T + (\textbf{R}=0)} \rightarrow \textbf{K} = \textbf{C}^{-1}\]

Substitute into the estimation equation:

\[\begin{split}\hat{x} ^+ &= \hat{x} ^{-} + \textbf{K} \cdot (y ^{-} - \textbf{C} \cdot \hat{x} ^- )\\ &= \hat{x} ^{-} + \textbf{C}^{-1} \cdot (y ^{-} - \textbf{C} \cdot \hat{x} ^- )\\ &= \hat{x} ^{-} + \textbf{C}^{-1} \cdot y ^{-} - \textbf{C}^{-1} \cdot \textbf{C} \cdot \hat{x} ^{-}\\ &= \textbf{C}^{-1} \cdot y ^{-}\end{split}\]

\(C^{-1}\) is equal to 1 in our case. Therefore the estimated value is only depend on the measured value, not prior estimates.

\[\begin{split}\hat{x} ^+ &= y ^{-}\\\end{split}\]

Note

If in the first case the prior estimate covariance is zero (P -> 0), then only prior estimates contribute to our current estimation.

\[\lim_{\textbf{P} ^{-} \to 0} \textbf{K} = \frac{(\textbf{P}^- = 0) \cdot \textbf{C}^T}{\textbf{C} \cdot (\textbf{P}^- = 0) \cdot \textbf{C}^T + \textbf{R}} \rightarrow \textbf{K} = \frac{0}{\textbf{R}} = 0\]

Substitute into the estimation equation:

\[\begin{split}\hat{x} ^+ &= \hat{x} ^{-} + \textbf{K} \cdot (y ^{-} - \textbf{C} \cdot \hat{x} ^- )\\ &= \hat{x} ^{-} + \textbf{0} \cdot (y ^{-} - \textbf{C} \cdot \hat{x} ^- )\end{split}\]

Therefore the estimated value is only depend on the prior estimates, not measurements.

\[\begin{split}\hat{x} ^+ &= \hat{x} ^{-}\\\end{split}\]

Warning

The Kalman filter is only applicable in casual, linear and time-invariant systems. If the system model is not satisfy these three conditions, then another type of filter/estimator/observer or a different variation of Kalman filter should be implemented.

Experimental Process¶

To implement those four orientation estimation techniques, we will use MATLAB and a smartphone.

On your smartphone

In Google Store, install Sensor Fusion App (IOS phones can follow the Follow the link and use the sensorLog_2pitch.txt instead. They don’t need to follow these steps.)
Select the first item Select Sensor and check if your accelerometer works fine. During a steady mode of your phone while its screen facing upwards, only the z-axis of the accelerometer should give 9.8 \(m/s^2\) and other axes should be 0.
Check if your gyroscope works fine. During a steady mode of your phone all axes should be 0. If there is a little fraction, it is the bias on your gyroscope data.
In the main screen, select the second item Log Data.
Check Accelerometer and Gyroscope and uncheck other sensors. We will not need them for this tutorial.
Check the Log option on the upper right corner so that the app can create a log file.
As soon as you hit the Start button, the log file is started to be written on. It is always better to start measurement from a steady state. Too long data will incrase the processing time in your code.
When you are done, hit Stop button.
Send the sensorLog_date&hour.txt file to your computer.

On your computer

Follow the link. There are 3 files. read_log_script.m is a script which reads your log data and extracts the accelerometer and gyroscope values into corresponding variables. Edit your log data file name in the \(10^{th}\) line.
input_filename = 'sensorLog_2pitch.txt';
In our main file, we first call our script.
read_log_script

You are going to use rotation matrix equations in the first section:

for i = 2:number_of_data
   p = Gx(i);
   q = Gy(i);
   r = Gz(i);
   
   phi_prev   = phi_hat_gyr(i - 1);
   theta_prev = theta_hat_gyr(i - 1);
    
   phi_hat_gyr(i)   = ...;
   theta_hat_gyr(i) = ...;
end

You are going to use the geometrical approach in Inclination sensing section:
phi_hat_acc = ...; theta_hat_acc = ...;

You are going to use your estimated \(\phi\) and \(\theta\) values in which you have calculated in the previous steps. :

for i=2:number_of_data
    p = Gx(i);
    q = Gy(i);
    r = Gz(i);
   
    phi_prev   = phi_hat_complimentary(i - 1);
    theta_prev = theta_hat_complimentary(i - 1);
    
       
    % Complementary filter equations
    % Hint: Use the gyro and acc values which you found in Section 1-2.
    phi_hat_complimentary(i)   = ...;
    theta_hat_complimentary(i) = ...;
end

You are going to fill the missing equations in Kalman filter.

for i=2:number_of_data
    
    p = Gx(i);
    q = Gy(i);
    r = Gz(i);
   
    phi_prev   = phi_hat_kalman(i - 1) + bias_phi_kalman(i-1);
    theta_prev = theta_hat_kalman(i - 1) + bias_theta_kalman(i-1);
    
    % Rotated gyroscope values: - hint: the same as you found in Section 1.
    phi_dot   = ...;
    theta_dot = ...;
          
    % Predict
    state_estimate = A * ...;
    P = A * ...;
    
    % Update (Correct)
    measurement = [phi_hat_acc(i) theta_hat_acc(i)]';
    y_tilde = measurement - C * state_estimate;
    S = R + C * P * C';
    K = ...;
    state_estimate = state_estimate + K * y_tilde;
    P = ...;
    
    phi_hat_kalman(i)    = state_estimate(1);
    bias_phi_kalman(i)   = state_estimate(2);
    theta_hat_kalman(i)  = state_estimate(3);
    bias_theta_kalman(i) = state_estimate(4);
    
end

Conclusion and Further Readings¶

We have seen some filtering algorithms applied on IMU in order to get some orientation data. The most important lesson in this tutorial is to realize that the sensor systems are not completely reliable if you are reading the raw data. As it is mentioned at the beginning, other localization solutions such as using GPS data or camera systems are also requires after-processing as we did on IMU in the tutorial. Today, some of the expensive sensor systems have their own filtering circuits inside the sensor.