Adaptive Kalman Filter for Indoor Localization Using Bluetooth

Low Energy and Inertial Measurement Unit

Paul K. Yoon, Student Member, IEEE, Shaghayegh Zihajehzadeh, Student Member, IEEE, Bong-Soo
Kang, and Edward J. Park, Senior Member, IEEE

Abstract This paper proposes a novel indoor localization

method using the Bluetooth Low Energy (BLE) and an inertial
measurement unit (IMU). The multipath and non-line-of-sight
errors from low-power wireless localization systems commonly
result in outliers, affecting the positioning accuracy. We address
this problem by adaptively weighting the estimates from the
IMU and BLE in our proposed cascaded Kalman filter (KF).
The positioning accuracy is further improved with the
Rauch-Tung-Striebel smoother. The performance of the
proposed algorithm is compared against that of the standard KF
experimentally. The results show that the proposed algorithm
can maintain high accuracy for position tracking the sensor in
the presence of the outliers.

I. INTRODUCTION
More indoor localization systems are being deployed as
wearable devices in entertainment, sports, and health. In
health care, for example, wearable inertial measurement units
(IMU), consisting of an accelerometer, a gyroscope, and a
magnetometer, is worn at the waist and fused with a Bluetooth
localization system to track the patients movements [1]-[3].
The key advantage of the IMU compared to existing tracking
systems is that they are highly portable, provide inertial data at
a high update rate, and do not suffer from a signal blockage.
However, estimating the position with the IMU alone leads to
the accumulation of the drift due to the instability of sensor
bias [4]. Some researchers have proposed the zero velocity
update (ZUPT) to correct the velocity error with a ground
contact, but the drift will still grow based on the total walking
distance [5].
The received signal strength indicator (RSSI) from a
low-power wireless technology such as Bluetooth Low
Energy (BLE) can be deployed to estimate a distance of a
receiver relative to a transmitter. With the multiple distance
measurements from the fixed anchor nodes (i.e. transmitters),
a trilateration can be used to estimate the absolute position of
the target node (i.e. receiver) [6]. However, the position
accuracy suffers from the non-line-of-sight (NLOS) and
multipath [7]. Therefore, the IMU and BLE measurements can
be fused to accurately estimate the drift-free position of the
The work is supported by the Natural Sciences and Engineering Research
Council of Canada (NSERC).
P. K. Yoon, S. Zihajehzadeh, and E. J. Park are with the School of
Mechatronic Systems Engineering, Simon Fraser University, 250-13450
102nd Avenue, Surrey, BC, Canada, V3T 0A3 (email: pky@sfu.ca,
szihajeh@sfu.ca, ed_park@sfu.ca).
B.-S. Kang is with the Department of Mechanical Engineering, Hannam
University, 70 Hannam-ro, Daejeon, 306-791, South Korea (email:
bskang@hnu.kr).

978-1-4244-9270-1/15/$31.00 2015 IEEE

target node at a high update rate.

Kalman filtering is a widely used algorithm for estimating
the state of the system from multiple sensor measurements
with a white Gaussian noise [8]. However, based on the
frequent outliers from the multipath and the NLOS, the
measurement noise of wireless tracking systems cannot be
modeled as the Gaussian distribution. Researchers have
addressed this issue by introducing the adaptive Kalman filter
(KF), such as the multiple-model adaptive estimation
(MMAE) and the innovation-based adaptive estimation (IAE),
to accommodate for the changes in these dynamic conditions
[9]. The MMAE estimates the states based on multiple KFs
with different process and measurement noise matrices, but
the high computational cost of running the KFs in parallel
makes it unsuitable for low-cost microcontrollers in wearable
devices. The IAE adapts new process and measurement noise
covariances based on the innovation sequence, but the
estimates can diverge [10].
This paper proposes a novel three-step cascaded Kalman
filter (CKF) to accurately estimate the sensor position in the
presence of the outliers. The position is estimated by fusing
the external acceleration from the IMU and the trilateration
estimation from the BLE. Using the estimated roll, pitch, and
yaw, the acceleration measurements are rotated from the
moving sensor to the fixed navigation frames. The weight of
the trilateration estimation is adaptively set based on the
residue between the distance measurement and the
trilateration estimation. The position accuracy is further
improved with the Rauch-Tung-Striebel (RTS) smoother. The
experimental results show that the residue has a strong
correlation with the trilateration estimation error, and the
proposed algorithm can estimate the position more robustly
compared to the standard KF in the face of outliers.
II. THEORETICAL METHOD
A. Tilt and Yaw Kalman Filters
This paper employs our previously proposed tilt and yaw
KFs to estimate the (yaw), (pitch), and (roll), which
represent the rotation angles about the Z-, Y-, and X- axes of
the fixed navigation frame N, respectively. The algorithms are
described in [11] and [12].
In the tilt KF, the state is set as the unit gravity vector of the
sensor frame (i.e. the third row of the rotation matrix NS R of
the sensor frame S with respect to N). The states are calculated
using triaxial accelerometer and triaxial gyroscope
measurements. and are calculated with the states [11].

