# Estimation of Skid-Steered Wheeled Vehicle States Using STUKF with Adaptive Noise Adjustment
## 4 Observer Design
### Unscented Kalman Filter  
For a given discrete system:  
\[\left\{\begin{array}{l}x_{k+1}=f(x_{k},u_{k},w_{k})\\ y_{k}=h(x_{k},u_{k},v_{k})\end{array}\right. \tag{14}\]  
The UKF computational procedure is described in the following steps.  
**Step 1** Initiate the mean value and variance matrix.  
\[\begin{array}{l}\hat{x}_{0}^{+}=E(x_{0})\\ P_{0}^{+}=E\Big{[}\big{(}x_{0}-\hat{x}_{0}^{+}\big{)}\big{(}x_{0}-\hat{x}_{0}^ {+}\big{)}^{T}\Big{]}\end{array} \tag{15}\]  
where \(\hat{x}_{0}^{+}\) is the posteriori estimation value of the expected mean value and \(P_{0}^{+}\) is the posteriori estimation of variance matrix when the computing step \(k=0\).  
**Step 2** Time update.  
Figure 4: The vertical force estimation as compared with the TruckSim simulation result.  
(a) The sigma points are chosen by:  
\[\begin{gathered}\hat{x}_{k-1}^{(i)}=\hat{x}_{k-1}^{+}+\widetilde{x} ^{(i)},\;i=0,\ldots,2n\\ \widetilde{x}^{(i)}=0,\;i=0\\ \widetilde{x}^{(i)}=\left(\sqrt{(n+k)P_{k-1}^{+}}\right)_{i}^{T},\;i=1,\ldots,n\\ \widetilde{x}^{(n+i)}=-\left(\sqrt{(n+k)P_{k-1}^{+}}\right)_{i}^{T},\;i=1,\ldots,n\\ \end{gathered} \tag{16}\]  
(b) Use the system function _f_(0) transforms the sigma points:  
\[\hat{x}_{k}^{(i)}=f\Big{(}\hat{x}_{k-1}^{(i)},u_{k},t_{k}\Big{)} \tag{17}\]  
(c) Compute the a priori estimation value of the expected mean value:  
\[\hat{x}_{k}^{-}=\sum_{2n}^{i=0}W_{m}^{(i)}\hat{x}_{k}^{(i)} \tag{18}\]  
where \(W^{(i)}\) represents the unscented transform weighted average coefficient.  
\[\begin{gathered} W_{m}^{(0)}=\frac{t}{n+t}\\ W_{c}^{(0)}=\frac{t}{n+t}+\left(1-a^{2}+b\right)\\ W_{m}^{(i)}=W_{c}^{(i)}=\frac{1}{2(n+t)},\;(i=1,\ldots,2n)\\ \end{gathered} \tag{19}\]  
where \(\tau=\alpha^{2}(n+\kappa)-n\); \(\kappa\) is the adjust coefficient to decrease the high-order error, \(n+\kappa\
eq 0\); \(\kappa\) is usually chosen as a small positive value, it reflects the distribution of sigma points at the \(\hat{x}_{k}\). The smaller the \(\alpha\) value, the smaller the influence of the nonlinear high order of the function; \(\beta\) describes the system distribution information and \(\beta=2\) when the system is a Gaussian distribution.  
(d) Compute the a priori estimation of the variance matrix:  
\[P_{k}^{-}=\sum_{2n}^{i=0}W_{c}^{(i)}\Big{(}\hat{x}_{k}^{(i)}-\hat{x}_{k}^{-} \Big{)}\Big{(}\hat{x}_{k}^{(i)}-\hat{x}_{k}^{\top}\Big{)}^{T}+Q_{k-1} \tag{20}\]  
**Step 3** Measurement update.  
(a) Refresh the sigma points:  
\[\begin{gathered}\hat{x}_{k}^{(i)}=\hat{x}_{k}^{-}+\widetilde{x} ^{(i)},\;i=0,\ldots,2n\\ \widetilde{x}^{(i)}=0,\;i=0\\ \widetilde{x}^{(i)}=\left(\sqrt{(n+k)P_{k}^{-}}\right)_{i}^{T},\;i=1,\ldots,n\\ \widetilde{x}^{(n+i)}=-\left(\sqrt{(n+k)P_{k}^{-}}\right)_{i}^{T},\;i=1,\ldots,n\\ \end{gathered} \tag{21}\]  
(b) Transform sigma points using measurement function _h_(0):  
\[\hat{y}_{k}=\sum_{2n+1}^{i=1}W_{m}^{(i)}\hat{y}_{k}^{(i)} \tag{22}\]  
(c) Compute the estimation of measurement:  
\[\hat{y}_{k}=\sum_{2n+1}^{i=1}W_{m}^{(i)}\hat{y}_{k}^{(i)} \tag{23}\](d) Compute the measurement estimation variance:  
\[P_{y}=\sum\limits_{2n}^{i=0}W_{c}^{(i)}\Big{(}\hat{y}_{k}^{(i)}-\hat{y}_{k} \Big{)}\Big{(}\hat{y}_{k}^{(i)}-\hat{y}_{k}\Big{)}^{T}+R_{k} \tag{24}\]  
(e) Compute the covariance between \(\hat{x}_{k}^{-}\) and \(\hat{y}_{k}\):  
\[P_{xy}=\sum\limits_{2n}^{i=0}W_{c}^{(i)}\Big{(}\hat{x}_{k}^{(i)}-\hat{x}_{k}^{- }\Big{)}\Big{(}\hat{y}_{k}^{(i)}-\hat{y}_{k}\Big{)}^{T} \tag{25}\]  
(f) Compute the a posteriori estimation of expected mean value and variance matrix:  
\[\begin{gathered} K_{k}=P_{xy}P_{y}^{-1}\\ \hat{x}_{k}^{+}=\hat{x}_{k}^{-}+K_{k}(y_{k}-\hat{y}_{k})\\ P_{k}^{+}=P_{k}^{-}-K_{k}P_{y}K_{k}^{T}\end{gathered} \tag{26}\]