# Robust Estimation of Vehicle Motion States Utilizing an Extended Set-Membership Filter
## 3 Extended Set-Membership Filter  
The ESMF is an algorithm that relies on unknown but bounded noise. This assumption for noise distribution can be generally satisfied in practical application, which makes the ESMF insensitive to the changes of model and measurement errors. Therefore, it can solve the defect that traditional filtering algorithms obtain the best estimation only with prior knowledge of system noise. The result of the set-membership estimation is a set of feasible solutions, and it can be guaranteed to include the real states. Generally, the center of the ellipsoid set is selected as the estimated value of each time step to make comparison with the real value. Consider the following discrete form of the state space expressed in Equation (3):  
\[ \begin{split} x_{k+1}&=f(\mathbf{x}_{k})+\mathbf{w}_{k}\\ z_{k+1}&=h(\mathbf{x}_{k+1})+\mathbf{v}_{k+1}\end{split}  \tag{6}\]  
where \(f()\) and \(h()\) are the process and measurement functions of the discrete system, \(\mathbf{x}_{k}\) is the \(k\)th system state vector, \(\mathbf{z}_{k+1}\) is the \((k+1)\)th system measurement vector, \(\mathbf{w}_{k}\) and \(\mathbf{v}_{k+1}\) are the \(k\)th system process and \((k+1)\)th measurement noises, respectively, which are assumed to be limited by the following ellipsoid sets:  
Figure 2: Block diagram of robust estimation strategy. \[ \begin{split} W_{k}&=\{\mathbf{w}_{k}:\mathbf{w}_{k}^{\mathsf{T}} \mathbf{Q}_{k}^{-1}\mathbf{w}_{k}\leq 1\}\\ V_{k+1}&=\{\mathbf{v}_{k+1}:\mathbf{v}_{k+1}^{\mathsf{T}} \mathbf{R}_{k+1}^{-1}\mathbf{v}_{k+1}\leq 1\}\end{split}  \tag{7}\]  
where \(\mathbf{Q}=\mathbf{Q}^{\mathsf{T}}\) and \(\mathbf{R}=\mathbf{R}^{\mathsf{T}}\) are the known and positive definite matrices representing the shape of the ellipsoid sets of the process and measurement noises. Furthermore, the initial state \(\mathbf{x}_{0}\) belongs to the following ellipsoid set:  
\[(\mathbf{x}_{0}-\mathbf{\hat{x}}_{0})^{\mathsf{T}}\mathbf{P}_{0}^{-1}(\mathbf{x}_{0}-\mathbf{\hat{ x}}_{0})\leq 1 \tag{8}\]  
where \(\mathbf{\hat{x}}_{0}\) is the ellipsoid center assumed to be known, \(\mathbf{P}_{0}=\mathbf{P}_{0}^{\mathsf{T}}\) is the known and positive definite matrix which denotes the shape of the initial ellipsoid set. In the ESMF algorithm, the result of the time update, called one-step prediction \(E_{k+1}^{-}\), is an ellipsoid set containing the vector sum of the state transition set and process noise set:  
\[\mathbf{E}_{k+1}^{-}=\{\mathbf{x}_{k+1}^{-}:(\mathbf{x}_{k+1}^{-}-\mathbf{\hat{x}}_{k+1}^{-})^ {\mathsf{T}}(\mathbf{P}_{k+1}^{-})^{-1}(\mathbf{x}_{k+1}^{-}-\mathbf{\hat{x}}_{k+1}^{-}) \leq 1\} \tag{9}\]  
where \(\mathbf{x}_{k+1}^{-}\) is the ellipsoid center, \(\mathbf{P}_{k+1}^{-}\) is the matrix that defines its shape, and it should be noted that \(\mathbf{E}_{k+1}^{-}\) is the prior estimated ellipsoid set obtained before the utilization of the measurement vector \(\mathbf{z}_{k+1}\). The result of the measurement update is an ellipsoid set which contains the intersection of the one-step prediction set and measurement set:  
\[\mathbf{E}_{k+1}^{+}=\{\mathbf{x}_{k+1}^{+}:(\mathbf{x}_{k+1}^{+}-\mathbf{\hat{x}}_{k+1}^{+})^ {\mathsf{T}}(\mathbf{P}_{k+1}^{+})^{-1}(\mathbf{x}_{k+1}^{+}-\mathbf{\hat{x}}_{k+1}^{+}) \leq 1\} \tag{10}\]  
where \(\mathbf{x}_{k+1}^{+}\) and \(\mathbf{P}_{k+1}^{+}\) are the posterior estimations obtained after utilizing the measurement \(\mathbf{z}_{k+1}\). On the assumption that \(\mathbf{F}\) and \(\mathbf{H}\) are the Jacobian matrices of the discrete system, the specific steps for the ESMF algorithm are as follows:  
**Time update:**  
1. Determine the center of the one-step prediction ellipsoid set: \[\mathbf{\hat{x}}_{k+1}^{-}=f(\mathbf{\hat{x}}_{k}^{+})\] (11)
2. Update the matrix that defines the shape of the one-step prediction ellipsoid set: \[\mathbf{P}_{k+1}^{-}=\bigg{(}1+\frac{1}{\gamma_{k+1}}\bigg{)}\mathbf{F}_{k}\mathbf{P}_{k} ^{+}\mathbf{F}_{k}^{\mathsf{T}}+(1+\gamma_{k+1})\mathbf{Q}\] (12)  
**Measurement update:**  
1.