In this paper, we propose a pedestrian cooperative localization (CL) method to estimate the position of multi-pedestrians using the pedestrian-to-pedestrian (P2P) ranging measurement of ultra-wideband (UWB). In the case of using an inertial sensor mounted on a shoe, the pedestrian position is estimated by applying integration approach-based pedestrian dead reckoning (PDR). Unfortunately, the observability of the position error cannot be guaranteed in a PDR with a zero-velocity update. When several pedestrians are working together, a CL method using UWB-based P2P ranging measurements between pedestrians can be used to correct the position error of each pedestrian. In an indoor environment, however, ranging measurements may contain errors in the form of bias, impulse, ramp, etc. due to obstacles such as walls and furniture. These errors may cause a positioning error in CL. In this paper, these measurement errors are filtered through a maximum correntropy criterion-based extended Kalman filter (MCEKF). MCEKF is a kind of adaptive filter and adjusts the filter covariance based on the residual. When a measurement error occurs, the measurement noise covariance increases depending on the residual. Measurement updates can be performed with increased covariance, providing a robust positioning solution to measurement errors. The proposed MCEKF-based CL is experimentally verified to provide accurate positioning information even in the presence of various uncertainty errors of UWB.
The positioning of multi-mobile agents has been of interest to many researchers. The mobile
agent can be a robot or a pedestrian, and in this paper, multi-pedestrians are targeted.
Multipedestrians’ positioning is in high demand for lifesaving and safe tasks such as tracking the
location of firefighters at a fire scene and monitoring the location of workers in a complex factory.
In indoor environments where Global Navigation Satellite System (GNSS) signals are blocked or
distorted, wireless infrastructure can be used to estimate the position of pedestrians. However,
the infrastructure must be installed in advance, and the installed infrastructure may be destroyed
at a fire site, etc [
As a non-infrastructure-based positioning method, a pedestrian dead reckoning (PDR) utilizes
an inertial measurement unit (IMU) [
Use permitted under Creative Commons License Attribution 4.0 International (CC BY 4.0).
CEUR Workshop Proceedings (CEUR-WS.org) required to eliminate the error. In IA-based PDR, it is possible to eliminate a part of the errors of PDR by recognizing the moment when the foot velocity is zero when the foot touches the ground and performing a zero-velocity update (ZUPT).
Nevertheless, the position error cannot be limited within a certain range. In this paper, the
error of PDR is corrected by using ultra-wideband (UWB) with ZUPT. We propose a method for
cooperatively correcting PDR errors by acquiring relative ranging information between
multipedestrians. This technique is called pedestrian cooperative localization (CL) [
One of the important considerations in the CL method is the accuracy of the ranging
information measured based on UWB. UWB is known to measure ranging information within an
error range of less than 30cm. However, these excellent features can only be guaranteed under
Line of Sight (LoS) conditions and cannot be guaranteed under NLoS (Non-Line-of-Sight)
conditions due to various indoor environments [
In this paper, to solve the problem caused by measurement errors, maximum correntorpy extended Kalman filter (MCEKF) based CL is proposed. The proposed method has an implementation advantage because it uses only the residual between the ranging measurement and the filter estimate. In addition, by changing the state error covariance matrix and measurement noise covariance matrix in LoS and NLoS conditions through maximum correntropy criterion (MCC), it is possible to obtain robust position estimation performance in both conditions with only one filter [10-13]. The performance of the proposed method is verified experimentally.
The remaining paper consists of the following. System description and problem statement in Section 2. In Section 3, MCEKF-based pedestrian CL is described. The experimental results discussed are presented in Section 4, and the conclusion is described in section 5.
Among the methods for positioning pedestrians indoors, this paper deals with IA-based PDR. This method can be applied when the IMU is attached to the pedestrian’s shoe and navigation is performed. As shown in Figure 1, IA-based PDR calculates navigation information with the INS algorithm and has a structure that corrects INS errors based on ZUPT. INS is a method of calculating attitude, velocity, and position using information on acceleration and angular velocity measured from the IMU. Although INS has the advantage of calculating navigation information without infrastructure, it has the disadvantage that the error gradually increases with time. In IAbased PDR, when the foot equipped with the IMU touches the ground is detected and ZUPT is performed to compensate for INS error by using the zero-velocity information.
ZUPT can compensate for errors in INS, but not all errors. In EKF, the error estimation possibility is determined based on observability, but unfortunately, zero-velocity information alone does not guarantee the observability of the position error. To correct this position error, another non-inertial information is needed. In this paper, the ranging information measured based on wireless signals is used. If a general wireless positioning technique can be used, the position errors of the PDR can be corrected by using ranging information measured between multiple anchor nodes and a terminal owned by a pedestrian. In addition, it is possible to improve the accuracy of navigation by enhancing the observability of the filter, which is limited only by ZUPT. However, in this study, considering conditions such as emergency rescue environments, the following problem can be defined.
The problem is the uncertainty error involved in the ranging measurements used in this method applied in an indoor environment. The accuracy of the UWB-based ranging measurement is satisfied only under the LoS condition. The ranging measurement error occurring in the NLoS condition always has positive values. If the error of the PDR is corrected using the ranging measurement, including this error, the performance of the PDR may be worse. Considering this problem, this paper designs a filter for CL using MCEKF. MCEKF is a filter with robust characteristics against non-Gaussian noise such as heavy-tailed impulsive noise. Therefore, when the UWB measurement includes an uncertainty error having a non-Gaussian error distribution, the influence of the measurement error can be reduced by the MCEKF.
The proposed method is MCEKF-based CL using IA-based PDR and UWB-based P2P (Pedestrian-to-Pedestrian) ranging measurements to solve the described two problems. Figure 2 shows the overall structure of the proposed method. Each pedestrian performs navigation using IA-based PDR. And it has a CL structure in which errors of each PDR are corrected together using UWB-based P2P ranging measurements. Here, the filter for CL is implemented with MCEKF, considering the measurement uncertainty error of UWB.
The MCEKF-based CL proposed in this paper performs time-propagation in synchronization with the IMU output cycle, and the measurement-updates are performed independently using two asynchronous measurements and once again using one additional measurement. In Figure 2, the first measurement update is performed in the main-filter with ZUPT, and the second measurement-update is performed in sub-filter 1. And sub-filter 2 is driven based on the result of sub-filter 1. Detailed information related to this is described in each sub-section.
3.1. Main-filter with ZUPT In the main-filter, the IA-based PDR shown in Figure 1 is driven. This filter is divided into two stages. First, the navigation information consisting of attitude, velocity, and position is calculated using the IMU measurements based on the INS algorithm. Then, the navigation error is corrected based on the ZUPT if the moment when the foot touches the ground is detected. The INS algorithm for pedestrian navigation can be simplified and expressed as follows: q 12 q (ωibb )T 0T (1) vl Cblf b gl pl vl (2) (3) (4) (6) (7) where l and b denote the local tangent coordinate system, and the body coordinate system of the IMU, respectively. pl is the position, vl is the velocity, and q and Cbl are the quaternion and direction cosine matrix that mean the conversion from b-frame to l-frame. fb and ωibb are the accelerometer and gyro outputs, respectively, and gl is the gravitational acceleration vector.
When ZUPT is implemented using EKF, the error state variables are set as follows: xmain ( pl )T ( vl )T ( l )T
where p is the position error, v is the velocity error, is the attitude error, and is (b )T T the acceleration bias. The filter does not take the gyro bias into account because it can be estimated by averaging the gyro measurements in the stationary state before gait start [14].
The measurement update is performed using the zero-velocity information if a zero-velocity is detected. Then, the state variables are updated as follows:
xˆmain,k Kk vlk (5) where K is the Kalman gain.
The upper process is repeatedly performed synchronizing with the IMU output cycle and the gait cycle.
3.2. Sub-filter 1: MCEKF-based Ranging Measurement Update Sub-filter 1 performs a measurement-update using ranging measurement. In this case, since the position information of the counterpart with which ranging is performed must be used together with the ranging information, the position error of the counterpart must be considered. For this, the state vector and the state error covariance matrix are expanded as follows: T xi xTmain,i pTj(i)
Pmain,i Psub1 0212 0122
Ppos, j where xmain,i is the error state vector of pedestrian-i driving the sub-filter 1, p j(i) is the position error of the pedestrian-j to be estimated in the sub-filter 1 of pedestrian-i, P is the state error covariance matrix. Superscript (-) means time-propagation, and (+) means measurementupdate. Pmain,i Pk is the state error covariance matrix calculated by the main-filter of the pedestrian-i, and Ppos, j corresponds to the position error among the state error covariance matrix calculated by the main-filter of pedestrian-j.
The state variables in (6) are not fully observable using only one ranging information. And the degree of observability of the observable state variables is low. However, the degree of observability of state variables is gradually improved by continuously acquiring the ranging measurements among moving pedestrians. Continuous measurement update has a similar effect to performing the measurement update once using ranging measurements obtained from a plurality of anchor nodes. Therefore, the observability condition for sub-filter 1 is satisfied over time.
Based on the prior information denoted in the previous subsection, sub-filter 1 is designed
using MCEKF. First, the UWB-based ranging measurement between two pedestrians is modeled
as follows [
Considering the extended state variables in (8), the Jacobian matrix of the measurement function is constructed as follows:
where Pk and Rk are adjusted through the weighting matrix calculated by the kernel function as follows:
R 0
The difference between MCEKF and EKF is not only the residual but also the cost function. In EKF, the state variable that minimizes the mean square error is estimated. On the other hand, MCEKF estimates the state variable that maximizes the correntropy-based cost function. The cost function of MCEKF is as follows:
where L is the sum of the dimensions of the state and the measurement, ek (i) is the i-th element of ek , and G (e) exp(e2 / (2 2)) is a Gaussian kernel function, which is used to calculate the correntropy.
The state variable that maximizes the cost function is difficult to obtain analytically and can be obtained through fixed-point iteration [11].
The measurement-update in sub-filter 1 can be summarized as follows: xˆk,t xˆk Kk (zk Hk ) xˆk (15)
J (xk ) 1 L
G (ek (i))
L i1 Kk PkHkT (Hk PkHkT Rk )1
Pk Bp,kCx,1k Bp,k
T Rk Br,kCy,1k Br,k
T Hi,k Hi,k
H j,k Hi, k ( pix,k p xj,k ) / dij,k ( piy, k p yj,k ) / dij,k
0110 H j,k ( p xj,k pix,k ) / dij,k ( p yj,k piy,k ) / dij,k where dij is the calculated distance between pedestrian-i, j.
Unlike EKF, the residual in MCEKF is calculated as follows:
T ek Bk1 xˆk xk
zk h(xk ) Bk BkT Psub1 0 0 T Bp,k Bp,k 0
T Br,k Br,k (9) (10) (11) (12) (13) (14) (16) (17) (18) where t is the iteration order in the fixed-point iteration algorithm, and n is the extended system dimension.
As this process is repeated in the fixed-point iteration algorithm, the state variables converge while satisfying the maximum correntropy. After this process, the state error covariance matrix is updated as follows:
Pk (I Kk Hk )Pk (I Kk Hk )T Kk Rk KkT (22)
The reason that MCEKF has robust characteristics against measurement uncertainty is in (18). If an uncertainty error having a non-Gaussian characteristic occurs in the measurement, it is reflected in the residual of (21). This value affects the value of the kernel function in (20), and the Cy matrix deviates from the identity matrix and decreases to a small value. Since the inverse of this matrix is used as a weighting matrix in (18), the R matrix has a larger value than the previously set R matrix. As a result, the reliability of the measurement with uncertainty is lowered, so MCEKF has a robust characteristic against measurement uncertainty.
3.3. Sub-filter 2: EKF-based position measurement update Sub-filter 1 estimates not only its state variables but also the horizontal position error of the other pedestrian. As shown in Figure 2, the position of pedestrian-i estimated in sub-filter 1 of pedestrian-j and the corresponding error covariance matrix are transmitted to pedestrian-i. This information is used as a measurement in sub-filter 2 to perform additional measurement update. The state vector of the sub-filter 2 is the same as that of the main-filter, and the error covariance matrix is set as follows:
Cx,k diag G (ek (1)),...,G (ek (n)) Cy,k diag G (ek (n 1)),...,G (ek (L)) ek Bk1 xˆk xˆk,t1 zk h( xˆk,t1)
T (19) (20) (21) (23) (24) (25) (26) And the measurement information is set as follows:
Psub2,k Psub1,k (1:12,1:12) zsub2,k pˆx i( j),k
y T pˆi( j),k
Rsub2,k Psu(b1j,)k (13:14,13:14) Hsub2 I22 021 023 023 023 where Pˆi(xj) is the position information of pedestrian-i estimated in the sub-filter 1 of pedestrian-j.
Additional measurement-update using the position information is performed based on EKF. The result of the sub-filter 2 is fed back to the main-filter.
An experiment was performed to analyze the performance of the proposed MCEKF-based CL. Figure 3 shows the experimental equipment. The IMU is Xsens’s MTw and was attached to the side of the shoe. And the UWB ranging module is Decawave’s TREK1000 and was attached to the shoulder of the experimenter.
The parameters defined in this paper were set as follows. The output frequency of the IMU is 100 Hz, and the output frequency of the UWB is set to about 3.4 Hz. The kernel bandwidth was set to 1.6. The kernel bandwidth for passing Gaussian noise and detecting non-Gaussian noise is set to 1.6. And it was verified through various experiments and set to this value. (c) impulse error (d) ramp error Figure 4: Types of ranging measurement errors occurring depending on the experimental condition
The experiments were conducted in two ways. First, the type of error that can be included in the UWB measurement was analyzed, and the response in the MCEKF according to each error factor was analyzed. Then, the results of performing CL through the three walking experimenters with EKF and MCEKF, respectively, were analyzed comparatively.
4.1. Analysis of UWB measurement error, and MCEKF characteristics according to measurement error Experiments were performed to analyze the UWB measurement error according to the environment. To this end, two experimenters stood at a distance of 5m, and then an obstacle was placed between the experimenters to generate a ranging measurement error. Experiments were performed 5 times under the 4 types of conditions: ① there is no obstacle, ② the obstacle exists for 10 seconds, ③ the obstacle exists intermittently, and ④ one experimenter was stationary and the other experimenter walked away along the corner of the corridor. The error of the measurements obtained in each condition is shown in Figure 4.
In the first case, it is an LoS condition with no obstacles between the two experimenters, and the calculated mean value of the measurement errors was 0.002m. It can be seen that the ranging measurement under the LoS condition is very accurate. The second condition is when an obstacle exists between the two experimenters, and the measurement error can be seen in the blue region of Figure 4(b). It can be seen that the measurement error of a constant value of about 0.5m occurs during the time period in which the obstacle is present. The magnitude of this error may vary depending on the type of obstacle and the experimental environment [9]. In the third case, an obstacle exists intermittently, and the error characteristics can be confirmed in Figure 4(c). It can be seen that impulsive errors occur intermittently because the time that the obstacle exists between the two experimenters is short. The last condition is when an experimenter walks through a corner of a corridor and is obscured by a wall. In Figure 4(d), it can be seen that the ranging error gradually increases from the occurrence of the NLoS condition due to the wall, and (c) impulse error (d) ramp error Figure 5: Square root of the adjusted measurement error covariance according to error types then decreases at the experimenter returns. Therefore, the ranging measurement error of UWB can be defined as noise, bias error, impulse error, and ramp error.
The characteristics of the MCEKF-based CL were analyzed under NLoS conditions. The analysis of changes in MCEKF internal variables, when errors due to NLoS occur in measurements, is as follows. For example, when an error as shown in Figure 4 occurs, the residual and Cy change in (20) and (21). In this figure, the residual increase due to the ranging measurement error, and due to this, the component of Cy is calculated as a value less than 1. And since the inverse matrix of Cy acts as a weight, the measurement error covariance adjusted in (18) increases. Therefore, the uncertainty contained in the measurement is filtered out by adjusting the measurement error covariance matrix in the MCEKF.
Figure 5 shows the square root of the measurement error covariance adjusted in MCEKF according to each measurement error type, and it is the adjusted value in conjunction with the measurement error case of Figure 4. Hereinafter, for convenience of description, it is denoted by
R . In this figure, (a) is R adjusted under LoS condition, and it can be seen that the covariance matrix adjustment hardly occurs in the measurement where only noise exists. (b) is a case in which there is a bias error in the measurement, and it can be seen that the R value increases from the point in time when the bias error is generated and returns to the initial value when the bias error disappears. (c) is a case where impulse errors exist in the measurement. Comparing with Figure 4(a), it can be seen that the R value considering the magnitude of the impulse error increases only at the moment when the impulse errors occur. And (d) is a case in which a ramp error exists in the measurement, and it can be seen that the R value increases and then decreases similarly to the form of the ramp error. However, when the measurement error occurs in the size of 2m, it can be seen that the R value increases significantly to 20m, which is because the R value is adjusted to increase exponentially for the linearly increasing residual by the correntropy expressed in the Gaussian kernel.
4.2. Performance analysis of CL performed by 3 pedestrians
The performance of the proposed MCEKF-based CL was analyzed based on experiments performed by 3 experimenters. The test trajectory is shown in Figure 6, and each pedestrian walked the elliptical trajectory eight times and then returned to the starting position. The experimenters were asked to walk while stepping on the markers at the same time for every step. There is no system that time-synchronizes multiple IMUs and multiple UWBs and stores data based on it. Therefore, IA-based PDR was performed by acquiring only IMU data, and UWB data (a) experimenter 1
(b) experimenter 2 for CL was created by setting the distance between markers to the actual distance between pedestrians and adding noise and specific errors to it.
UWB ranging measurements between the experiments were generated periodically. The errors added here are the bias error between experimenters 1 and 2 for 50~90 sec, the impulse error between experimenters 1 and 3 for 140~190 sec, and the ramp error between experimenters 2 and 3 for 240~280 sec.
CLs were performed based on EKF and MCEKF, respectively, and the results are shown in Table I. The estimated position in the CL performed based on EKF appears to be similar to that based on MCEKF. However, suppose the positions estimated on each marker are analyzed while walking the elliptical trajectory eight times repeatedly. In this case, it can be seen that in the MCEKF-based method, similar positions are estimated for each marker for eight times, whereas in the EKF-based method, different positions are estimated for each marker.
This experiment was performed 3 times, and Figure 7 shows the RMSE of the positioning result for each gait by each experimenter. In the case of the EKF-based CL, the positioning error of experimenter 1 increases between 50 and 90 secs by the bias error. However, it can be seen that relatively small errors occur in the MCEKF-based CL. This phenomenon can also be confirmed in the positioning error of experimenter 2 in the same period. It is also confirmed that the MCEKF produces better results than the EKF in the positioning of experimenters 2 and 3 in the presence of the ramp error for 140~190 sec. However, the effect of the impulse error does not appear to be prominent in this positioning result. However, if only impulse errors occur, it is judged that the performance difference between the filters will appear.
In this paper, MCEKF-based pedestrian CL using UWB-based P2P ranging measurement is proposed. In the proposed method, the IA-based PDR using an IMU is driven for each pedestrian navigation, and the CL method is used to correct the error of each PDR with UWB-based P2P ranging measurement. Although the UWB measurement is accurate in an LoS environment, it may include various types of errors in an NLoS environment that frequently occurs in an indoor space. The proposed method uses MCEKF to take into account uncertainty due to measurement error when updating the measurement. Residual increases due to measurement error, and in MCEKF, measurement noise covariance increases due to residual. Therefore, a measurement update with increased covariance is performed to make the positioning solution robust to measurement errors. The robustness of the proposed method was verified through an experiment considering various NLoS conditions that may cause measurement errors. In the walking experiment, the proposed method reduced the position error by 0.05m compared to the EKF-based method. The proposed method is expected to help in determining the exact location of each firefighter when performing firefighting activities while multiple firefighters walk at the same time indoors without infrastructure.
This work was supported by the National Research Foundation of Korea funded by the Ministry of Science and ICT, the Republic of Korea, under Grant NRF-2022R1A2C2012166. [9] J. Zhu, and S. K. Solmaz. "UWB ranging aided pedestrian geolocation with GPB-based filtering for LoS and NLoS measurement processing." 2020 IEEE/ION Position, Location and Navigation Symposium (2020) [10] L. Shi, Y. Lin. “Convex combination of adaptive filters under the maximum correntropy criterion in impulsive interference.” IEEE Signal Process Lett 21.11 (2014): 1385-1388 [11] B. Chen, X. Liu, H. Zhao, J. C. Principe. “Maximum correntropy Kalman filter.” Automatica 76 (2017): 70-77 [12] H. Zhao, B. Tian, and B. Chen. "Robust stable iterated unscented Kalman filter based on maximum correntropy criterion." Automatica 142 (2022): 110410 [13] S. Liu, T. Zhang, and L. Zhang. "A SINS aided correct method for USBL range based on maximum correntropy criterion adaptive filter." IEEE Transactions on Instrumentation and Measurement 71 (2022): 1-13. [14] S. Y. Cho, J. H. Lee, and C. G. Park. "A Zero-Velocity Detection Algorithm Robust to Various Gait Types for Pedestrian Inertial Navigation." IEEE Sensors Journal 22.6 (2022): 4916-4931