Adaptive Decentralized Cooperative Localization for Firefighters Based on UWB and Autonomous Navigation

Abstract

Cooperative localization (CL) is a popular research topic in the area of localization. Research is becoming more focused on Unmanned Aerial Vehicles (UAVs) and robots and less on pedestrians. This is because UAVs and robots can work in formation, but pedestrians cannot. In this study, we develop an adaptive decentralized cooperative localization (DCL) algorithm for a group of firefighters. Every member maintains a local filter and estimates the position and the relative measurement noise covariance is estimated rather than a fixed value. We derived the explicit expressions for the inter-member collaboration instead of using approximations. This method reduces the influence of non-line-of-sight (NLOS) errors in the ultra-wideband (UWB) ranging on the CL, eliminating the need for fixed UWB anchors. The proposed algorithm was validated by two experiments designed in the building and forest environments. The experimental results demonstrate that the proposed algorithm improved the accuracy of localization, and the proposed algorithm suppressed the localization errors by 14.23% and 47.01% compared to the decentralized cooperative localization extended Kalman filter (DCLEKF) algorithm, respectively.

1. Introduction

Nowadays, the topic of CL has caught much attention [1,2,3]. CL is a technique that enables a group of agents to estimate their positions and orientations relative to each other and their surroundings. This method has many practical applications, including in robotics, where it is essential for enabling robots to navigate and operate in complex environments. One specific use case of CL is in the development of firefighter rescue systems. The precise localization of firefighters is critical in this context to ensure their safety and improve their response effectiveness during emergencies. A group of firefighters can commonly be separated into the commander and soldiers in firefighting rescue scenarios, depending on the responsibilities they undertake. As illustrated in Figure 1 and Figure 2, the commander usually stands in the open sky far away from the fires, observing the fire situation and judging the possibility of danger. However, soldiers are frequently close to the front lines of firefighting in global navigation satellite system (GNSS) denied environments, such as forests and indoors. Because sensor nodes should be arranged in advance, it is infeasible to use the existing infrastructure to locate them in such a case [4,5]. Therefore, inertial navigation as an autonomous navigation scheme is preferred [6,7,8].

Figure 1. CL in the building environment.

Figure 2. CL in the forest environment.

Inexpensive, compact, and power-efficient microelectromechanical system inertial measurement unit (IMU) sensors have been widely used in pedestrian inertial navigation [9,10,11]. Existing research generally fixes the IMU on a pedestrian body and uses kinematic constraints to reduce localization errors. The Zero Velocity Update algorithm is usually used to restrain the divergence of the localization errors [12,13,14]. However, these methods can only maintain the localization results for some time without divergence. It is essential to introduce other sensors to assist with inertial navigation. The geomagnetic field can be measured by the magnetometer to obtain the heading [15,16,17], the barometer measures pressure to calculate altitude [18,19,20], and GNSS can be used to obtain a position directly [21,22,23]. The above methods are commonly used in individual localization, but the relationship of firefighters can also be utilized in CL. UWB can provide a relative range of measurements among firefighters so that it can be used in CL.

UWB technology stands out among many radio technologies because of its high temporal resolution, high-ranging precision, and resistance to multipath effects [24,25,26]. Many studies have been conducted on the combination of an inertial navigation system (INS) and UWB technology in CL. Most of them required fixed anchors and were only suitable for individual localization. In [27], the error divergence of the INS was constrained by a loosely coupled system of INS and UWB that used the UWB position estimation from the trilateration method. In [28], a tightly coupled method was applied to the foot-mounted INS and UWB system, and from this, precise position results could be obtained. In addition, UWB range measurements may suffer from NLOS errors due to environmental effects, and the direct use of inaccurate range measurements can lead to poor localization results.

CL can be divided into two main structures: centralized cooperative localization (CCL) and the DCL. In the CCL structure, there is a need for a fusion center that analyzes information from the entire team. Although this method can achieve accurate estimates of position, it has a high communication cost and lacks robustness when some members are disconnected [29,30]. In the DCL structure, every member could serve as a local fusion center and exchange information with each other. However, this method suffers from a double-counting problem in calculating the cross-correlations between the state estimates of members. In [31], a covariance intersection method that inflated the measurement noise variance was proposed. However, it was conservative, and the localization accuracy was not good. In [32], a cross-correlations compensation method was introduced to the DCL but suffered from a high computational cost. In [33], a recursive DCLEKF algorithm was proposed that was able to obtain satisfactory CL accuracy at a small computation cost. However, this does not take into account the effects of the inaccurate measurement noise covariance matrix and how the CL accuracy degrades if inaccurate measurement noise is used.

