Cooperative Localization of Firefighters Based on Relative Ranging Constraints of UWB and Autonomous Navigation

Abstract

There are many demands for the cooperative localization (CL) of multiple people, such as firefighter rescue. The classical foot-mounted inertial navigation based on zero velocity update (ZUPT) suffers from accumulating error due to the low-cost inertial sensor, and the pre-placed anchors in the ultra-wideband (UWB) system limit the application in an unknown environment. In this study, a group of sensors including the inertial measurement unit (IMU), magnetometer, barometer, and UWB sensor is used. Through the different characteristics of sensors and the position relationship between people, a cooperative localization system using an extended Kalman filter for three-dimensional firefighter tracking is proposed. Ranging information between firefighters from UWB is utilized, and couplings introduced by relative measurement are estimated. Two experiments are designed to verify the proposed algorithm in building and forest environments. Compared with the results of single-person inertial navigation, the average positioning precision of the algorithm in the building and forest is, respectively, improved by 38.93% and 79.01%. This approach successfully suppresses the divergence of positioning errors, and fixed UWB anchors are not needed.

1. Introduction

Nowadays, the issue of cooperative localization has attracted much attention [1,2,3], such as among firefighters. In a firefighter location system, firefighters can usually be divided into the commander and soldiers depending on the tasks they undertake. As shown in Figure 1 and Figure 2, the commander usually stands in the open, far away from the fires and watches out for soldiers, observes the fire situation, and judges the possibility of danger, whereas soldiers are often near the front line of firefighting in environments where the global navigation satellite system (GNSS) cannot be used, such as indoors and in forests. In firefighting, localization using the existing infrastructure may be infeasible, because a large number of sensor nodes should be arranged in advance [4,5]. Therefore, autonomous navigation, such as inertial navigation, is preferred [6].

Inexpensive, compact, and power-efficient microelectromechanical system (MEMS) sensors have been widely used in pedestrian inertial navigation [7,8]. To prevent the dispersion of positioning errors, the IMU is typically attached to the body utilizing kinematic constraints during movement. A common practice is to mount the IMU on the feet of pedestrians and use the ZUPT algorithm [9,10]. When the foot comes into contact with the ground, the pedestrian’s speed is zero, and this is employed as the speed observation to suppress the divergence of the speed error. However, according to the analysis in the literature [11], the ZUPT method can only observe some state quantities such as velocity and zero bias of gyroscope in pedestrian localization, but not heading angle and position, so the cumulative positioning error will become large over time. Therefore, it is necessary to introduce other sensors to assist inertial navigation. A magnetometer is typically used to measure the geomagnetic field, and the calculated heading result is accurate and reliable in environments free from magnetic interference [12,13]. The position solved by inertial sensors in the altitude direction is unstable compared to the other directions, so it is a common practice to introduce barometers to constrain the error growth in the altitude direction [14]. The GNSS technology can constrain the divergence of inertial navigation positioning errors. However, in some indoor and forest environments, the positioning of GNSS will weaken and even fail [15,16]. UWB can provide relative ranging information in case of satellite positioning failure, so it is commonly used in pedestrian positioning.

UWB technology stands out among many techniques for measuring distances by radio due to its decimeter-level ranging accuracy, high temporal resolution, and resistance to multipath effects [17,18]. There has been a large amount of research on UWB positioning. In [19,20], the positioning is achieved by using only UWB technology. However, the positioning precision hinges on the base stations’ number and the spatial layout. Some investigators explored the combination of the inertial navigation system (INS) and UWB technology. In [21], a loosely coupled INS and UWB position is introduced, which uses the UWB position estimate from trilateration to constrain the error divergence of INS. However, the localization result will be not robust if the UWB cannot provide the position information. In [22], the shoe-mounted INS and UWB measurements are fused using a tightly coupled method, and precise position results can be obtained. However, this method neglects the non-line-of-sight (NLOS) error of UWB and can only be used in an indoor environment. The localization solutions for the aforementioned research are applied to single-person positioning, and the UWB base stations are essential. However, it is challenging and time-consuming to set many UWB base stations in firefighting. Therefore, there is a need to investigate how UWB ranging can be used to constrain firefighters and, thus, achieve localization solutions in the absence of fixed base stations.

