## 3 Path Tracking Control Based on States Estimation
### Patch Tracking Control Based on State Estimation
For a random vector \(\mathbf{y}\), which is the nonlinear transformation of an N-dimensional independent variable \(\mathbf{x}\) by \(f(\mathbf{x})\), we take \(\left.\mathbf{P}_{x}\right|_{N\times N}\) as the covariance matrix of \(\mathbf{x}\), and \(\overline{x}\) as the mean of \(\mathbf{x}\); to obtain the statistics of \(\mathbf{y}\), a sigma-point vector matrix \(\mathbf{x}\) is required and is formed by using sigma-point sampling method:  
\[\mathbf{x}=[\overline{x}\mid\ _{i=0}\qquad\overline{x}+(\sqrt{(N+\lambda) \mathbf{P}_{x}})_{i}\bigg{|}\ _{i=1,\cdots,N}\qquad\qquad\overline{x}-(\sqrt{(N+\lambda)\mathbf{P}_{x}})_{i }\big{|}_{i=N+1,\cdots,2N}\ ] \tag{11}\]  
where \(\lambda=\alpha^{2}(N+\kappa)-N\) is the scale parameter, in which \(\alpha\) decides the diffusion range of the sigma-points and is a small positive constant; \(\kappa\) is a sub-scale parameter taking a value of 0 in general state estimation and 3-N in parameter estimation; \((\sqrt{(N+\lambda)\mathbf{P}_{x}})_{i}\) is the i-th column of the weighted square root matrix \(\sqrt{(N+\lambda)\mathbf{P}_{x}}\). In normal cases, \(\mathbf{P}_{x}\) is a positive definite symmetric matrix, and to figure out \(\sqrt{(N+\lambda)\mathbf{P}_{x}}\), the eigenvalue decomposition of \(\mathbf{P}_{x}\) is taken. Suppose \(\mathbf{P}_{x}=\mathbf{Q}\Lambda\mathbf{Q}^{-1}\) (where \(\mathbf{Q}\) is a N \(\times\) N matrix with the eigenvectors of \(\mathbf{P}_{x}\) as its columns; \(\mathbf{\Lambda}\) is the diagonal matrix with the corresponding eigenvalues as its diagonal elements, \(\mathbf{P}_{x}^{1/2}=\mathbf{Q}\Lambda^{1/2}\mathbf{Q}^{-1}\)). On the other hand, for a positive definite symmetric matrix, its eigenvalue decomposition is equivalent to its SVD decomposition. Suppose \(\mathbf{P}_{x}=\mathbf{U}\Lambda\mathbf{V}^{T}\) (where \(\mathbf{U}=\mathbf{V}=\mathbf{Q}\quad,\quad\mathbf{Q}^{T}\mathbf{Q}=\mathbf{I}\), and \(\mathbf{\Lambda}\) is the same as just introduced above), therefore \(\mathbf{P}_{x}^{1/2}=\mathbf{U}\Lambda^{1/2}\mathbf{V}^{T}\). Notice that \(\sqrt{(N+\lambda)\mathbf{P}_{x}}\) stands with the precondition that \(\mathbf{P}_{x}\) is a positive definite symmetric matrix. However, a morbid covariance matrix is often encountered in the implementation of UKF, i.e., the covariance matrix fails to be positive symmetric during filtering. To solve this, SVD decomposition is always taken instead of eigenvalue decomposition in the Unscented Transform, which is called SVD-Unscented Kalman Filtering. The procedure of state estimation based on SVD-Unscented Kalman Filtering is as follows:  
* Initialization \[\overline{x}_{0}=\text{E}[x_{0}];\ \mathbf{P}_{0}=\text{E}[(x_{0}- \overline{x}_{0})(x_{0}-\overline{x}_{0})^{T}]\]\(\overline{x}_{0}\) is the initial mean value of \(x\), while \(\textbf{P}_{0}\) is the initial covariance matrix of \(x\). We construct the augmented matrix about \(\overline{x}_{0}\) with the mean value of the process noise and measurement noise, which are both zero initially, and the augmented matrix about \(\textbf{P}_{0}\):  
\[ \begin{array}{cccc}\textbf{x}^{a}&=&-\\ \textbf{P}_{0}^{a}&=&diag(\textbf{P}_{0},&\textbf{P}_{\varepsilon},&\textbf{P} _{\delta})\end{array} \]  
where the superscript 'a' means 'augmented'. \(\textbf{P}_{\varepsilon}\) and \(\textbf{P}_{\delta}\) are the covariance of process noise \(\varepsilon\) and measurement noise \(\delta\), respectively. * We take a set of sigma-points and form the vector matrix \(\textbf{x}_{k}\) by using Equation (11) and \(\textbf{P}_{x}^{1/2}=\textbf{U}\textbf{A}^{1/2}\textbf{V}^{T}\). * Time updating We produce samples \(\textbf{x}^{\star}_{k+1|k}=f(\textbf{x}_{k},\textbf{u}_{k})\) through function \(f(x)\), obtaining the new augmented matrices: \[