The adaptive Kalman filter is an effective method to solve the unknown noise covariance matrix [34,35]. It can estimate system noise and measure noise online. In this study, we proposed an adaptive extended Kalman filter decentralized cooperative localization (DCLAEKF) algorithm for firefighters. In the DCL system of firefighters, sensors of IMU, magnetometer, barometer, GNSS and UWB were used to obtain an accurate localization result. Every firefighter acted as a local fusion center, and the exchange of information took place only when relative measurements occurred. In this way, the burden of system communication was reduced significantly. The NLOS error of UWB was identified, and its adverse influence on the system accuracy was reduced by the adaptive filter. This avoided discarding the NLOS measurements directly, which reduced the effectiveness of measurement feedback.

The main contributions of the research are outlined as follows:

(1)

The DCL for firefighter localization was proposed. This method differed from the CCL structure, and it is also distinct from the other distributed structures proposed by other researchers. In our proposed DCL structure, we derived explicit expressions for the inter-member collaboration instead of using approximations, which led to higher localization accuracy.

(2)

To address the issue of the reduced localization accuracy caused by NLOS errors in UWB systems, we proposed using an adaptive extended Kalman filter algorithm to estimate measurement noise. In contrast to fixed noise parameter settings, which reduce the adverse effects of NLOS errors, the adaptive filter showed greater adaptability to the time-varying noise caused by environmental changes.

The structure of this paper is as follows. Section 2 introduces the system model, and the DCLAEKF algorithm is derived. Section 3 introduces the experiments and validates the proposed algorithm. Finally, Section 4 concludes this work.

2. System Model

2.1. Inertial Navigation System Model

For the foot-mounted inertial navigation system, the error-state Kalman filter was used to estimate the state error and the position change [36]. The misalignment angle and gyroscope deviation were chosen as the estimation states so that the error state could be taken as $\delta x={\left[\begin{array}{cc}\delta \theta & \delta {\omega }_b\end{array}\right]}^T$ and the state without error quantities was $x={\left[\begin{array}{cc}q& {\omega }_b\end{array}\right]}^T$, where $\delta \theta$ is the misalignment angle error, $\delta {\omega }_b$ is the angular rate gyro bias error, $T$ is the transpose operation of a matrix, $q={\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]}^T$ is a Hamiltonian quaternion, and ${\omega }_b$ is the angular rate gyro bias. We defined $x_t={\left[\begin{array}{cc}{q}_t& {\omega }_{bt}\end{array}\right]}^T=x\oplus \delta x$ as the true state, where $q_t$ is the true quaternion and ${\omega }_{bt}$ is the true angular rate gyro bias, $\oplus$ is a generic composition consisting of quaternion multiplication and vector addition. The corresponding specific expansion forms were $q_t=q\left\{\delta \theta \right\}\circ q$ and ${\omega }_{bt}={\omega }_b+\delta {\omega }_b$, where $\circ$ is quaternion multiplication.

On the basis of the error equations of inertial navigation, the discrete-time motion model was formulated as follows:

$$\delta x_k=F_k\delta x_{k-1}+W_{k-1}$$

(1)

where $\delta x_k$ and $\delta x_{k-1}$ are the error states at time $k$ and $k-1$ respectively, $F_k=\left[\begin{array}{cc}{I}_{3\times 3}& -C_b^{n}T\\ O_{{}_{3\times 3}}& I_{3\times 3}\end{array}\right]$ is the state transition matrix at time $k$, $W_{k-1}$ is the system noise with a zero mean Gaussian distribution, $I_{3\times 3}$ is an identity matrix, and $O_{{}_{3\times 3}}$ is a zero matrix. $C_b^n$ is the direction cosine matrix from the body frame to the navigation frame, which can be denoted by the quaternion $q$ as [37]:

$$C_b^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$$

(2)

In this system, two kinds of measurement were considered, including the accelerometer and magnetometer measurements at the moment of zero velocity. For the acceleration measurements, the following equation was available:

$$a_b=C_n^b{g}_n$$