Relative ranging fusion research is often used for cooperative localization [23]. In [24], a method of installing IMUs on both feet and using the ranging measurement constraints between pedestrians is proposed. The step-wise dead reckoning is used to replace the commonly used ZUPT method. This method reduces the computational effort and realizes the cooperative localization. The coupling relationship between pedestrians should be considered during the cooperative localization so as to improve positioning accuracy. In [25], a cross-correlations compensation method is introduced to the decentralized cooperative localization. However, it suffers from a high computational cost. The IMU and radio measurements (i.e., UWB ranging and Wi-Fi received signal strength) can be incorporated into the particle filtering, and the positioning of people in the two-dimensional plane is realized [26]. However, the performance of positioning will degrade in an outdoor three-dimensional environment.

In this study, we propose a cooperative localization algorithm for firefighters that fuses data from the IMU, magnetometer, barometer, and UWB ranging. The method employs an extended Kalman filter to achieve autonomous localization in three-dimensional space without infrastructure. Every firefighter uses the INS to obtain the position estimate, and the filter update of the cooperative localization algorithm occurs when there are relative range measurements among firefighters. We make full use of UWB fusion technology to achieve high-precision ranging between firefighters, and the adverse influence of the NLOS errors will be mitigated by setting different range measurement noise parameters according to whether the environment condition is the NLOS condition. The proposed algorithm has high accuracy and is suitable for the positioning of multiple mobile pedestrians.

The paper is organized as follows: Section 2 describes the formulation of the problem to be solved and the algorithm is derived. Section 3 introduces the experiments and validates the proposed approach. Finally, Section 4 concludes this work.

2. Modeling

2.1. INS

In the process of inertial navigation calculations, the state error is estimated by using the error state Kalman filter (ESKF) [27]. This method minimizes rotation processing compared to the traditional Kalman filter, as three error angles will be estimated rather than the quaternion. Additionally, it operates close to the origin, so it is easier to ensure the effectiveness of linearization. In this research, we only model the estimation of the misalignment angle and gyroscope deviation, as the attitude calculation accuracy impacts the final position accuracy and picking too many state values will increase the computational cost. Then, the pseudo-observation with zero speed is used to adjust the speed once the attitude has been determined, and the updates of speed and position are appropriately simplified in the following way:

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

(1)

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

(2)

where $a_{k-1}$ is the acceleration in the body frame at time $k-1$, $C_{b(k-1)}^n$ is the direction cosine matrix from the body frame to the navigation frame at time $k-1$, $g$ is the gravity acceleration, $\Delta t$ is the sampling time interval, $v_k$ and $v_{k-1}$ are the velocity at time $k$ and $k-1$, respectively, and $p_k$ and $p_{k-1}$ are the position at time $k$ and $k-1$, respectively.

In the attitude estimation process, the error state is taken as $\delta x={\left[\begin{array}{cc}\delta \theta & \delta {\omega }_b\end{array}\right]}^T$ and states without error quantities are $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, $q={\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]}^T$ is a Hamiltonian quaternion, ${\omega }_b$ is the angular rate gyro bias, and $T$ is the transpose operation of a matrix. The true state is $x_t={\left[\begin{array}{cc}{q}_t& {\omega }_{bt}\end{array}\right]}^T=x\oplus \delta x$, where $q_t$ is the true quaternion and ${\omega }_{bt}$ is the true angular rate gyro bias. The specific expansion forms are $q_t=q\left\{\delta \theta \right\}\circ q$ and ${\omega }_{bt}={\omega }_b+\delta {\omega }_b$, where $\circ$ represents quaternion multiplication.

According to the error equations of inertial navigation and considering the influence of device accuracy, the motion equation under the linearization of discrete form is given by

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

(3)

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 system noise with a zero mean Gaussian distribution, $O_{{}_{3\times 3}}$ is a zero matrix, $I_{3\times 3}$ is an identity matrix, and $C_b^n$ is the direction cosine matrix from the body frame to the navigation frame and can be represented by the quaternion $q$ as in [28]:

$$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]$$

(4)

The outputs of the accelerometer and magnetometer at the moment of zero velocity are selected as measurement vectors. The following equation is available for acceleration measurements:

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