825

Accelerometer

The residue ri (= [ri, x ri, y ]T ) along X- and Y- axes for each

Tilt KF

distance d i is determined by (6) and (7) in a sequence:

Gyr oscope

Yaw KF
Magnetome ter

ri [ xi x

ri ri

Position/
Velocity KF and
Smoother

IMU
BLE

Figure 1. Overview of the proposed algorithm, including tilt, yaw, and

position/velocity KFs and smoother

The states of the yaw KF are set as the first row of NS R ,

which are calculated using triaxial gyroscope and triaxial
magnetometer measurements along with the estimated tilt
angles from the tilt KF. is estimated from the tilt angles and
the states [12].
The parameters of the tilt and yaw KFs are the
accelerometer noise variance A2 , the gyroscope noise
variance G2 , the magnetometer noise varaince M2 , and the
external acceleration model-related constant c A .
B. Trilateration
By assuming that the receivers and transmitters have
omnidirectional antennas and the transmitter has a constant
transmit power, the receive power Pr (i.e. RSSI) on the
receiver can be determined by [7]:

z t Ct x t v t

yt

T
y t

tI 22

I 22

xt

1 2
t I 22
B t 1 2
tI
22

0 21
0 21

u t N a t NS R t S a t N g

Ct 1 I 22

I
A t 1 22
0 22

solved as the least-squares problem, i.e. x (AT A) 1 AT b :

( xi x) 2 ( y i y ) 2 d i

ri , y

i 1

x t A t 1x t 1 B t 1u t 1 w t 1

x t xt

x12 x n2 y12 y n2 d n2 d12

2
2
2
2
2
2
x x y y d d
n 1
n
n
n 1
n1 n
The residue between the estimated distance d i and the
distance to the location estimate x is calculated as following:

1n
ri , x
n i 1

where x t is the state vector consisting of position and velocity

of the target node; A t 1 and B t 1 are the state transition and
input matrices; u t 1 is the input vector; w t 1 is the process
noise vector; z t is the measurement vector; C t is the
observation matrix; and v t is the measurement noise vector.
These variables are defined as follows:

distances d i and the known positions ( xi , yi ) of the anchor

nodes, the position x is expressed in the form of Ax b and

ri

C. Position and Velocity Kalman Filter

In the position/velocity KF, the position of the target node
is estimated with the external acceleration and the trilateration
estimation. The KF is governed by following equations:

The trilateration is deployed to estimate the position of the

target node x (= [ x y]T ) [6]. Based on the estimated

2( y1 y n )
2( x1 x n )

2( x n 1 x n ) 2( y n 1 y n )

ri
ri

averaged over the multiple ri as follows:

where B is the power offset constant; a is the environmental

variable; d is the distance between a transmitter and a receiver;
and the random noise. For the fixed environment, a is set as
a constant. d is estimated from Pr , a, and B by:
d 10Pr B / a

The overall residue r (= [rx ry ]T ) along X- and Y- axes is

xt

Pr B a log10 d

| yi y |]T

0 22

1 2

t I 23
w t 1 2
nA
tI

23

z t x BLE ,t

where xt and yt are the positions in the X- and Y- axes; x t

and y t are the velocities in the X- and Y- axes; n A is the
accelerometer measurement noise vector; x BLE,t is the
estimated positon from the trilateration; t is the sampling
period of the IMU; N a t is the external acceleration in
N-frame; S a t is the triaxial accelerometer measurements; and
N

g (= [0 0 g]T ) is the gravity vector in the N- frame where

g is 9.8m/s 2 . The process and measurement noise covariances

matrices Q t and R t are defined as following:

826

1
Q t t 2 I 22
2

1
tI 22 acc t 2 I 22

r 2
R t t,x
0

tI 22

rt2,y

Target
Node

Reference
Cameras

where acc is the accelerometer noise covariance. By

assuming that the accelerometer noise variances on the all
three axes are equal to A2 , acc is set as A2 I . rt , x and rt , y
are the trilateration residues along the X- and Y- axes,
respectively. With the variables defined as above, the
procedure for estimating the states are found in [8].
D. RTS Smoother
For the post-processing, the RTS smoother is applied to
improve the position accuracy of the forward position
estimate from the position/velocity KF as following [8]:

