## 4 Design of the Control Algorithm
### Sakf  
The SAKF module is designed based on the state space model of the OES, which can be transformed from the OES transfer function (4) as  
\[\left\{\begin{array}{c}\dot{\mathbf{x}}=\mathbf{A}\mathbf{x}+\mathbf{B}(u-d)\\ \mathbf{y}=\mathbf{C}\mathbf{x}\end{array}\right. \tag{5}\]  
where \(\mathbf{x}=\left[\begin{array}{c}\theta\\ v\end{array}\right]\mathbf{A}=\left[\begin{array}{cc}0&1\\ 0&-p\end{array}\right]\mathbf{B}=\left[\begin{array}{c}0\\ K\end{array}\right]\) and \(\mathbf{C}=\left[\begin{array}{cc}0&1\end{array}\right]\).  
Since the actual algorithm always runs in a digital signal processor (DSP), the state space model (5) needs to be discretized according to the sampling period \(T_{8}\). Meanwhile, considering that the SAKF is designed to observe and compensate the disturbance torque, the torque disturbance \(d\) is extended to the state variable \(\mathbf{x}\) of (5) as:  
\[\left\{\begin{array}{c}\left[\begin{array}{c}\mathbf{x}(k+1)\\ d(k+1)\end{array}\right]=\mathbf{A_{\mathrm{a}}}\cdot\left[\begin{array}{c}\mathbf{x} (k)\\ d(k)\end{array}\right]+\mathbf{B_{\mathrm{a}}}\cdot u(k)+\mathbf{G}\cdot\left[\begin{array} []{c}n(k)\\ \omega_{\mathrm{n}}(k)\end{array}\right]\\ \mathbf{y}(k)=\mathbf{C_{\mathrm{a}}}\cdot\left[\begin{array}{c}\mathbf{x}(k)\\ d(k)\end{array}\right]+\mathbf{n_{\mathrm{m}}}(k)\end{array}\right. \tag{6}\]  
where \(\mathbf{A_{\mathrm{a}}}=\left[\begin{array}{cc}\mathbf{A_{\mathrm{d}}}&-\mathbf{B_{ \mathrm{d}}}\\ 0&\mathbf{I_{1\times 1}}\end{array}\right]\mathbf{A_{\mathrm{d}}}=\mathrm{e}^{\mathbf{A}T_{ \mathrm{s}}}\), \(\mathbf{B_{\mathrm{d}}}=\int_{0}^{T_{\mathrm{s}}}\mathrm{e}^{\mathbf{A}T_{\mathrm{s}}} \mathrm{B}\mathrm{d}t\), \(\mathbf{x}(k)=\left[\begin{array}{c}\theta(k)\\ v(k)\end{array}\right]\mathbf{I_{1\times 1}}\) is a \(1\times 1\) unit matrix, \(\mathbf{B_{\mathrm{a}}}=\left[\begin{array}{c}\mathbf{B_{\mathrm{d}}}\\ 0\end{array}\right]\), \(\mathbf{G}=\left[\begin{array}{cc}\mathbf{B_{\mathrm{d}}}&0\\ 0&\mathbf{I_{1\times 1}}\end{array}\right]\), \(\mathbf{C_{\mathrm{a}}}=\left[\begin{array}{cc}\mathbf{C}&0\end{array}\right]\), \(\mathbf{V}=\mathbf{I_{1\times 1}}\). \(\omega_{\mathrm{n}}\) is the difference between \(d(k+1)\) and \(d(k)\), which can be regarded as a white noise; meanwhile, \(\mathbf{n_{\mathrm{m}}}(k)\) is the measurement noise of the speed obtained from encoder, and \(n(k)\) is the process noise of the state variable \(\mathbf{x}(k)\).  
According to (5) and (6), a relationship between \(\mathbf{A_{\mathrm{d}}}\), \(\mathbf{B_{\mathrm{d}}}\) and \(p\), \(K\) can be derived as:  
\[\left\{\begin{array}{c}\mathbf{A_{\mathrm{d}}}=\left[\begin{array}{cc}1&\frac{ 1-\mathrm{e}^{-pT_{\mathrm{s}}}}{p}\\ 0&\mathrm{e}^{-pT_{\mathrm{s}}}\end{array}\right]\\ \mathbf{B_{\mathrm{d}}}=\frac{K}{p^{2}}\left[\begin{array}{cc}pT_{\mathrm{s}}+ \mathrm{e}^{-pT_{\mathrm{s}}}-1\\ p-p\mathrm{e}^{-pT_{\mathrm{s}}}\end{array}\right]\end{array}\right.. \tag{7}\]  
Figure 3: Schematic diagram of the proposed control method.  
Therefore, the estimation result of the state variable \([x(k),d(k)]^{\rm T}\) can be obtained according to:  
\[\left[\begin{array}{c}\hat{x}(k+1)\\ \hat{d}(k+1)\end{array}\right]=A_{\bf a}\left[\begin{array}{c}\hat{x}(k)\\ \hat{d}(k)\end{array}\right]+B_{\bf a}u(k)+L(y(k)-C_{\bf a}\hat{x}(k)). \tag{8}\]  
Here, the gain coefficient matrix \(L\) in (8) can be obtained by solving  
\[L=A_{\bf a}MC_{\bf a}^{\rm T}(R_{\bf r}+C_{\bf a}MC_{\bf a}^{\rm T})^{-1} \tag{9}\]  
and the Riccati equation  
\[A_{\bf a}MA_{\bf a}^{\rm T}-A_{\bf a}MC_{\bf a}^{\rm T}\Big{[}R_{\bf r}+C_{\bf a }MC_{\bf a}^{\rm T}\Big{]}^{-1}C_{\bf a}MA_{\bf a}^{\rm T}-M+R_{\bf w}={\bf 0} \tag{10}\]  
where \(R_{\bf w}\) is the noise covariance matrix of \(u(k)\), and \(R_{\bf r}\) is the noise covariance matrix of the process variable \([x(k),d(k)]^{\rm T}\).