*5.4. Kalman Filter*

As mentioned above, we decided to use a Kalman filter for the fusion of the two high-level localization subsystems. This choice is motivated by the fact that we have found a simple linear model with Gaussian noise that is sufficient to represent our fusion problem, and the Kalman filter is the optimal solution under these assumptions. It should be noted that the aim of this filter is not the localization itself, but to fuse sources from complex localization algorithms that provide filtered information. For this reason, we can simplify the model and avoid the Extended Kalman filter and its problems derived from linearization. The state vector in this model is a pose vector: **x** = (*x y θ*)*T*. The state transition equations **<sup>x</sup>** ← *<sup>f</sup>*(**x***k*−1, **<sup>u</sup>**, **<sup>w</sup>**) are defined as:

$$
\Delta \mathbf{x}\_k = \mathbf{x}\_{k-1} + \Delta \mathbf{u}\_{\mathbf{x}} + \mathbf{w}\_{\mathbf{x}} \tag{3}
$$

$$y\_k = y\_{k-1} + \Delta u\_y + w\_y \tag{4}$$

$$
\theta\_k = \theta\_{k-1} + \Delta u\_\theta + w\_\theta,\tag{5}
$$

where **u** = (*ux uy u<sup>θ</sup>* )*<sup>T</sup>* is the control signal, which in this case is the odometry generated by our low-level system, being <sup>Δ</sup>**<sup>u</sup>** <sup>=</sup> **<sup>u</sup>***<sup>k</sup>* <sup>−</sup> **<sup>u</sup>***k*−<sup>1</sup> = (Δ*ux* <sup>Δ</sup>*uy* <sup>Δ</sup>*u<sup>θ</sup>* )*T*, where **<sup>w</sup>** = (*wx wy <sup>w</sup><sup>θ</sup>* )*<sup>T</sup>* is the system perturbation noise. As the state is fully observable by the two subsystems described, the observation equation is defined as:

$$\mathbf{y} = \mathbb{H}\mathbf{x}\_k + \boldsymbol{\upsilon}\_\prime \tag{6}$$

where **y** = (*yx yy y<sup>θ</sup>* )*<sup>T</sup>* is the observation vector from the localization subsystems, and where **v** = (*vx vy v<sup>θ</sup>* )*<sup>T</sup>* is the observations noise. As the observations are of the same nature as the state vector, the matrix that relates them is an identity matrix *H* = *I*3*x*3. Using this Kalman filter implementation, we can obtain a high-frequency prediction step which rate is given by the odometry (see (3)–(5)), while applying the filter correction step only when a new high-level observation becomes available. In this way, the positioning rate is sufficient for the control loop. We follow the notation used in [40] where a thorough explanation of Kalman filter equations can be found.