(5)

where $g_n$ and $a_b$ are the gravity vector under the navigation and body frames, 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 magnetometer measurements, we have

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

(6)

where $m_n$ and $m_b$ are the geomagnetic field vector under the navigation and body frames, respectively. Because the geomagnetic field does not have any component in the east direction, it can be modified as

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

(7)

Then, the corrected geomagnetic field vector of the navigation frame is converted into the body frame as the observation values, and the equation is

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

(8)

According to Equations (5) and (8), the observation equation is

$$z_k=h(x_t)+V_k$$

(9)

where $z_k$ is the observation vector consisting 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_k$ is the observation noise with a zero mean Gaussian distribution. The Jacobian matrix $H_k$ of the observation equation is required during filter updates, which can be obtained according to the following chain rule:

$$H_k=\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}$$

(10)

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

$$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]$$

(11)

$$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]$$

(12)

According to the derived equations of state and observation equations, the ESKF can be realized with the following equation. The prediction equations of ESKF can be presented as follows:

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

(13)

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

(14)

where $P_{k-1}$ is the error covariance matrix related to the error states and $Q_k$ is the process noise covariance matrix.

The update equations of ESKF can be presented as follows:

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

(15)

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

(16)

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

(17)

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

After each ESKF update, we can use the estimated error state $\delta x$ to compensate the state $x$, so as to obtain the true state $x_t$. The compensation process is shown in the following equations:

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

(18)

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

(19)

In order to prevent the error state estimation from being included in the subsequent update, the error state needs to be reset as soon as the true state has been determined.

2.2. UWB

The double-sided two-way ranging (DS-TWR) method is used in this study considering the ranging accuracy. However, the ranging error may have different error characteristics depending on the environment [29,30]. In the line-of-sight (LOS) conditions, the ranging value is relatively accurate, and the standard deviation of the ranging error is small. Therefore, the ranging error can be easily previously calibrated. In NLOS conditions, the error distribution model of ranging measurement is complex and there is no unified model.

In contrast to many studies where NLOS measurements are identified and excluded, the NLOS measurements are used in the study. The received power and signal power in the first path are selected as the identification condition for LOS and NLOS conditions.

The received power is

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

(20)

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

The signal power in the first path is

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

(21)

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

The difference between the received power and signal power in the first path could distinguish the LOS and NLOS conditions [31], and the discrimination equation can be described as follows:

$${\epsilon }_P=R{X}_L-F{P}_L$$

(22)

In this study, the critical threshold for NLOS conditions judgment is 10 dB, which is based on the experimental results in Figure 3. Therefore, if the ${\epsilon }_P$ is less than 10 dB, the UWB range measurements are in the LOS condition. In contrast, if the ${\epsilon }_P$ is larger than 10 dB, the UWB range measurements are in the NLOS condition. In the subsequent Kalman filter correction process of the cooperative localization algorithm, we choose different ranging noise parameters according to the judged environmental conditions. This approach will reduce the adverse impact of NLOS errors of UWB on the CL algorithm.

2.3. Cooperative Localization

We first build the motion model of a single firefighter, and then the motion models of multiple firefighters will be achieved correspondingly. Only the person’s position is chosen as the system state vector to reduce the system dimension and lighten the computation load. We denote $X_i={[\begin{array}{ccc}{p}_i^x& p_i^y& p_i^z\end{array}]}^T$ as the state vector for firefighter $i$, where $p_i^x$, $p_i^y$, and $p_i^z$ are the east, north, and up positions, respectively. The motion model for the ith firefighter is, thus, as below:

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

(23)

where $X_i(k-1)$ and $X_i(k)$ are the position state vector at time $k-1$ and $k$, respectively, $F_i(k|k-1)=I_{3\times 3}$ is the state transition matrix, $B_i(k-1)=I_{3\times 3}$ is the input matrix, $U_i(k-1)={\left[\begin{array}{ccc}\Delta p_i^x(k-1)& \Delta p_i^y(k-1)& \Delta p_i^z(k-1)\end{array}\right]}^T$ is the input vector consisting of three position changes, $\Delta p_i^x(k-1)$, $\Delta p_i^y(k-1)$ and $\Delta p_i^z(k-1)$ are the east, north, and up position changes at time $k-1$, respectively, and $W_i(k-1)$ is the vector of the system noise and accords with the zero mean Gaussian white noise distribution.

