Orientation Estimation

Using Extended Kalman Filter And DCM Method

Tung Dang-Anh
INTRODUCTION
The backbone of this project is the
processing for signal from a Inertial
Measurement Unit (IMU). Specifically, the
IMU measures the objects orientation of
three Euler angular coordinates: roll ( ),
pitch ( ) and yaw ( ) and is popularly
utilized in industry and balance control field
due to its low cost, compactness and low
power
consumption.
With
three
componential MEMS sensors combined:
accelerometer - gyroscope - magnetometer,
to determine the accurate angles, we must
estimate them from those three sensors by a
fusion algorithm.

IMU SYSTEM
The IMU system used in this report includes
ARM-Cortex M4 STM32F405 MCU with
FPU instructions and ADIS16405 inertial
sensor with high precision tri-axis
gyroscope, accelerometer, magnetometer.

Figure 5. Simulink & KeilC Code Generation Sequence

Figure 3a . IMU assembled in Automatic Control Lab,

Department of Automation and Control, FEEE, HCMUT
Timer 10ms

Accelerometer

GPIO
Gyroscope
SPI

Magnetometer

ARMCortex
M4/FPU
UART

ADIS16405

RESULTS & DISCUSSION

During experiments, the proposed method
proves its advantage and shows reliable
result with RMS Error less than 3.
However, this method has not been adapted
for stability against magnetic disturbance.

, ,
, ,

STM32F40x

Temperature

Figure 3b. IMU Hardware Block Diagram

Figure 1. IMU & Applications

This report presents the Extended Kalman

Filter (EKF) and DCM (Direct Cosine
Matrix) method for orientation estimation.
EKF is applied for nonlinear systems and
DCM method is then required for dealing
with the 3 3 rotation matrix as deduced
below.
.
.

.
.

.
.

.
.

.
.

.
.

3D ROTATABLE TABLE
To verify the accuracy of the IMU, this
report uses a 3D rotatable table with three
encoders on 3-DOF axes. Estimated angular
values from IMU will be compared with
encoder values to compute RMS error:

where:

Figure 6a. Results on the 3 axes when rotating the IMU

METHOD
The main advantage of DCM method is
removal for the effect of magnetic
interference on the roll and pitch angles
without concerning about the parameter of
the earths magnetic vector in comparison
with quaternion-based method. Moreover,
DCM method can avoid the singularity
problem in Euler based method. The method
includes a two-step algorithm:
1st: E-KF1 estimates
and from the
accelerometer and gyroscope (variables at
the 3rd row of DCM).
2nd: E-KF2 estimates from roll pitch
angles and magnetometer (determination
for the remaining variable from 1st and 2nd
row of DCM).
Besides, we added three state variables to
estimate external acceleration. This helps the
IMU achieve better estimation during high
external acceleration.
Accelerometer
Gyroscope

Magnetometer

E-KF 1
=

= .

,
,

E-KF 2
=

Figure 2. EKF & DCM Method Block Diagram

IMU
Y
X

Mch c
encoder
Z

Figure 4a. Rotation Table constructed in Automatic

Control Lab, Department of Automation and Control.
IMU

UART

MAX II
EPM21xxx

FSMC

Encoder x
Encoder y

UART

PC

STM32F4
Discovery Board

Encoder z
IMU
ADIS16480

Figure 6b. High error of yaw angle in magnetic

disturbance

IMU can be integrated with GPS to create an

Inertial Navigation System (INS) that is used
in positioning and navigation. Also, IMU
also finds wide application in fields of
Humanoid Robot, Quad-rotor Robot,
Human-Robot
Interactions,
Motion
Tracking

SPI

Figure 4b. 3D Rotatable Table Hardware Block Diagram

MATLAB SIMULINK - KEILC

In this project, the MATLAB Embedded
Coder Toolbox is used to generate C code
from Simulink model. The source code,
compiled by KeilC and then downloaded to
MCU, is the combination between
MATLAB C code and MCU STM32F405
drivers. Total time for each estimation loop
over the MCU is 8.5ms. The update
frequency for estimation angles is 100Hz.

ACKNOWLEDGEMENTS
This project is completed with special thanks
to the thesis adviser, PhD. Hao NguyenVinh, for his thorough and supportive
guidance and mentoring.

REFERENCES
[1] Greg Welch, and Gary Bishop (2006). An
Introduction to the Kalman Filter
[2] Le Manh Thang (2010). A Master Thesis on
IMU, HCMUT.
[3] Nguyen Ho Quoc Phuong. Study On
Orientation Estimation With Three Different
Representations.

The 2013 PIF TechXchange. January 17th, 2013, Ho Chi Minh city.