A reliable underground navigation system for task forces in emergency scenarios increases safety and effectiveness. Since smoke, explosions or disturbed power supply complicate the operations below ground, the correct visualization of the task forces' positions has become a key element in obtaining a successful outcome. A foot-mounted inertial navigation system provides an autonomous way to navigate in Global Navigation Satellite Systems (GNSS)-denied environments. The additional use of maps and absolute positioning methods (Wi-Fi Round Trip Time (RTT), Ultra-Wideband (UWB), ...) represent one way to build a robust navigation system for emergency operations underground. This study proposes a sensor fusion concept to combine a foot-mounted inertial navigation system (INS) with distance measurements and map information. However, the main emphasis in this paper is on the implementation of a foot-mounted INS. Therefore, a Zero-Velocity-Aided Error-State (Extended) Kalman Filter is utilized. The filter is based on the mechanization equation that expresses the position in the earth-fixed frame, the velocity in the local-level frame and the attitude in the body frame. The performance analysis of the foot-mounted INS as a single system showed that a stable position solution over several hundred meters could be achieved.
One possible sensor is an Inertial Measurement Unit (IMU). An IMU is a common sensor for
autonomous positioning techniques [
The fusion of multiple sensors makes a navigation system more robust and reliable. Studies
from [
In this project, an INS, distance measuring techniques (Wi-Fi RTT or UWB) and map information
are fused to obtain a reliable underground navigation system for task forces in GNSS-denied
environments. A possible concept for an underground navigation system is illustrated in Figure 1.
The basic idea is to integrate the foot-mounted IMUs into a Bayesian filtering framework [ 11].
Thus, a dual foot-mounted INS will be developed that provides a robust estimation of, among
others, (step-wise) position changes. The use of IMUs on both feet can significantly reduce the
systematic heading drift as proposed in [
The particle filter provides an easy integration of nonlinear observations such as map information (probability maps) [11]. Maps of tunnel structures are used as artificial sensors. For instance, the lateral position error can be bounded by the tunnel cross-section. The absolute position information is derived from distance measurements (Wi-Fi RTT or UWB) to known anchors. In case that at least three distance measurements are available, trilateration delivers an absolute position. If not, the distance measurements are used to re-weight the particles [13]. The position estimates of the task forces will be visualized in a virtual reality environment.
The use of absolute positioning methods require a preinstalled infrastructure. However, during emergency scenarios below ground such infrastructure is not available or, even if they are, they could be damaged. The idea in this project is to built the needed infrastructure during operating time, in other words, to install dynamically the Wi-Fi/UWB access points. The approximate coordinates of the access points are derived from the INS and the map information and stored in the corresponding database (Figure 1). Once the access points are installed, all available sensors (IMU, Wi-Fi RTT/UWB, map) are fused to obtain the task force’s positions and also to update the positions of the access points. The determination of the start position is still an open topic.
However, the main focus is put on the implementation and performance analysis of a “single” foot-mounted INS. In future studies, an approach to fuse the data of a dual foot-mounted INS will be developed as well as the concrete integration into the particle filter.
For the foot-mounted PDR system, the XSens Dot [14], produced by XSens, is utilized as IMU. The XSens Dot is composed of triaxial micro-electro-mechanical systems (MEMS) accelerometers, triaxial MEMS gyroscopes and triaxial MEMS magnetometers. It is a wearable sensor and features wireless data transmission based on Bluetooth 5.0. The acceleration and angular data are sensed internally at a high-frequency rate (800 Hz). Due to limitations of the Bluetooth’s channel bandwidth, the data is finally provided in real-time at 60 Hz using an intern strap-down integration by XSens [14]. The data is recorded using a smartphone and the “Xsens Dot” App, which is available in the App Store as well as in the Play Store. Furthermore, multiple sensors can be time-synchronized with each other [14]. For the test setup, two IMUs are mounted onto the left and the right foot, respectively, as seen in Figure 2a.
The raw measurements (accelerations, angular rates, magnetic field strength) refer to the sensor frame (s-frame). The s-frame on Xsens Dot [14] is displayed in Figure 2b. The body frame ys
IMU zs zb yb IMU xb (a) Test Setup. (b) Sensor Orientation. The sensor frame and body frame are indicated by the superscripts s and b, respectively. (b-frame) here is defined so that the y-axis points in the forward direction, the x-axis to the right and the z-axis is orthogonal to them and points upwards [15]. Thus, a right-handed system is completed (Figure 2b). The local-level frame (l-frame) is expressed in terms of east north up (ENU). The Earth-Centered Earth-Fixed Frame (ECEF) is denoted as e-frame. The WGS 84 ellipsoid is used [16].
The sensors are calibrated according to [17]. The IMU calibration was done just for investigation purposes, whether the in-factory calibration is satisfactory or not. To get an first impression, the focus was put on biases only. The highest obtained biases were 0.12 m/s2 and 0.45 ∘ /s for the accelerometer and gyroscope, respectively. However, since the gyroscope biases can vary strongly between diferent measurement sets, it is recommended to estimate them from stationary phases during each set [17]. The magnetometer is calibrated using the Magnetic Field Mapper provided by XSens. Furthermore, the measurements need to be transformed from the s-frame into the b-frame. This is accomplished by performing a simple 90-degree rotation around the -axis ( = = ) (cf. Figure 2b).
The following subsection presents the used approach for the foot-mounted Zero-VelocityAided INS. The measured accelerations (specific forces) and the measured angular rates are denoted as and , respectively. Both refer to the b-frame.
The used mechanization equation is expressed as a set of three first-order diferential
equations [
0 = ⎣ cos sin
⎡ ⎤
0 ⎦ → Ωℓ = ⎣ sin − cos − sin cos ⎤ 0 0 ⎦ 0 0 ⎡
− +ℎ = ⎢ +ℎ ⎣ tan +ℎ ⎤ ⎡
0 ⎥⎦ → Ωℓℓ = ⎢⎣ ta+n ℎ
− +ℎ − tan +ℎ
0 − +ℎ
⎤ +ℎ
+ℎ ⎦⎥ 0
There are dependencies to the geodetic position (, ℎ ), the current velocity, the Earth’s rotation and the two radii of curvature ( and ). However, the Coriolis part for a pedestrian is relatively small and could be neglected. Future studies will show whether the consideration of the Coriolis term leads to a noticeable model improvement. The quantity Ω is composed, according to [15], of: (1) (2) (3) (4) Ω = Ω + Ω, where Ω describes the Earth’s rotation in the b-frame and Ω expresses the rate of the orientation change between the e- and l-frame expressed in the b-frame. The term Ω is a skew-symmetric matrix containing the gyroscope measurements.
The position, velocity and attitude (PVA) are obtained by integrating Equation 1 over the
interval ∆ = 1/60 s. Each estimation of the time epoch is based on the previous PVA
solution at − 1 and the currently measured accelerations f () and angular rates (). The
position/velocity term is obtained by adding the linear integrated velocity/acceleration term
to the previous position/velocity solution. The quaternion q is updated via the closed-form
solution shown in [15]. However, more detailed information can be found in [
Equation 1 provides a PVA computation based on the integration of accelerations and angular
rates obtained from the IMU and the previous PVA solution. Due to that, sensor errors and
measurement noise are accumulated and result in a rapid error growth (drift). However, those
efects can be bounded by introducing so-called Zero-Velocity Updates (ZUPT). In this study,
the ZUPTs are implemented in form of an Error-State (Extended) Kalman Filter (ESKF). Since
the IMUs are mounted onto the shoes, the stance phases of a person are well sensed. During
the stance phases, the sensor should output zero velocity. However, this is typically not the
case. The zero-velocity is introduced as pseudo-observation in the ESKF [
The system state consists of the position errors = [ , , ] , the velocity errors
= [ , , ] and the attitude errors = [, , ] . Sensor errors like biases are
not taken into account, since the velocity errors are assumed to be zero-mean, afected by white
noise and independent [
The state-space model [
̇+1 = + ⎡ ̇ ⎤ ⎣ ̇ ⎦ ̇ +1 + ⎣ 0 0 ⎤ 0 ⎦ (5) where 0,1 = , 1,2 = ⎣ − ⎡ 0 0 − ⎤ − 0 ⎦ , 2,1 = ⎢⎣ − +ℎ
tan − +ℎ ⎡ 0 1 +ℎ 1 0 0 , where and are the variances of the measurement noise of the specific force and the angular rates. Equation 8 shows the corresponding observation model [15]: = + = ⎣ 0 0 0 0 1 0 0 0 0 ⎦ x + , where refers to the observation noise. The observation noise is assumed to be normally distributed with zero-mean. The corresponding covariance matrix = 2 contains the variances of the measurement noise .
To improve the performance of the proposed Zero-Velocity-Aided INS, prolonged standing is treated diferently. Therefore, a modified stand-still mechanization is implemented according to [21]. This approach freezes the position as well as the heading under particular standstill conditions.
To detect zero-velocity events, the stance hypothesis optimal estimation (SHOE) detector [
= ⎧ ⎪⎨ 1, if 1 ∑︀+− 1
= ⎪⎩ 0, otherwise.
︃(
1 2 ⃦ ⃦⃦ − (, ℎ ) ‖¯¯‖ ⃦⃦⃦⃦ 2 ⃦
1 + 2 ⃦⃦ ⃦ 2 ⃦ )︃ < (9)
The initial sensor orientation can be determined when the system is stationary. The accelerometer measurements are used to compute the initial values for roll and pitch, which are then used to level the system [15]. The initialization of yaw is based on the magnetic heading [23]. Thus, the first 2 seconds of a stationary measurement phase from the accelerometer and magnetometer data is averaged and used for the computation of the initial attitude values. Roll and pitch are computed according to the Formulas 10 and 11 [15].
The heading (yaw) is initialized using the magnetic heading. The magnetic heading is computed using the normalized magnetic data ( ¯, ¯, ¯) [23]: 0 = arctan ︂(
¯ cos + ¯ sin ¯ cos + ¯ sin sin − ¯ cos sin ︂) .
However, the magnetic heading deviates from the true heading . To improve the heading, the magnetic variation, which is currently about 4∘ 28’ (positive to east) for Graz (Austria) [24], is taken into account.
for the diferent trails. are recorded: The results refer to the foot-mounted INS as a single system. Therefore, two areas in Graz (Austria) are chosen as test environments. The initial position is deviated from GNSS-data (if available) or manually set. The threshold for the zero-velocity detection was manually adapted For the first test environment no reference data is available. In this area, three diferent tracks • Track A: Walking along a straight street (∼ 1150 m) • Track B: Walking (∼ 750 m) • Track C: Mixed-motions like slow/fast walking with forward/backward/lateral movements and jogging (∼ 750 m) wide. The test subject walks along the street 4 times (< 1 km). As it can be seen, in the first 270 meters, the trajectories are approximately aligned with the path walked. Then the solutions start to drift away. The final horizontal positioning errors (comparison of start- and endpoint) after over 1 km are about 13 m and 45 m for the left and right trajectory, respectively. The vertical error is under 1 m in both cases (Figure 4b). (10) (11) (12) trajectory (left foot) trajectory (right foot) start point end point (left) end point (right) 15.3910 [°] (a) INS trajectories. left foot right foot 0.0 2.5 5.0 (b) Ellipsoidal height. Dotted lines: actual heights at both ends of the street.
Figure 5 pictures Tracks B and C. The first few meters show a satisfying positioning solution. The limitations are visible in Track C. Due to the use of a zero-velocity detector, which is based on a fixed threshold, irregular motions are not treated in the best way possible.
For the second test environment, a reference path with GNSS-based on real-time kinematic positioning (RTK) is generated. Therefore, the test subject carried a GNSS antenna during the recordings. The height component was corrected accordingly. For those tracks, another pair of sensors was used in comparison to the first measurement campaign. In total, three tracks are recorded: • Track D: Walking forward • Track E: Walking with forward/backward/lateral movements • Track F: Walking forward combined with stair climbing
Figure 6 shows Track F as example. It pictures the INS trajectories as well as the ellipsoidal height over time. Between minute 5 and 6 the test subject goes down the steps, turns around, and goes back up. As time passes, the height drifts away. As it can be seen, the height component of the right foot performs better than the one of the left foot.
Table 1 lists the average root-mean-square error (ARMSE) of all tracks where reference data is available. The tests show reasonable results. In general, it is noticeable that the sensor of the right foot performs significantly better concerning the height estimation than the sensor mounted on the left foot.
The vertical accuracy in all tests is pretty good. Previous analysis, according to [25, 26], of
inertial sensor errors based on Allan variance data showed that the gyroscope bias instability of
the z-component is around 6 ∘ /hr, whereas the bias instabilities of the x- and y-component vary
from 8 ∘ /hr to 12 ∘ /hr. So, the gyroscope bias of the z-component shows a more stable behavior
than the biases of the horizontal plane. The accelerometer biases do not difer significantly
(≈ 30 g). Although the accelerometer and gyroscope biases result in an unbounded error
growth of the vertical component [
(a) INS trajectories.
(b) Ellipsoidal height.
The proposed navigation system relies on a dynamically built network of Wi-Fi RTT/UWB anchors. The coordinates of those anchors are derived from the INS and map solution. The position solutions from the foot-mounted INS as a single system reached satisfactory results over several hundred meters and that without any external support. Therefore, it should provide an accurate enough initial position estimate for the Wi-Fi/UWB access points.
The sensors showed diferent but manageable drift behaviors. A combined INS solution of
the right and left leg could reduce the individual drifts. Studies [
The used zero-velocity detector (SHOE detector) is based on a fixed threshold approach. Thus,
only uniform motion are treated well. Mixed motions, such as walking combined with running,
are problematic. As shown in [
The fusion of the foot-mounted INS with distance measurements and map information within a particle filter represents a promising approach to obtain a reliable navigation system for GNSS-denied environments.
This work is funded by the Austrian Federal Ministry of Agriculture, Regions and Tourism via the Austrian Promotion Agency (FFG) in course of the program line “FORTE”. The consortium is composed of 5 partners: OHB Digital Solutions GmbH (Lead), Austrian Federal Ministry of Defence (Research Group NIKE), Montan University Leoben (Chair of Subsurface Engineering), Graz University of Technology (Institute of Geodesy - Working Group Navigation) and Ingenieurbüro Laabmayr & Partner ZT GesmbH. Thank you for all your cooperation!
Thanks to Bernd Mölg (Institute of Geodesy) for helping with the construction of the fixture. inertial sensors and inter-agent ranging, CoRR abs/1304.3663 (2013). URL: http://arxiv.org/ abs/1304.3663. arXiv:1304.3663. [8] S. Han, L. Wei, R. Li, W. Jingzhe, H. Yu, A Novel Cooperative Localization Method Based on IMU and UWB, Sensors 20 (2020) 467. doi:10.3390/s20020467. [9] Y. Yang, R. Mu, J. Liang, Z. Xiao, B. Yan, S. Lin, G. Li, Accurate dual-foot micro-inertial navigation system with inter-agent ranging, in: 2016 Fourth International Conference on Ubiquitous Positioning, Indoor Navigation and Location Based Services (UPINLBS), 2016, pp. 152–161. doi:10.1109/UPINLBS.2016.7809964. [10] M. Sun, Y. Wang, S. Xu, H. Qi, X. Hu, Indoor Positioning Tightly Coupled Wi-Fi FTM Ranging and PDR Based on the Extended Kalman Filter for Smartphones, IEEE Access 8 (2020) 49671–49684. doi:10.1109/ACCESS.2020.2979186. [11] B. Krach, P. Robertson, Integration of foot-mounted inertial sensors into a bayesian location estimation framework, in: 2008 5th Workshop on Positioning, Navigation and Communication, 2008, pp. 55–61. doi:10.1109/WPNC.2008.4510357. [12] I. Skog, J.-O. Nilsson, D. Zachariah, P. Händel, Fusing information from two navigation system using an upper bound on their maximum spatial separation, in: International Conference on Indoor Positioning and Indoor Navigation (IPIN), 2012, pp. 1–5. doi:10. 1109/IPIN.2012.6418862. [13] F. Uth, T. Hartmann, B. Späth, Real-time optimization of extraction and the logistic process in highly complex geological and selective mining settings, in: Horizon 2020 – the Framework Programme for Research and Innovation (2014-2020), 2017, p. 9–12. [14] Xsens DOT User Manual, Document XD0502P, Revision D, XSens, 2020. https://www.
xsens.com/xsens-dot. [15] A. Noureldin, T. B. Karamat, J. Georgy, Fundamentals of inertial navigation, satellite positioning and their integration, Springer, Heidelberg, 2013. [16] Koordinatensysteme, BEV – Bundesamt für Eich- und Vermessungswesen, 2006. https://www.bev.gv.at/pls/portal/docs/PAGE/BEV_PORTAL_CONTENT_ALLGEMEIN/ 0200_PRODUKTE/PDF/KOORD-SYS.PDF. [17] T. Moder, C. Reitbauer, M. Dorn, M. Wieser, Calibration of smartphone sensor data usable for pedestrian dead reckoning, in: 2017 International Conference on Indoor Positioning and Indoor Navigation (IPIN), 2017, pp. 1–8. doi:10.1109/IPIN.2017.8115910. [18] K. Legat, Untersuchungen zur GPS/INS-Integration, VGI – Österreichische Zeitschrift für
Vermessung und Geoinformation 93 (2005) 72–82. [19] R. M. Rogers, Applied Mathematics in Integrated Navigation Systems, Third Edition, American Institute of Aeronautics and Astronautics, Reston ,VA, 2007. doi:10.2514/4. 861598. [20] J. Wendel, Integrierte Navigationssysteme: Sensordatenfusion, GPS und Inertiale Navigation, 2011. [21] J.-O. Nilsson, P. Händel, Standing still with inertial navigation, in: 2013 International
Conference on Indoor Positioning and Indoor Navigation (IPIN), 2013. [22] I. Skog, P. Händel, J.-O. Nilsson, J. Rantakokko, Zero-Velocity Detection—An Algorithm Evaluation, Biomedical Engineering, IEEE Transactions on 57 (2010) 2657 – 2666. doi:10. 1109/TBME.2010.2060723. [23] P. D. Groves, Principles of GNSS, inertial, and multi-sensor integrated navigation systems,
Technology and applications series, Artech House, Boston, 2008. [24] Zentralanstalt für Meteorologie und Geodynamik (ZAMG), IGRF Deklinationsrechner, 2021. URL: https://www.zamg.ac.at/cms/de/geophysik/produkte-und-services-1/ online-deklinationsrechner. [25] J. Farrell, F. O. Silva, R. F., J. Wendel, IMU Error State Modeling for State Estimation and Sensor Calibration: A Tutorial, in: UC Riverside: Bourns College of Engineering, 2020. https://escholarship.org/uc/item/1vf7j52p. [26] J. Jurado, C. M. Schubert Kabban, J. Raquet, A regression-based methodology to improve estimation of inertial sensor errors using Allan variance data, NAVIGATION 66 (2019) 251–263. URL: https://onlinelibrary.wiley. com/doi/abs/10.1002/navi.278. doi:https://doi.org/10.1002/navi.278. arXiv:https://onlinelibrary.wiley.com/doi/pdf/10.1002/navi.278. [27] J. Lee, H. Ju, C. Park, Error Analysis of PDR System Using Dual Foot-mounted IMU, E3S Web of Conferences 94 (2019) 02007. doi:10.1051/e3sconf/20199402007.