## 3 Robust Extended Kalman Filter
### Robust Extended Kalman Filter Model  
The state space representation for the extended Kalman filter model is defined by two equations as follows:  
State Equation :  
\[X_{k}=f\left(X_{k-1}\right)+\epsilon_{k} \tag{11}\]  
Observation Equation :  
\[Z_{k}=h\left(X_{k}\right)+e_{k} \tag{12}\]  
where \(X_{k}\) is the state vector and \(Z_{k}\) is the measurement vector at time \(t_{k}\); \(\epsilon_{k}\) is the vector that conveys the system error sources; \(e_{k}\) is the vector that represents the measurement error sources; \(f()\) is the function for state transition and \(h()\) is the function for the measurement. IRWLS works on linear regression to handle the problem of outliers in the data. Therefore, converting a non-linear model into a linear model is necessary to apply the robust-EKF that consists in three steps: Linearizing equations, reforming filter and updating. _Step 1_: _Linearizing equations_  
The linearized state in Equation (11) is described as:  
\[A_{k}=\frac{\partial f\left(X_{k-1}\right)}{\partial X} \tag{13}\]  
where \(A_{k}\) is the Jacobian of the process model. The predicted state is determined by:  
\[X_{k}^{-}=A_{k}X_{k-1} \tag{14}\]  
The relation between the true state \(X_{K}\) and its prediction \(\left(X_{k}^{-}\right)\) can be written as:  
\[X_{k}^{-}=X_{k}-\delta_{k} \tag{15}\]  
where \(\delta_{k}\) is the error between the true state and its prediction. Linearizing Equation (12) using the first order Taylor series expansion with the predicted state vector \(\left(X_{k}^{-}\right)\):\[Z_{k}=h\left(X_{k}^{-}\right)+\left.\frac{\partial h}{\partial X}\right|_{X=X_{k}^ {-}}\left(X_{k}-X_{k}^{-}\right)+e_{k} \tag{16}\]  
By noting: \(\left.\frac{\partial h}{\partial X}\right|_{X=X_{k}^{-}}=H_{k}\), Equation (16) can be written as:  
\[Z_{k}=h\left(X_{k}^{-}\right)+H_{k}\left(X_{k}-X_{k}^{-}\right)+e_{k} \tag{17}\]  
Or  
\[Z_{k}-h\left(X_{k}^{-}\right)+H_{k}X_{k}^{-}=H_{k}X_{k}+e_{k} \tag{18}\]  
_Step 2_: _Reforming the filter_  
This step will reform the filter to a regression equation. To perform this, combining Equations (15) and (18) together in a matrix form yields the following:  
\[\left[ \begin{array}{c}X_{k}^{-}\\ Z_{k}-h\left(X_{k}^{-}\right)+H_{k}X_{k}^{-}\end{array} \right]=\left[ \begin{array} []{c}I\\ H_{k}\end{array} \right]X_{k}+\left[ \begin{array}{c}-\delta_{k}\\ e_{k}\end{array} \right] \tag{19}\]  
where \(I\) is the identity matrix. Equation (19) is expressed in a compact form as:  
\[\hat{Z}_{k}=\tilde{H}_{k}X_{k}+\xi_{k} \tag{20}\]  
The covariance matrix of \(\xi_{k}\) is given by:  
\[\tilde{R}_{k}=\left[ \begin{array}{cc}P_{k}^{-}&0\\ 0&R_{k}\end{array} \right]=L_{k}L_{k}^{T} \tag{21}\]  
where \(R_{k}=E\left(e_{k}\epsilon_{k}^{T}\right)\) is the measurement covariance matrix and \(P_{k}^{-}=E\left[\delta_{k}\delta_{k}^{T}\right]\) is the covariance matrix of the predicted error. \(P_{k}^{-}\) is determined by:  
\[P_{k}^{-}=A_{k}P_{k-1}A_{k}^{T}+Q_{k} \tag{22}\]  
where \(Q_{k}\) is the covariance matrix of the process. \(L_{k}\) in Equation (21) is calculated using Cholesky decomposition. To perform pre-whitening, Equation (20) is multiplyed by \(L_{k}^{-1}\) and by defining: \(y_{k}=L_{k}^{-1}\hat{Z}_{k}\), \(G_{k}=L_{k}^{-1}\hat{H}_{k}\), \(\zeta_{k}=L_{k}^{-1}\xi_{k}\). Equation (20) becomes the regression equation written as:  
\[y_{k}=G_{k}X_{k}+\zeta_{k} \tag{23}\]  
_Step 3_: _Updating_  
This step consists in updating the estimated state and checking the position accuracy using RAIM algorithm presented in section \"RAIM algorithm\" (Section 3.4). The estimated state \(\hat{X}_{k}\) in Equation (23) is optimized by the IRWLS algorithm presented in section \"Iterative Reweighted Least Squares Algorithm\" (Section 3.3). The final state estimate \(\hat{X}_{k}\) is given by:  
\[\hat{X}_{k}=\left(G_{k}^{T}W_{k}G_{k}\right)^{-1}G_{k}^{T}W_{k}y_{k} \tag{24}\]  
where \(W_{k}\) is the diagonal weight matrix at time \(t_{k}\).