# Loosely Coupled GNSS and UWB with INS Integration for Indoor/Outdoor Pedestrian Navigation +
## 3 Material and Methods
### Methods
The EKF measurement model for GNSS/UWB-INS integration is:  
\[\delta\mathbf{y}_{k}=\mathbf{H}_{k}\delta\mathbf{x}_{k}+\mathbf{v}_{k} \tag{11}\]  
In the loosely coupled integration, this measurements vector is formed as the difference between GNSS/UWB and INS position and velocity (\(\delta\mathbf{y}_{k}=\ \delta\mathbf{x}_{INS}-\delta\mathbf{x}_{GNSS/UWB}\)) and the \(\mathbf{H}\) matrix, that relates the observations to the state vector \(\delta\mathbf{x}_{k}\), can be defined as:  
\[\delta\dot{\mathbf{p}}=\mathbf{T}^{\prime}\delta\mathbf{p}^{n}+T\delta\mathbf{v}^{n} \tag{3}\]0  
Finally, it is possible to estimate the measurement noise vector \(\mathbf{v}_{k}\), represented by its var-covariance matrix \(\mathbf{R}_{k}\) defined a priori. This measurement model is used to update the state vector and its var-covariance matrix. Therefore, the Kalman gain matrix is required and can be estimated using the following equation:  
\[\delta\dot{\mathbf{p}}=\mathbf{T}^{\prime}\delta\mathbf{p}^{n}+T\delta\mathbf{v}^{n} \tag{3}\]1  
Finally, the pre-GNSS/UWB measurement estimates of the state error \(\delta\mathbf{x}_{k}\) and the correspondent \(\mathbf{P}_{k|k}\) matrix are refined with equation:  
\[\delta\dot{\mathbf{p}}=\mathbf{T}^{\prime}\delta\mathbf{p}^{n}+T\delta\mathbf{v}^{n} \tag{3}\]2  
\[\delta\dot{\mathbf{p}}=\mathbf{T}^{\prime}\delta\mathbf{p}^{n}+T\delta\mathbf{v}^{n} \tag{3}\]3  
Then, the INS parameters are corrected for the estimated state error vector:  
\[\delta\dot{\mathbf{p}}=\mathbf{T}^{\prime}\delta\mathbf{p}^{n}+T\delta\mathbf{v}^{n} \tag{3}\]4  
The GNSS receiver and the Pozyx GNSS inertial sensor unit are the systems integrated in a loosely coupled algorithm that represents the starting navigation solution for the outdoor space. The GNSS receiver provide position at 1 Hz, while the IMU works at 4 Hz. This high rate is fundamental to fill the gap between two subsequent GNSS measurements. The GNSS position and velocity are used to estimate the INS error. When the systems move indoor the GNSS receiver is no more able to provide measurements so it has been substituted by the UWB receiver that enters in the UWB network of fixed anchor nodes. Also, the UWB can work at 1 Hz providing positioning in the indoor environment. The previous loosely coupled integration is used in this case simply using the UWB data to update the INS navigation. In the proposed work the 3D localization problem of the UWB positioning is solved with an iterative non-linear least square estimation. This method is applied iteratively in order to refine the linearization point which is obtained when the error \(r\) is lower than the preset tolerance. If \(m\) is the number of anchors in the network, the \(i_{\text{th}}\) residuals can be written as a function of the coordinates of the target node:  
\[\delta\dot{\mathbf{p}}=\mathbf{T}^{\prime}\delta\mathbf{p}^{n}+T\delta\mathbf{v}^{n} \tag{3}\]5  
with \(d_{i}\) the distance between two nodes, \((\mathbf{x},y,z)^{T}\) the coordinates of the target node and \((\mathbf{x}_{i},y_{i},z_{i})^{T}\) the known position of the \(i_{\text{th}}\) anchor. At each step of the iteration, starting from an initial guess \(\mathbf{x}_{0}\) for the position of the target node, the algorithm returns a value \(\mathbf{x}_{k+1}\) that minimizes the sum of the residuals \(S\):  
\[\delta\dot{\mathbf{p}}=\mathbf{T}^{\prime}\delta\mathbf{p}^{n}+T\delta\mathbf{v}^{n} \tag{3}\]6  
the estimation value is given by:  
\[\delta\dot{\mathbf{p}}=\mathbf{T}^{\prime}\delta\mathbf{p}^{n}+T\delta\mathbf{v}^{n} \tag{3}\]7  
where \(\mathbf{J}\) is the Jacobian matrix of the residual vector function.