## 3 Estimation Algorithms Design for Vehicle State Estimation
### Moving Horizon Estimation  
The MHE algorithm is able to solve the estimation problem of constrained linear or nonlinear systems, using a horizon covering the prior measurement information. In the same manner as UKF, MHE is also based on the least-squares objective function. UKF employs sigma point sampling to estimate the covariance matrices within linear update derived from the objective function via the maximum likelihood estimation [34], while MHE solves the objective function as a mathematical  
Figure 2: The schematic of an unscented Kalman filter. programming problem. Additionally, both MHE Sequential Quadratic Programming (SQP) and UKF algorithm, employ second-order estimates at each iteration; however, the SQP solver continues to iterate until the convergence tolerance is satisfied, which leads to a robust means of guaranteeing local optimality without tuning. From the Bayesian theory point of view, the constrained state estimation problem can be formulated as the solution of a full information estimation (FIE) problem [35]. Therefore, the FIE objective function for Equation (22) is written as:  
\[\min_{\mathbf{x}_{\mathbf{0}},\{\mathbf{w}_{k=0}^{T-1}\}}\Phi_{T}\{\mathbf{x}_{\mathbf{0}},\{\mathbf{w }_{k}\}\}=\min_{\mathbf{x}_{\mathbf{0}},\{\mathbf{w}_{k}\}_{k=0}^{T-1}\}\Big{\{}\sum_{k=0} ^{T-1}\parallel\mathbf{w}_{k}\parallel_{\mathbf{Q}^{-1}}^{2}+\sum_{k=0}^{T}\parallel \mathbf{v}_{k}\parallel_{\mathbf{R}^{-1}}^{2}+\parallel\mathbf{x}_{\mathbf{0}}-\widetilde{\mathbf{ x}}_{\mathbf{0}}\parallel_{\mathbf{P}_{\mathbf{0}}^{-1}}^{2}\Big{\}}, \tag{39}\]  
subject to  
\[\mathbf{x}(k+1)=f(\mathbf{x}(k),\;\mathbf{u}(k))+\mathbf{w}(k)\] \[\mathbf{y}(k)=h(\mathbf{x}(k),\;\mathbf{u}(k))+\mathbf{v}(k),\]  
where \(\parallel*\parallel_{A}^{2}=*^{T}\mathbf{A}*\); \(\{\mathbf{w}_{k}\}_{k=0}^{k=T-1}\) is the state disturbance sequence from time \(k=0\) to \(k=T-1\); and \(\widetilde{\mathbf{x}}_{\mathbf{0}}\) is the initial estimate of vehicle states with corresponding covariance matrix \(\mathbf{P}_{\mathbf{0}}\). Additionally, it is worth noting that, in practice, the vehicle speed should be less than \(\max\big{(}\omega_{ij}\big{)}\cdot R\) on driving conditions and greater than \(\min\big{(}\omega_{ij}\big{)}\cdot R\) on braking conditions. However, measurement noise or some other reasons may occasionally result in accidental error, which affects the accuracy of the subsequent estimation. Thus, the following additional constraints for the vehicle longitudinal velocity are given and applied to reduce this effect:  
\[u=\left\{ \begin{array}{ll}\leq\max\big{(}\omega_{ij}\big{)}\cdot R,\;\text{ driving conditions}\\ \geq\min\big{(}\omega_{ij}\big{)}\cdot R,\;\text{ braking conditions}\end{array}  \right.. \tag{40}\]  
However, real-time implementations of FIE are computationally infeasible due to the infinite growth of the number of sampling points. To make the objective function tractable, the estimation size needs to be bounded. One common strategy to estimate the \(\{\mathbf{x}_{\mathbf{k}},\mathbf{w}_{k}\}_{k=T-N}^{T-1}\) instead of the full-state sequence \(\{\mathbf{x}_{\mathbf{k}},\mathbf{w}_{\mathbf{k}}\}_{k=0}^{T-1}\), where \(N\) is the fixed time horizon of Equation (39), just like a moving \"sampling window\". Therefore, Equation (39) is rewritten as,  
\[\begin{array}{ll}\min_{\mathbf{x}_{T-N},\{\mathbf{w}_{k}\}_{k=T-N}^{T-1}}\Phi_{T}\{ \mathbf{x}_{T-N},\{\mathbf{w}_{k}\}\}=\parallel\mathbf{x}_{\mathbf{0}}-\hat{\mathbf{x}}_{\mathbf{0}} \parallel_{\mathbf{p}_{\mathbf{0}}^{-1}}^{2}+\sum_{k=0}^{T-N-1}\parallel\mathbf{w}_{\mathbf{k }}\parallel_{\mathbf{Q}^{-1}}^{2}+\sum_{k=0}^{T-N-where \(p=\frac{2\lambda-1}{2(L+\lambda)\left(S_{r}-(2L+1)\left(\sqrt{L+\lambda}\right) \right)}\), \(q=\frac{1}{2(L+\lambda)}-\frac{2\lambda-1}{2(L+\lambda)\left(S_{r}-(2L+1)\left( \sqrt{L+\lambda}\right)\right)}\) and \(S_{r}=\sum_{i=1}^{2L}r_{i}\). \(r_{i}\) is the step size in each direction of the set of sigma points with constraints, which is calculated as follows [36]:  
\[