(3)

where $a_b$ and $g_n$ are the gravity vector under the body and navigation frame, respectively. $C_n^b$ is the direction cosine matrix from the navigation frame to the body frame, and the relationship between $C_n^b$ and $C_b^n$ is $C_n^b={\left(C_b^n\right)}^T$.

For the magnetometer measurements, the following equation was available:

$$m_n=C_b^n{m}_b$$

(4)

where $m_b$ and $m_n$ are the geomagnetic field vector under the body and navigation frames, respectively. Additionally, $m_n$ can be converted to the following form because there is no easterly component to the geomagnetic field, and the geographic frame (east-north-up) was chosen as the navigation frame.

$$m_n=[\begin{array}{ccc}0& \sqrt{m_{nx}{}^2+m_{ny}{}^2}& m_{nz}\end{array}]$$

(5)

The modified geomagnetic field vector can be converted into the body frame through the directional cosine matrix as the observation vector, and the equation for this is:

$$m_b=C_n^b{m}_n$$

(6)

According to Equations (3) and (6), the observation equation is:

$$z=h(x_t)+V$$

(7)

where the observation vector $z$ is made up of the accelerometer measurements and the magnetometer measurements, $h()={\left[\begin{array}{cc}{C}_n^b{g}_n& C_n^b{m}_n\end{array}\right]}^T$ is the nonlinear function of $x_t$, and $V$ is the observation noise with a zero mean Gaussian distribution. The Jacobian matrix $H$ of the observation equation needs to be obtained during the filter update, which can be obtained on the basis of the following chain rule:

$$H=\frac{\partial h}{\partial \delta x}{|}_x=\frac{\partial h}{\partial x_t}{|}_x\frac{\partial x_t}{\partial \delta x}{|}_x=H_x{X}_{\delta x}$$

(8)

where $H_x$ and $X_{\delta x}$ can be derived as follows:

$$H_x=\left[\begin{array}{cc}\frac{\partial a_b}{\partial q}& O_{3\times 3}\\ \frac{\partial m_b}{\partial q}& O_{3\times 3}\end{array}\right]$$

(9)

$$X_{\delta x}=\left[\begin{array}{cc}\frac{\partial (\delta q\circ q)}{\partial \delta \theta }& O_{4\times 3}\\ O_{3\times 3}& \frac{\partial ({\omega }_b+\delta {\omega }_b)}{\partial \delta {\omega }_b}\end{array}\right]$$

(10)

Based on the state and observation equations, the ESKF can be implemented using the following equation. The prediction equations of ESKF can be expressed as follows:

$$\delta x_{k|k-1}=F_k\delta x_{k-1}$$

(11)

$$P_{k|k-1}=F_k{P}_{k-1}{F}_k^T+Q_k$$

(12)

where $P_{k-1}$ represents the error covariance matrix associated with the error states while $Q_k$ represents the covariance matrix of the process noise.

The updated equations for ESKF can be expressed as follows:

$$K_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R_k)}^{-1}$$

(13)

$$\delta x_k=K_k(z_k-h(x_{k|k-1}))$$

(14)

$$P_k=(I-K_k{H}_k)P_{k|k-1}$$

(15)

where $K_k$ is the Kalman gain used to provide the correction, and $R_k$ denotes the measurement noise covariance matrix.

Following each ESKF update, we utilized the estimated error state $\delta x$ to correct the state $x$ and obtain the true state $x_t$. This compensation process can be expressed by the following equations:

$$q_t=q\left\{\delta \theta \right\}\circ q$$

(16)

$${\omega }_{bt}={\omega }_b+\delta {\omega }_b$$

(17)

To avoid introducing the error estimation in the next update, the error states need to be reset as soon as the true value has been determined. The attitude can be estimated by the above filter estimation, and then the speed and position can be obtained using the following equations.

$$v_k=v_{k-1}+(C_{b(k-1)}^n{a}_{k-1}-g)\mathrm{\Delta }t$$

(18)

$$p_k=p_{k-1}+\frac{v_k+v_{k-1}}{2}\mathrm{\Delta }t$$

(19)

where the subscript $k$ and $k-1$ represent time, $a$ is the acceleration the in body frame, $g$ is the gravity acceleration in the navigation frame, $\mathrm{\Delta }t$ is the sampling time interval, and $v$ and $p$ are the velocity and positions, respectively.

