## 3 Mathematical Foundations
### Non-Uniform Dual-Rate Extended Kalman Filter (NUDREKF)
#### 3.2.2 Formulation of the NUDREKF  
The previous reasoning can be applied to the well-known Extended Kalman Filter equations [19]:  
\[ \begin{split}\xi_{k+1}^{T}&=f\Big{(}\xi_{k}^{T} \Big{)}+h\Big{(}\xi_{k}^{T}\Big{)}\cdot u_{k}^{T}\\ z_{k}^{T}&=g\Big{(}\xi_{k}^{T}\Big{)}+\eta_{k}^{T}\end{split}  \tag{14}\]  
where \(\xi_{k}^{T}\) is the vehicle state, which is composed of \(\big{[}(V_{x},V_{y},W_{\psi},X,Y,\psi)_{k}^{T}\big{]}^{\top}\); the control signal is \(u_{k}^{T}=\big{[}(\omega_{1},\omega_{2},\omega_{3},\omega_{4})_{k}^{T}\big{]}^{\top}\); \(\eta_{k}^{T}\) is a possible measurement noise, which is assumed to be zero mean multivariate Gaussian noise with covariance \(R_{k}^{T}\); and the output is \(z_{k}^{T}=\big{[}(V_{x},V_{y},W_{\psi},\psi,\gamma_{1}\cdot d_{1},\gamma_{2} \cdot d_{2},\gamma_{3}\cdot d_{3},\gamma_{4}\cdot d_{4})_{k}^{T}\big{]}^{\top}\), being \(d_{i}\
eq 0,\;(i=1\ldots 4)\) at times \(k=NT\) and \(\gamma_{i}=1\) if no measurement loss is produced (otherwise, \(\gamma_{i}=0\)). The nonlinear functions \(f\) and \(g\) can be linearized as follows:  
\[ \begin{split} f\Big{(}\xi_{k}^{T}\Big{)}&\approx f \Big{(}\xi_{k}^{T}\Big{)}+\frac{\partial f}{\partial\xi_{k}^{T}}\bigg{|}_{\xi_{k }^{T}}\cdot\Big{(}\xi_{k}^{T}-\xi_{k}^{T}\Big{)}\\ g\Big{(}\xi_{k}^{T}\Big{)}&\approx g\Big{(}\xi_{k |k-1}^{T}\Big{)}+\frac{\partial g}{\partial\xi_{k}^{T}}\bigg{|}_{\xi_{k-1}^{T}} \cdot\Big{(}\xi_{k}^{T}-\xi_{k|k-1}^{T}\Big{)}\end{split}  \tag{15}\]where, as usual, the notation \(\widehat{\xi}_{k|k-1}^{T}\) means the state estimated for the instant \(k\) from the instant \(k-1\). If the previous nonuniform multirate modeling is applied to the linearized model, it yields  
\[f_{MR}\left(\widehat{\xi}_{k}^{T}\right)=\left[ \begin{matrix}f\left(\widehat{ \xi}_{k}^{T}\right)\\ \left(\Delta^{y}\right)_{k}^{T}\cdot g\left(\widehat{\xi}_{k}^{T}\right)+(I- \left(\Delta^{y}\right)_{k}^{T})\cdot\left(\
u^{y}\right)_{k}^{T}\end{matrix} \right]\]  
\[\frac{\partial f_{MR}}{\partial\xi}\bigg{|}_{\widehat{\xi}_{k}^{T}}=\left[ \begin{matrix}\frac{\partial f}{\partial\xi}&0\\ \left(\Delta^{y}\right)_{k}^{T}\cdot\frac{\partial g}{\partial\xi}&I-\left( \Delta^{y}\right)_{k}^{T}\end{matrix} \right] \tag{16}\]  
\[h_{MR}\left(\widehat{\xi}_{k}^{T}\right)=\left[ \begin{matrix}h\left(\xi_{k}^{ T}\right)\\ 0\end{matrix} \right]\]  
\[g_{MR}\left(\widehat{\xi}_{k}^{T}\right)=\left[\left(\Delta^{y}\right)_{k}^{ T}\cdot g\left(\widehat{\xi}_{k}^{T}\right)+(I-\left(\Delta^{y}\right)_{k}^{T}) \cdot\left(\
u^{y}\right)_{k}^{T}\right]\]  
\[\left.\frac{\partial g_{MR}}{\partial\xi}\bigg{|}_{\widehat{\xi}_{k}^{T}}= \left[\left(\Delta^{y}\right)_{k}^{T}\cdot\frac{\partial g}{\partial\xi} \bigg{|}_{\widehat{\xi}_{k}^{T}}\quad I-\left(\Delta^{y}\right)_{k}^{T}\right]\]  
Then, the NUDREKF is formulated as follows:  
* Initialization: \[ \begin{split} P_{0|0}^{T}&=\left[\begin{matrix}Cov( \xi_{0}^{T})\\ 0\end{matrix}\right]\\ \widehat{\xi}_{0|0}^{T}&=\left[\begin{matrix}\xi_{0}^{T} \\ \
u^{y}\end{matrix}\right]_{0|0}^{T}=\left[\begin{matrix}E(\xi_{0}^{T})\\ 0\end{matrix}\right]\end{split} \] (17) where \(Cov(\cdot)\) denotes covariance, and \(E(\cdot)\) denotes the expectation. * Algorithm \(\forall k\geq 1\): \[