Partical filtering For Orientation Determining using
Inertial sensors IMU
Abstract—Using a low cost Inertial Measurement Sensor (IMU)
to estimate an accurate orientation of human body is a challeng-
ing work. Several approaches has been proposed to minimize the
sensor error as the Particle filter which has gain a popularity
with nonlinear and non-gaussian noise system models. In the
case of orientation estimation and positioning the PF has been
employed in the frame of hybridization with other measurement
systems such as visual systems, radio-localisation , GPS etc. In
this study, we propose to use only the data obtained from inertial
sensor, with a constrained Particle filter (PF). The Iterative Mean
Density Truncation algorithm (IMeDeT) has been employed with
a quaternion algorithm based on acceleration and magnetic field.
An improvement in term of accuracy has been clearly detected
as well as the filter convergence, against the Extended Kalman
Filer (EKF)and a optical measurement system.
Index Terms—constraint particle filter, IMeDeT, IMU, MARG,
inertial sensor, quaternion
I. I NTRODUCTION
Orientation estimation using Inertial sensors has been used
widely in various application such as machine interaction
[1], robotics [1][2], aerospace[3], navigation[4][5] and human
motion analyses [6][7]. Using a low cost Inertial Measurement
Unit (IMU) or Magnetic Angular Rate and gravity (MARG)
sensor is a chalenging work. In this context, several ap-
proaches were designed to solve this problem. Namely, various
deterministic approches based on QUaternion ESTimation
algorithm (QUEST) have been for this pirpose; odometry
algorithm has been presented by Iwaneczko et al.[8] which
fuses only gyros and accelerometer data in indoor activities
, in the same context also Duong et al [9] proposed a fuzzy
turned Complemetary filter which uses only accelerometter
and gyroscope data, in this both methods its not necessary
to use MARG sensor . However, an IMU can’t be enough
to satisfy the required data in some other approches. Gaus
Newteon Algorithm (GNA) has been proposed by Liu et al
[10] which fuses all of gyros, accelerometer and magnetome-
ter data, in the context of using MARG sensor, Fourati et
al [11] employed Levenberg Marquadt algorithm (LMA) to
estimate the attitude. Magdwick et al[12] proposed a simple
method which can be applied with IMU or MARG sensor,
this approche is, gradient descent algorithm, efficient in term
of accuracy and ease of implemntation. This approches has
been implemented in several motion-tracking devices [13][14].
Kalman filter and their extentend have been extensively used
to solve the orientation problem. Linear Kalman filter has
been proposed by Zhou et al [15] also by Kannan[16] with
two different measurement model, both of them worked on
biais measurement estimation, also it has been proposed by
valenti et al [17] which uses in the measurement model an
externel algebraic algorithm, Ahmed and Tahir[18] proposed a
kalman filter based on the direction of linear acceleration. The
extended Kalman Filter (EKF) has been proposed by Sebatini
[19] in aim to estimate the orientation basing on measurement
biais, the same of Ghodha and Lachapelle[20] and Valerie
and Cristophe[21]. However, a clear a low performance in
term accuracy and efficiency is detected in presence of non-
gaussian noise. In the other hand, Particle Filter (PF) has
gained popularity in the case of non linear and non gausian
noise [22]. In literature, the PF has been employed only in
the case of fussing the IMU Data with an other system data,
as Choi and Ahn[23] employed data obtained from range
sensor with the IMUs data, GPS has been employed with IMU
by Touil et al[24], both of Ascher et all[25] and Gustafsson
et al[26] worked on map matching with IMU, Li et al[27]
porposed to fuse the IMUs data with radiolocation based
navigation system. In this work we propose an orientation
estimation algorithm based on Particle filtering using only
inertial sensor data.
II. D EFINITION AND NOTATION
In this section we present the necessary tools that we will
use in next sections.
A. Quaternion and rotation
We can define the orientation as a parameters set which
relates the angular position of two different frames (fixed
coordinate system N, and mobile coordinate B). This relation
was described with several methods, each method has a kind of
limits. Orientation is represented by Quaternions. A quaternion
can be defined as
q = q0 + q+ = q0 + iq1 + jq2 + kq3 (1)
With q0 is the real part, represents the amount of rotations
which going to occur about the rotation axes of quaternion
and q+ is the vector part ∈ R3 contains three complex number
which represents the axis where the rotation occur. In term of
m
quaternion the rotation matrix R could be written as:
 2
