## 3 Applications to Kinetic Models
### EKF Recursive Algorithm Application  
We extended our previous example of an object moving along a straight line by adding the orientation of the vehicle \(\theta\) to the position vector (\(x\),\(y\)). Then, we estimated the state vector \(\mathbf{p}\), i.e., \(\mathbf{p}=\) (\(x\),\(y\),\(\theta\)).  
The kinematic model is given by the following system:  
\[\left\{\begin{array}{l}x_{t}=x_{t-1}+v_{t}\cdot\Delta_{t}\cdot\cos\Bigl{(} \theta_{t-1}+w_{t}\cdot\frac{\Delta_{t}}{2}\Bigr{)},\\ y_{t}=y_{t-1}+v_{t}\cdot\Delta_{t}\cdot\sin\Bigl{(}\theta_{t-1}+w_{t}\cdot \frac{\Delta_{t}}{2}\Bigr{)},\\ \theta_{t}=\theta_{t-1}+w_{t}\cdot\Delta_{t},\end{array}\right. \tag{24}\]  
where \(\Delta_{t}\) is the time interval, while \(v\) and \(w\) are the speed and the yaw rate of the vehicle. We also denote \(\hat{v}\) and \(\hat{w}\) as the speed and yaw rate measurements. We assumed that their errors followed the Gaussian distribution as \(\Delta v_{t}\sim N\bigl{(}0,\sigma_{v_{0}}^{2}\bigr{)}\) and \(\Delta w_{t}\sim N\bigl{(}0,\sigma_{v_{0}}^{2}\bigr{)}\).  
Now, suppose that the vehicle is equipped with a GPS, and the GPS measurements are denoted by vector \(\mathbf{z}\) with a measurement model given by:  
\[\mathbf{z}_{t}=\mathbf{H}\mathbf{p}_{t}+\gamma_{t}\mathbf{H}=\left(\begin{array}{ccc}1&0&0\\ 0&1&0\end{array}\right), \tag{25}\]  
where \(\gamma\sim N\Bigl{(}0,\sigma_{\gamma}^{2}\Bigr{)}\) is the measurement error that follows a Gaussian distribution with zero mean and variance \(\sigma_{\gamma}^{2}\).  
This is an EKF model, which could be locally linearized as follows:  
\[\left(\begin{array}{c}x_{t}\\ y_{t}\\ \theta_{t}\end{array}\right)\approx\left(\begin{array}{c}\overline{x}_{t}\\ \overline{y}_{t}\\ \overline{\theta}_{t}\end{array}\right)+\mathbf{A}\bigl{(}\mathbf{p}_{t-1},\mathbf{u}_{t} \bigr{)}\left(\begin{array}{c}\Delta x_{t-1}\\ \Delta y_{t-1}\\ \Delta\theta_{t-1}\end{array}\right)+\mathbf{B}\bigl{(}\mathbf{p}_{t-1},\mathbf{u}_{t} \bigr{)}\left(\begin{array}{c}\Delta v_{t}\\ \Delta\omega_{t}\end{array}\right),\]  
\[\left\{\begin{array}{l}\overline{x}_{t}=x_{t-1}+v_{t}\cdot\Delta_{t}\cdot \cos\Bigl{(}\theta_{t-1}+w_{t}\cdot\frac{\Delta t}{2}\Bigr{)},\\ \overline{y}_{t}=y_{t-1}+v_{t}\cdot\Delta_{t}\cdot\sin\Bigl{(}\theta_{t-1}+w_{ t}\cdot\frac{\Delta t}{2}\Bigr{)},\\ \overline{\theta}_{t}=\theta_{t-1}+w_{t}\cdot\Delta_{t},\end{array}\right.\]  
\[\mathbf{A}\bigl{(}\mathbf{p}_{t-1},\mathbf{u}_{t}\bigr{)}=\left(\begin{array}{ccc}1&0&-v _{t}\cdot\Delta_{t}\cdot\sin\Bigl{(}\theta_{t-1}+w_{t}\cdot\frac{\Delta t}{2} \Bigr{)}\\ 0&1&v_{t}\cdot\Delta_{t}\cdot\cos\Bigl{(}\theta_{t-1}+w_{t}\cdot\frac{\Delta t }{2}\Bigr{)}\\ 0&0&1\end{array}\right),\]  
\[\mathbf{B}\bigl{(}\mathbf{p}_{t-1},\mathbf{u}_{t}\bigr{)}=\left(\begin{array}{ccc}\Delta t \cdot\cos\Bigl{(}\theta_{t-1}+w_{t}\cdot\frac{\Delta_{t}}{2}\Bigr{)}&-v_{t} \cdot\Delta t^{2}\cdot\sin\Bigl{(}\theta_{t-1}+w_{t}\cdot\frac{\Delta_{t}}{2} \Bigr{)}\\ \Delta t\cdot\sin\Bigl{(}\theta_{t-1}+w_{t}\cdot\frac{\Delta_{t}}{2}\Bigr{)}&v_ {t}\cdot\Delta t^{2}\cdot\cos\Bigl{(}\theta_{t-1}+w_{t}\cdot\frac{\Delta_{t}} {2}\Bigr{)}\\ 0&\Delta t\end{array}\right), \tag{26}\]  
where \(\mathbf{u}_{t}=(v_{t},w_{t})\).  
As we can see, the matrices \(\mathbf{A}\bigl{(}\mathbf{p}_{t-1},\mathbf{u}_{t}\bigr{)}\) and \(\mathbf{B}\bigl{(}\mathbf{p}_{t-1},\mathbf{u}_{t}\bigr{)}\) of the state evolution function are Jacobian with respect to \(\mathbf{p}_{t-1}\) and \(\mathbf{u}_{t}\), respectively.  
Hence, we have a locally linearized EKF model. The prediction of the state vector is:  
\[\left(\begin{array}{c}\overline{x}_{t}\\ \overline{y}_{t}\\ \overline{\theta}_{t}\end{array}\right)=\left(\begin{array}{c}\hat{x}_{t-1}+ \hat{v}_{t}\cdot\Delta_{t}\cdot\cos\Bigl{(}\hat{\theta}_{t-1}+\hat{w}_{t} \cdot\frac{\Delta_{t}}{2}\Bigr{)}\\ \hat{y}_{t-1}+\hat{v}_{t}\cdot\Delta_{t}\cdot\sin\Bigl{(}\hat{\theta}_{t-1}+ \hat{w}_{t}\cdot\frac{\Delta_{t}}{2}\Bigr{)}\\ \hat{\theta}_{t-1}+\hat{w}_{t}\cdot\Delta_{t}\end{array}\right). \tag{27}\]