The implemented hybrid cooperative positioning algorithm is based on Kalman filter (KF), which is an efficient and recursive estimator for discrete time linear filtering problem [55]. There are many extensions and generations of KF, and here the standard extended KF (EKF) is adopted due to its simplicity. Here the formulation of EKF is simply introduced in order to have a better understanding of the proposed positioning system.

### 4.1 EKF introduction

EKF is a simple extension of KF for nonlinear problems [55] and is widely applied in navigation and tracking systems. In principle, EKF includes two phases: prediction phase, during which the system state is estimated based on system behaviors, and update phase, during which the system state is corrected by using the available observations.

#### Algorithm 1 Hybrid location engine procedure

#### 4.1.1 Prediction

In prediction phase, the current *a priori* estimates of state {\widehat{\mathbf{x}}}_{k|k-1} and of error covariance **P**_{k|k−1} are drawn from the previous *a posteriori* ones of state {\widehat{\mathbf{x}}}_{k-1|k-1} and of error covariance **P**_{k−1|k−1} by using the following two equations:

{\widehat{\mathbf{x}}}_{k|k-1}=f\left({\widehat{\mathbf{x}}}_{k-1|k-1},{\mathbf{w}}_{k-1}\right),

(1)

{\mathbf{P}}_{k|k-1}={\mathbf{A}}_{k}{\mathbf{P}}_{k-1|k-1}{\mathbf{A}}_{k}^{T}+{\mathbf{Q}}_{k-1}.

(2)

where *f* is the state transition function and {\mathbf{A}}_{k}={\left(\right)close="|">\frac{\mathrm{\partial f}}{\partial \mathbf{x}}}_{}\n \n \n \n \n \n x\n \n \u0302\n \n \n \n k\n \u2212\n 1\n |\n k\n \u2212\n 1\n \n \n \n is the corresponding Jacobian matrix calculated at the previous *a posteriori* state estimate {\widehat{\mathbf{x}}}_{k-1|k-1}. **w**_{k−1} is the process noise and is assumed as Gaussian distributed with covariance **Q**_{
k
}, that is, {\mathbf{w}}_{k-1}\sim \mathcal{N}(0,{\mathbf{Q}}_{k-1}).

#### 4.1.2 *Update*

In update phase, the *a priori* estimates ({\widehat{\mathbf{x}}}_{k|k-1} and **P**_{k|k−1}) are corrected the *a posteriori* estimates ({\widehat{\mathbf{x}}}_{k|k} and **P**_{k|k}) by using the available measurements **z**_{
k
}. In more detail, innovation vector {\stackrel{~}{\mathbf{y}}}_{k} and optimal Kalman gain **K**_{
k
} are computed as (3) and (4), respectively, as follows:

{\stackrel{~}{\mathbf{y}}}_{k}={\mathbf{z}}_{k}-h\left({\widehat{\mathbf{x}}}_{k|k-1},{\mathbf{v}}_{k}\right),

(3)

{\mathbf{K}}_{k}={\mathbf{P}}_{k|k-1}{\mathbf{H}}_{k}^{T}{\left({\mathbf{H}}_{k}{\mathbf{P}}_{k|k-1}{\mathbf{H}}_{k}^{T}+{\mathbf{R}}_{k}\right)}^{-1},

(4)

where *h* is the observation function and {\mathbf{H}}_{k}={\left(\right)close="|">\frac{\mathrm{\partial h}}{\partial \mathbf{x}}}_{}\n \n \n \n \n \n x\n \n \u0302\n \n \n \n k\n |\n k\n \u2212\n 1\n \n \n \n is the corresponding Jacobian matrix evaluated around the *a priori* state estimate {\widehat{\mathbf{x}}}_{k|k-1}. **v**_{
k
} is the measurement noise and is also assumed as Gaussian distributed with covariance **R**_{
k
}, that is, {\mathbf{v}}_{k}\sim \mathcal{N}(0,{\mathbf{R}}_{k}).