Because of the characteristic of firefighters working in teams, we used a method to calculate the altitude from differential barometer measurements. Although barometer measurements are prone to temperature, humidity and so on, the working area of firefighters is within a certain range, and the barometer measurements are basically affected the same. The altitude of the commander is known or can be accurately measured, and we could make use of the differential barometer measurements between the commander and soldiers to calculate the relative altitude accurately. Therefore, the altitude did not need to be considered in the CL, and we could research the CL model in a two-dimensional space.

2.2. Non-Line-of-Sight Identification of Ultra-Wideband

A double-sided two-way ranging method was used in this study to obtain the relative range measurements among the firefighters. However, the ranging error may show different error characteristics according to the environment [38,39]. In the LOS conditions, the range measurements were accurate, and the standard deviation of the ranging errors was small. Hence, the ranging errors could be easily calibrated. In the NLOS conditions, the error distribution models of range measurements were complicated, and there were no unified models.

Inaccurate range measurements caused by NLOS conditions lead to poor localization results in CL; here, we utilized the received power and signal power in the first path to determine the LOS and NLOS conditions.

The received power was:

$$R{X}_L=10\times {\log }_{10}\left(\frac{C\times 2^{17}}{N_r^2}\right)-A$$

(20)

where $C$ is the channel impulse response power in the register, $N_r$ is the preamble accumulation count value in the register, and $A$ is the constant related to the register configuration.

The signal power in the first path was:

$$F{P}_L=10\times {\log }_{10}\left(\frac{F_1{}^2+F_2{}^2+F_3{}^2}{N_r^2}\right)-A$$

(21)

where $F_1$, $F_2$ and $F_3$ are the first path amplitude magnitude values in the registers.

The difference values between the $R{X}_L$ and $F{P}_L$ were used as the criterion for LOS and NLOS conditions in this study [40], and the threshold was selected as 10 dB in Figure 3. The determined NLOS values were subsequently used in the adaptive estimation algorithm to enhance the localization accuracy.

Figure 3. The difference between the received power and signal power in the first path.

2.3. Decentralized Cooperative Localization Model

Considering a team of $N$ firefighters, every firefighter carries the IMU, magnetometer, barometer, GNSS and UWB sensor. Because the altitude can be obtained by the differential barometer measurements, we only modelled the CL system model in a two-dimensional space. We took the position as the state vector and obtained $X_i={\left[\begin{array}{cc}{p}_i^x& p_i^y\end{array}\right]}^T$ for firefighter $i$, where $p_i^x$ and $p_i^y$ represented the east and north positions. The discrete-time motion model of firefighter $i$ was formulated as follows:

$$X_i(k)=F_i(k|k-1)X_i(k-1)+B_i(k-1)U_i(k-1)+W_i(k-1)$$

(22)

where $X_i(k-1)$ and $X_i(k)$ are the positions at time $k-1$ and $k$ respectively, $F_i(k|k-1)=I_{2\times 2}$ is the state transition matrix, $B_i(k-1)=I_{2\times 2}$ is the input matrix, $U_i(k-1)={\left[\begin{array}{cc}\mathrm{\Delta }{p}_i^x(k-1)& \mathrm{\Delta }{p}_i^y(k-1)\end{array}\right]}^T$ is the input vector consisting of position changes at time $k-1$, and $\mathrm{\Delta }{p}_i^x(k-1)$, $\mathrm{\Delta }{p}_i^y(k-1)$ are the east and north position changes at time $k-1$ respectively, $W_i(k-1)$ is the system noise with a zero mean Gaussian distribution.

In the study, two kinds of measurements were considered. One was that the commander used GNSS to obtain the absolute position so as to understand the characteristics of buildings or forests using the map of the Geographic Information System. The absolute measurement equation can be formulated as follows:

$$Z_i(k)=H_i(k)X_i(k)+V_i(k)$$

(23)

where $Z_i(k)$ is the position obtained by GNSS at time $k$, $H_i(k)=I_{2\times 2}$ is the identity matrix, and $V_i(k)$ is the measurement noise with a zero mean Gaussian distribution.

The relative measurement was the UWB ranging among firefighters and the relative measurement model between firefighter $i$ and $j$, which can be formulated as follows:

$$\begin{array}{ll}{Z}_{ij}(k)& =h_{ij}(X_i(k),X_j(k))+V_{ij}(k)\\ & =\sqrt{{(p_i^x(k)-p_j^x(k))}^2+{(p_i^y(k)-p_j^y(k))}^2}+V_{ij}(k)\end{array}$$

(24)

where $Z_{ij}(k)$ is the range measurement of firefighter $i$ to firefighter $j$ at time $k$, $h_{ij}(X_i(k),X_j(k))$ is the relative measurement function, $p_j^x(k)$ and $p_j^y(k)$ are the east and north positions of firefighter $j$, and $V_{ij}(k)$ is the ranging noise with zero mean Gaussian white distribution.

The purpose of this distributed algorithm is to spread the calculation among individual nodes, eliminating the need to maintain a central node. The distributed algorithm using EKF consists of three parts in the CL model, including no measurement update, the absolute measurement update, and the relative measurement update.

When no measurement update occurred, the update equations for firefighter $i$ could be formulated as follows:

$${\widehat{X}}_i(k|k-1)=F_i(k|k-1){\widehat{X}}_i(k-1)+B_i(k-1)U_i(k-1)$$

(25)

$$P_{ii}(k|k-1)=F_i(k|k-1)P_{ii}(k-1)F{(k|k-1)}^T+Q(k-1)$$

(26)

$$P_{ij}(k|k-1)=F_i(k|k-1)P_{ij}(k-1)$$

(27)

$${\widehat{X}}_i(k)={\widehat{X}}_i(k|k-1)$$

(28)

$$P_{ii}(k)=P_{ii}(k|k-1)$$

(29)

$$P_{ij}(k)=P_{ij}(k|k-1)$$

(30)

where $P_{ii}$ is the covariance matrix of firefighter $i$, $P_{ij}$ is the covariance matrix between firefighter $i$ and firefighter $j$, and $Q(k-1)$ is the system noise covariance matrix.

When the absolute measurement update of firefighter $i$ occurred, the update equations for firefighter $i$ could be formulated as follows:

$$K_i(k)=P_{ii}(k|k-1)H_i^T(k){(H_i(k)P_{ii}(k|k-1)H_i^T(k)+R_i(k))}^{-1}$$

(31)

$${\widehat{X}}_i(k)={\widehat{X}}_i(k|k-1)+K_i(k)(Z_i(k)-H_i(k){\widehat{X}}_i(k|k-1))$$

(32)

$$P_{ii}(k)=(I-K_i(k)H_i(k))P_{ii}(k|k-1)$$

(33)

$$P_{ij}(k)=(I-K_i{H}_i(k))P_{ij}(k|k-1)$$

(34)

where $K_i(k)$ is the Kalman gain and $R_i(k)$ is the covariance matrix of the position observation noise.

When the relative measurement update of firefighter $i$ and firefighter $j$ occurred, and firefighter $i$ detected firefighter $j$, the update equations for firefighter $i$ are formulated were as follows:

$$H_{ir}(k)=\frac{\partial h_{ij}(X_i(k),X_j(k))}{\partial X_i(k)}({\widehat{X}}_i(k|k-1),{\widehat{X}}_j(k|k-1))$$

(35)

$$H_{jr}(k)=\frac{\partial h_{ij}(X_i(k),X_j(k))}{\partial X_j(k)}({\widehat{X}}_i(k|k-1),{\widehat{X}}_j(k|k-1))$$

(36)

$$K_{ir}(k)=(P_{ii}(k|k-1)H_{ir}^T(k)+P_{ij}(k|k-1)H_{jr}^T(k)){(H_r{P}_r(k|k-1)H_r^T+R_r(k))}^{-1}$$

(37)

$${\widehat{X}}_i(k)={\widehat{X}}_i(k|k-1)+K_{ir}(k)(Z_{ij}(k)-h_{ij}({\widehat{X}}_i(k|k-1),{\widehat{X}}_j(k|k-1)))$$

(38)

$$P_{ii}(k)=(I-K_{ir}(k)H_{ir}(k))P_{ii}(k|k-1)-K_{ir}(k)H_{jr}(k)P_{ji}(k|k-1)$$

(39)

$$P_{ij}(k)=(I-K_{ir}(k)H_{ir}(k))P_{ij}(k|k-1)-K_{ir}(k)H_{jr}(k)P_{jj}(k|k-1)$$