There are two types of observations for firefighters. One is for the commander to use the GNSS to obtain the absolute position so that the map of the geographic information system can be utilized to understand the characteristics of buildings or forests. The observation equation obtained is

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

(24)

where $Z_i(k)$ is the observation vector consisting of the GNSS position information, $h_i(k)=I_{3\times 3}$ is the observation matrix, and $V_i(k)$ is the GNSS position measurement noise with a zero mean Gaussian distribution.

Another observation is the UWB ranging between the commander and soldiers. Then, the relative ranging observation of two firefighters i and j at a certain moment is

$$\begin{array}{l}{Z}_{ij}(k)=h_{ij}(X_i(k),X_j(k))+V_{ij}(k)\\ \hspace{1em}\hspace{1em}\hspace{0.17em}\hspace{0.17em}=\sqrt{{(p_i^x(k)-p_j^x(k))}^2+{(p_i^y(k)-p_j^y(k))}^2+{(p_i^z(k)-p_j^z(k))}^2}+V_{ij}(k)\end{array}$$

(25)

where $Z_{ij}(k)$ is the ranging observation of firefighters, $V_{ij}(k)$ is the ranging noise with zero mean Gaussian white distribution, and $h_{ij}(X_i(k),X_j(k))$ is the nonlinear vector observation function.

To use the Kalman filter algorithm, we linearize Equation (25) by employing first-order Taylor expansion to obtain

$$\begin{array}{l}{Z}_{ij}(k)=\nabla h_{ij}{}^i{X}_i(k)+\nabla h_{ij}{}^j{X}_j(k)+h_{ij}({\widehat{X}}_i(k),{\widehat{X}}_j(k))\\ \hspace{1em}\hspace{1em}\hspace{0.17em}\hspace{0.17em}-\nabla h_{ij}{}^i{\widehat{X}}_i(k)-\nabla h_{ij}{}^j{\widehat{X}}_j(k)+V_{ij}(k)\end{array}$$

(26)

where $\nabla h_{ij}{}^i=\frac{\partial h_{ij}{}^i}{\partial X_i(k)}|{}_{X_i(k)={\widehat{X}}_i(k)}$, $\nabla h_{ij}{}^j=\frac{\partial h_{ij}{}^j}{\partial X_j(k)}|{}_{X_j(k)={\widehat{X}}_j(k)}$.

According to the above model of a single firefighter, the system model consisting of N firefighters in the cooperative localization algorithm can be formulated as

$$X(k)=F(k|k-1)X(k-1)+B(k-1)U(k-1)+W(k-1)$$

(27)

where $X(k-1)=\left[\begin{array}{c}{X}_1(k-1)\\ X_2(k-1)\\ ⋮\\ X_N(k-1)\end{array}\right]$, $U(k-1)=\left[\begin{array}{c}{U}_1(k-1)\\ U_2(k-1)\\ ⋮\\ U_N(k-1)\end{array}\right]$, $W(k-1)=\left[\begin{array}{c}{W}_1(k-1)\\ W_2(k-1)\\ ⋮\\ W_N(k-1)\end{array}\right]$.

The observation equation of the ith firefighter is

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

(28)

where $H_i(k)=\left[\begin{array}{ccccc}0& \cdots & h_i(k)& \cdots & 0\end{array}\right]$.

The relative observation equation of the i and j firefighters is

$$Z_{ij}(k)=H_{ij}(k)X(k)-H_{ij}(k)\widehat{X}(k)+h_{ij}({\widehat{X}}_i(k),{\widehat{X}}_j(k))+V_{ij}(k)$$

(29)

where $H_{ij}(k)=\left[\begin{array}{ccccccc}0& \cdots & \nabla h_{ij}{}^i(k)& \cdots & \nabla h_{ij}{}^j(k)& \cdots & 0\end{array}\right]$.

In this study, an extended Kalman filter is selected for cooperative localization, which is a straightforward method for calculating the mutual coupling relationships to obtain accurate position estimates. It can consist of two steps, prediction and update.