After that, the *a posteriori* estimates are obtained as follows:

{\widehat{\mathbf{x}}}_{k|k}={\widehat{\mathbf{x}}}_{k|k-1}+{\mathbf{K}}_{k}{\stackrel{~}{\mathbf{y}}}_{k},

(5)

{\mathbf{P}}_{k|k}=\left({\mathit{I}}_{n}-{\mathbf{K}}_{k}{\mathbf{H}}_{k}\right){\mathbf{P}}_{k|k-1}.

(6)

The recursive computation of Equations 1 to 6 makes up the EKF solutions of a dynamic system.

### 4.2 Measurement modeling

The available measurements are related to the distance between two RF devices using different models, which are adopted in hcEKF.

#### 4.2.1 *WSN measurement model*

The RSSI measurements performed among WSN nodes are linked to the distance observation by adopting the *log-normal shadowing path loss model*[56], where the received power \stackrel{~}{P} (expressed in dBm) is seen as a logarithmic function of the distance (*d* in meters) between the transmitter and the receiver:

\stackrel{~}{P}\left(d\right)={P}_{0}-10\alpha \phantom{\rule{0.3em}{0ex}}{log}_{10}\left(d/{d}_{0}\right)+{X}_{\sigma},

(7)

where *P*_{0} (expressed in dBm) is the mean power received at the reference distance *d*_{0} (typically 1 m), *α* is the path loss exponent, and *X*_{
σ
} is an additive measurement noise. For simplicity, *X*_{
σ
} is assumed to be Gaussian distributed with zero mean and variance {\sigma}_{\text{dB}}^{2}, that is, {X}_{\sigma}\sim \mathcal{N}(0,{\sigma}_{\text{dB}}^{2}). This model only considers the path loss of RF signal and does not takes into account multipath or any other effects. It is worth reminding that these parameters depend greatly on the environment and the operating frequency. Calibrations are required before applying this model.

#### 4.2.2 *UHF-RFID measurement model*

Concerning the UHF-RFID measurements, as in [11], each detection event is translated to a distance measurement equal to half of the reader interrogation range, *r*. In other words, the UHF-RFID detection is seen as constant distance measurement,

\stackrel{~}{d}=r/2+n,

(8)

where *n* is the measurement noise, and it is hard to know the exact distribution of this noise. Here we assume it satisfies Gaussian distribution with zero mean and a variance depending on the radio coverage *r*. This assumption may be not true but it is suitable for EKF to use the UHF-RFID observations. It is worth reminding that the RFID detection is treated as distance measurement equal to *r*/2, and this measurement is always positive.

### 4.3 Hybrid cooperative EKF

The adopted hybrid cooperative EKF (hcEKF) is first proposed in [12], and it adds hybrid and cooperative features onto the standard EKF. In principle, the hcEKF algorithm is divided into three parts: state modeling, hybridization, and cooperation, which are introduced in the following.

#### 4.3.1 *State modeling*

The positioning complexity strongly depends on the modeling of the system dynamics, and in this algorithm we choose the system state which is the position of unknown mobiles, that is, {\widehat{\mathbf{x}}}_{k}=[{\widehat{x}}_{k},{\u0177}_{k}]. Here only 2D localization is considered but the extension to 3D case is straightforward.

According to the this model, the state transition function *f* is a linear function of the state:

{\widehat{\mathbf{x}}}_{k|k-1}=f\left({\widehat{\mathbf{x}}}_{k-1|k-1},{\mathbf{w}}_{k-1}\right)={\widehat{\mathbf{x}}}_{k-1|k-1}+{\mathbf{w}}_{k-1}.

(9)

In this case, the process noise **w**_{k−1} models the unknown movements along *x* and *y* axes. We let △*t*_{
k
} denote the time difference between {\widehat{\mathbf{x}}}_{k|k-1} and {\widehat{\mathbf{x}}}_{k-1|k-1}, and the covariance matrix **Q**_{k−1} can be expressed as:

{\mathbf{Q}}_{k-1}=\left[\u25b3{t}_{k}{\mathbf{I}}_{2}\right]\phantom{\rule{0.3em}{0ex}}\text{diag}\left({\sigma}_{\stackrel{\u0307}{x}}^{2},{\sigma}_{\stackrel{\u0307}{y}}^{2}\right)\phantom{\rule{0.3em}{0ex}}{\left[\u25b3{t}_{k}{\mathbf{I}}_{2}\right]}^{T}.

(10)

where **I**_{2} is a 2 ×2 identity matrix and \text{diag}({\sigma}_{\stackrel{\u0307}{x}}^{2},{\sigma}_{\stackrel{\u0307}{y}}^{2}) is a 2 ×2 diagonal matrix whose diagonal elements are corresponding to the moving speed, which are the differentials of system state.

#### 4.3.2 *Hybridization*

The art of hybridization is to fuse heterogeneous measurements together and to build the corresponding observation functions. Let \mathcal{A}=\{1,2,\dots \phantom{\rule{0.3em}{0ex}}A\}, \mathcal{\mathcal{M}}=\{1,2,\dots \phantom{\rule{0.3em}{0ex}}M\}, in which \mathcal{R}=\{1,2,\dots \phantom{\rule{0.3em}{0ex}}R\} denote the sets of fixed WSN anchors, WSN mobiles, and fixed RFID readers, respectively. For a generic mobile node *m* at time *k*, {\mathcal{A}}_{k}\subseteq \mathcal{A}, {\mathcal{\mathcal{M}}}_{k}\subseteq \mathcal{\mathcal{M}}, and {\mathcal{R}}_{k}\subseteq \mathcal{R} denote the subsets of connected devices (WSN anchors, WSN mobiles, and RFID readers). Note that here *m* is abbreviated for simplicity of denotation.

Therefore, the generic observation vector can be written as

{\mathbf{z}}_{k}={\left[{\stackrel{~}{\mathbf{P}}}_{{\mathcal{A}}_{k}}\phantom{\rule{0.3em}{0ex}}\phantom{\rule{0.3em}{0ex}}{\stackrel{~}{\mathbf{P}}}_{{\mathcal{\mathcal{M}}}_{k}}\phantom{\rule{0.3em}{0ex}}\phantom{\rule{0.3em}{0ex}}{\stackrel{~}{\mathbf{d}}}_{{\mathcal{R}}_{k}}\right]}^{T},

(11)

where {\stackrel{~}{\mathbf{P}}}_{{\mathcal{A}}_{k}} and {\stackrel{~}{\mathbf{P}}}_{{\mathcal{\mathcal{M}}}_{k}} denote the sets of RSSI measures from WSNs, while {\stackrel{~}{\mathbf{d}}}_{{\mathcal{R}}_{k}} denotes the set of RFID-based distance measurements. Note that the RSSI is not transformed into distance measurement and is directly used to feed the positioning algorithm, because the assumption of Gaussian measurement errors does not hold for RSS-based distance measurements [57].

For the *a priori* estimate {\widehat{\mathbf{x}}}_{k|k-1}, the corresponding observation function could be one of the three forms

\phantom{\rule{-12.0pt}{0ex}}h\left({\widehat{\mathbf{x}}}_{k|k-1}\right)\in \left[\begin{array}{l}{h}_{{\mathcal{A}}_{k}}\left({\widehat{\mathbf{x}}}_{k|k-1}\right)\phantom{\rule{0.3em}{0ex}}\phantom{\rule{0.3em}{0ex}}{h}_{{\mathcal{\mathcal{M}}}_{k}}\left({\widehat{\mathbf{x}}}_{k|k-1}\right)\phantom{\rule{0.3em}{0ex}}\phantom{\rule{0.3em}{0ex}}{h}_{{\mathcal{R}}_{k}}\left({\widehat{\mathbf{x}}}_{k|k-1}\right)\end{array}\right],