(40)

$$P_{im}(k)=(I-K_{ir}(k)H_{ir}(k))P_{im}(k|k-1)-K_{ir}(k)H_{jr}(k)P_{jm}(k|k-1)$$

(41)

where $H_r(k)=[H_{ir}(k),H_{jr}(k)]$ is the Jacobian matrix of the relative measurement model, $K_{ir}(k)$ is the Kalman gain of the relative measurement update, $P_r(k|k-1)=\left[\begin{array}{cc}{P}_{ii}(k|k-1)& P_{ij}(k|k-1)\\ P_{ji}(k|k-1)& P_{jj}(k|k-1)\end{array}\right]$ is the covariance matrix, $R_r(k)$ is the ranging noise covariance matrix of UWB sensors, and the subscript $m$ is the firefighter that did not participate in the relative measurement.

2.4. Adaptive Extended Kalman Filter Algorithm

The relative range measurements obtained by UWB sensors could have an adverse effect on CL due to the NLOS errors. Although we can set a large-ranging noise covariance matrix to reduce the adverse effect in NLOS conditions, the ranging noise covariance matrix might vary due to environmental influences. It was not appropriate to set a fixed nominal value for the ranging noise covariance matrix. In the study, the Sage-Husa adaptive filter algorithm was utilized to estimate the ranging noise.

The innovation of the extended Kalman filter is expressed below:

$${\tilde{Z}}_{ij}(k|k-1)=Z_{ij}(k)-h_{ij}({\widehat{X}}_i(k|k-1),{\widehat{X}}_j(k|k-1))$$

(42)

The adaptive filter algorithm of the range measurement noise variance matrix can be formulated as follows:

$${\widehat{R}}_r(k)=(1-\beta (k)){\widehat{R}}_r(k-1)+\beta (k)({\tilde{Z}}_{ij}(k|k-1){\tilde{Z}}_{ij}^T(k|k-1)-H_r(k)P_r(k|k-1)H_r^T(k))$$

(43)

$$\beta (k)=\frac{\beta (k-1)}{\beta (k-1)+b}$$

(44)

where $\beta (0)=1$, $b$ is the forgetting factor that satisfies $0<b<1$.

To maintain the positive definiteness of ${\widehat{R}}_r(k)$, the threshold boundary values $R_{r\max }$ and $R_{r\min }$ were selected. We defined the discriminant equation as:

$$\epsilon ={\tilde{Z}}_{ij}(k|k-1){\tilde{Z}}_{ij}^T(k|k-1)-H_r(k)P_r(k|k-1)H_r^T(k)$$

(45)

Therefore, ${\widehat{R}}_r(k)$ can be modified to:

