^{1}

^{*}

^{2}

This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).

We propose a novel hybrid inertial sensors-based indoor pedestrian dead reckoning system, aided by computer vision-derived position measurements. In contrast to prior vision-based or vision-aided solutions, where environmental markers were used—either deployed in known positions or extracted directly from it—we use a shoe-fixed marker, which serves as positional reference to an opposite shoe-mounted camera during foot swing, making our system self-contained. Position measurements can be therefore more reliably fed to a complementary unscented Kalman filter, enhancing the accuracy of the estimated travelled path for 78%, compared to using solely zero velocities as pseudo-measurements.

Indoor pedestrian positioning, a prominent example of where Global

Various indoor pedestrian positioning methods have been proposed through the years. Most of the approaches rely either on a modified environment (e.g., radio beacons or fixed visual markers), or some a priori knowledge about it (e.g., radio fingerprinting). In emergency, life-critical scenarios, where time and reliability are of utmost importance, such solutions could be easily stretched beyond their limits. Beauregard [

With the help of modern self-contained inertial sensors the aforementioned shortcomings could be overcome to a certain degree at least. However, the hard to ignore issue of low cost Inertial Measurement Unit (IMU)-based personal navigation systems lays in the inaccuracy of their microelectromechanical system (MEMS)-type sensors. Even with theoretically perfect initial alignment, accurate position tracking can only be successfully performed for a few seconds using commercial grade inertial sensing alone [

To limit the error growth characteristics of low-cost IMU-based pedestrian Inertial Navigation System (INS), also known as Pedestrian Dead Reckoning (PDR), an inherent property of the human gait has been widely exploited—the fact that cyclically one foot at a time stays still on the ground for a short period of time (stance phase) while the opposite one is moving (swing phase) [

Considerable research has been carried out recently in the hybrid indoor positioning field. Having in mind that most of the presented hybrid approaches are not self-contained or rely on environmental features [

Our approach is similar to the one proposed by Do and Suh with their Gait Analysis System [

We therefore propose a novel, self-contained machine vision-aided hybrid PDR aiming to improve foot trajectory estimation in an IMU-based PDR system with ZUPTs. The idea is to minimize foot trajectory error during the swing phase, particularly during slow or disturbed walking, when the swing phase, and thus the error integration time, may last longer. We achieve our goal by taking advantage of a novel visual marker-based setup, where traditional environmental markers are being replaced by a user-worn marker, fixed on the user's shoe, while an IMUCC unit is placed on the opposite one. From the time it enters in the camera's field of view, the marker's pose with respect to the camera can be determined by means of an augmented reality (AR) machine vision algorithm and from then on it can serve as a positional reference to the IMU, since there is a fixed, known spatial relationship between the camera coordinate frame and the IMU coordinate frame. Position measurements can be therefore fed to a complementary unscented Kalman filter (UKF), operating in a unit quaternion space in feedback configuration.

In this section a description of the proposed algorithm will be given. Its schematic representation is shown in

We make use of the following Cartesian, orthogonal coordinate reference systems (frames) throughout the article:

All frames are right-handed, the third axes are thus defined with the first two. In this article superscript is used to denote the coordinate system in which a variable is represented. Bold text is used for vector and matrix variables.

The main idea behind our approach was motivated by the fact that SDINS positional uncertainty grows cubically over time. Developing a method to somehow anchor subsequent foot position measurements during swing phase, when SDINS calculations keep losing accuracy, to a prior point in time, when accuracy was greater, was our goal. The use of a shoe-worn marker would allow us to perform positional measurements with accuracy that is substantially decoupled from time, similarly to using an outdoor operating GNSS receiver. The cubical position error growth of the inertial-only SDINS solution during the swing phase should therefore become limited to the sum of the positional error, which occurred up to the time of the reference marker image acquisition and the inherent ARToolKitPlus [

The coordinate systems transformations involved, leading to the actual positional measurement being fed to the UKF, can be thought of as a two-step process (

When the marker is first recognized in an image during the swing phase, its 3D pose is calculated with the aid of the IMU frame calculated pose in the nav-frame, the known IMU-Camera lever arm and the current ARToolKitPlus measurement of the pose of the marker in the cam-frame. We call this resulting frame the reference marker coordinate frame (mref-frame).

From now on the system's computational workflow reverses with respect to the first step. The inverse of the ARToolkitPlus homogeneous matrix is used to perform the transform from the previously calculated mref-frame, expressed in the nav-frame, to the cam-frame and again the known IMU-Camera lever arm transformations are used at the end for calculating the actual homogeneous matrix, describing the pose of the IMU frame in the navigation frame.

The visual marker-based relative 3D positional measurement architecture that we are using relies on the well-known ARToolKitPlus AR framework, which is based on the pose estimation algorithm developed by Schweighofer and Pinz [

A common characteristic of optical tracking AR systems with fiducial markers are their viewing angle W-shaped rotation error functions [

Our camera is rigidly connected to the IMU. Using the calibration algorithm developed by Hol

Having three separate measurement data streams of mutually dependent quantities, one for visual marker pose measurements, one for inertial measurements and one for magnetic compass heading measurements, gives rise to the data synchronization issue. The most accurate and straightforward solution in our case would be to use a hardware-based time synchronization mechanism for the three streams, similarly to [

Complementary filtering involves system error estimation through system error modeling. Therefore, the states used in our complementary filter architecture are SDINS velocity and position errors in the n-frame and quaternion attitude error between the p-frame and n-frame. All states used in a Kalman filter are considered to be white-noise driven signals. Any time-correlated driving noise should be properly shaped. In most cases, a first-order Gauss-Markov process model is accurate enough for modeling such errors. The state vector is then augmented with additional states to accommodate these additional colored noise states. To use this approach is particularly important in long lasting missions using sensors with non-negligible sensor drift, where the filter is augmented with accelerometer and gyro bias states. However, because the noise power spectral density (PSD) curve of the IMU outputs showed negligible drifts for the duration of our proof-of-concept experiments, we considered accelerometer and gyro biases as zero mean white noises in our article for simplicity and clarity reasons.

Therefore, considering velocity, position and attitude as the main quantities of interest in a navigation solution and sensor biases as time uncorrelated states, we can define the following error state vector for our complementary UKF:

As it can be observed above, the filter error state vector is composed of a translational part (Δ^{n}^{nav}

The unscented Kalman filter constitutes an alternative to the extended Kalman filter, which is a suboptimal implementation of the recursive Bayesian estimation framework applied to Gaussian random variables [

A complementary filter operates on the navigation errors in the error state space, recursively estimating them, making it possible to correct navigation states in the total state space SDINS. We have not employed any small angle assumption in the development of the algorithm. A complementary UKF in a unit quaternion space was developed for the rotational (unit quaternion) part of the state vector propagation, while a vector space complementary UKF was used for the Euclidean vector space part of the state vector. Hereafter in this chapter we will describe the fundamental algorithm of the latter.

Considering the following discrete-time process governed by the nonlinear stochastic difference equation:
_{k}_{k} ∈ ℜ^{n × 1} being the state vector, _{k} ∈ ℜ^{m×1} the measurement vector, _{k}_{k} the process and measurement zero mean Gaussian noises with covariances given by _{k−1} and _{k}, respectively, an UKF tries to estimate the state vector _{k} by the following procedure: given an n × n covariance matrix _{k}_{i,k} can be generated and column-wise concatenated to form the matrix _{k−1} of size n × (2n + 1):
_{k-1} is the distribution mean at time step k − 1 and γ is a composite scaling parameter. From _{k} is added to_{k}_{i,k−1} are then propagated through the nonlinear function _{i,k}_{k}_{i,k}_{i}_{i,k}_{i}_{,}_{k}_{k}

In contrast to vector quantities, rotations lie on a nonlinear manifold and quaternions, used in our system to represent them, are constrained to a unit radius hypersphere in a four-dimensional Euclidean space (a 3-sphere). This is the reason why quaternions are not closed for addition and scalar multiplication (operations that constitute the core of the weighted sum calculations in an UKF) and consequently why using unscented filtering directly with a unit quaternion attitude parametrization generally yields a non-unit quaternion estimate [

The original vector space UKF algorithm has thus to be modified accordingly to ensure that during the weighted sum of the unscented transform, the quaternion does not depart from the unit sphere. This was achieved with the help of the rotation vector attitude representation for sigma point rotation vector generation, followed by a quaternion-based weighted mean computation based on the quaternion distance metric formulated in [

We chose to treat the quaternion noise
_{k}_{−1} in the vector space UKF, where the superscript + denotes the post-update process noise estimate when gyro bias error is being considered) as a rotation vector, because this way the transformed sigma points are more narrowly scattered around the current estimate compared to the alternative noise representation as a vector part of the quaternion [_{k}_{−1}) before the process model. To avoid using addition and multiplication in the quaternion unit sphere domain, we use quaternion multiplication in sigma point generation instead, multiplying the quaternion error by the current quaternion estimate
^{q}_{i,k}_{-1} is the resulting transformed

Dealing with a combined translation-rotational UKF, we would like to emphasize that (2^{q}^{q}_{k}

The predicted mean quaternion part

The associated predicted rotational covariance
_{i} between the single sigma quaternion and the predicted mean quaternion:

Each quaternion distance _{i}_{,}_{k}_{i,k}_{i}_{,}_{k}_{i,k}_{i}_{,}_{k}

Propagating the quaternion sigma points through the measurement model:

Since we are using three different measurement modalities, namely ZUPT, visual position measurement (ARTK) and heading measurement (

The measurement switching module sets the ARTK measurement mode each time the marker is recognized in the image and a position measurement is therefore available, while an effective and straightforward gyro signals thresholding technique similar to Foxlin's approach in [

Dealing with complementary filter architecture, we have to stress the fact that the measurements _{k} involved in the update stage of the filter are actually difference quantities _{k}^{v}_{Z}_{k}^{ZUPT}_{k}^{ZUPT}_{k}^{ZUPT}_{k}

In heading measurement mode we are dealing with rotation measurements represented by unit quaternions, which are not mathematically closed for subtraction. We thus opted for the following quaternion multiplication approach to get the heading error measurement to be fed to the UKF in a complementary configuration. Heading was first extracted from the SDINS attitude quaternion
^{q}_{k},which represents the measured difference rotation quaternion to be fed to the UKF:
^{Heading}_{k}^{Heading}_{k}^{q}_{k}

It has to be noted that heading measurements are presented as attitude measurements to the UKF in the heading measurement mode, therefore the generally nonlinear measurement transfer model ^{Heading}_{i,k}^{q}_{i,k}^{q}_{i,k}

In the ARTK measurement mode the difference ^{r}_{k} between the IMU's ARTK measured position and the SDINS estimated position of the IMU sensor is being fed to the UKF:
^{ARTK}_{k}^{ARTK}_{k}

Regarding the ARTK measurement mode, a few words need to be devoted to its triggering. First of all, only pose measurements with high confidence are taken into account in the UKF, since frames (measurements) with lower ARToolkitPlus confidence tended to produce poor 3D cube overlays (

The UKF gain _{k}_{i}_{,}_{k}_{i}_{,}_{k}

The cross correlation matrix

We can define the error state vector
_{k}

Since we are dealing with a complementary filter architecture, SDINS states have to be updated by the error state vector

We accomplish the update for the predicted vector part

The following hardware is used in our proof-of-concept PDR system:

low-cost Analog Devices ADIS 16354AMLZ IMU

3-axis tilt-compensated magnetic compass Ocean Server OS-5000

grayscale video camera The Imaging Source DMK 41 AF02 with a Computar 3.5–8 mm 1:1.4 1/3″ CS lens

All data preprocessing and computations are performed offline with main algorithms running in MATLAB Simulink environment. Experiments were conducted to evaluate the methods we proposed. The intrinsic parameters of the camera-lens combination and the turn-on biases of the inertial sensors were determined as described in Sections 3.1 and 3.2, respectively. MATLAB Simulink was used to perform offline computations.

Accelerometer and gyro triads error covariance matrices (cov(∇) and cov(ε), respectively) were determined by logging several minutes of IMU data, while leaving the IMUCC unit at rest:

Since visual marker pose estimation accuracy is proportional to the marker physical size, we opted for a relatively big (79.3 mm wide), but still feasible marker, for our proof-of-concept PDR system.

The following parameters were used in our experimental setup:
_{ZUPT}_{Heading}_{ARTK}

Not having a 3D position measurement device at hand, we first decided to check the behaviour of our system by executing a round trip sequence (

Having checked that the system improves the SDINS-only solution when being used for prolonged periods, we wanted to try it in a real slow walking scenario, to assess the amount of positional error correction during slow paced pedestrian navigation. This second experiment was performed walking slowly down the corridor of our lab, travelling with the IMUCC-equipped foot for 14.09 m in 44 s.

After having preprocessed all the data, ZUPT triggering parameters were set by trial and error:
_{x}_{v}_{z}_{ZUPT}

We repeated the calculations on the earlier acquired experimental data with ARTK measurement mode disabled, without altering all the remaining parameters, to be able to compare our proposed solution to a state-of-the-art PDR. A comparison of both positional graphs showed a reduction in ZUPT position corrections in the ARTK-enabled graph (

The upper graph in

The final calculated point in 3D space for the ARTK-disabled SDINS system, therefore aided by ZUPT and compass measurements only, is 14.36 m distant from the starting point, resulting in a 27 cm (1.92%) travelled distance error. Comparing it to the 6 cm error with ARTK mode enabled, an 78% increase in accuracy can be observed in our slow walking experiment with our proposed hybrid PDR system.

A significant improvement (25%) in velocity estimation has been observed during the experiments through beginning-of-stance-phase data analysis (

We would like to stress the fact that navigation accuracy improvement is expected to be inversely proportional to the pace of the user and that we have chosen to test our system in a slow pace walk because it would represent a plausible, real-use scenario in which a substantial improvement in navigation accuracy would arise. Conversely, in a normal or fast pace walking, less or even no improvement is expected, due to the shorter SDINS integration time during swing phase. Moreover, using the same visual setup, image blurring would also occur due to the higher camera travel speed, making marker pose estimation less accurate. The mentioned effects of walking speed on accuracy improvement could therefore constitute the subject of further experimental investigation.

Besides walking speed, there is another limitation of our system—the fact that our visual aiding algorithm is based on the assumption of a stationary marker during stance phase of the marker-equipped foot. In fact, any movement of the marker would compromise the visual pose measurement of the marker and consequently the calculated pose of the camera. By examining the video acquired during our experimental walking, we could confirm that we succeeded in achieving no visible marker motion during the experiments in our controlled laboratory environment. However, if intended to be brought to practical use, the system should incorporate some kind of marker movement detection or limitation at least.

A proof-of-concept hybrid, inertial sensors-based indoor PDR system, aided by a novel position measurement technique, relying on a shoe-attached marker, has been proposed in this paper. Using a visual marker in this novel way was expected to enhance the navigation solution of an inertial-only PDR, while retaining independence from environmental markers or features, making the proposed approach potentially interesting, especially for first responders' tasks. However, additional sensors could be added to our system in cases when other sensory modalities would be available.

Since a comparison to other indoor pedestrian navigation systems, relying on external aids (e.g., environmental markers, radio fingerprinting,

Being proof-of-concept, our proposed hybrid PDR cannot be directly used in practice. However, in this paper we have shown that if brought to real-time operation and to a practically acceptable size and weight, the use of a hybrid PDR, aided by visual positional measurements, relying on a shoe-worn marker, can represent a viable solution for improving indoor PDR navigation for first responders in the first place.

This research has been co-financed by the European Union—European Social Fund and Slovenian research agency—ARRS, under grants P2-0095, P2-0098 and J2-4284.

The authors declare no conflict of interest.

The SDINS is the main navigation algorithm in our system, based on IMU outputs. The SDINS algorithm takes IMU-measured accelerations (specific forces) ^{b}_{x}_{y}_{z}^{p}^{p}^{p}

In

Before using the IMU ^{b}^{init}^{b}^{b}^{b}^{init}

Generally, Earth rate is used to derive gyros' turn-on biases, but using a low-cost category IMU this term can be ignored and applying simple mean removal leads to turn-on bias-free data, which can be directly used for navigation.

Our proof-of-concept PDR.

Flowchart of the proposed algorithm.

Schematics of our two-step pose calculation process, yielding the IMU position in the navigation frame as the final result. If

Schematic diagram of the rotational part of the UKF filter.

Typical rejected ARTooKitPlus measurements. (

(

ARToolKitPlus-corrected positional navigation solution. The edgier red, green and dark blue curves are ARToolKitPlus measurements being fed to the UKF at the moments, represented by the blue spikes in the graph beneath.

2D top view graph of the reconstructed experimental slow walk. The path started at the entrance to our Lab and following the wall finished at the other end of the corridor.

Close-ups of the two positional graphs obtained with the slow walking experimental data with ARTK mode disabled (

IMU frame orientation during the slow walking experiment, converted to Euler angles (

IMU velocity vector norm error just before ZUPT triggering occurred, for each step of the slow walking experiment.

1 | 0.1144 | 0.0294 |

2 | 0.0872 | 0.0892 |

3 | 0.0980 | 0.0608 |

4 | 0.0325 | 0.0769 |

5 | 0.1293 | 0.0642 |

6 | 0.0736 | 0.0283 |

7 | 0.0533 | 0.0442 |

8 | 0.0713 | 0.0623 |

9 | 0.0914 | 0.0747 |

10 | 0.0793 | 0.0670 |

11 | 0.1030 | 0.0826 |

12 | 0.0463 | 0.0195 |

13 | 0.0617 | 0.0725 |

14 | 0.0294 | 0.0270 |

15 | 0.0604 | 0.0533 |

Mean value, standard deviation and maximum value of data, presented in

Mean of velocity vector norm error (m/s) | 0.0754 | 0.0568 |

Standard deviation of velocity vector norm error (m/s) | 0.0291 | 0.0222 |

Minimum value of velocity vector norm error (m/s) | 0.0294 | 0.0195 |

Maximum value of velocity vector norm error (m/s) | 0.1293 | 0.0892 |