(12)

where {h}_{{\mathcal{A}}_{k}}\left({\widehat{\mathbf{x}}}_{k|k-1}\right), {h}_{{\mathcal{\mathcal{M}}}_{k}}\left({\widehat{\mathbf{x}}}_{k|k-1}\right), and {h}_{{\mathcal{R}}_{k}}\left({\widehat{\mathbf{x}}}_{k|k-1}\right) are the relative observation functions, referring to the subsets of connected WSN anchors, WSN mobiles, and RFID readers, respectively. More specifically, {h}_{{\mathcal{A}}_{k}}\left({\widehat{\mathbf{x}}}_{k|k-1}\right) is calculated by using Equation 7,

{h}_{{\mathcal{A}}_{k}}\left({\widehat{\mathbf{x}}}_{k|k-1}\right)={P}_{0}-10\alpha \phantom{\rule{0.3em}{0ex}}{log}_{10}\left(\text{dist}\left({\widehat{\mathbf{x}}}_{k|k-1},{\mathit{p}}_{k}^{i}\right)/{d}_{0}\right),

(13)

where {\mathit{p}}_{k}^{i} (i\in {\mathcal{A}}_{k}) is the position of *i* th WSN anchor at time k and dist(·) is the operator of the Euclidean distance computation, e.g., \text{dist}({\mathbf{x}}_{1},{\mathbf{x}}_{2})=\sqrt{{({x}_{1}-{x}_{2})}^{2}+{({y}_{1}-{y}_{2})}^{2}}.

In Equation 13, {h}_{{\mathcal{\mathcal{M}}}_{k}}\left({\widehat{\mathbf{x}}}_{k|k-1}\right) is calculated similarly, but the positions of mobile are used instead of those of anchors. Note that the uncertainty of mobile’s position is considered on the measurement noise and it is found in Section 4.3.3. In addition, {h}_{{\mathcal{R}}_{k}}\left({\widehat{\mathbf{x}}}_{k|k-1}\right) can be computed by using (8)

{h}_{{\mathcal{R}}_{k}}\left({\widehat{\mathbf{x}}}_{k|k-1}\right)=\text{dist}\left({\widehat{\mathbf{x}}}_{k|k-1},{\mathit{p}}_{k}^{l}\right),

(14)

where {\mathit{p}}_{k}^{l} (l\in {\mathcal{R}}_{k}) is the position of *l* th RFID reader at time *k*. More details about how to set the observation function can be found in [12].

#### 4.3.3 *Cooperation*

Cooperations among mobile nodes increase the signal of opportunities for more range or range-related measurements. Uncertainty about mobile’s position, however, should be taken into appropriate considerations. Otherwise, cooperation might do harm to the position estimation, that is, the estimated positions could even diverge further away from the real ones than the noncooperative case. Usually, the uncertainty of mobile’s position is evaluated as the trace of its error covariance matrix.

In hcEKF this uncertainty is mapped on the RSSI measurement and is modeled as additional additive noise on the RSS measurements. In other words, the measurement noise variance from mobile nodes is as the sum of the intrinsic measurement variance plus a contribution from the neighboring mobile position’s uncertainty, i.e.,

{\sigma}_{{\mathcal{\mathcal{M}}}_{k}^{j}}^{2}={\sigma}_{\text{dB}}^{2}+{\sigma}_{{\mathcal{X}}_{k}^{j}}^{2},

(15)

where j\in {\mathcal{\mathcal{M}}}_{k} is the *j* th connected mobile nodes and {\sigma}_{\text{dB}}^{2} is the intrinsic noise variance. Moreover, {\sigma}_{{\mathcal{X}}_{k}^{j}}^{2} is the additional noise variance and is a function of the trace of error covariance matrix (\text{trace}\left({\mathbf{P}}_{k}^{j}\right)), which is indicated in more details in [12].

Supposing that all the available measurements are independent with each other, the measurement error covariance matrix **R**_{
k
} is a diagonal matrix given by:

{\mathbf{R}}_{k}=\text{diag}\left(\phantom{\rule{2.22198pt}{0ex}}\underset{i\in {\mathcal{A}}_{k}}{\underset{\u23df}{\dots \phantom{\rule{2.22198pt}{0ex}}{\sigma}_{{\mathcal{A}}_{k}^{i}}^{2}\phantom{\rule{2.22198pt}{0ex}}\dots}}\phantom{\rule{2.22198pt}{0ex}}\phantom{\rule{2.22198pt}{0ex}}\underset{j\in {\mathcal{\mathcal{M}}}_{k}}{\underset{\u23df}{\dots \phantom{\rule{2.22198pt}{0ex}}{\sigma}_{{\mathcal{\mathcal{M}}}_{k}^{j}}^{2}\phantom{\rule{2.22198pt}{0ex}}\dots}}\phantom{\rule{2.22198pt}{0ex}}\phantom{\rule{2.22198pt}{0ex}}\underset{l\in {\mathcal{R}}_{k}}{\underset{\u23df}{\dots \phantom{\rule{2.22198pt}{0ex}}{\sigma}_{{\mathcal{R}}_{k}^{l}}^{2}\phantom{\rule{2.22198pt}{0ex}}\dots}}\phantom{\rule{2.22198pt}{0ex}}\right).

(16)

In distributed localization systems, cooperations among mobile nodes increase network traffic to transmit the cooperation packets. In this case, however, cooperations are done in the centralized location engine and no network traffic is generated. The whole hcEKF procedure is presented as pseudo code as Algorithm 2.

### 4.4 Complexity analysis

The computational complexity of EKF is mainly upon the matrix inversion and matrix multiplication. For each state estimate, in (4), matrix inversion is computed with asymptotic complexity \mathcal{O}\left({\mathbb{R}}^{3}\right)[58], where \mathbb{R} is the dimension of measurement noise covariance **R** or the number of available measurements; in (6), matrix multiplication is computed with asymptotic complexity \mathcal{O}\left({\mathbb{P}}^{3}\right)[58], where \mathbb{P} is the dimension of error covariance or the dimension of the state vector. In the positioning applications, the number of measurements is usually larger than the dimension of state in order to solve the ambiguity of position estimate. Hence, the complexity of EKF is the computation of inverting matrices in our application. Let \left|{\mathcal{A}}_{k}\right|, \left|{\mathcal{\mathcal{M}}}_{k}\right|, and \left|{\mathcal{R}}_{k}\right| denote the cardinality of the corresponding sets {\mathcal{A}}_{k}, {\mathcal{\mathcal{M}}}_{k}, and {\mathcal{R}}_{k}. The complexity of adopted hcEKF is asymptotically \mathcal{O}\left({(\left|{\mathcal{A}}_{k}\right|+\left|{\mathcal{\mathcal{M}}}_{k}\right|+\left|{\mathcal{R}}_{k}\right|)}^{3}\right). For the standard EKF algorithm, the used measurements are only in set {\mathcal{A}}_{k}, and the complexity is asymptotically \mathcal{O}\left({\left|{\mathcal{A}}_{k}\right|}^{3}\right). Therefore, the complexity of hcEKF is increased {(1+\frac{\left|{\mathcal{\mathcal{M}}}_{k}\right|+\left|{\mathcal{R}}_{k}\right|}{\left|{\mathcal{A}}_{k}\right|})}^{3} times with respect to standard EKF. For example, suppose that at a specific time, there are two RSSI measures from anchors \left|{\mathcal{A}}_{k}\right|=2, one RSSI measure from mobile node \left|{\mathcal{\mathcal{M}}}_{k}\right|=1, and one RFID observation \left|{\mathcal{R}}_{k}\right|=1, the computational complexity of hcEKF is increased about eight times. It is worth reminding that the hcEKF can still localize the mobile node in this case by using the observations from RFID technology.