$${\widehat{R}}_r(k)=\left\{\begin{array}{c}{R}_{r\max }\\ (1-\beta (k)){\widehat{R}}_r(k-1)+\beta (k)R_{r\min }\\ (1-\beta (k)){\widehat{R}}_r(k-1)+\beta (k)\epsilon \end{array}\right.\begin{array}{c}\\ \\ \end{array}\begin{array}{c}\\ \\ \end{array}\begin{array}{c}(\epsilon >R_{r\max })\\ (\epsilon <R_{r\min })\\ (others)\end{array}$$

(46)

We drew a corresponding algorithm block diagram for the proposed DCLAEKF algorithm, as shown in Figure 4.

Figure 4. Block diagram of DCLAEKF algorithm.

3. Experiments and Analysis

3.1. Experimental Setup

We finally validated the proposed algorithm through a hardware experimental platform built by ourselves. The ICM-42688-P was the IMU that integrated a three-axis gyroscope and a three-axis accelerometer. The IST8310 was a three-axis magnetometer. SPL06-001 was selected as a digital barometric air pressure sensor. The SKG122S was selected as the GNSS module owing to its multi-system dual-band performance. The DW1000 chip was selected as the UWB because it has low power and can be compliant with the IEEE 802.15.4-2011 UWB standard. The STM32F407VET6 was selected as the microcontroller of the system. The sensors were mounted as illustrated in Figure 5, where the IMU, the magnetometer, and the barometer were placed on a shoe, and the GNSS and UWB modules were fixed on the helmet. The parameters of the sensors are listed in Table 1.

Figure 5. Sensor layout.

Table 1. Configuration parameters of sensors and experimental setup.

Sensors	Sampling Frequency (Hz)	Experimenters	Experimental Sites
ICM-42688-P	100	Six soldiers and one commander	Building environment site
IST8310	100
SPL06-001	100		Forest environment site
SKG122S	1
DW1000	40

3.2. Building Environment Experiment

In order to be similar to the actual situation, we chose seven experimenters. The experimenters consisted of one commander and six soldiers. The six soldiers were divided into three groups, and the same group performed the same tasks. The first experimental site was chosen inside a building. The commander was outside the building, while three groups of soldiers were inside the building. Each group of soldiers walked along a pre-measured closed trajectory, and different groups of soldiers walked along a different trajectory. The walking track of each group of soldiers is shown in Figure 6. We compared the DCLEKF algorithm and the proposed DCLAEKF algorithm, and the localization results are shown in Figure 7. The MSEs of the localization are listed in Table 2.

Figure 6. Reference track diagram in the building environment.

Figure 7. Localization results of soldiers using different algorithms in a building environment experiment. (a) Localization results of soldier 1; (b) Localization results of soldier 2; (c) Localization results of soldier 3; (d) Localization results of soldier 4; (e) Localization results of soldier 5; (f) Localization results of soldier 6.

Table 2. Localization MSEs of soldiers using different algorithms in a building environment experiment.

Soldiers	DCLEKF	DCLAEKF
Soldier 1	0.69 m	0.68 m
Soldier 2	0.70 m	0.54 m
Soldier 3	0.63 m	0.59 m
Soldier 4	0.49 m	0.42 m
Soldier 5	1.21 m	1.01 m
Soldier 6	0.67 m	0.50 m

Figure 7a–f shows the localization results of two kinds of algorithms. The red line is the true trajectory, the green line is the localization result of the DCLEKF algorithm, and the blue line is the localization result of the DCLAEKF algorithm. Soldiers 1 and 2 are in the same group and are shown in Figure 7a,b. Compared to the DCLEKF algorithm, the DCLAEKF algorithm suppressed the localization errors of soldiers 1 and 2 by 1.19% and 21.98%, respectively. Similarly, soldiers 3 and 4 were in the same group, as shown in Figure 7c,d. Compared to the DCLEKF algorithm, the DCLAEKF algorithm suppressed the localization errors of soldiers 3 and 4 by 5.49% and 14.29%, respectively. Soldiers 5 and 6 were in the same group, as shown in Figure 7e,f. Compared to the DCLEKF algorithm, the DCLAEKF algorithm suppressed the localization errors of soldiers 5 and 6 by 16.36% and 26.03%, respectively.

Figure 8 shows the results of MSE, which were obtained by utilizing two kinds of algorithms. We compared the performance of the algorithms more clearly by analyzing the error curves. The findings of Figure 8a indicate that the DCLAEKF algorithm outperformed the DCLEKF algorithm in terms of precision within the 0–50 s time range, whereas the performance of the DCLAEKF algorithm declined afterward. The results depicted in Figure 8b indicate that during the early stage, the DCLAEKF algorithm had comparable MSEs to DCLEKF, but during the later stage, the DCLAEKF algorithm attained lower MSEs. Similar results were obtained from Figure 8c–f. In the presence of NLOS errors, the DCLAEKF algorithm outperformed the DCLEKF algorithm by effectively handling the NLOS errors through the adaptive estimation of range measurement noise parameters, thus reducing the final localization errors.

Figure 8. MSE curves of soldiers in a building environment experiment. (a) MSE of soldier 1; (b) MSE of soldier 2; (c) MSE of soldier 3; (d) MSE of soldier 4; (e) MSE of soldier 5; (f) MSE of soldier 6.

The localization results and MSE curves in the figures indicate that the DCLAEKF algorithm displayed higher errors than the DCLEKF algorithm at some times but demonstrated lower errors most of the time. This was due to the complexity of the indoor environment, limited space, and the presence of various types of obstructions, such as walls, desks, chairs, doors, and windows. The sole use of the differential method, as described in Section 2.2, could lead to inaccurate NLOS detection, thereby limiting the effectiveness of adaptive algorithms. The MSEs in Table 2 indicate that the DCLAEKF algorithm achieved better localization accuracy than the DCLEKF algorithm. The experimental results demonstrate that the proposed algorithm was suitable for the cooperative localization of firefighters in the building environment.

3.3. Forest Environment Experiment

The second experimental site was selected in the forest land and is shown in Figure 9. The groups of experimenters are the same as in the previous experiment. The soldiers were under dense trees, and the commander was in the open sky. Each group of soldiers walked along a pre-measured closed trajectory, and different groups of soldiers walked along different trajectories. The walking tracks of each group of soldiers were L-shaped areas, and the length and width were $35\times 6$ m, $20\times 6$ m, and $10\times 6$ m, respectively. The experimental results are shown in Figure 10, and the MSEs of localization are listed in Table 3.

Figure 9. Reference track diagram in the forest environment.

Figure 10. Localization results of soldiers using different algorithms in a forest environment experiment. (a) Localization results of soldier 1; (b) Localization results of soldier 2; (c) Localization results of soldier 3; (d) Localization results of soldier 4; (e) Localization results of soldier 5; (f) Localization results of soldier 6.

Table 3. Localization MSEs of soldiers using different algorithms in a forest environment experiment.

Soldiers	DCLEKF	DCLAEKF
Soldier 1	2.62 m	2.09 m
Soldier 2	2.33 m	1.87 m
Soldier 3	3.38 m	1.03 m
Soldier 4	2.65 m	0.81 m
Soldier 5	1.60 m	0.94 m
Soldier 6	2.10 m	0.80 m

Soldiers 1 and 2 were in the same group, and they walked along the $35\times 6$ m L-shaped trajectory shown in Figure 10a,b. Compared to the DCLEKF algorithm, the DCLAEKF algorithm suppressed the localization errors of soldiers 1 and 2 by 20.46% and 19.69%, respectively. Similarly, soldiers 3 and 4 were in the same group, and they walked along the $20\times 6$ m L-shaped trajectory shown in Figure 10c,d. Compared to the DCLEKF algorithm, the DCLAEKF algorithm suppressed the localization errors of soldiers 3 and 4 by 69.55% and 69.32%, respectively. Soldiers 5 and 6 were in the same group, and they walked along the $10\times 6$ m L-shaped trajectory shown in Figure 10e,f. Compared to the DCLEKF algorithm, the DCLAEKF algorithm suppressed the localization errors of soldiers 5 and 6 by 41.09% and 61.96%, respectively.

Figure 11 shows the results of MSE, which were obtained by utilizing two kinds of algorithms. Figure 11a indicates that the DCLAEKF algorithm outperformed the DCLEKF algorithm with smaller errors for the majority of the time. Nevertheless, between 15–35 s, the DCLAEKF algorithm exhibited a larger error than that of the DCLEKF algorithm. Furthermore, the DCLAEKF algorithm displayed a superior performance to the DCLEKF algorithm in terms of both maximum error and error stability. Similar results were obtained from Figure 11b,f. Due to the existence of NLOS errors in UWB ranging, the DCLEKF algorithm experienced sudden spikes in localization errors when NLOS occurred. On the contrary, the DCLAEKF algorithm effectively reduced the impact of NLOS errors on localization accuracy through an adaptive estimation of the range measurement noise. For example, during the time interval of 40–60 s in Figure 11b, the localization errors of the DCLAEKF algorithm were significantly smaller than that of the DCLEKF algorithm. The MSEs in Table 3 indicate that the DCLAEKF algorithm achieved a better localization accuracy than the DCLEKF algorithm. The experimental results indicate that the proposed algorithm was suitable for the cooperative localization of firefighters in the forest environment.

Figure 11. MSE curves of soldiers in a forest environment experiment. (a) MSE of soldier 1; (b) MSE of soldier 2; (c) MSE of soldier 3; (d) MSE of soldier 4; (e) MSE of soldier 5; (f) MSE of soldier 6.

4. Conclusions

A DCLAEKF algorithm for firefighters was proposed in this work. For each firefighter, the use of self-positioning information and mutual range measurements between firefighters constrained the divergence of the localization error. The DCL structure was adopted, and the relative measurement noise covariance was estimated adaptively. Experiments were performed, and the results showed that the proposed algorithm was more accurate in NLOS conditions than the DCLEKF algorithm. In this study, only the range of measurements between firefighters was used as observations. Future work should incorporate more measurements (e.g., relative bearing) to improve the accuracy of CL.