# Trajectory-Tracking Control of Unmanned Vehicles Based on Adaptive Variable Parameter MPC
## 2 Unmanned Vehicle Trajectory-Tracking Model
### Lateral Force Estimation  
In order to obtain real-time front and rear tire lateral forces, we designed a lateral force estimator based on SCKF. SCKF is the square root form of CKF, which can guarantee symmetry and a positive semi-qualitative covariance matrix. Compared with the EKF and UKF algorithms, the SCKF algorithm has higher estimation accuracy and filtering stability. It is assumed that the variation of lateral forces in each tire during vehicle travel is consistent with a Markov process, and the variation is described using a second-order stochastic wandering model. \[ \begin{pmatrix}\dot{F}_{0}\\ \dot{F}_{1}\end{pmatrix} = \begin{pmatrix}0&1\\ 0&0\end{pmatrix}\begin{pmatrix}F_{0}\\ F_{1}\end{pmatrix} +w_{F} \tag{9}\]  
where \(F_{0}\) is the tire force to be estimated; \(F_{1}\) is the first-order derivative of \(F_{0}\); \(w_{F}\) is random noise. If the state variable is defined to be \(x= \begin{bmatrix}\dot{\varphi},F_{yf},\dot{F}_{yf},F_{yr},\dot{F}_{yr}\end{bmatrix} ^ {T}\) and the measurement variable is defined to be \(z= \begin{bmatrix}a_{y},\dot{\varphi}\end{bmatrix} ^{T}\), combining Equations (1) and (9), the state-space equation in discrete form can be expressed as follows:  
\[\left\{ \begin{array}{l}x_{k}=f(x_{k-1})+\omega_{k-1}\\ z_{k}=h(x_{k})+v_{k}\end{array} \right. \tag{10}\]  
where \(f(\cdot)\) is the state transfer equation; \(h(\cdot)\) is the measurement equation; \(\omega_{k-1}\) is the process noise with covariance \(Q_{k-1}\); \(v_{k}\) is the measurement noise with covariance \(R_{k}\). Using the first-order Euler equation with a sampling time of \(T\) to approximate the discretization of the system, the state transfer equation and the measurement equation can be expressed as, respectively:  
\[f(x_{k-1})= \begin{bmatrix}x_{1,k-1}+\frac{1}{l_{x}}(l_{f}x_{2,k-1}-l_{r}x_{4, k-1})T\\ x_{2,k-1}+x_{3,k-1}T\\ x_{3,k-1}\\ x_{4,k-1}+x_{5,k-1}T\\ x_{5,k-1}\end{bmatrix} \;h(x_{k})= \begin{bmatrix}\frac{x_{2,k}+x_{4,k}+h\dot{ \varphi}-g\dot{\varphi}}{m}\\ x_{1,k}\end{bmatrix}  \tag{11}\]  
Figure 4: Effect of road surface adhesion coefficient on tires’ lateral force. The lateral force estimation based on the SCKF algorithm is implemented as follows. Step 1: Set the initial value of the state \(\delta_{0/0}\) and its error covariance matrix square root factor \(S_{0/0}\). Step 2: Time update. Calculate the volume points and transfer them using the state transfer equation. \[ \begin{array}{l}x_{k-1/k-1}^{i}=\hat{x}_{k-1/k-1}+S_{k-1/k-1}\xi_{i}\ \ \ i=1,2,\ldots,2n\\ x^{i*}_{k-1/k-1}=f(x_{k-1/k-1}^{i})\end{array}  \tag{12}\]  
where \(\xi_{i}\) is the column \(i\) of the volume point weight matrix \((\sqrt{n}I_{n},-\sqrt{n}I_{n})\); \(I_{n}\) is the unit matrix of \(n\times n\); \(n\) is the dimension of the state variable. Calculate the predicted value of the state and the square root factor of its error covariance matrix:  
\[x_{k/k-1}=\frac{1}{2n}\sum_{i=1}^{2n}x^{i*}_{k/k-1} \tag{13}\]  
\[S_{k/k-1}=qr(X^{i*}_{k/k-1},\sqrt{Q_{k-1}}) \tag{14}\]  
where \(qr\) denotes \(QR\) the decomposition; the weighted center matrix \(X^{i*}_{k/k-1}\) is  
\[X^{i*}_{k/k-1}=\frac{1}{\sqrt{2n}}(x^{1*}_{k/k-1}-\hat{x}_{k/k-1},x^{2*}_{k/k- 1}-\hat{x}_{k/k-1},\cdots x^{2n*}_{k/k-1}-\hat{x}_{k/k-1}) \tag{15}\]  
Step 3: Measurement update. Update the volumetric points and transfer them using the measurement equation. \[ \begin{array}{l}x_{k/k-1}^{i}=\hat{x}_{k/k-1}+S_{k/k-1}\xi_{i}\\ x_{k/k-1}^{i}=h(x_{k/k-1}^{i})\end{array}  \tag{16}\]  
Calculate the predicted values of the measured variables and the square root factors of their new interest covariance matrices:  
\[ \begin{array}{l}z_{k/k-1}^{i}=\frac{1}{2n}\sum_{i=1}^{2n}z_{k/k-1}^{i}\\ S^{zz}_{k/k-1}=qr(Z_{k/k-1},\sqrt{R_{k}})\end{array}  \tag{17}\]  
where the weighted center matrix \(Z_{k/k-1}\) is  
\[Z_{k/k-1}=\frac{1}{\sqrt{2n}}(z^{1}_{k/k-1}-\hat{z}_{k/k-1},z^{2}_{k/k-1}-\hat {z}_{k/k-1},\cdots z^{2n}_{k/k-1}-\hat{z}_{k/k-1}) \tag{18}\]  
Calculate measurement covariance matrix and mutual covariance matrix:  
\[