# Precise Positioning of Autonomous Vehicles Combining UWB Ranging Estimations with On-Board Sensors
## 4 Design of the Outdoor Positioning System
### Proposed Extended Kalman Filter  
The proposed Extended Kalman Filter makes use of the travelled distance and the orientation of the RC car provided by the odometry and IMU, respectively. These data are used to periodically update its position during the prediction phase. This position is corrected over time in the correction phase by using UWB ranging estimates. Therefore, the RC car position is the only navigation state included in the state vector \(\mathbf{\Theta}\). \[\mathbf{\Theta}_{k}= \begin{bmatrix}x_{k}\\ y_{k}\end{bmatrix} . \tag{2}\]  
Each time the algorithm receives new data from the on-board sensors of the RC car, the a priori state vector \(\mathbf{\hat{\Theta}}\) is updated by using the following equation:  
Figure 2: Architecture of the positioning system developed. \[\mathbf{\tilde{\Theta}}_{k}=\mathbf{A}\mathbf{\hat{\Theta}}_{k-1}+\mathbf{B}\mathbf{u }_{k}. \tag{3}\]  
where \(\mathbf{\tilde{\Theta}}_{k}\) is the new a priori state at timestamp \(k\), \(\mathbf{\hat{\Theta}}_{k-1}\) is the previous a posteriori state at timestamp \(k\)-1 and \(\mathbf{u}_{k}\) is the input vector that includes the travelled distance by the RC car in the \(x\) and \(y\) directions between timestamps \(k\)-1 and \(k\). During the prediction phase of the algorithm, the a priori error covariance matrix \(\mathbf{\hat{P}}_{k}\) is also computed based on the a posteriori error covariance matrix \(\mathbf{\hat{P}}_{k-1}\) and the process noise covariance matrix \(\mathbf{Q}\). This matrix includes the noise from both the odometry and IMU data. Note that, if no new UWB ranging estimates are available after the prediction phase, the computed a priori estimate is used as the previous a posteriori estimate (\(\mathbf{\hat{\Theta}}_{k-1}\mathbf{\hat{P}}_{k-1}\)) during the next iteration of the EKF. The computed a priori estimation is corrected over time in the correction phase by making use of the UWB ranging estimates. This phase is applied each time a new UWB ranging estimate arrives into the system after post-processing it. The main idea behind post-processing the UWB ranging estimates is to discard Non Line-Of-Sight (NLOS) measurements that may compromise the accuracy and robustness of the proposed solution. The developed algorithm for post-processing the UWB ranging estimates is described in depth in Section 4.2. Therefore, if an UWB ranging estimate is classified as an outlier, it is discarded and the correction phase is not applied. In this situation, the a priori estimate of the EKF is used as the previous a posteriori estimate in the next iteration of the EKF. Otherwise, this measurement is employed for correcting the a priori information. For this, the measurement estimation \(h(\mathbf{\hat{\Theta}}_{k})\) has to be computed. This function relates the a priori state \(\mathbf{\hat{\Theta}}_{k}\) with the UWB ranging estimate by calculating the euclidean distance between the position of the UWB tag node located in the RC car and one of the anchors that takes place in the UWB ranging estimate. Note that, the position of the anchor is well-known. Hence, the position of the tag by knowing its relative position with regard to the RC car position is computed. Due to the fact that Equation (4) is not linear, an EKF has been considered. \[h(\mathbf{\hat{\Theta}}_{k})=\sqrt{(x_{tag_{k}}-x_{an})^{2}+(y_{tag_{k}}-y_{an })^{2}}. \tag{4}\]  
Once the function \(h(\mathbf{\hat{\Theta}}_{k})\) has been computed, the Jacobian matrix \(\mathbf{H}_{k}\) can be directly derived as:  
\[\mathbf{H}_{k}=\frac{\delta(h(\mathbf{\hat{\Theta}}_{k}))}{\delta\mathbf{ \Theta}_{k}}=\left[(x_{tag_{k}}-x_{an})/h\quad(y_{tag_{k}}-y_{an})/h\right]. \tag{5}\]  
The matrix \(\mathbf{H}_{k}\) allows to compare the a priori error covariance matrix \(\mathbf{\tilde{P}}_{k}\) with the measurement noise covariance matrix \(\mathbf{R}\), which represents the error of the UWB ranging estimates. By making use of these two sources of information the algorithm is able to compute the matrix \(\mathbf{K}_{k}\), also known as the Kalman gain.