Anchor
Nodes
(a)
(b)
Figure 2. (a) Experimental lab setup with the target node, the anchor nodes,
and reference cameras and (b) Target node with the CC2540 BLE SoC with
the omnidirectional antenna, the MTx sensor, and an optical camera marker

measurements with the stationary IMU. G2 , A2 , and M2

I t1 (Pt1 ) 1

K t Pt ATt I t1

were calculated as 104 m 2 /s4 , 4 105 rad 2 /s2 , and

2 103 mT 2 , respectively. c A was set to 0.1 which provides a
good result for estimating the tilt angles under various
dynamic conditions [10].

Pt Pt K t (Pt1 Pt 1 )K Tt

IV. EXPERIMENTAL RESULTS AND DISCUSSION

x t x t K t (x t 1 x t1 )

where Pt and Pt1 are the posteriori and priori covariance

estimates; K t is the smoother gain; and x t , x t1 , and x t are
the posteriori, priori and smoothed state estimates.
The overall structure of the proposed algorithm is shown in
Fig. 1.
III. EXPERIMENTAL SETUP AND PROTOCOL
The performance of the proposed algorithm was tested in an
indoor space. As shown in Fig. 2(a), three anchor nodes were
placed close to the outside line of the test area. Both target and
anchor nodes are equipped with the CC2240 BLE
system-on-chip (SoC) (from Texas Instruments) connected to
the omnidirectional ANT-DB1-RAF antenna (from Linx
Technologies). The transmit power of the anchor nodes is set
at 23dBm. The target node received the RSSI from each
transmitter at different times with a sampling rate of about
80Hz. The RSSIs from each transmitter are averaged at 10Hz
when using the trilateration. The target node is additionally
equipped with the MTx IMU (from Xsens Technologies) at
the sampling rate of 100Hz. The Qualisys optical tracking
system was used as a reference system at the sampling rate of
100Hz. Fig. 2(b) shows the target node with the BLE SoC with
the omnidirectional antenna, the IMU, and an optical marker.
Proir to the experiments, the target node moved away from
each anchor node from 6 cm to 90 cm, and the RSSI
measurements were best fitted to calculate the parameters a
and B in (2) for each anchor node.
In each experimental trial, the target node was continuously
moved in the rectangular trajectories of 80cm by 70cm in
about 60s. The test was repeated for 10 times. The parameters
of the proposed algorithm were estimated by the inertial

Fig. 3 compares the trilateration estimation against the

proposed algorithm on the Y- axis. Fig. 3(a) shows that the
proposed algorithm is robust against the outliers from the
trilateration. Fig. 3(b) shows that the residue maintains a
strong correlation with the absolute error of trilateration
estimation throughout the experiment. The maximum
trilateration error is 4.49m at 108.65s, where the residue is
3.60m. Fig. 4(a) shows the residues against the trilateration
errors for all 10 tests. The correlations of 0.896 and 0.949 on
the X- and Y- axes suggest that the residue can provide the
estimate to the reliability of the trilateration estimation well.
Next, the performance of the proposed algorithm is
compared against that of a standard KF on the Y- axis. The
standard KF assumes a constant trilateration measurement
noise. The noise variances (the diagonal elements along R t )
along the X- and Y- axes are tuned as 9 104 m 2 . As shown in
Fig. 5(a), the standard KF produces a large average error of
0.841m from t = 79 to 82s. Fig. 6(a) shows this drift that
ranges from 0.95m to 1.35m on the Y- axis at the x position of
about 0.5m. Our proposed algorithm, on the other hand,
rejects the outliers and accurately tracks the position with an

Figure 3. (a) Position estimate from the BLE trilateration, the proposed
algorithm, and the reference; and (b) residue on the Y- axis

827

(a)
(b)
Figure 6. Horizontal trajectory of (a) forward and (b) smoothed estimates
from t = 78 to 107.3s
(a)
(b)
Figure 4. (a) Absolute errors of trilateration estimations against the residues
on the X- and Y- axes and (b) Kalman gain of the position/velocity KF on the
standard KF and the proposed algorithm

average error of 0.205m during the time interval. As shown in

