# Interval Extended Kalman Filter--Application to Underwater Localization and Control
## 4 Test-Cases  
In this section, we propose to compare the three localization methods presented in Section 3. For this, we consider an autonomous underwater vehicle (AUV), the _Riptide_, represented by Figure 2. For its locomotion, this robot uses three fins and one propeller. The inputs are \(u_{0}\), the rotation speed of the propeller and the angles \(u_{1},u_{2},u_{3}\) of the fins. We assume that we have no _side slip_ effect. This means that the velocity vector \(\mathbf{v}_{r}\) has always the direction of the robot, or equivalently, the robot has no lateral speed. For each vector \(\mathbf{w}=(w_{x},w_{y},w_{z})\), we may associate the skew-symmetric matrix:  
\[\mathbf{w}\wedge=\left( \begin{array}{ccc}0&-w_{z}&w_{y}\\ w_{z}&0&-w_{x}\\ -w_{y}&w_{x}&0\end{array} \right)\]  
Figure 2: The Riptide robot has one propeller and three fins  
The state space equation of the robot is given by [30] with an adaptation in the orientation variables:  
\[ \begin{cases}\hat{\mathbf{p}}&=\mathbf{R}\cdot(v_{r}\,0\,0)^{T}\\ \hat{\mathbf{R}}&=\mathbf{R}\cdot(\mathbf{w}_{r}\wedge)\\ \hat{v}_{r}&=q_{1}\cdot u_{0}^{2}-q_{2}\cdot v_{r}\cdot|v_{r}|\\ \mathbf{w}_{r}&=v_{r}\cdot\mathbf{B}\cdot(u_{1}\,u_{2}\,u_{3})^{T}\end{cases}  \tag{4}\]  
The Euler angles have been replaced by a rotation matrix to remove the Gimbal lock for the controller. In this model:  
* \(q_{1}\) and \(q_{2}\) are known parameters. \(q_{1}\) represents the effect of the propeller's speed on the acceleration while \(q_{2}\) represents the water friction. * \(\mathbf{B}\) is the known repartition matrix. * \(\mathbf{p}=(p_{x},p_{y},p_{z})^{T}\) is the position of the robot. * \(\mathbf{R}\) is the orientation matrix. * \(v_{r}\) is its longitudinal speed (lateral speed are supposed to be null). * \(\mathbf{w}_{r}\) is its angular speed vector. This robot is controlled in speed and orientation with the following controller:  
\[ \begin{cases}u_{0}&=\sqrt{\frac{q_{2}}{q_{1}}}v_{d}\\ \left(\begin{array}{c}u_{1}\\ u_{2}\\ u_{3}\end{array}\right)&=\frac{1}{\hat{v}_{r}}\mathbf{B}^{-1}\mathbf{R}^{T} \cdot\text{Log}(\mathbf{R}_{d}\cdot\mathbf{R}^{T})\end{cases}  \tag{5}\]  
where Log is the capitalized log map used in Lie groups theory [31]. In this controller, \(v_{d}\) is the desired speed, the matrix \(\mathbf{R}_{d}\) is the desired orientation, and \(\hat{v}_{r}\) is the estimated speed. While \(v_{d}\) is constant, a planner computes \(\mathbf{R}_{d}\) to make the robot follow a series of connected lines. When the drone follows the line from the point \(\mathbf{a}_{j}\) to the point \(\mathbf{a}_{j+1}\) with the unit vector  
which corresponds to a selection of the equations of the complete model (4) that are needed for the localization. In this model, the state vector is \(\mathbf{x}=(p_{x},p_{y},v_{r})^{T}\), the input is the propeller speed \(u_{0}\). \(R_{11}\) and \(R_{21}\) are the two first elements of the first column of \(\mathbf{R}\). The observation equation given by  
\[\mathbf{y}=\mathbf{g}(\mathbf{x})+\boldsymbol{\beta}=\left( \begin{array}{c}p _{z}\\ \left\|\mathbf{p}-\mathbf{m}_{1}\right\|\\ \left\|\mathbf{p}-\mathbf{m}_{2}\right\|\end{array} \right)+\boldsymbol{\beta} \tag{8}\]  
corresponds to a situation where the robot measures its depth \(p_{z}\) and the Euclidean distances to two fixed markers \(\mathbf{m}_{1},\mathbf{m}_{2}\). The measurement noise is denoted by \(\boldsymbol{\beta}\). We assume that the orientation matrix \(\mathbf{R}\) is measured with a high accuracy. Let us discretize the state equations with an Euler method:  
\[ \begin{cases}\mathbf{x}_{k+1}&=\mathbf{x}_{k}+dt\cdot\mathbf{f}_{c}(\mathbf{x }_{k},\mathbf{u}_{k})=\mathbf{f}_{k}(\mathbf{x}_{k},\mathbf{u}_{k})\\ \mathbf{y}_{k}&=\mathbf{g}(\mathbf{x}_{k})+\boldsymbol{\beta}_{k}=\mathbf{g}_ {k}(\mathbf{x}_{k})+\boldsymbol{\beta}_{k}\end{cases}  \tag{9}\]  
with the sampling time \(dt=0.1s\) which has the same form than Equation (1) without process noise. That discretization will be used by the extended Kalman filters.