# Autonomous Vehicles: Vehicle Parameter Estimation Using Variational Bayes and Kinematics
## 3 SGP Motion Model Implementation
### SGP Model Training  
To train the motion in the sGP models a dataset is used that is based on the following Equation (6).  
\[\mathcal{D}=\left\{\left[\left[\begin{pmatrix}\Delta\phi_{1}\\ \dots\\ \Delta\phi_{m}\\ \theta\end{pmatrix}_{0},\begin{pmatrix}\Delta x\\ \Delta y\\ \Delta\theta\end{pmatrix}_{0}\end{pmatrix}_{0}\right],\dots,\left[\begin{pmatrix} \Delta\phi_{1}\\ \dots\\ \Delta\phi_{m}\\ \theta\end{pmatrix}_{n},\begin{pmatrix}\Delta x\\ \Delta y\\ \Delta\theta\end{pmatrix}_{n}\right]\right\}. \tag{6}\]  
We then estimate the vehicle parameters for a differential driven mobile robot and a car. The kinematic models used for this calculation are depicted in Figure 4 and described in [45]. Note that we use a virtual wheel for the Ackermann kinematics. This approach is similar to the bicycle simplification described in [46].  
Both models use the instantaneous center of rotation (ICR), sometimes known as instantaneous center of curvature (ICC).  
The movement of the mobile robot refers to the center of the baseline \(P\). The kinematic model is described using the vehicle parameters \(\bar{r}\) (mean wheel radius) and \(B\) (distance between wheels), the simulated ground truth data and wheel movement \(\Delta\phi_{t,\{r,l\}}\) at time \(t\) as denoted in Equation (7):  
Figure 4: Visualization of the kinematic models used for the performed mobile robot (**a**) and car experiments (**b**). For both models, we assumed a rigid frame. For the car model, we used a simplified version of Ackermann steering using a virtual wheel. The doted lines are the zero-motion lines of the wheels and thus represent kinematic constrains of the wheels.  
Figure 3: Graphical model including the vehicle parameters (**a**). Since we perform simulation from a known pose using the movement model, the connections to \(x_{t}\) disappear (**b**).  
\[\begin{pmatrix}\Delta x\\ \Delta y\\ \Delta\theta\end{pmatrix}_{t}=\begin{pmatrix}\frac{(\Delta\phi_{t,r}+\Delta\phi_{t, l})\cdot\tilde{r}}{2}\cdot\cos(\theta_{t-1}+\Delta\theta_{t-1})\\ \frac{(\Delta\phi_{t,r}+\Delta\phi_{t,l})\cdot\tilde{r}}{2}\cdot\sin(\theta_{t -1}+\Delta\theta_{t-1})\\ \frac{(\Delta\phi_{t,r}-\Delta\phi_{t,l})\cdot\tilde{r}}{b}\end{pmatrix}. \tag{7}\]  
Note that this representation is similar to the data set that we used for the sGP formulation in Equation (6).  
To develop our model for evaluation we relied on the approach described in [43] and implemented in [47]. The parameter estimation and data pre-processing was done in R. We divided the data into 25% for training and 75% for testing. For each element in the state vector \(\vec{x}_{t}=\begin{pmatrix}x_{t}&y_{t}&\theta_{t}\end{pmatrix}^{T}\), we trained an sGP model using an RBF kernel without automatic relevance determination. We estimated the models for the following \(\{1,2,5,10,20,30,50,100,200,500\}\) auxiliary points. For each model, we analyzed the log likelihood \(log(p(\mathbf{X}))=\mathcal{L}(q)+KL(q||p)\)[35]. \(\mathcal{L}(q)\) is known as the lower bound and \(KL(q||p)\) is the Kullback-Leibler divergence [35]. We selected the number of auxiliary points when the model seemed to be converged.