Fig. 3(a), most of the trilateration estimation from t = 78 to 80s
deviate from the reference trajectory, resulting in an average
error of 1.073m. As shown in Fig. 4(b), the proposed
algorithm has a lower average Kalman gain of 0.0014
compared to 0.0449 of the standard KF. The smaller Kalman
gain of the proposed algorithm indicates that a greater weight
is put to the IMU measurements compared to the trilateration.
The Kalman gain is reduced due to the large residue that exists
during this time period as shown in Fig 3(b). With the RTS
smoother, Fig. 5(b) and Fig. 6(b) show that the positioning
performance is further improved. However, the small drift on
the standard KF is still present from t = 78 to 80s.
Table I compares the root-mean-square error (RMSE) in
position tracking between the standard KF and proposed
algorithm for all tests. For the real-time estimates, the position
accuracy of the proposed algorithm is improved by 40.0% and
20.0% compared to the BLE trilateration and the standard KF,
respectively. With the smoother, its accuracy is further
improved by 6.45% and 50.6% compared to the standard KF
with the smoother and the proposed algorithm without the
smoother, respectively. Our future work will be to extend and
validate the proposed indoor localization system for the
purpose of a wider range, in-home patient monitoring system.

Figure 5. (a) Forward and (b) smoothed position estimates from the standard
KF (KF), the proposed algorithm, and the reference on the Y- axis
TABLE I.

Filtering
Smoothing

RMSE for the Position Tracking (cm)

BLE
61.3
-

KF
45.9
19.4

Proposed
36.7
18.1

V. CONCLUSION
In this paper, a novel three-step cascaded Kalman filter for
accurate estimation of the position trajectories with the IMU
and BLE trilateration measurement is proposed. Based on the
strong correlation between the trilateration residue and
trilateration error, the proposed algorithm uses the residue to
adaptively weight the trilateration estimate and the external
acceleration, thus the algorithm not requiring manual tuning
of the filter parameters. The experimental results have shown
that the proposed algorithm can accurately track the moving
sensor in the presence of the outliers, and the accuracy is
further improved by post-processing using the RTS smoother.
REFERENCES
[1]

D. Kelly, S. McLoone, and R. Farrell, Minimal hardware Bluetooth

tracking for long-term at-home elder supervision, in Proc. IEEE
EMBC. Buenos Aires, Argentina, Sept. 2010, pp. 2136-2140.
[2] H. T. Cheng and W. Zhuang, Bluetooth-enabled in-home patient
monitoring system: early detection of Alzheimers disease, IEEE Wirel.
Commun., vol. 17, no. 1, pp. 74-79, Feb. 2010.
[3] A. Colombo, D. Fontanelli, D. Macii, and L. Palopoli, Flexible indoor
localization and tracking based on a wearable platform and sensor data
fusion, IEEE Trans. Instrum. Meas., vol. 63, no. 4, pp. 864-876, Apr.
2014.
[4] D. Roetenberg, P. J. Slycke, and P. H. Veltink, Ambulatory position
and orientation tracking fusing magnetic and inertial sensing, IEEE
Trans. Biomed. Eng., vol. 54, no. 5, pp. 883-890, May 2007.
[5] H. Fourati, Heterogeneous data fusion algorithm for pedestrian
navigation via foot-mounted inertial measurement unit and
complementary filter, IEEE Trans. Instrum. Meas., vol. 64, no. 1, pp.
221-229, Jan. 2015.
[6] K. Langendoen and N. Reijers, Distributed localization in wireless
sensor networks: a quantitative comparison, Elsevier Computer
Networks, vol. 43, pp. 499-518, 2003.
[7] G. Blumrosen, B. Hod, T. Anker, D. Dolev, and B. Rubinsky,
Enhancing RSSI-based tracking accuracy in wireless sensor
networks, ACM Trans. Sensor Networks, vol. 9, no. 3, pp. 29:1-29:28,
May 2013.
[8] D. Simon, Optimal State Estimation: Kalman, H Infinity, and
Nonlinear Approaches, Hoboken, New Jersey: Wiley, 2006.
[9] A. H. Mohamed and K. P. Schwarz, Adaptive Kalman filtering for
INS/GPS, Jour. of Geod., vol. 73, pp. 193-203, 1999.
[10] C. Hide, T. Moore, and M. Smith, Adaptive Kalman filtering for
low-cost INS/GPS, Jour. of Navig., vol. 56, pp. 143-152, 2003.
[11] J. K. Lee, E. J. Park, and S. R. Robinovitch, Estimation of attitude and
external acceleration using inertial sensor measurement during various
dynamic conditions, IEEE Trans. Instrum. Meas., vol. 61, no. 8, pp.
2262-2273, Aug. 2012.
[12] S. Zihajehzadeh, D. Loh, M. Lee, R. Hoskinson, and E. J. Park, A
cascaded two-step Kalman filter for estimation of human body segment
orientation using MEMS-IMU, in Proc. IEEE EMBC. Chicago, USA,
Aug. 2014, pp. 6270-6273.

828