q0 + q12 − q22 − q32 2(q1 q2 − q3 q0 ) 2(q1 q3 − q2 q0 )

m
R =  2(q1 q2 + q3 q0 ) q02 − q12 + q22 − q32 2(q2 q3 − q1 q0 )
2(q1 q3 − q2 q0 ) 2(q2 q3 + q1 q0 ) q02 − q12 − q22 + q32
(2)
 a
B. Measure model of inertial/Magnetic sensor fact, a unique solution is required. the New rotation matrix R
• Tri-axial accelerometer can be resumed by:
It measures the acceleration of the movement object where it a
is attached. The output of accelerometer in frame (B) is given R(q)Gvr = accv (9)
by:
a m
f = R(accv − Gvr + va (3) The following system is obtained from developing the above
f
equation:
Where accv , and Gvr represent respectively the acceleration 
m  2(q 1 q 3 − q 0 q 2 ) = ax
and the gravity in frame (N). va ∈ R3 is a white noise and R
f 2(q 0 q 1 + q 2 q 3 ) = ay
is the rotation matrix defined in (2. 
(1 − 2q 1 2 − q 2 2 ) = az
• Tri-axial gyroscope
• Magnetic field quaternion
This sensor measures the angular speed of his own system
compared to a fixed coordinate system. The output of gyros To obtain the rotated magnetic field vector hr = [hrx hry hrz ]T
in frame (B) is done by: resumed by equation (10) we used quaternion in term of
a
ω acceleration q instead of using quaternion (6). Respecting the
f = ω v + bg + vω (4) characteristic of magnetic field, we are limited to use the
f
rotation around the x and z axis. Applying the same concept
Where ω v = [ωx ωy ω z ] ∈ R3 is the angular speed, bg is used with the acceleration, to obtain the quaternion in term of
the bias vector and vω is white noise. magnetic field, we obtain equation (11).
f
• Tri-axial magnetometer a a
R(q)mv = hr (10)
This sensor measures the direction and intensity of the mag-
netic field. The output of magnetometer in frame (B) is given
by:  q 
m m

f = Rm + m v v
(5) rLx rL2x + rL2y
M R (q m ) rLy   (11)
 
f
0 
v
Where m is the tri-axial magnetic field vector in frame (N). rLz rLz

C. Orientation in term of quaternion B. Iterative Mean Density Truncation algorithm

The orientation is obtained directly from integrating the Traditionally, Particle Filter proves its reliability in
gyroscope data. nonlinear and non Gaussian state space models. In a such
1
q = qi ⊗ [0 ω v ] (6) application as orientation estimation requires some constraints
2
which PFs cannot handle it. [niss]. To deal with this problem
where ⊗ denotes the quaternion product. several method presents constraints for each particle in the
particle filter which lead to umpteen iteration and it doesn’t
III. P ROPOSED M ETHOD
guaranteed to lead to a stronger condition. In this paper,
A. Quaternion estimation the Iterative Mean Density Truncation (IMeDeT) introduced
Acceleration and magnetic field are widely used in aim by Amor et al[28] has been employed in aim to optimally
to correct the gyros errors. In this Framework, acceleration estimate the orientation with additional constraints expected
and magnetic field are used to compute the quaternion. The value of the state. This approach aim to satisfy the desired
n n n constraints on the mean state by inductively drawing particles.
rotation matrix R(q) and quaternion q in term of acceleration
For the convenience readers, the IMeDeT [28] is resumed by
and magnetic field can be resumed by equation (7) and (8)
the Algorithm 1.
respectively.
n n a a m m
R(q) = R(q)R( q ) (7) In this Study, state vector is resumed by the quaternion, which
n a m is typical technique of state augmentation. The measurements
q=q⊗ q (8)
bias are not estimated.
a m
where q and q are the quaternion in term of acceleration and
magnetic field respectively. x̂k = [q0 q1 q2 q3 ]T (12)
• Acceleration quaternion

Supposing that the acceleration depict the gravity solely. there- We expected the estimated quaternion to be ∈ [−1, 1]. We
m
fore the rotation matrix R(2)can extracted proportionally to the therefore add the constraint E[quat] ∈ [−1, 1]. Improvement
gravity vector. Delivred system has an infinity of solution, in in accuracy expected by adding this constraint.
 ALGORITHM1:Inductive Mean Density truncation (IMeDeT). Static Condition
0

−5
Unconstrained sampling

φ
−10 EKF
for k = 1, 2, ..., tl(tl is the time lengh) do IMeDeT
for n = 1, 2, ..., pN ( pN : the number of particles) do −15
real
(n) −20
Generate samples from an accessible proposal distribution xk ∼ qk (xk ). 0 5 10 15 20 25 30
(n) (n) Time step
Compute the weights wk of xk and normalize them.
IMeDeT 2
for i=1,2,...,n do
n 0
(i) (i)
wk xk 6∈ C r then
P
if

θ
i=1 −2
(n)
Find a new particle xk which satisfy the above condition:
−4
pN
P (i) (i) 0 5 10 15 20 25 30
wk xk ∈ C r . Time step
i=1
20
end if
end for
10
end for

ψ
pN
P (i) (i) 0
x̂k = wk xk .
i=1
−10
end for 0 5 10 15 20 25 30
Time step
Cr denotes the constraint region: Cr = {xk : c1k ≤ φ(x̂k ) ≤ c2k }.

Fig. 1. Euler angles,Φ is the pitch, θ is the roll and ψ is the yaw angle, in
static Condition
IV. S IMULATION ADN R ESULTS
Real data has been collected from a prototype MARG 100
Dynamic Condition

sensor. All data was sampled with sampling frequency equal

0
to 200Hz, the gravity is defined by a constant norm ||g|| =
φ

9.81m/s2 . Performance are benchmarked using, Qualisys sys- −100

0 5 10 15 20 25
tem, visual system. The application of interest in this study is EKF
Time step
100
indoor activities. We assume the same constraint E[quat] ∈ IMeDeT
real
[−1, 1] for all time steps k . We used 1000 particles in the 0
θ

implemented IMeDeT. We show the orientation estimation −100

0 5 10 15 20 25
results on both of static and dynamic measurement Figs. 1 and Time step
2. In each figure, the top shows the roll angle, the middle row 50

shows the pitch angle and the bottom shows the yaw angle over 0
ψ

time obtained from Extended Kalma Filter (EKF), with red,

and Iterative Mean Density Truncation algorithm (IMeDeT),
with green. Figure 3, shows the estimated quaternion over
time, we can clearly observe that the constraint is satisfied,
the blue signal is the scalar of quaternion vector. Figure 4
show an exemplary of result of 3D position using IMeDeT.
The MSE of IMeDeT and EKF have been shown in figure
5. An average of ±3 with the pitch angle, an average of ±2
with to roll angle and around ±5 with the yaw angle, when we
obtain from the EKF an average of 4 , 2.5 and 8 with pitch, roll
and yaw respectively. The comparison results of the average
orientation error among the EKF indicate that IMeDeT has
achieved a better accuracy.
V. C ONCLUSION
This paper present an orientation estimation algorithm based
on acceleration and magnetic field quaternion using a con-
strained Particle Filter named Iterative Mean Density Trun-
cation algorithm (IMeDeT)without using any external sensor
to estimate the orientation neither the position. The proposed
constraint has satisfied the expected estimate. The proposed
work has been evaluated using experiments for different angles
−50
0 5 10 15 20 25
Time step
Fig. 2. Euler angles,Φ is the pitch, θ is the roll and ψ is the yaw angle, in
Dynamic Condition
and its proves its effectiveness with the good experimental
results. Our Future work will focus on outdoor activities and
long distances walking. More motion patterns experiments and
stair climbing.
R EFERENCES
[1] B. Barshan and H. F. Durrant-Whyte, Inertial navigation systems for
mobile robots, vol. 11, pp. 328342, June 1995.
[2] L. Ojeda and J. Borenstein, Flexnav: fuzzy logic expert rule-based
position estimation for mobile robots on rugged terrain, in Proc. IEEE
International Conference on Robotics and Automation ICRA 02, vol. 1,
pp. 317322, May 1115, 2002.
[3] S. K. Hong, Fuzzy logic based closed-loop strapdown attitude system
for unmanned aerial vehicle (uav), Sensors and Actuators A: Physical,
vol. 107, no. 2, pp. 109 118, 2003.
[4] D. H. Titterton and J. L. Weston, Strapdown inertial navigation technol-
ogy. The Institution of Electrical Engineers, 2004
 1.6 1 12
EKF
1.4 IMeDeT 10
0.8
1.2
8
1 0.6
0.8 6

ψ
φ

θ
0.6 0.4
4
0.4
0.2
2
0.2

0 0 0
0 5 10 15 20 25 0 5 10 15 20 25 0 5 10 15 20 25
Time step Time step Time step

Fig. 3. The performance of orientation estimation using MSE, pitch in the top, roll in the middle and yaw in the botom

1
[11] H. Fourati, N. Manamanni, L. Afilal, and Y. Handrich, A Nonlinear
Filtering Approach for the Attitude and Dynamic Body Acceleration
0.8
Estimation Based on Inertial and Magnetic Sensors: Bio-Logging Ap-
0.6 plication, IEEE Sensors Journal, vol. 11, no. 1, pp. 233244, 2011.
[12] S. O. H. Madgwick, A. J. L. Harrison, and R. Vaidyanathan, Estimation
0.4
of IMU and MARG orientation using a gradient descent algorithm, IEEE
0.2 International Conference on Rehabilitation Robotics, 2011.
0
[13] Ya Tian; Hamel, W.R., J. Tan ”Accurate human navigation using
wearable monocular visual and inertial sensors”, IEEE Trans. On In-
−0.2
strumentation and Measurement, 63-1, pp. 203-213, Jan. 2014.
−0.4
[14] Maxim Integrated, ”MAX21100, Low-Power, Ultra-Accurate 6+3
DoF IMU, the Industry’s Smallest and Thinnest 6DoF IMU
−0.6
0 5 10
Time Step
15 20 25 with Stable and Accurate Gyroscope and Reliable Accelerome-
ter”, 2014:http://www.maximintegrated.com/en/products/analog/sensors-
andsensor-interface/MAX21100.html.
Fig. 4. Quaternion obtained from IMeDeT estimation [15] Z. Zhou, Y. Li, J. Liu, and G. Li, Equality constrained robust measure-
ment fusion for adaptive kalman-filter-based heterogeneous multi-sensor
navigation, IEEE Transactions on Aerospace and Electronic Systems,
vol. 49, no. 4, pp. 21462157, 2013.
[16] R. Kannan, Orientation Estimation Based on LKF Using Differential
2
State Equation, IEEE Sensors Journal, vol. 15, no. 11, pp. 61566163,
1 2015.
0 [17] R. G. Valenti, I. Dryanovski, J. Xiao, and S. Member, A Linear Kalman
−1 Filter for MARG Orientation Estimation Using the Algebraic Quaternion
Z(m)

2
Algorithm, IEEE Transactions on Instrumentation and Measurement,
1
vol. 65, no. 2, pp. 467481, 2016.
0
[18] H. Ahmed and M, Tahir ”Improving the Accuracy of Human Body
−1
2 Orientation Estimation With Wearable IMU Sensors”, IEEE TRANS-
5
1
3
4 ACTIONS ON INSTRUMENTATION AND MEASUREMENT, 2016
2
0
0
1 [19] Angelo Maria Sabatini Kalman-Filter-Based Orientation Determination
−2

Y(m)
−1 −1
Using Inertial/Magnetic Sensors: Observability Analysis and Perfor-
X(m)
mance Evaluation Sensors 2011.
[20] S. Godha and G. Lachapelle, Foot mounted inertial system for pedestrian
Fig. 5. Example of a 3D indoor displacement navigation, Meas. Sci. Technol., vol. 19, no. 7, pp. 075202-1075202-9,
2008.
[21] R. Valrie and C. Christophe ”Magnetic, Acceleration Fields and Gy-
roscope Quaternion MAGYQ)-Based Attitude Estimation with Smart-
[5] S. Beauregard, Omnidirectional pedestrian navigation for rst responders, phone Sensors for Indoor Pedestrian Navigation ” Sensors , 14, pp
in Proc. 4th Workshop on Positioning, Navigation and Communication 22864-22890,2014.
WPNC 07, pp. 3336, Mar. 2222, 2007. [22] A. Doucet, N. De Freitas, and N. Gordon” An Intro-
[6] H. J. Luinge and P. H. Veltink, Inclination measurement of human duction To Sequential Monte Carlo Methods”:http :
movement using a 3-d accelerometer with autocalibration, vol. 12, pp. //www.stats.ox.ac.uk/doucet/doucetd ef reitasg ordons mcbookintro.pdf
112121, Mar. 2004. [23] Choi Y. C. and Ahn H. S. ”A fusion of a range sensor and IMU
[7] H. Zhou and H. Hu, Human motion tracking for rehabilitationa survey, using EM-particle filter for measuring distance between quadrotors”,The
Biomedical Signal Processing and Control, vol. 3, no. 1, pp. 1 18, 2008 2014 International Conference on Control, Automation and Information
[8] P. Iwaneczko, K. Jrdrasiak, A. Nawrat, ” Advanced Technologies in Sciences (ICCAIS ),978-1-4799-7204,2014.
Practical Applications for National Security”.Springer, SSDC, volume [24] K. Touil, M. Zribi, J. B. Choquel,and M. Benjelloun,Bayesian bootstrap
106, pp. 337-346,2017. filter for integrated GPS and dead reckoning 3 positioning, in Proc. IEEE
[9] D. D. Duong, J. Sun, T.P. Nguyen, L. Luo, ”Attitude estimation by using ISIE, Vigo, Spain, pp. 15201524,2007.
MEMS IMU with Fuzzy Tuned Complementary Filter”, IEEE ICEICT, [25] C. Ascher, C. Kessler, M. Wankerl and G.F. Trommer, ”Dual IMU Indoor
2016 pp. 372-378. Navigation with Particle Filter based Map-Matching on a Smartphone
[10] F. Liu, J. Li, H. Wang, and C. Liu, An improved quaternion Gauss ”, 2010 International Conference on Indoor Positioning and Indoor
Newton algorithm for attitude determination using magnetometer and Navigation (IPIN),978-1-4244-5864-6, 2010.
accelerometer, Chinese Journal of aeronautics, vol. 27, no. 4, pp. 986 [26] F. Gustafsson, F. Gunnarsson, N. Bergman, U. Forssell, J. Jansson, R.
993, 2014. Karlsson, and P.-J. Nordlund, ”Particle Filters for Positioning, Naviga-
 tion,and Tracking”,IEEE TRANSACTIONS ON SIGNAL PROCESS-
ING, VOL. 50, NO. 2, FEBRUARY 2002.
[27] W. W. L. Li, R. A. Iltis and M. Z. Win,”Integrated IMU and
Radiolocation-based Navigation Using A Rao-Blackwellized Particle
Filter”, IEEE ICASSP ,978-1-4799-0356-6, 2013.
[28] N. Amor, N. Bouaynaya, P. Georgieva, R. Shterenberg and S. Chebbi,
”EEG Dynamic Source Localization using Constrained Particle Filter-
ing”, IEEE Symposium Series on Computational Intelligence (IEEE
SSCI),Athens, Greece, 2016