## 2 Methodology
### Design of the Enhanced Error-Adaptive ESKF-MPC  
In order to use the transfer function model in a digital controller, it needs to be converted from a continuous time domain (S-domain) to a discrete time domain (Z-domain). In a real system, the input signal \(u(\mathbf{t})\) is usually kept sampled by zero-order, i.e., constant over each sampling period \(\Delta t\). Therefore, the discretization equation of the transfer function is as follows:  
\[G(z)=(1-z^{-1})\mathcal{Z}\bigg{\{}\mathcal{L}^{-1}(\frac{G(s)}{s})\bigg{|}_{t =k\Delta t}\bigg{\}} \tag{4}\]  
where \(\mathcal{L}^{-1}\) denotes the inverse Laplace transform, \(\mathcal{Z}\) denotes the Z-transform, and \(\Delta t\) is the sampling period. The discrete transfer function can be converted into a difference equation. Take single-input single-output as an example:  
\[G(z)=\frac{b_{m}z^{m}+b_{m-1}z^{m-1}+\ldots+b_{0}}{a_{n}z^{n}+a_{n-1}z^{n-1}+ \ldots+a_{0}} \tag{5}\]  
The corresponding difference equation is summarized as follows:  
\[y(k)=-\sum_{i=1}^{n}\frac{a_{n-i}}{a_{n}}y(k-i)+\sum_{j=0}^{m}\frac{b_{m-j}}{ a_{n}}u(k-j) \tag{6}\]  
For a multiple-input multiple-output system, each output \(y_{i}(k)\) is a linear combination of all inputs \(u_{j}(k)\):  
\[y_{i}(k)=-\sum_{p=1}^{n_{i,j}}a_{ij,p}y_{i}(k-p)+\sum_{j=1}^{\text{nu}}\sum_{q =0}^{m_{i,j}}b_{ij,q}u_{j}(k-q) \tag{7}\]  
where \(a_{ij,p}\) and \(b_{ij,q}\) are the coefficients of \(G_{i,j}(z)\), and \(n_{i,j}\) and \(m_{i,j}\) are the orders of the corresponding transfer function. For ESKF state estimation, the transfer function is converted into an observable canonical form:  
\[x_{\text{e},k+1}=A_{e}x_{e,k}+B_{e}u_{k}+\xi_{k} \tag{8}\]  
\[y_{k}=C_{e}x_{e,k} \tag{9}\]\[A= \begin{bmatrix}A&I\\ 0&I\end{bmatrix}  \tag{10}\]  
\[B= \begin{bmatrix}B\\ 0\end{bmatrix}  \tag{11}\]  
\[C= \begin{bmatrix}C,&0\end{bmatrix}  \tag{12}\]  
\[x_{e,k}= \begin{bmatrix}xk\\ wk\end{bmatrix}  \tag{13}\]  
where \(wk\) is the disturbed state, and \(\xi_{k}\) is the disturbed rate of change noise. The equation for prediction is as follows:  
\[\hat{x}_{e,k|k-1}=A_{e}\hat{x}_{e,k-1}+B_{e}u_{e,k-1} \tag{14}\]  
\[P_{k|k-1}=A_{e}P_{k-1}A_{e}^{T}+Q_{e} \tag{15}\]  
The equation for updating is as follows:  
\[e(k)=y(k)-C_{e}\hat{x}_{e}(k\mid k-1) \tag{16}\]  
\[S(k)=C_{e}P(k|k-1)C_{e}^{T}+R \tag{17}\]  
\[K(k)=P(k|k-1)C_{e}^{T}S(k)^{-1} \tag{18}\]  
\[\hat{x}_{e}(k|k)=\hat{x}_{e}(k|k-1)+K(k)e(k)+\beta\cdot fal(e(k),\alpha,\delta) \tag{19}\]  
\[P(k|k)=(I-K(k)C_{e})P(k\mid k-1)+\Gamma_{k} \tag{20}\]  
\[fal(e,\alpha,\delta)= \begin{cases}|\mathbf{e}|^{\alpha}sign(e),|e|>\delta\\ \frac{e}{\delta^{1-\alpha}},&|e|\leq\delta\end{cases}  \tag{21}\]  
The prediction model of MPC predicts the future step output based on the difference equation:  
\[y(k+i|k)=-\sum_{p=1}^{n}a_{p}y(k+i-p|k)+\sum_{q=0}^{m}b_{q}u(k+i-q|k) \tag{22}\]  
The prediction vector is as follows:  
\[Y=[y(k+1|k),\ldots,y(k+Np|k)]^{T}=F\overline{x}(k)+\Phi\Delta U \tag{23}\]  
\[\overline{x}(k)=[y(k),\ldots,y(k-n+1),u(k),\ldots,u(k-m+1)]T \tag{24}\]  
\[\Delta U=[\Delta u(k|k),\ldots,\Delta u(k+Nc-1|k)]^{T} \tag{25}\]  
The objective function is as follows:  
\[J=(Y-Y_{r})^{T}S_{q}(Y-Y_{r})+\Delta U^{T}S_{r}\Delta U \tag{26}\]  
The secondary form is as follows:  
\[J=\frac{1}{2}\Delta U^{T}(2\omega)\Delta U+(2 FF^{T})\Delta U \tag{27}\]  
\[\omega=\Phi^{T}S_{q}\Phi+rwS_{r} \tag{28}\]  
\[FF=\Phi^{T}S_{q}(F\overline{x}-Y_{r}) \tag{29}\]where \(S_{q}\) and \(S_{r}\) are, respectively, the output error weight matrix and control increment weight matrix. The constraint is set to the following:  
\[\Delta u_{min}\leq\Delta u(k)\leq\Delta u_{max} \tag{30}\]  
\[u_{min}\leq u(k-1)+\sum_{i=0}^{c-1}\Delta u(k+i)\leq u_{max} \tag{31}\]  
In this paper, the core innovation of the adaptive mechanism lies in incorporating a dynamic weight \(Q_{k}\) into the optimization objective of MPC, which modulates the error weight Sq for each prediction step.