The prediction equations are as below:

$$\widehat{X}(k|k-1)=F(k|k-1)\widehat{X}(k-1)+B(k-1)U(k-1)$$

(30)

$$P(k|k-1)=F(k|k-1)P(k-1)F{(k|k-1)}^T+Q(k)$$

(31)

where $P(k-1)$ is the covariance matrix of state vectors and $Q(k)$ is the system noise covariance matrix.

There are two types of update equations. One is the position update, and the relevant update equations are as follows about the ith firefighter:

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

(32)

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

(33)

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

(34)

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

The other type of update equations are about the relative ranging between the i and j firefighters:

$$K_{ij}(k)=P(k|k-1)H_{ij}^T(k){(H_{ij}(k)P(k|k-1)H_{ij}^T(k)+R_{ij}(k))}^{-1}$$

(35)

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

(36)

$$P(k)=(I-K_{ij}(k)H_{ij}(k))P(k|k-1){(I-K_{ij}(k)H_{ij}(k))}^T+K_{ij}(k)R_{ij}(k)K_{ij}(k)$$

(37)

where $K_{ij}(k)$ is the Kalman gain of relative ranging and $R_{ij}(k)$ is the covariance matrix of the relative ranging observation noise.

For the cooperative localization system of firefighters, the position information can already be calculated by the above cooperative localization algorithm. However, if the same range measurement noise parameter is used in the NLOS and LOS conditions, the positioning results will also deteriorate accordingly. Therefore, we can set different range measurement noise parameters based on the judgement results of UWB in Section 2.2, which can improve the positioning accuracy of cooperative localization in different environments.

3. Experiment and Results

3.1. Experimental Setup

In order to verify the proposed algorithm, a corresponding hardware experimental platform is built. The ICM-42688-P (Invensense, San Jose, CA, USA) is the IMU that integrates a three-axis gyroscope and a three-axis accelerometer. The IST8310 (Isentek, New Taipei City, Taiwan) is a three-axis magnetometer. The SPL06-001 (Goertek, Weifang, China) is a digital barometric air pressure sensor. The SKG122S (Skylab) is selected as the GNSS module owing to its multi-system dual-band performance. The DW1000 (Qorvo, Greensboro, NC, USA) is chosen as the UWB because it is low-power and complies with the IEEE 802.15.4-2011 UWB standard. The STM32F407VET6 (STMicroelectronics, Geneva, Switzerland) is chosen as the controller of the system. The sensors are mounted as shown in Figure 4, where the IMU, magnetometer, and barometer are placed on a shoe, and the GNSS and UWB modules are fixed on the helmet. The sensor parameters are shown in

Table 1. Sensor configuration parameters.

Sensors	Sampling Frequency (Hz)
ICM-42688-P	100
IST8310	100
SPL06-001	100
SKG122S	1
DW1000	50

.

3.2. Building Environment Experiment

The first experimental site was chosen inside a building, and the experiment participants consist of one commander and three soldiers. The commander is outside the building while the soldiers are inside the building. There are NLOS situations when the soldiers are blocked by walls. Soldiers walk along a pre-measured closed trajectory as shown in Figure 4 and they are blocked by walls at corners. The trajectory is an L-shaped area, 12.6 m long and 7.8 m wide, and it is made of 0.6 × 0.6 m square tiles. The experimental results are plotted in Figure 5 and Figure 6.

Figure 5a shows the positioning results of three soldiers calculated through inertial navigation. Affected by the sensor accuracy and the magnetic interference environment in the building, the positioning results gradually diverged. In Figure 5b, the results of cooperative localization without dealing with NLOS errors are presented. Because the NLOS errors are not processed, the system positioning accuracy will be affected when the NLOS value occurs. In Figure 5c, dealing with NLOS errors by adjusting the corresponding filter ranging noise parameter $R_{ij}$ from 0.03 to 10 in the CL algorithm, the error introduced by the NLOS error is compensated, and the positioning accuracy is the best.

Figure 6 shows the mean square error (MSE) curves of the positioning results using different methods. We are able to more clearly compare the performance of the algorithm through the error curves. In Figure 6a–c, the INS error shows an increasing characteristic compared with the initial stage, which conforms to the characteristic of using IMU alone for positioning. By introducing the CL algorithm among firefighters, it can be found that the final positioning results of the CL algorithm are better than those of INS. Due to the NLOS errors of UWB, whether the NLOS errors are dealt with will also affect the accuracy of CL. For example, in the period of 20–40 s in Figure 6a, the positioning errors of the CL that does not deal with NLOS errors are larger than those of the CL that deals with NLOS errors. Similar results can also be found in Figure 6b,c. It can be found that the error of the CL algorithm is larger than that of INS at some moments because the cumulative error of INS is not large at the beginning stage. With the increase in experimental time and walking distance, the final error of INS will be larger and larger, whereas the error of the CL algorithm still remains within a certain range.

Table 2. Positioning RMSE of soldiers using different methods in building environment experiment.

	INS	CL (without Dealing with NLOS)	CL (Dealing with NLOS)
Soldier 1	1.06 m	1.48 m	0.67 m
Soldier 2	2.13 m	1.96 m	0.73 m
Soldier 3	1.08 m	1.70 m	0.92 m

lists the positioning root mean square errors (RMSE) of soldiers using the INS and CL methods. As shown in Table 2, the CL algorithm dealing with NLOS errors has the best positioning accuracy in the three methods. The experimental results show that the proposed algorithm is suitable for firefighters in the building environment.

3.3. Forest Environment Experiment

The second experimental scene was selected in undulating forest land. The soldiers are under dense trees, while the commander is the open. The soldiers walk three circles along the reference trajectory shown by the red solid line in Figure 7, and their actual positions are surveyed by the total station.

The experimental analysis method is similar to the previous experiment. Figure 8a,b is the positioning results of inertial navigation in two-dimensions (2D) and three-dimensions (3D), respectively, and the positioning results gradually diverged over time. NLOS errors may occur when the soldiers walk in the forest due to environmental impact. Figure 8c,d shows the location results for 2D and 3D cooperative localization without dealing with NLOS errors, respectively. It can be seen that the positioning results become correspondingly worse when NLOS occurs. The localization results of Figure 8e,f are better than the previous results due to the consideration of NLOS error factors, and the filter ranging noise parameter $R_{ij}$ is adjusted from 0.03 to 10 in the CL algorithm.

Figure 9 shows the MSE curves of the positioning results using different methods. In Figure 9a–c, it can be clearly seen that the INS errors present a growth characteristic. Compared with the building environment experiment, the INS errors increase more clearly due to the increase in experimental time and walking distance. By introducing the CL algorithm among firefighters, the positioning errors do not increase with time. The result shows that the CL algorithm has more advantages than the INS in long-term movement. The NLOS errors of UWB will have an adverse effect on the CL accuracy. For example, in the period of 120–160 s in Figure 9a, the positioning errors of the CL that does not deal with NLOS errors are larger than those of the CL that deals with NLOS errors. Similar results can also be found in Figure 9b,c.

The positioning RMSE of soldiers using the INS and CL methods are listed in

Table 3. Positioning RMSE of soldiers using different methods in forest environment experiment.

	INS	CL (without Dealing with NLOS)	CL (Dealing with NLOS)
Soldier 1	4.87 m	2.59 m	1.57 m
Soldier 2	10.21 m	3.13 m	1.69 m
Soldier 3	8.51 m	2.39 m	1.20 m

. According to the data in Table 3, the CL algorithm dealing with NLOS errors has the best positioning accuracy in the three methods. The experimental results show that the proposed algorithm is suitable for firefighters in the forest environment.

4. Conclusions

This work proposed a cooperative localization algorithm in 3D space for firefighters. The use of self-positioning information and mutual ranging information between firefighters constrains the divergence of the overall positioning error. Experiments are designed to validate the proposed algorithm in building environments and forest environments. The results of the experiments show that compared with the inertial navigation method, this method can enable firefighters to achieve accurate positioning in 3D space. By identifying the NLOS errors of UWB, the adverse impact of NLOS errors on system positioning accuracy can be reduced by setting different ranging noise parameters. In this study, only the range measurements among firefighters are used as the observation. More observations, such as relative direction, can be added to improve the accuracy of cooperative localization in the future.