\chapter{Results and Analysis}
\label{details}
After introducing the basic deep learning knowledge that would be used in the project, I will start the experiments and analyze the results. The main experiments would be,
\begin{enumerate}
    \item Do the simulation based one computer physics library(\textit{pybox2d}). Totally, 100 different rigid motion simulation would be carried out. For every simulation, I would record the fixed number of simulation steps, $600$.
    \item \textbf{XML} formulation would be used to store every step of all simulation. The data of each state would involve positions, velocities, contact forces, etc.
    \item Once obtaining the \textbf{XML} files. I would transform the \textbf{XML} files into a \textit{pybox2d} world, and then carry out some experiments to determine the suitable grid size and smoothing length of SPH.
    \item After choosing the proper parameter of the SPH kernel(types, grid cell size, and smoothing length), I would transform these \textbf{XML} files to grid images, which are used as training dataset.
    \item Design a deep learning model and do the training on the training dataset.
    \item Apply the trained model to predict the starting iterate values of contact forces($\pmb{\lambda}$). Then I would compare the learning model with other classical methods.
\end{enumerate}

\section{Rigid Motion Simulation}

\textit{pybox2d}\footnote{\url{https://github.com/pybox2d/pybox2d}} is chosen as the main physics engine to implement computer simulation experiments. \textit{pybox2d} is a 2D physics library for your games and simple simulations. It's based on the Box2D library, which is written in \textit{C++}. It supports several shape types (circle, polygon, thin line segments), and quite a few joint types (revolute, prismatic, wheel, etc.). In my experiment, I would first try to apply deep learning in the simple simulations. So I would mainly simulation with circle rigid objects sharing the same radius.

\subsection{Simulation Configuration}
Simulation configs are listed below,
    \label{simconfig}
    \begin{itemize}
        \item \textbf{World Setting}, the world box size is $30\times30$,   and there are $100$ circle rigid bodies($r=1$, all circle rigid bodies in the same size.) inside the box. Initially, the rigid circles will be located following gaussian distribution\footnote{\url{https://en.wikipedia.org/wiki/Normal_distribution}}. Then, all rigid circles will fall down by gravity. The visualization of simulation is shown in Figure \ref{fig:imsim}.
        \item \textbf{Simulation Setting}, there will be totally $600$-steps simulation. For each step, $\Delta t = 0.01s$, and the number of iteration in each step will be set as fixed, $3000$. Then I will use the average convergence rate to describe the performances of models.
    \end{itemize}
\subsection{Simulation Details}
    Before generating data, one dynamic simulation was run to check how \textit{pybox2d} works and some figures have been obtained. Figure \ref{fig:contactnum} describe the relationship between time step and contacts number, and Figure \ref{fig:contacttime} gives the relationship between time spend and contacts number.
    \begin{figure}[!h]
        \centering
        \begin{subfigure}[b]{0.3\textwidth}
            \includegraphics[width=\textwidth]{Figures/sim0.png}
            \caption{Time Step=$0$}
        \end{subfigure}
        \begin{subfigure}[b]{0.3\textwidth}
            \includegraphics[width=\textwidth]{Figures/sim2.png}
            \caption{Time Step=$200$}
        \end{subfigure}
        \begin{subfigure}[b]{0.3\textwidth}
            \includegraphics[width=\textwidth]{Figures/sim3.png}
            \caption{Time Step=$400$}
        \end{subfigure}
        \caption{Visualization for experiment simulation}
        \label{fig:imsim}
    \end{figure}
    \begin{figure}[!ht]
        \centering
        \begin{subfigure}[b]{0.7\textwidth}
            \includegraphics[width=\textwidth]{Figures/contact_num}
            \caption{The number of contacts in each step.}
            \label{fig:contactnum}
        \end{subfigure}
        \begin{subfigure}[b]{0.7\textwidth}
            \includegraphics[width=\textwidth]{Figures/contact_time}
            \caption{Time spent for contact solver(only the time for interation).}
            \label{fig:contacttime}
        \end{subfigure}
        \caption{Details of a normal simulation. Figure \ref{fig:contactnum} shows contact numbers in each step, while Figure \ref{fig:contacttime} shows the time spent for contact iteration.}
    \end{figure}

\subsection{SPH Parameters}
    \label{SPH-setting}
    In Section \ref{sphtest}, it has been tested that \textbf{SPH} is a good strategy to generate grid images representing a discrete snapshot of the dynamics. Then, the grid size and smoothing length $h$ are imported into our data generation. The grid size should be reasonable so that there is no two contact points are mapped into the same cell, since if there are more than two contact values in one cell, the rid node might store mixture information state from two objects. which would be hard for CNN to find the relationship between one objects, which will decrease the accuracy of prediction. This was mentioned in section \ref{gs}.
    \begin{figure}[!h]
        \centering
        \includegraphics[width=0.4\textwidth]{Figures/SPHvi.pdf}
        \caption{Example visiualization for Smoothed Particle Hydrodynamics. The small red circles stand for the grid nodes, and the blue circles stand for kernel size.}
    \end{figure}

    \subsubsection{Grid Size and Kernel Length}
    I define $\pmb{d}=(d_x, d_y)$ as grid cell size and $h$ as smoothing length. I conclude some rules for determining grid cell size and smoothing length.
    \begin{itemize}
        \item Since the objects are circles, the ideal cell size should like,
            $$d_x = d_y = d $$
        \item Since there can not be two contact points are mapped into the same cell, $d$ must be less than the distance of nearest two contact points. It can be defined.
            $$d \le r = 1$$
        \item Similarly, if one contact point can only be mapped to one cell, the smooth length $h$ should be less than the minimum distance between two contact points $d$,
            $$h \le d$$
        \item For a given $d$, whatever the contact position $\pmb{q}=(q_x, q_y)$ is, its information can be restore in nearby nodes. So it can be,
            $$h \ge \frac{\sqrt{2}}{2}d \approx 0.71 d$$
    \end{itemize}
    One experiment has been designed to test whether the rules can be applied in this case. Still using the simulation config introduced in section \ref{simconfig}. The cell size is fixed, $d=0.25$. Different smoothing length is appiled. The results are shown in Figure \ref{fig:dh}.

    \begin{figure}[!ht]
     \centering
    \begin{subfigure}[b]{0.7\textwidth}
        \includegraphics[width=\textwidth]{Figures/size25.pdf}
        \caption{The grid size $d$ is set $0.25$. $h=0.1, 0.2, 0.25, 0.5$ is tested respectively. This figure shows different coveragence rate based on different $h$ value.}
        \label{fig:dh}
    \end{subfigure}
    \begin{subfigure}[b]{0.7\textwidth}
        \includegraphics[width=\textwidth]{Figures/hd.pdf}
        \caption{Coveragence rate for different $d$ value.}
        \label{fig:testd}
    \end{subfigure}
    \caption{Experiments from kernel \textbf{Poly6}}
    \label{poly6}
    \end{figure}

    As what is shown in Figure \ref{fig:dh}, when $h=0.4d$ or $h=2d$, the solver coverages more slowly and unstable. Overall, for the next step, data generation, I will make $h=d$ always. The next step is to explore what $d$ value will be good. Once $h=d$ has been determined, the next step is to choose the value of $d$. Another experiment is taken to check the convergence rate with using diffrent $d$ values. The reults are showns in Figure \ref{fig:testd}. From this figure, it is obviously, when $d=0.5$, iterative solver converages the most rapidly.
    \textbf{Importantly}, $d=0.5~~\text{and}~~h=0.5$ is not always the best experimented with different numbers of rigid inside the world box. But, it generally performs better than other values. As a result, I will choose $h=d=0.5$ as the parameters for SPH method.
    $$d_x = d_y = 0.5~~\text{and}~~h=0.5$$
    \subsubsection{Kernel Choosing}
    The choice of kernel also needs to be addressed. In the paper, two kernels, \textbf{Poly6} and \textbf{Spiky}, have been introduced in section \ref{sec:kernels}. In order to compare these two kernels and determine the best one, finding the best parameters for \textbf{Spiky} is essential. I did the same experiment with kernel \textbf{Spiky}. Find a good scale between $h$ and $d$ firstly and then choose $d$. The results are shown in Figure \ref{spiky}. The plot in Figure \ref{fig:spikynum} reveals that $h=d$ is also a good choice for kernel \textbf{Spiky}. Although $d$s perform similarly, $d=0.25$ performs more stable and a bit fater in convergence.
    \begin{figure}[!ht]
        \centering
        \begin{subfigure}[b]{0.7\textwidth}
            \includegraphics[width=\textwidth]{Figures/spiky25.pdf}
            \caption{The grid size $d$ is set $0.25$. $h=0.1, 0.2, 0.25, 0.5$ is tested respectively. This figure shows different coveragence rate based on different $h$ value.}
            \label{fig:spikynum}
        \end{subfigure}
        \begin{subfigure}[b]{0.7\textwidth}
            \includegraphics[width=\textwidth]{Figures/spikysize.pdf}
            \caption{Coveragence rate for different $d$ value.}
            \label{fig:spikyttime}
        \end{subfigure}
        \caption{Experiments for kernel \textbf{Spiky}.}
        \label{spiky}
    \end{figure}
    These two kernels are separately used based on Algorithm \ref{testsph}. In order to compare these two kernels carefully, the fixed iteration number was extended to $5000$. Then, the plot about the convergence rate of different kernels is obtained in Figure \ref{fig:testdkernel}. From the plot, it is clear that kernel \textbf{Poly6} perform better.
    \begin{figure}[!ht]{}
        \centering
        \includegraphics[width=0.8\textwidth]{Figures/kerneltest.pdf}
        \caption{Coveragence rate for kernel \textbf{Poly6} anf \textbf{Spiky}. $h_\text{poly6} = d_\text{poly6} = 0.5$, while $h_\text{spiky} = d_\text{spiky} = 0.25$}
        \label{fig:testdkernel}
    \end{figure}
    As a conclusion, SPH with \textbf{Poly6} kernel($\pmb{d} = (0.5, 0.5)$, smoothing length $h=0.5$), will be used for particle-grid transformation. \\

    After determining grid size and smoothing length, I will compare this specific \textbf{SPH-Model} with other models mentionde in section \ref{sphtest}, following Algorithm \ref{testsph}. The result is shown in Figure \ref{fig:final_test}.
     \begin{figure}[!ht]
        \centering
        \includegraphics[width=0.8\textwidth]{Figures/final_SPH.pdf}
        \caption{Coveragence rate for models(different initial values for $\pmb{\lambda}$).}
        \label{fig:final_test}
    \end{figure}
    Unlike the result shown in Figure \ref{fg:addsph}. Compared with other models, \textbf{SPH-Model} gets convergence much faster. So, $h=d=0.5$ is a good choice for this project and the future steps. Some example visualization of images is shown in Figure \ref{fig:examsph}.
    \begin{figure}[!h]
        \centering
        \begin{subfigure}[b]{0.45\textwidth}
            \includegraphics[width=\textwidth]{Figures/sphmass}
            \caption{The grid image for mass distribution.}
        \end{subfigure}
        \begin{subfigure}[b]{0.45\textwidth}
            \includegraphics[width=\textwidth]{Figures/sphy}
            \caption{The grid image for $v_x$ distribution.}
        \end{subfigure}
        \caption{Example visualization for bodies image with $d=0.5$ and $h=0.5$}
        \label{fig:examsph}
    \end{figure}

\section{Data Generation}
To create data that are more accessible to learning, I will map a discrete element method into a continuum setting use techniques from smooth particle hydrodynamics(SPH). In order to get enough available training data and remove useless information, I used,
\begin{itemize}
    \item Totally $150$ simulations with different initial configuration. The number of circle objects is not fixed as well. However, if there are too few objects inside the box, contacts might not happen until the simulation ends up. So the number of objects will be in the range of $[70, 110]$.
    \item If there is no contact in one state, the state means nothing to the learning. So the state files without any contacts will be removed.
\end{itemize}

\subsection{Simulation World to XML}
For one simulation world, every time-step state would be stored in one \textbf{XML} file. The structure of one rigid body is consist of \textbf{mass}, \textbf{position}, \textbf{velocity}, \textbf{spin omega} and \textbf{inertia}. The structure of one contact consists of \textbf{postion}, \textbf{impulse}(including in normal direction and tangent direcrion), \textbf{master} body and \textbf{slave} body. Two examples are given in the following.\\

This is an example of one body information is given below.
\begin{lstlisting}[language=XML]
    <body index="86" type="free">
        <mass value="3.14159274101"/>
        <position x="7.79289388657" y="2.62924313545"/>
        <velocity x="2.7878344059" y="-1.45545887947"/>
        <orientation theta="-0.115291565657"/>
        <inertia value="1.57079637051"/>
        <spin omega="-2.33787894249"/>
        <shape value="circle"/>
    </body>
    ...
\end{lstlisting}
Another example \textit{XML} code is for contatc force.
\begin{lstlisting}[language=XML]
    <contact index="1" master="2" master_shape="b2CircleShape(childCount=1, pos=b2Vec2(0,0), radius=1.2000000476837158, type=0,)" slave="97" slave_shape="b2CircleShape(childCount=1, pos=b2Vec2(0,0), radius=1.2000000476837158, type=0, )">
        <position x="0.21963849663734436" y="13.875240325927734"/>
        <normal normal="b2Vec2(-1,2.9819e-05)"/>
        <impulse n="0.005236322991549969" t="-0.002184529323130846"/>
    </contact>
    ...
\end{lstlisting}

\subsection{SPH Configuration}
As it is talked in \ref{SPH-setting},
\begin{itemize}
    \item \textbf{Kernel}, Poly6, which you can see in section \ref{poly6}.
    \item \textbf{Kernel Settings}, cell size $d_x=d_y=0.5$, smoothing length $h=0.5$.
\end{itemize}
\subsection{XML to Grid}
After getting a set of \textbf{XML} file, the next step is to load state information from \textbf{XML} file, and then map them to grid images with SPH based method. Since for any contact point, there are two normal contacts, one with $[n_x, n_y]$ while the other with $[-n_x, -n_y]$. So the grid values for $\pmb{n}$ would be always $\pmb{0}$, and the same  with $\pmb{t}$. So finally, the channel number of each image will be $6$, including $[m, v_x, v_y, \omega, \lambda_n, \lambda_t]$. \\

For the learning, I divide 8 channels into features(input) and label(output).
    $$\text{Feature} = [m, v_x, v_y, \omega]$$
    $$\text{Label} = [\lambda_n, \lambda_t]$$

\section{CNN Training}
\subsection{CNN  Architecture}
    The neural network was designed using Keras\cite{chollet2015keras}. Keras is a neural networks Application Programming Interface (API) written in Python, it runs on top of either TensorFlow. With some inspiration from AlexNet\cite{Krizhevsky:2012:ICD:2999134.2999257}, the networks shows five constructing stacks of layers and each stack is consist of two or three convolutional layers in the same size. To avoid overfitting, each stack is followed by one dropout layer. One full-connected layer is set after the last convolutional layer. This input layer is followed by a batch normalization layer, normalizing images within a batch, which is discussed in Section \ref{batchnor}. This architecture includes $51,599,092$ weights for an input size of ($61\times61\times4$). All convolutional layers have kernels of size $(3\times 3\times d)$, and are followed by ReLU activation function. Figure \ref{fig:art} shows the visualization of model architecture, and Table \ref{table:layers} shows the specific information about parameters when input images go through the CNN. The keras code impletementing this CNN structure can be reviewed in Appendix \ref{cnnn}.
    \begin{itemize}
        \item \textbf{Input Size}, the input will be $61\times61\times4$. Since the orginal world is $30\times30$ and grid size is $d=0.5$, the generated grid would be $61\times61$. There would $4$ channels
        \item \textbf{Output Size}, output size depends on the label size. The orginal label image would be $[\lambda_n, \lambda_t]$, so the label image size would be $61\times61\times2$, which should be flattened as the actual training label. The label size would be $61\times 61 \times 2$
        \item \textbf{Weights Number}, the total weights number is $64,498,866$.
    \end{itemize}
    Then we can define $cnn$ as the overall function of CNN model, where $\pmb{G}_{\lambda}$ is the image of contact forces(label) and $\pmb{G}_{\mathcal{B}}$ is the image of bodies state(feature).
    \begin{equation}
        \pmb{G}_{\lambda} = cnn(\pmb{G}_{\mathcal{B}})
    \end{equation}

    \begin{figure}[!h]
        \centering
        \includegraphics[width=\textwidth]{Figures/cnn_arc.pdf}
        \caption{Architecture of CNN model}
        \label{fig:art}
    \end{figure}

    \begin{table}[h!]
        \centering
        \begin{tabular}{ l | c  }
            Layer & Output Shape  \\ \hline
            Input & $n_b\times61\times61\times4$  \\
            Convolution(32) & $n_b\times61\times61\times32$  \\
            Convolution(32) & $n_b\times61\times61\times32$  \\
            Dropout & $n_b\times61\times61\times32$  \\
            MaxPooling & $n_b\times31\times31\times32$ \\
            Convolution(64) & $n_b\times31\times31\times64$  \\
            Convolution(64) & $n_b\times31\times31\times64$  \\
            Convolution(64) & $n_b\times31\times31\times64$  \\
            Dropout & $n_b\times31\times31\times64$  \\
            MaxPooling & $n_b\times16\times16\times64$ \\
            BatchNormalization & $n_b\times16\times16\times64$ \\
            Convolution(128) & $n_b\times16\times16\times128$  \\
            Convolution(128) & $n_b\times16\times16\times128$  \\
            Convolution(128) & $n_b\times16\times16\times128$  \\
            Dropout & $n_b\times16\times16\times128$  \\
            MaxPooling & $n_b\times8\times8\times128$ \\
            Convolution(256) & $n_b\times8\times8\times256$  \\
            Convolution(256) & $n_b\times8\times8\times256$  \\
            Convolution(256) & $n_b\times8\times8\times256$  \\
            Dropout & $n_b\times8\times8\times256$  \\
            MaxPooling & $n_b\times4\times4\times256$ \\
            BatchNormalization & $n_b\times4\times4\times512$  \\
            Convolution(512) & $n_b\times4\times4\times512$  \\
            Convolution(512) & $n_b\times4\times4\times512$  \\
            Convolution(512) & $n_b\times4\times4\times512$  \\
            Dropout & $n_b\times4\times4\times512$ \\
            Flatten & $n_b\times8192$ \\
            Dense & $n_b\times7442$ \\
        \end{tabular}
        \caption{Feature map (tensor) sizes through the network, the input has size $n_b\times61\times61\times5$, with batch size $n_b$ and patches of size $61\times61\times5$.}
        \label{table:layers}
    \end{table}

\subsection{Training Details}
    \label{trde}
    The learning happened on GPU(\textit{GeForce GTX 1080 Ti, 11 Gbps GDDR5X memory})\footnote{\url{https://www.nvidia.com/en-us/geforce/products/10series/geforce-gtx-1080-ti/}} held by Image Section, DIKU\footnote{\url{https://di.ku.dk/english/research/imagesection/}}. The whole learning takes nearly 24 hours. The trained model can be downloaded in my personal dropbox\footnote{\url{https://www.dropbox.com/s/jrwzqib6ghrq59i/model.h5?dl=0}}. I gave hyperparameters setting below,
    \begin{table}[h!]
        \centering
        \begin{tabular}{ l | c  }
            Hyperparameter  & Setting \\ \hline
            Activation function & ReLU \\
            Weight initilization & He normal\cite{sutskever2013importance} \\
            Weight regularizer &  L2 \cite{hinton2006fast} \\
            Convolution border mode  & Same \\
            Stride & 2 \\
            Kernel size & $(3, 3)$ \\
            Dropout rate & $0.1$ \\
            Optimizer & SGD \\
            Initial Learning Rate & $1\times 5\times10^{-3}$ \\
            Batch Size & 200 \\
            Epoch & 1000 \\
            Validation Rate & 0.2 \\
        \end{tabular}
        \caption{Hyperparameter settings.}
        \label{table:hyper}
    \end{table}
    \subsubsection{Loss Function}
        Firstly, we define a filter funtion,
        \begin{equation}
            g(x)= \begin{cases} 0, \quad x = 0 \\ 1, \quad x \ne 0 \end{cases}
        \end{equation}
        Then, we can update the loss function based on Euqation \ref{eq:mse}. The code can be viewed in Appendix \ref{lf}.

        \begin{equation}
            L = \frac{1}{N}\sum_{i}^{N}g(\hat{y}_i)(y_i-\hat{y}_i)^2
        \end{equation}
    \subsubsection{Learning Rate}
    The learning rate will change with the number of epoch, as talked in section \ref{learning}. I will give a specific value as the learning rate depending on the number of epoch. The learning rate will become smaller with increasing epoch.
    data. The overall algorithm is concluded in Algorithm \ref{ls}.
    \begin{algorithm}[!h]
        \KwData{$epoch$}
        \KwResult{learning rate $\eta$}
        \If {$epoch < 100$}{$\eta = 5\times10{-3}$}
        \If {$100< epoch < 300$}{$\eta = 2\times10^{-3}$}
        \If {$300< epoch < 500$}{$\eta = 1\times10^{-3}$}
        \If {$epoch > 300$}{$\eta = 2\times10^{-4}$}
        \caption{Learning Rate Scheduling}
        \label{ls}
    \end{algorithm}

    \subsubsection{Conclusion about Training process}
    \begin{itemize}
        \item The size of the input image is $41\times41\times4$, and all pixels are important since all of them retore useful information. If we want to extract the features from $41\times 41$, $3\times 3$ kernel would be a good choice. But the disadvantages is that there would be a lot of training parameters, which might slow down the learning process.
        \item I do have nearly $18208$ samples for training. However, it is hard for both the GPU and CPU to load all the data at one time. Dividing all training samples to a couple of batches would not only speed up the single training time and release memory, but also reduce the risk of overfitting.
        \item The learning rate will change with the number of the epoch, as analyzed in section \ref{learning}. I will give a specific learning rate values depending on the number of the epoch. The learning rate will become smaller with With the epoch increase, I hope the value of learning rate can decrease. Since the high learning rate would cause suboptimal performance at the end of training. The overall algorithm is concluded in Algorithm \ref{ls} and code in Appendix \ref{lr}.
        \item Validation data set is essential to test the accuracy of the model and whether it is overfitting. Before each epoch, validation would be randomly selected, $20\%$ of total data.
    \end{itemize}

    After a couple of CNN designing and training, Some conclusion can be summarized,
    \begin{itemize}
        \item The results are tightly related to CNN structure. If CNN architecture is simple, it is easy to get underfitting results. However, too complicated CNN architecture would cause overfitting easily. My solution for that is to extend the number of epochs. Finally, the loss of the model performed well.
        \item Another advantage of enough epochs is that the model would get convergence completely.
        \item Overall, the results are not too sensitive to the training configuration except learning rate. If we keep the same learning rate during the whole training, it is hard to get more optimal when epoch reaches a high number.
    \end{itemize}

\section{Simualtion based on Trained CNN Model}
Once getting the trained model, the next step is to apply this model in simulation based on Algorithm \ref{al:basic} and compare it with other solutions. Similarly, I would define a \textbf{CNN-Model}. The details of the workflow are described in Algorithm \ref{al:basic}.
\begin{itemize}
    \item \textbf{CNN-Model}, for each time-step simulation, the bodies state would be mapped into a bodies image $\pmb{G}_{\mathcal{B}}$ firstly. Then, the contact image $\pmb{G}_{\lambda}$ would be predicted based on $\pmb{G}_{\mathcal{B}}$ through the CNN model. Then interpolated values in contact position would be used as warm starting for contact solver.
\end{itemize}
\begin{figure}[!h]
    \centering
    \includegraphics[width=0.8\textwidth]{Figures/cnn.pdf}
    \caption{The final result. Add the final CNN solution to compare with other methods.}
    \label{testoneworld}
\end{figure}

I applied it in one test world built by the setting mentioned in section \ref{simconfig}(the number of objects is selected randomly). The test world is consist one $30 \times 30 $ box and $K$($50<K<100$) randomly distributing balls($r=1$). The balls will fall down due to gravity. Figure \ref{testoneworld} shows the details. As what is expected, CNN model performs similar with \textbf{SPH-Model}(mentioned in section \ref{sphtest}). Although the model is not as good as \textbf{SPH-Model}, its convergence rate is obviously faster than built-in ones.

\begin{figure}[!h]
        \centering
        \begin{subfigure}[b]{0.8\textwidth}
            \includegraphics[width=\textwidth]{Figures/cnntime}
            \caption{Time spent for contact solver iteration}
            \label{fig:iter}
        \end{subfigure}
        \begin{subfigure}[b]{0.8\textwidth}
            \includegraphics[width=\textwidth]{Figures/cnntotal}
            \caption{Time spend for the whole contact solution(including warm starting calculation).}
            \label{Total}
        \end{subfigure}
        \caption{Time spent for simulation.}
        \label{fig:time}
\end{figure}

From Figure \ref{testoneworld}, it can be indicated that
\begin{itemize}
    \item \textbf{CNN-Model} actually makes the iterative solver converge faster. But it is not a great improvement to buit-in warm starting.
    \item \textbf{CNN-Model} performs very similar to \textbf{SPH-Model}, which means the CNN predicts contact image well.
    \item Due to the limitation of the SPH-based method, \textbf{CNN-Model} just gets a small improvement compared with \textbf{Builtin-Model}. And it cannot perform as good as \textbf{SPH-Model}.
\end{itemize}

\begin{algorithm}[!ht]
        \KwData{
            Given a set of bodies $\mathcal{B}_{t}~~t\in T$ in time $t$, a trained CNN model $CNN$, spatial positions of all grid nodes and one .
        }
        \KwResult{Get the contact forces $\pmb{\lambda}_{t}$ and make simulation keep running.}
        \For{all $t$ in $T$}
        {
            1. Read $\mathcal{B}_t$. \\
            2. map the bodies state to a gird image,
                $$\pmb{G}_{\mathcal{B}} \gets Bodies2grid(\mathbf{x}, \mathcal{B}_t)$$ \\
            3. Transform bodies imgae $\pmb{G}_{\mathcal{B}}$ to a contact grid image $\pmb{G}_{\lambda}$,
            $$\pmb{G}_{\lambda} \gets cnn(\pmb{G}_{\mathcal{B}})$$
            4. Once the contact force image $\pmb{G}_{\lambda}$ is obtained,  then \\
            \For{all $j \in \mathcal{C}$}{$$\pmb{\lambda}_{j} \gets interpolate(\pmb{G}_{\lambda}, \mathbf{x}, \pmb{q}_{j})$$}
            4. Using $\pmb{\lambda}$ as a warm starting to the iterative solver to get the final solution,
            $$\pmb{\lambda} \gets Solver(\pmb{A}, \pmb{b}, \pmb{\lambda})$$
            where $\pmb{A}$ and $\pmb{b}$ would be computed by simulation engine.\\
            5. Updating the simulation world(velocity and position) by $\pmb{\lambda}$ and $t = t+\Delta t$
        }
        \caption{Algorithm describing the workflow of \textbf{CNN-Model}.}
        \label{al:basic}
\end{algorithm}

In the next step, I want to review whether the CNN model can speed up the simulation. I change the fixed iteration steps to a threshold($5\times10^{-4}$), which means when the convergence rate is less than $5\times10^{-4}$, the solver will stop the iteration.

The results is indicated in Figure \ref{fig:time}. The left figure, \ref{fig:iter}, just describes the time purely spent on iteraction solver. Overall, \textbf{CNN-Model} iterates to convergence faster than \textbf{Builin-Model}. It seems a good solution. However, the whole solution would include interpolation computing time. The time spent on whole contact solution is described on Figure \ref{Total}. The total time spent by \textbf{Builtin-Model} and \textbf{CNN-Model}. So, if we can find a more efficient interpolation algorithm or improve the performance of \textbf{CNN-Model} on convergence, the CNN-based method would be a better warm starting.


\subsection{Unconditional Queries}
\label{uncondq}
The unconditional probability of an atom can be asked using \verb|pita| with the predicate
\begin{verbatim}
prob(:Query:atom,-Probability:float) is nondet
\end{verbatim}
as in
\begin{verbatim}
?- prob(heads(coin),P).
\end{verbatim}
If the query is non-ground, \verb|prob/2| returns in backtracking the successful instantiations together with their probability.

When using \verb|mcintyre|, the predicate for querying is
\begin{verbatim}
mc_prob(:Query:atom,-Probability:float,+Options:list) is det
\end{verbatim} where
\verb|Options| is a list of options, the following are recognised by \verb|mc_prob/3|:
\begin{itemize}
\item \verb|bar(-BarChar:dict)|
BarChart is a dict for rendering with c3 as a bar chart with
a bar for the number of successes and a bar for the number
of failures.
\end{itemize}
For example
\begin{verbatim}
?- mc_prob(heads(coin),P,[]).
\end{verbatim}
You can also use
\begin{verbatim}
mc_prob(:Query:atom,-Probability:float) is det
\end{verbatim}
which is equivalent to \verb|mc_prob/3| with an empty option list. In general, all the predicates that admit
a list of options as an argument have a corresponding version without the list of options that is equivalente
to calling the first with an empty option list.

With \verb|mcintyre|, you can also take a given number of samples with
\begin{verbatim}
mc_sample(:Query:atom,+Samples:int,-Probability:float,
  Options:list) is det
\end{verbatim}
where
\verb|Options| is a list of options, the following are recognised by \verb|mc_sample/4|:
\begin{itemize}
\item \verb|successes(-Successes:int)|
Number of succesess
\item \verb|failures(-Failures:int)|
Number of failures
\item \verb|bar(-BarChar:dict)|
\verb|BarChart| is a dict for rendering with c3 as a bar chart with
a bar for the number of successes and a bar for the number
of failures.
\end{itemize}
For example (\href{http://cplint.eu/e/coinmc.pl}{\texttt{coinmc.pl}})
\begin{verbatim}
?- mc_sample(heads(coin),1000,P,[successes(S),failures(F)]).
\end{verbatim}
that samples \verb|heads(coin)| 1000 times and returns in \verb|S| the number of successes, in \verb|F| the number of failures and in \verb|P| the
estimated probability (\verb|S/1000|).
As another example, the call
\begin{verbatim}
?- mc_sample(heads(coin),1000,Prob).
\end{verbatim}
samples \verb|heads(coin)| 1000 times and returns the
estimated probability that a sample is true.

You can also sample using Gibbs sampling with
\begin{verbatim}
mc_gibbs_sample(:Query:atom,+Samples:int,-Probability:float,
  +Options:list) is det
\end{verbatim}
where
\verb|Options| is a list of options, the following are recognised by \verb|mc_gibbs_sample/4|:
\begin{itemize}
\item \verb|block(+Block:int)|
   Perform blocked Gibbs: \verb|Block| variables are sampled together, default value 1
\item \verb|mix(+Mix:int)|
   The first \verb|Mix| samples are discarded (mixing time), default value 0
\item \verb|successes(-Successes:int)|
Number of succesess
\item \verb|failures(-Failures:int)|
Number of failures
\end{itemize}
\verb|mc_gibbs_sample/3| is equivalent to \verb|mc_gibbs_sample/4| with an empty option list.

Moreover, you can sample arguments of queries with
\begin{verbatim}
mc_sample_arg(:Query:atom,+Samples:int,?Arg:var,
  -Values:list,+Options:list) is det
\end{verbatim}
The predicate samples \verb|Query| a number of \verb|Samples| times.
\verb|Arg| should be a variable in \verb|Query|.
The predicate returns in \verb|Values| a list of couples \verb|L-N| where
\verb|L| is the list of values of \verb|Arg| for which \verb|Query|
succeeds in a world sampled at random and \verb|N|
is the number of samples returning that list of values.
If \verb|L| is the empty list, it means that for that
sample the query failed.
If \verb|L| is a list with a
single element, it means that for that sample the query is
determinate.
If, in all couples \verb|L-N|, \verb|L|
is a list with a
single element, it means that the clauses in the program
are mutually exclusive, i.e., that in every sample, only
one clause for each subgoal has the body true. This is one
of the assumptions taken for programs of the PRISM system \cite{DBLP:journals/jair/SatoK01}.
For example
\href{http://cplint.eu/e/pcfglr.pl}{\texttt{pcfglr.pl}} and \href{http://cplint.eu/e/plcg.pl}{\texttt{plcg.pl}} satisfy this constraint while
 \href{http://cplint.eu/e/markov_chain.pl}{\texttt{markov\_chain.pl}} and \href{http://cplint.eu/e/var_obj.pl}{\texttt{var\_obj.pl}} doesn't.

\verb|Options| is a list of options, the following are recognised by \verb|mc_sample_arg/5|:
\begin{itemize}
\item \verb|successes(-Successes:int)|
Number of succeses
\item \verb|failures(-Failures:int)|
Number of failueres
\item \verb|bar(-BarChar:dict)|
\verb|BarChart| is a dict for rendering with c3 as a bar chart with
 with a bar for each possible value of \verb|L|, the list of values of
 \verb|Arg| for which the query succeeds in a world sampled at random.
 The size of the bar is the number of samples
returning that list of values.
\end{itemize}

An example of use of \verb|mc_sample_arg/4| is
\begin{verbatim}
?- mc_sample_arg(reach(s0,0,S),50,S,Values).
\end{verbatim}
of \href{http://cplint.eu/e/markov_chain.pl}{\texttt{markov\_chain.pl}}
that takes 50 samples of \verb|L| in \verb|findall(S,(reach(s0,0,S),L)|.

You can sample arguments of queries also with
\begin{verbatim}
mc_sample_arg_raw(:Query:atom,+Samples:int,?Arg:var,
  -Values:list) is det
\end{verbatim}
that samples \verb|Query| a number of \verb|Samples| times
The predicate returns in \verb|Values| a list of values
of \verb|Arg|  returned as the first answer by \verb|Query|  in
a world sampled at random.
The value is \verb|failure| if the query fails.

The predicate
\begin{verbatim}
mc_sample_arg_first(:Query:atom,+Samples:int,?Arg:var,
  -Values:list,+Options:list) is det
\end{verbatim}
samples \verb|Query| a number of \verb|Samples| times
and returns in \verb|Values| a list of couples \verb|V-N| where
\verb|V| is the value of \verb|Arg| returned as the first answer by \verb|Query| in
a world sampled at random and \verb|N| is the number of samples
returning that value.
\verb|V| is failure if the query fails.
\verb|mc_sample_arg_first/5| differs from \verb|mc_sample_arg/5| because the first just computes the first
answer of the query for each sampled world.

\verb|Options| is a list of options, the following are recognised by \verb|mc_sample_arg_first/5|:
\begin{itemize}
\item \verb|bar(-BarChar:dict)|
  \verb|BarChart| has a bar for each value of \verb|Arg| returned as a first answer for the query in
   a world sampled at random.
   The size of the bar is the number of samples that returned that value.
\end{itemize}

The predicate
\begin{verbatim}
mc_sample_arg_one(:Query:atom,+Samples:int,?Arg:var,
  -Values:list,+Options:list) is det
\end{verbatim}
 samples \verb|Query| a number of \verb|Samples| times
and returns in \verb|Values| a list of couples \verb|V-N| where
\verb|V| is a value sampled with uniform probability from those returned
by \verb|Query| in a world sampled at random and \verb|N| is the number of samples
returning that value.
\verb|V| is failure if the query fails.

\verb|Options| is a list of options, the following are recognised by \verb|mc_sample_arg_one/5|:
\begin{itemize}
\item \verb|bar(-BarChar:dict)|
\verb|BarChart| has a bar for each value of \verb|Arg| returned by sampling with uniform
    probability one answer from those returned by the query in a world sampled
    at random.
    The size of the bar is the number of samples.
\end{itemize}

The predicate
\begin{verbatim}
mc_gibbs_sample_arg(:Query:atom,+Samples:int,?Arg:var,-
  Values:list,+Options:list) is det
\end{verbatim}
samples an argument of the query using Gibbs sampling. The same options as those of \verb|mc_gibbs_sample/4| are
recognized.

Finally, you can compute expectations with
\begin{verbatim}
mc_expectation(:Query:atom,+N:int,?Arg:var,-Exp:float) is det
\end{verbatim}
that computes the expected value of \verb|Arg| in \verb|Query| by
sampling.
It takes \verb|N| samples of \verb|Query| and sums up the value of \verb|Arg| for
each sample. The overall sum is divided by \verb|N| to give \verb|Exp|.

An example of use of the above predicate is
\begin{verbatim}
?- mc_expectation(eventually(elect,T),1000,T,E).
\end{verbatim}
of \href{http://cplint.eu/e/pctl_slep.pl}{\texttt{pctl\_slep.pl}}
that returns in \verb|E| the expected value of \verb|T| by taking 1000 samples.

The predicate
\begin{verbatim}
mc_gibbs_expectation(:Query:atom,+N:int,?Arg:var,
  -Exp:float) is det
\end{verbatim}
computes an expectation with Gibbs sampling.

\subsubsection{Drawing BDDs}

With \verb|pita|, you can obtain the BDD for a query with the predicates
\begin{verbatim}
bdd_dot_file(:Query:atom,+FileName:string,-Var:list) is nondet
bdd_dot_string(:Query:atom,-DotString:string,-Var:list) is nondet
\end{verbatim}
The first write the BDD to a file, the latter returns it as a string.
The BDD is represented in the dot format of graphviz.
Solid edges indicate 1-children, dashed edges indicate 0-children and dotted
edges indicate 0-children with negation applied to the sub BDD.
Each level of the BDD is associated to a variable of the form XI\_J indicated on the left:
I indicates the multivalued variable index and J the index of the Boolean variable of rule I.
The hexadecimal number in each node is part of its address in memory and is not significant.
The table \verb|Var| contains the associations between the rule groundings and the
multivalued variables: the first column contains contains the multivalued variable index,
the second column contains the rule index, corresponding
to its position in the program, and the last column contains the list
of constants grounding the rule, each replacing a variable in the order of appearance in the
rule.

The BDD can be drawn in \verb|cplint| on SWISH by using the \verb|graphviz| renderer
by including
\begin{verbatim}
:- use_rendering(graphviz).
\end{verbatim}
before \verb|:- pita.|

For example (\href{http://cplint.eu/e/coin.pl}{\texttt{coin.pl}})
\begin{verbatim}
?- bdd_dot_string(heads(coin),BDD,Var).
\end{verbatim}
returns the BDD for the query \verb|heads(coin)| and the list of associations between rule groundings and
multivalued variables.

\subsection{Conditional Queries on Discrete Variables}
\label{condq}
The conditional probability of an atom query given another atom evidence can be asked using \verb|pita| with the predicate
\begin{verbatim}
prob(:Query:atom,:Evidence:atom,-Probability:float) is nondet
\end{verbatim}
as in
\begin{verbatim}
?- prob(heads(coin),biased(coin),P).
\end{verbatim}
If the query/evidence are non-ground, \verb|prob/3| returns in backtracking ground instantiations together with their probability. The query and the evidence can be conjunctions of literals (positive or negative).

You also have
\begin{verbatim}
prob(:Query:atom,:Evidence:atom,-Probability:float,
  +Options:list) is nondet
\end{verbatim}
where \verb|Options| is a list of options, the following are recognised by \verb|prob/4|:
\begin{itemize}
\item \verb|bar(-BarChar:dict)|
\verb|BarChart| is a dict for rendering with c3 as a bar chart with
a bar for the number of successes and a bar for the number
of failures.
\end{itemize}
as in
\begin{verbatim}
?- prob(heads(coin),biased(coin),P,[bar(Chart)}).
\end{verbatim}
\verb|prob/3| is equivalent to \verb|prob/4| with an empty option list.

When using \verb|mcintyre|, you can ask conditional queries with rejection sampling, Metropolis-Hastings Markov Chain Monte Carlo or Gibbs sampling.
In rejection sampling \cite{von195113}, you first query the evidence and, if the query is successful, query the goal in the same sample, otherwise
the sample is discarded.
In Metropolis-Hastings MCMC, \verb|mcintyre| follows the algorithm proposed in \cite{nampally2014adaptive} (the non adaptive version).
A Markov chain is built by building an initial sample and by generating successor samples.

The initial sample is built by  randomly sampling choices so that the evidence is true. This is done with
a backtracking meta-interpreter that starts with the goal and
randomizes the order in which clauses are selected during the search so that the initial sample is unbiased. Each time the meta-interpreter encounters
a probabilistic choice, it first checks whether a
value has already been sampled, if not, it takes
a sample and records it. If a failure is obtained,
the meta-interpreter backtracks to other clauses but
without deleting samples. Then the goal is queries using
regular MCINTYRE.

A successor sample is obtained by deleting a
fixed number (parameter \verb|Lag|) of sampled probabilistic choices. Then the
evidence is queried using regular MCINTYRE starting with the undeleted choices.
If the query succeeds, the goal is queried using regular MCINTYRE.
The sample is accepted with a probability of $\min\{1,\frac{N_0}{N_1}\}$ where $N_0$ is the number of choices sampled
in the previous sample and $N_1$ is the number of choices sampled in the current sample.
In \cite{nampally2014adaptive} the lag is always 1 but the proof in \cite{nampally2014adaptive} that the above acceptance
probability yields a valid
Metropolis-Hastings algorithm holds also when forgetting more than one
sampled choice, so the lag is
user defined in \verb|cplint|.

Then the number of successes of the query is increased by 1 if the query succeeded in the last accepted
sample. The final probability is given by the number of successes over the total
number of samples.

You can take a given number of sample with rejection sampling using
\begin{verbatim}
mc_rejection_sample(:Query:atom,:Evidence:atom,+Samples:int,
  -Probability:float,+Options:list) is det
\end{verbatim}
where
\verb|Options| is a list of options, the following are recognised by \verb|mc_sample_arg/5|:
\begin{itemize}
\item \verb|successes(-Successes:int)|
Number of succeses
\item \verb|failures(-Failures:int)|
Number of failueres
\end{itemize}
as in (\href{http://cplint.eu/e/coinmc.pl}{\texttt{coinmc.pl}})
\begin{verbatim}
?- mc_rejection_sample(heads(coin),biased(coin),1000,P,
  [successes(S),failures(F)]).
\end{verbatim}
that takes 1000 samples where \verb|biased(coin)| is true and returns in \verb|S| the number of samples where
\verb|heads(coin)| is true, in \verb|F| the number of samples where \verb|heads(coin)| is false and in \verb|P| the
estimated probability (\verb|S/1000|).

The query and the evidence can be conjunctions of literals.

You can take a given number of sample with Metropolis-Hastings MCMC using
\begin{verbatim}
mc_mh_sample(:Query:atom,:Evidence:atom,+Samples:int,
  -Probability:float,+Options:list) is det
\end{verbatim}
where \verb|Lag| (that is set with the options, default value 1) is the number of sampled choices to forget before taking a new sample.

\verb|Options| is a list of options, the following are recognised by \verb|mc_mh_sample/5|:
\begin{itemize}
\item \verb|mix(+Mix:int)|
The first Mix samples are discarded (mixing time), default value 0
\item \verb|lag(+Lag:int)|
lag between each sample, Lag sampled choices are forgotten, default value 1
\item \verb|successes(-Successes:int)|
Number of succeses
\item \verb|failures(-Failures:int)|
Number of failueres
\item \verb|bar(-BarChar:dict)|
BarChart is a dict for rendering with c3 as a bar chart with
a bar for the number of successes and a bar for the number
of failures.
\end{itemize}
With \verb|Mix| specified it takes \verb|Mix+Samples| samples and discards the first \verb|Mix|.

For example (\href{http://cplint.eu/e/arithm.pl}{\texttt{arithm.pl}})
\begin{verbatim}
?- mc_mh_sample(eval(2,4),eval(1,3),10000,P,
  [successes(T), failures(F)]).
\end{verbatim}
takes 10000 accepted samples and returns in \verb|T| the number of samples where
\verb|eval(2,4)| is true, in \verb|F| the number of samples where \verb|eval(2,4)| is false and in \verb|P| the
estimated probability (\verb|T/10000|).

The predicate
\begin{verbatim}
mc_gibbs_sample(:Query:atom,:Evidence:atom,+Samples:int,
  -Probability:float,+Options:list) is det
\end{verbatim}
performs Gibbs sampling.
\verb|Options| is a list of options, the following are recognised by \verb|mc_gibbs_sample/5|:
\begin{itemize}
\item \verb|block(+Block:int)|
   Perform blocked Gibbs: \verb|Block| variables are sampled together, default value 1
\item \verb|mix(+Mix:int)|
   The first \verb|Mix| samples are discarded (mixing time), default value 0
\item \verb|successes(-Successes:int)|
Number of succesess
\item \verb|failures(-Failures:int)|
Number of failures
\end{itemize}
Moreover, you can sample arguments of queries with rejection sampling, Metropolis-Hastings MCMC or Gibbs sampling using
\begin{verbatim}
mc_rejection_sample_arg(:Query:atom,:Evidence:atom,
  +Samples:int,?Arg:var,-Values:list,+Options:list) is det
mc_mh_sample_arg(:Query:atom,:Evidence:atom,
  +Samples:int,?Arg:var,-Values:list,+Options:list) is det
mc_gibbs_sample_arg(:Query:atom,+Samples:int,
  ?Arg:var,-Values:list,+Options:list) is det
\end{verbatim}
that return the distribution of values for \verb|Arg| in \verb|Query| in \verb|Samples| of
\verb|Query| given that \verb|Evidence| is true.
\verb|Options| is a list of options, the following are recognised:
\begin{itemize}
\item \verb|mix(+Mix:int)|
The first  \verb|Mix| samples are discarded (mixing time), default value 0 (only MH and GIbbs)
\item \verb|lag(+Lag:int)|
lag between each sample, \verb|Lag| sampled choices are forgotten, default value 1 (only MH)
\item \verb|block(+Block:int)|
   Perform blocked Gibbs: \verb|Block| variables are sampled together, default value 1 (only Gibbs)
\item \verb|bar(-BarChar:dict)|
    \verb|BarChart| is a bar chart of the possible values
\end{itemize}
The predicates return in \verb|Values| a list of couples \verb|L-N| where
\verb|L| is the list of values of \verb|Arg| for which \verb|Query|
succeeds in a world sampled at random where \verb|Evidence| is true and \verb|N|
is the number of samples returning that list of values.
\begin{verbatim}
mc_gibbs_sample_arg(:Query:atom,+Samples:int,
  ?Arg:var,-Values:list,+Options:list) is det
\end{verbatim}
An example of use of the above predicates is
\begin{verbatim}
?- mc_mh_sample_arg(eval(2,Y),eval(1,3),1000,Y,V,[]).
\end{verbatim}
of \href{http://cplint.eu/e/arithm.pl}{\texttt{arithm.pl}}.

To compute conditional expectations, use
\begin{verbatim}
mc_rejection_expectation(:Query:atom,:Evidence:atom,+N:int,
  ?Arg:var,-Exp:float) is det
mc_mh_expectation(:Query:atom,:Evidence:atom,+N:int,
  ?Arg:var,-Exp:float,+Options:list) is det
mc_gibbs_expectation(:Query:atom,:Evidence:atom,+N:int,
  ?Arg:var,-Exp:float,+Options:list) is det
\end{verbatim}
where \verb|Options| is a list of options, the same as those of the predicates for conditional argument
sampling are recognised.
For example
\begin{verbatim}
?- mc_mh_expectation(eval(2,Y),eval(1,3),1000,Y,E,[]).
\end{verbatim}
of \href{http://cplint.eu/e/arithm.pl}{\texttt{arithm.pl}}
computes the expectation of argument \verb|Y| of \verb|eval(2,Y)| given that
\verb|eval(1,3)| is true by taking 1000 samples using Metropolis-Hastings MCMC.

Note that conditional inference  is not allowed for PRISM programs with the setting \verb|prism_memoization| set to
\verb|false|, as sampled values are not stored in that case and conditioning would have no effect.

\subsection{Conditional Queries on Continuous Variables}
\label{condqcont}

When you have continuous random variables, you may be interested in
sampling arguments of goals representing continuous random variables.
In this way you can build a probability density of the sampled argument.
When you do not have evidence or you have evidence on atoms not depending
on continuous random variables, you can use the above predicates for sampling
arguments.

For example
\begin{verbatim}
?- mc_sample_arg(val(0,X),1000,X,L).
\end{verbatim}
from (\href{http://cplint.eu/e/gauss_mean_est.pl}{\texttt{gauss\_mean\_est.pl}})) samples 1000 values for \verb|X| in
\verb|value(0,X)| and returns them in \verb|L|.

When you have evidence on ground atoms that have continuous values as
arguments, you cannot use rejection sampling or Metropolis-Hastings,
as the probability of the evidence is 0. For example,
the probability of sampling a specific value from a Gaussian is 0.
Continuous variables have probability densities instead of distributions as
discrete variables.
In this case, you can use likelihood weighting or particle filtering \cite{fung1990weighing,koller2009probabilistic,Nitti2016} to obtain samples of
continuous arguments of a goal.

For each sample to be taken, likelihood weighting
uses a meta-interpreter to find a sample where
the goal is true, randomizing the choice of
clauses when more than one resolves with the goal
in order to obtain an unbiased sample.
This meta-interpreter is similar to the one used
to generate the first sample in Metropolis-Hastings.

Then a different meta-interpreter is used to evaluate
the weight of the sample.
This meta-interpreter starts with the evidence as the query and a weight of 1. Each time the meta-interpreter encounters
a probabilistic choice over a continuous variable, it first checks whether a
value has already been sampled.
If so, it computes the probability density of the sampled value and multiplies the weight by it. If the value has not been sampled, it takes a sample and records it,
leaving the weight unchanged.
In this way, each sample in the result has a weight that is 1 for the prior distribution and that may be different from the posterior distribution,
reflecting the influence of evidence.

In particle filtering, the evidence is a list of atoms. Each sample is weighted by the
likelihood of an element of the evidence and constitutes a particle.
After weighting, particles are resampled and the next element of the evidence
is considered.

 The predicate
\begin{verbatim}
mc_lw_sample(:Query:atom,:Evidence:atom,+Samples:int,
  -Prob:float) is det
\end{verbatim}
samples \verb|Query|  a number of \verb|Samples| times given that \verb|Evidence|
(a conjunction of atoms is allowed here) is true.
The predicate returns in \verb|Prob| the probability that the query is true.
It performs likelihood weighting: each sample is weighted by the
likelihood of evidence in the sample.
For example
\begin{verbatim}
?- mc_lw_sample(nation(a),student_gpa(4.0),1000,P).
\end{verbatim}
from \href{http://cplint.eu/e/indian_gpa.pl}{\texttt{indian\_gpa.pl}} samples 1000 times the query
\verb|nation(a)| given that \verb|student_gpa(4.0)| has been observed.

 The predicate
\begin{verbatim}
mc_lw_sample_arg(:Query:atom,:Evidence:atom,+N:int,?Arg:var,
  -ValList) is det
\end{verbatim}
returns in \verb|ValList| a list of couples \verb|V-W| where \verb|V| is a value of \verb|Arg|
for which \verb|Query| succeeds and \verb|W| is the
weight computed by likelihood weighting
according to \verb|Evidence| (a conjunction of atoms is allowed here).
For example
\begin{verbatim}
?- mc_lw_sample_arg(val(0,X),(val(1,9),val(2,8)),100,X,L).
\end{verbatim}
from \href{http://cplint.eu/e/gauss_mean_est.pl}{\texttt{gauss\_mean\_est.pl}} samples 100 values for \verb|X| in
\verb|val(0,X)| given that \verb|val(1,9)| and \verb|val(2,8)| have been observed.

You can compute conditional expectations using likelihood weighting with
\begin{verbatim}
mc_lw_expectation(:Query:atom,Evidence:atom,+N:int,?Arg:var,
  -Exp:float) is det
\end{verbatim}
that computes the expected value of \verb|Arg| in \verb|Query| given that
\verb|Evidence| is true.
It takes \verb|N| samples of  \verb|Arg| in \verb|Query|, weighting each according
to the evidence, and returns their weighted average.

The predicate
\begin{verbatim}
mc_particle_sample_arg(:Query:atom,+Evidence:list,
  +Samples:int,?Arg:var,-Values:list) is det
\end{verbatim}
samples argument \verb|Arg| of \verb|Query| using particle filtering
given that
\verb|Evidence|
is true. \verb|Evidence| is a list of goals and \verb|Query| can be either
a single goal or a list of goals.
When \verb|Query| is a single goal, the predicate returns in \verb|Values| a list of couples \verb|V-W| where
\verb|V| is a value of \verb|Arg| for which \verb|Query| succeeds in
a particle in the last set of particles and \verb|W| is the weight of the particle.
For each element of \verb|Evidence|, the particles are obtained by sampling \verb|Query|
in each current particle and weighting the particle by the likelihood of the evidence element.

When \verb|Query| is a list of goals,  \verb|Arg| is a list of variables, one for
each query of \verb|Query| and \verb|Arg| and \verb|Query| must have the same length of \verb|Evidence|.
\verb|Values| is then list of the same length of \verb|Evidence| and each of its
elements is a list of couples \verb|V-W| where
\verb|V| is a value of the corresponding element of \verb|Arg| for which the corresponding element of
\verb|Query| succeeds in
a particle and \verb|W| is the weight of the particle.
For each element of \verb|Evidence|, the particles are obtained by sampling the corresponding element of \verb|Query|
in each current particle and weighting the particle by the likelihood of the evidence element.

For example
\begin{verbatim}
?-[O1,O2,O3,O4]=[-0.133, -1.183, -3.212, -4.586],
mc_particle_sample_arg([kf_fin(1,T1),kf_fin(2,T2),kf_fin(3,T3),
  kf_fin(4,T4)],
  [kf_o(1,O1),kf_o(2,O2),kf_o(3,O3),kf_o(4,O4)],100,
  [T1,T2,T3,T4],[F1,F2,F3,F4]).
\end{verbatim}
from \href{http://cplint.eu/e/kalman_filter.pl}{\texttt{kalman\_filter.pl}} performs
particle filtering for a Kalman filter with four observations. For each observation, the value of the state
at the same time point is sampled. The list of samples is returned in \verb|[F1,F2,F3,F4]|, with each element
being the sample for a time point.

The predicate
\begin{verbatim}
mc_particle_sample(:Query:atom,:Evidence:list,
  +Samples:int,-Prob:float) is det
\end{verbatim}
samples \verb|Query|  a number of \verb|Samples| times given that
\verb|Evidence|
is true using particle filtering. \verb|Evidence| is a list of goals.
The predicate returns in \verb|Prob| the probability that the query is true.

You can compute conditional expectations using particle filtering with
\begin{verbatim}
mc_particle_expectation(:Query:atom,Evidence:atom,+N:int,
  ?Arg:var,-Exp:float) is det
\end{verbatim}
that computes the expected value of \verb|Arg| in \verb|Query| given that
\verb|Evidence| is true.
It uses \verb|N| particles.

\subsection{Causal Inference}
\label{causal}

\verb|pita| and \verb|mcintyre| support causal reasoning, i.e., computing the effect of actions using the
do-calculus \cite{Pea00-book}.

Actions in this setting are represented as literals of action predicates, that must be declared as such
with the directive
\begin{verbatim}
:- action predicate1/arity1,...,predicaten/arityn.
\end{verbatim}
When performing causal reasoning, action literals must be enclosed in the \verb|do/1| functor and included in the evidence conjunction. More than one action can be included (each with in a separate
\verb|do/1| term) and actions and observations can be freely mixed.
All conditional inference goals can be used except those for particle filtering.

For example
\begin{verbatim}
?- prob(recovery,do(drug),P).
\end{verbatim}
from \href{http://cplint.eu/e/simpson.swinb}{\texttt{simpson.swinb}}
computes the probability of recovery of a patient given that the action of administering a drug has
been performed.


\section{System model and simulations}
This section thoroughly covers the models and simulations of the two subsystems that constitute the mechatronical system:
the mobile platform and the arm which is attached to the platform.
The software used in this project is covered in section~\ref{sec:implementation}.

\subsection{Model}

The system is modeled in two parts: the moving base and the\ldots
% TODO: describe the moving base, and the attachment, whatever it ends up being.
% Include physical models.

% Describe the states of the system.
We argue for the following five states of the system:
\begin{inline-enum}
\item waiting for a command;
\item moving to a destination;
\item following a navigation-line;
\item picking an object up; and
\item dropping an object off.
\end{inline-enum}
The relation between these five states are visually described in the state machine of Fig~\ref{fig:state_machine}.
\begin{figure*}[ht]
  \centering
  \begin{tikzpicture}
    \node[state, initial, accepting] (wait) {waiting for \\ command};
    \node[state, right of=wait] (move) {moving to \\ destination};
    \node[state, below of=move] (follow) {following \\ line};
    \node[state, right of=follow] (pickup) {picking \\ up object};
    \node[state, left of=follow] (dropoff) {dropping \\ off object};

    \path[->] (wait) edge node{Arrowhead \\ signal} (move)
    (move) edge[sloped] node{line detected \\} (follow)
    (follow) edge[sloped] node{station detection \\ (system w/o object)} (pickup)
    (follow) edge[sloped] node{station detection \\ (system w/ object)} (dropoff)
    (pickup) edge[sloped] node{object picked up \\} (move)
    (dropoff) edge[sloped] node{object dropped \\} (wait)
    ;
  \end{tikzpicture}
  \caption{High-level state machine of the system.}
  \label{fig:state_machine}
\end{figure*}

\subsubsection{Motor}
\begin{description}
\item[PID contoller for the LEGO EV3 motor]
The transfer function of the LEGO EV3 motor is assumed as eq.~\eqref{eq1}.This equation is called as second order transfer function. The input refers to the input power and the output refers to to wheel rotation in degrees.

\begin{equation}\label{eq1}
    G(s)=\frac{Y(s)}{U(s)}=\frac{W_n^2}{s^2+2\xi W_n s + W_n^2}
\end{equation}
where $W_n$ is the natural frequency and $\xi$ is the damping ratio.  These constants are given as:

\begin{equation}
\begin{split}
     W_n & = 14.2 \\
    \xi & = 0.92
\end{split}
\end{equation}
Then equation \ref{eq1} becomes :
\begin{equation}
    G(s)=\frac{201.9}{s^2+26.17s+201.9}
\end{equation}
The PID controller in the Laplace transform is described as
    \begin{equation}
    C(s)= K_P+K_I \frac{1}{s}+K_Ds= \frac{K_Ps+K_I+K_Ds^2}{s}
\end{equation}
where $K_p$,$K_I$ and $K_D$ is the proportional, integral and derivative terms respectively.
\item[Discretization of PID controller]
The PID-controller was discretized by zero order hold method. The ZOH method give a perfect match between continuous and discrete time domain. This method can also be used for system with input delays, output delays or transport delays and provides an exact discretization.
The eq. (\ref{eq5}) describes how to transform from s-domain to z-domain using Zero order hold method
\begin{equation} \label{eq5}
    C(z)=(1-z^{-1}) Z(\frac{C(s)}{s})
\end{equation}
where C(s) is the controller and Z is the Z-transform
\begin{equation}
    C(z)=(\frac{z-1}{z})Z(\frac{k_Ps+k_I+k_Ds^2}{s} \Big/ s)
    \label{eq7}
\end{equation}
By looking at the z-transform table, the eq.~\eqref{eq7} yields to
\begin{equation}
    C(z)=\frac{z-1}{z}\bigg( k_p \frac{1}{1-z^{-1}}+k_I \frac{T z^{-1}}{(1-z^{-1})^2}+k_D\bigg)
\end{equation}
And finally the PID controller in discrete time domain is presented as
\begin{equation}
    C(z)= k_P + k_I \frac{1}{z-1} + k_D \frac{z-1}{z}
\end{equation}

\item[Motor simulation]
The PID parameters were chosen to get low settling time, low overshoot and fast response to disturbances for the system. The best parameters for the system resulted in
\begin{equation}
    \begin{split}
        K_P & = 1.35 \\
        K_I & = 6 \\
        K_D & = 0.08 \\
    \end{split}
\end{equation}
The simulation is made in discrete time domain with a sampling frequency 10 Hz.
\begin{figure}[ht]
    \centering
    \includegraphics[width=0.4\textwidth]{sections/assets/pid_motor.png}
    \caption{PID controller for the LEGO EV3 motor to rotate 10 degrees}
    \label{fig1}
\end{figure}
\end{description}
\subsubsection{Trajectory planning}
A point to point trajectory is described as a path from $q_{init}$ to $q_{final}$. A trajectory is a function of time such that $q(t_0) = q_{init}$ and $q(t_f) = q_{final}$. The difference between $t_0$ and $t_f$ represents the total elapsed time to execute the trajectory. The velocity and the acceleration can be computed by differentiation since the trajectory is depending of time.
At the time $t_0$ the joint variable satisfies

\begin{equation} \label{eq10}
\begin{split}
     q(t_0) & = q_0 \\
    \dot{q}(t_0) & = v_0
\end{split}
\end{equation}
And at the $t_f$ the desired joint variable will be
\begin{equation} \label{eq11}
\begin{split}
     q(t_f) & = q_f \\
    \dot{q}(t_f) & = v_f
\end{split}
\end{equation}

The trajectory between two configurations and the initial and final velocity is known. One way to solve this task is to solve it by polynomial function of t. Since there are four unknown constraints, it is required to use a polynomial function with four independent coefficient. Hence a cubic trajectory is considered
\begin{equation} \label{eq12}
    q(t) = a_0 + a_1t + a_2t^2 + a_3t^3
\end{equation}
Then the velocity of the cubic trajectory becomes
\begin{equation} \label{eq13}
    \dot{q}(t) = a_1 + 2a_2t+ 3 a_3t^2
\end{equation}
Combining eq.~\eqref{eq12} and ~\eqref{eq13} with eq.~\eqref{eq10} and \ref{eq11} returns four equations with four unknowns
\begin{equation} \label{14}
    \begin{split}
        q_0 &= a_0 + a_1t_0 + a_2t_0^2 + a_3t_0^3 \\
        v_0 & = a_1 + 2a_2t+ 3 a_3t^2 \\
        q_f &= a_0 + a_1t_f + a_2t_f^2 + a_3t_f^3 \\
        v_f & = a_1 + 2a_2t_f+ 3 a_3t_f^2
    \end{split}
\end{equation}
Suppose taking $t_0 = 0$ and $t_f = 1$ second with velocity constraints as $v_0 = 0$ and $v_f = 0$. Then the eq. (\ref{14}) can be written in matrix form.
\begin{equation*}
    \begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
 1 & 1& 1& 1 \\
0&1 &2 &3
\end{bmatrix} \begin{bmatrix}
a_0 \\
a_1 \\
a_2 \\
a_3
\end{bmatrix} = \begin{bmatrix}
q_0 \\
0\\
q_f \\
0
\end{bmatrix}
\end{equation*}
Then solving for the letter $a_i$ by linear algebra and this letters yields to
\begin{equation} \label{15}
    \begin{split}
        a_0 = q_0 \\
        a_1 = 0 \\
        a_2 = 3(q_f - q_0 ) \\
        a_3 = -2(q_f - q_0)
    \end{split}
\end{equation}
The eq. (\ref{eq12}) into eq. (\ref{15}) to get the cubic polynomial function. The required cubic polynomial function with the supposed constraints is therefore
\begin{equation}
    q_i(t) = q_0 + 3(q_f - q_0)t^2 - 2(q_f - q_0)t^3
\end{equation}
\begin{figure}[ht]
    \centering
    \includegraphics[width=0.4\textwidth]{sections/assets/cubic.png}
    \caption{Angle profile for cubic polynomial trajectory.}
    \label{fig2}
\end{figure}
Fig. (\ref{fig2}) shows the trajectory with $q_0 = 10^\circ$ and $q_f = -30^\circ$.

\subsubsection{Mobile platform}
\label{subsubsec:unicycle}
The model used in this case is the unicycle model, due to the differential steering.
This is because of the mobile platform has only two wheels/trucks and it is not able to apply any steering angle to its wheels.
The only way this robot can change orientation is by giving different velocity on each wheel-driving servo on left- and right- side.
With this feature it is also possible to change the orientation of the mobile platform without changing the position of the platform.
I.e. the robot is able to spin while the right-hand side wheels have the same velocity as the left-hand side wheels, in opposite direction.
\begin{figure}[h]
\centering
\includegraphics[width=0.4\textwidth]{sections/assets/car-unicycle.png}
\caption{Unicycle model of a car-like robot.
$v_L$ and $v_R$ represent the left- and right-hand side wheels' velocities respectively.
The robot follows a path around the instantaneous center of rotation (ICR) where $R_L$ and $R_R$ are the distances from left and right wheels to ICR respectively and $w$ is the distance between them.}
\label{fig:UnicycleModel}
\end{figure}

As shown in Fig.~\ref{fig:UnicycleModel} The robot follows a curved path with the instantaneous center of rotation at its center.
The left-hand side wheels have velocity $v_L$ and moves along an arc with radius $R_L$ during the time that right-hand side wheels moves along another arc with radius $R_R$ at the speed of $v_R$.
The turning rate of the body is
\begin{equation*}
\dot{\theta}= \frac{v_L}{R_L} = \frac{v_R}{R_R}
\end{equation*}
since $R_R = R_L + W$ the expression can be simplified as
\begin{equation}
\dot{\theta}= \frac{v_R - v_L}{W} = \frac{v_\Delta}{W}\label{eq:ThetaDot}
\end{equation}
the equations of motion for this model are
\begin{eqnarray}
\begin{aligned}
\dot{x} &= v\,cos(\theta)\\
\dot{y} &= v\,sin(\theta)\\
\dot{\theta} &= \omega = \frac{v_\Delta}{W}
\end{aligned}
\label{eq:MotionEq}
\end{eqnarray}
where the average velocity \parencite{Corke2011} is given by
\begin{equation}
v = \frac{v_R + v_L}{2}
\label{eq:av_velocity}
\end{equation}
Before the implementation, it is required to to find a discrete equivalent for the systems dynamics.
By being able to count the number of encoder ticks on each servo and some approximation it is possible to estimate the position of the robot.
In this approximative method we assume that the driven path in-between the two sampling points is a straight line.
The length of the driven path for each wheel can be computed using the following formula:

\begin{equation}
    D = 2\,\pi\,r\,\frac{\Delta tick}{N}
\end{equation}\label{eq:encoder-formula}
Where $D$ is the length of the driven path, $r$ is the wheel radius, $\Delta tick$ is the number of recorded ticks during the two sampling points and $N$ is the number of ticks per revolution.
In our case the numeric values are:

\begin{eqnarray*}
    r =& 0.021\, [m]\\
    N =& 360\, [\frac{tick}{2\,\pi}]
\end{eqnarray*}
In this report we will use the notations $D_r$ and $D_l$ for the length of the driven path for the right- and left- hand side wheels respectively.
$D_c$ is the notation we use for the length of the driven path for the body and is similar to Eq.~\eqref{eq:av_velocity}:

\begin{equation}
    D_c = \frac{D_r + D_l}{2}
\end{equation}
The discretized equations of motion is given by:

\begin{eqnarray}
\begin{aligned}
    x(k+1) =& x(k) + D_c(k)\,cos(\theta(k))\\
    y(k+1) =& y(k) + D_c(k)\,sin(\theta(k))\\
    \theta(k+1) =& \theta(k) + \frac{D_r(k)-D_l(k)}{W}
\end{aligned}
\end{eqnarray}\label{eq:Disc_EOM}
Where $W$ is the width of the robot, i.e. the distance between the two wheels.
The limitation of this approximative method is the sampling rate relative the average velocity of the body.
The arc shaped driven path is being approximated by dividing the curve in to several points and recreate a polygonal path using line segments to connect this points together.
E.g. if the average velocity is very high and the sampling rate is very low, the straight line will not be a good approximation for the driven arc shaped path.
To make this approximation more accurate it is required to divide the driven path in to more points.
This accuracy can be improved by having higher sampling rate or lower average velocity.

\subsubsection{Arm}

\subsection{Simulation}
% Simulte the models from the previous section and show that it will work.
% Motivate regulation approach.
\subsubsection{Mobile platform}
A simulation profile is made to make sure of the unicycle mode in sub section ~\ref{subsubsec:unicycle} is the suitable model for this application.
The simulation is made in discrete time domain with the sampling frequency of $10$~Hz.
This is done in the described way due to the robot is going to operate in discrete time.
The sampling frequency is assumed to be low and that is because the designed controllers are going to be able to work fine for higher samplings frequencies but that is not guaranteed for vise-versa.
As the unicycle model describes the systems dynamics.
The position of the robot $(x, y, \theta)$ is dependent on the two wheel velocities. Which in turn has dependencies to the average velocity $V$ and the angular velocity $\omega$. These relations are presented in Eq.~\eqref{eq:ThetaDot} -- Eq.~\eqref{eq:av_velocity}.
\begin{figure*}[h]
\centering
\includegraphics[width=\textwidth]{sections/assets/omegaCtrlr.png}
\caption{Overview of the systems block diagram where the angular velocity $\omega$ is controlled by a PID controller.}
\label{fig:overview}
\end{figure*}

A pole placement analyze is required for the design of a PID controller.
This analyze is done in continues time domain, due to its simplicity.
As mentioned in Eq.~\eqref{eq:MotionEq}
\begin{equation}
\dot{\theta} = \omega
\label{eq:omega}
\end{equation}
By taking the Laplace transform of Eq.~\eqref{eq:omega} gives:
\begin{equation}
s\cdot \Theta(s)= \Omega(s)
\end{equation}
Where $G(s)$ is the transfer function
\begin{equation}
\therefore G(s)=\frac{\Omega(s)}{s\cdot\Theta(s)} = 1
\end{equation}
However it is the angular position $\theta$ which is relevant in this case.
So it is required to implement an integrator in the feedback to make the angular position controllable.
\tikzset{
    block/.style = {draw, rectangle,
    minimum height=1cm,
    minimum width=2cm},
    input/.style = {coordinate,node distance=1cm},
    output/.style = {coordinate,node distance=2cm},
    arrow/.style={draw, -latex,node distance=2cm},
    pinstyle/.style = {pin edge={latex-, black,node distance=2cm}},
    sum/.style = {draw, circle, node distance=1cm}
}
\begin{figure}[h]
    \begin{center}
        \begin{tikzpicture}[auto, node distance=2.5cm,>=latex']
        \node [input, name=input] {};
        \node [sum, right of=input] (sum) {};
        \node [block, right of=sum] (controller) {$PID$};
        \node [block, right of=controller] (plant) {$G(s)$};
        \node [output, right of=plant] (output) {};
        \node [block, below of=plant] (feedback) {$\frac{1}{s}$};
        \draw [draw,->] (input) -- node {$U(s)$} (sum);
        \draw [->] (sum) -- node {} (controller);
        \draw [->] (controller) -- node {} (plant);
        \draw [->] (plant) -- node [name=y] {$Y(s)$}(output);
        \draw [->] (y) |- node [above,pos=0.79] {} (feedback) ;
        \draw [->] (feedback) -| node[pos=0.99] {$-$}
        node [near end] {} (sum);
        \end{tikzpicture}
    \end{center}
    \caption{Closed loop system with a integrator in the feedback.}\label{fig:closedloop}
\end{figure}

In this case the closed loop transfer function will be as shown in Eq.~\eqref{eq:closedloop}
\begin{equation}
G_{c}(s) = \frac{PID}{s + PID}
\label{eq:closedloop}
\end{equation}
As the closed loop transfer function of the system is describing a first order system, it is possible to use a P-controller.
I.e. the controller only has a proportional gain and doesn't require a integrative or derivative elements.
So the final closed loop transfer function will be
\begin{equation}
G_{c}(s) = \frac{K_p}{s + K_p}
\label{eq:kp}
\end{equation}

This system will have a pole at $-K_p$.
Any positive proportional gain will results in a stable pole and which in turn leads to a stable system.
\begin{figure*}[ht]
\centering
\includegraphics[width=\textwidth]{sections/assets/Theta_PID.PNG}
\caption{The complete control block for the angular position in discrete time domain.}
\label{fig:PID1}
\end{figure*}

It is required to make sure that $\theta$ error, $\theta_e$ is in $(-\pi,\pi]$ span. Due to the $\theta = \theta \pm 2 \cdot n \cdot \pi$, it is possible for the error be in out of this span.
By using a function called arctan2 or atan2 in many programming languages it is possible to accomplish this challenge.
This function often has two input argument (y,x).
By defining
\begin{equation}
\theta = \text{atan2}(\sin(\theta),\, \cos(\theta))
\end{equation}
$\theta$ will always be in $(-\pi,\pi]$ span.

Due to the physical limits the servos have, two saturation blocks are required.
These blocks are shown in Fig.~\ref{fig:PID1} and the assumption for their limits are $(-10,10)$~[cm/s].
This assumption is done to make the simulation more alike the real application.
\begin{figure}[ht]
\centering
\includegraphics[width=0.4\textwidth]{sections/assets/Theta_pi_stepresponse.png}
\caption{The step response for $\theta$ where the proportional gain $K_p = 4$ and the reference value is $\pi$.}
\label{fig:Theta_Step}
\end{figure}

The result of the simulation, the step response for the angular position control system is presented in Fig.~\ref{fig:Theta_Step}.
As it shown this is a stable system, it has no steady state error, no oscillation and relative fast rise-time.

In this part of the mobile platforms movement it is important to be able to control the spacial position $(x, y)$.
As shown in the kinematic equations Eq.~\eqref{eq:MotionEq} the positions are dependent on both the angular velocity $\omega$ and the average velocity $v$.
A design of a controller for the average velocity is required to make it possible to control the spacial position.
Take note of the two input \texttt{AverageVelocity}, \texttt{Desired\_Theta} and the two output \texttt{X}, \texttt{Y} in Fig.~\ref{fig:PID1}.

By redefining the spatial error from a Cartesian coordinate system to a polar coordinate system it is possible to design a controller for average velocity.
In this case the error will be defined as following:
\begin{eqnarray}
\begin{aligned}
\Delta x &= Desired\_X - X\\
\Delta y &= Desired\_Y - Y\\
Distance &= \sqrt{\Delta x^2 + \Delta y^2}\\
Desired\_\theta &= \text{atan2}(\Delta y, \Delta x)
\end{aligned}
\label{eq:cart2polar}
\end{eqnarray}

Now by defining the distance as an error to a PID controller for the average velocity, it makes it possible for the mobile platform to stop at the desired position.
In this case a P controller, a controller which has only a proportional gain, is enough.
\begin{figure*}[h]
\centering
\includegraphics[width=\textwidth]{sections/assets/V_PID.PNG}
\caption{Overview of the complete systems block diagram for the spatial position control. Fig.~\ref{fig:PID1} is an overview of the ``Steering'' block and Eq.~\eqref{eq:cart2polar} is the mathematical definition for the block function ``Cartesian- to Polar-coordinates Converter''.}
\label{fig:V_PID}
\end{figure*}

Here comes some of the results for the spacial position control simulation. In this case the desired position is $(100, 100)$, the proportional gain for the angular velocity controller is $K_p = 4$ and the proportional gain for the average velocity controller is $K_p = 5$. If you are interested to see more plots, you can go to the Git-repository under \texttt{simulation/Diff\_Drive/README.txt} and you will find out how to generate more relative plots for this system.

\begin{figure}[h]
\centering
\includegraphics[width=0.4\textwidth]{sections/assets/traj.png}
\caption{The driven trajectory.}
\end{figure}

\begin{figure}[h]
\centering
\includegraphics[width=0.4\textwidth]{sections/assets/WV.png}
\caption{The wheels' velocities during the drive time.}
\end{figure}

\subsubsection{Arm}

\subsection{Hardware}
% Explain the raspberry pi and it's attachments, the motors, and all the lego


In this Master's thesis we presented a solution, an early warning system, for minimising Human-Elephant Conflicts.
In the beginning we presented a Machine Learning approach for solving classification of thermal images and we outlined its strengths compared to classical problem solving.
We described in depth the knowledge required for understanding our work from various sub parts of Machine Learning, to wireless IoT technologies and thermal cameras.

We presented several Machine Learning workflows that we followed during the design and planning of our own Neural Network.
We analysed the thermal image dataset and recognized what kind of data it lacked.
We compensated for missing data by gathering more of them with our own image capturing setup and by resampling them.
We proposed a basic CNN structure that we used later in the model training phase.

As we wanted to execute Machine Learning algorithms on our selected STM32 microcontroller, we had to find a way to make this possible.
We ported a TensorFlow Lite for a Microcontrollers library to the libopencm3 platform and in the process created an open-source project MicroML.
In the process we familiarised ourselves with the specifics of cross compiling and build systems.

To implement our early warning system on the hardware we decided to use a two microcontroller setup, with an STM32 as a main inferencing processor and nRF52 as a low-power system controller.
To communicate with the FLIR Lepton camera we wrote the driver code, and we also wrote the LoRaWAN communication module.

We ran random search algorithms to find the proper hyperparameters for our proposed CNN model.
We did an in depth comparison of several trained models and compared them to the models trained with the help of commercial software.

We saw that we could train CNN models with an accuracy as high as 98.04 \% and we saw that models trained with the Transfer Learning technique reached 98.7 \% without much additional work.
We ran each trained model on the STM32 and measured its inference time.
From the results we saw that reaching an inference time below 200 \si{\milli\second} is not hard if special care is devoted to the low number of parameters and correct microcontroller optimisations.

We also evaluated the performance of the embedded system from the battery lifetime perspective and we saw promising results.
Assuming that our system would have to process 600 events per day, while connected from 6 battery cells, we estimated the lifetime of the system would be around 10 months.

This thesis shows that the field of Machine Learning on embedded devices has reached a point were it is viable enough to use it in real life applications.
Running inference directly on the embedded device can provide instant feedback and can extend the device's lifetime, as data do not need to be sent to the server for processing.
Many use cases from the Animal Conservation field would benefit from such technology, as there is a need for the embedded devices that require minimal manual care and can provide big value for their cost.

\section{ Future work}

Our research into elephant detection with the aid of Machine Learning models yielded promising results.
There are several aspects of it that can be improved.

In terms of model performance, we can always improve it by gathering more relevant training data.
The dataset that we used contained several thousand images of elephants, but only a couple of thousand images of humans and only a few hundred images of cows.
In order to train a more robust and reliable model, we should gather more thermal images of humans and livestock, especially goats.

It would be interesting to further explore models trained with the Transfer Learning technique.
We saw that Transfer Learning models reached higher accuracies with shorter inferencing times when compared to other models.
We expect that running a random hyperparameter search with a smaller version of the pre-trained MobileNetV2 model could produce optimal results.

In terms of the system performance, testing our early warning system in the field would give us key insights into what could be improved.
With a device deployed in a zoo, we could monitor its performance and see which conditions degrade its performance.
We could add an SD card to the system, and save every taken image and the result of its inference.

By observing the performance of the model in the field we would see if extremely low inference times are really needed.
It might be feasible to run CNN models on slower, low-power, Cortex-M4 microcontrollers.
Although we are expecting longer inference times, we would benefit from a simpler system design and a lower overall price of the embedded system.

In terms of battery life performance, creating a custom printed circuit board specially for our application would give us more control over the current consumption of the systems. Reaching lower low-power state current consumption should be easier.
At the same time, we should research how to optimise the detection sequence.
Decrease in either current consumption or the duration of detection sequence would provide us with huge improvements in battery life.

\section{ Final words}

Machine Learning on the embedded devices is opening doors to various, wonderful applications that were not possible a few years ago.
We can see that there is already a demand for intelligence for devices on the "edge" and we expect it to increase as the field matures.
It is quite possible that future embedded engineers would require some amount of Machine Learning knowledge to stay competitive and interesting to the market.

As Pete Warden said: "The Future of ML is Tiny and Bright".


    \filetitle{!measurement\_shocks}{List of measurement shocks}{modellang/measurementshocks}

	\paragraph{Syntax}\label{syntax}

\begin{verbatim}
!measurement_shocks
    ShockName, ShockName, ...
    ...
\end{verbatim}

\paragraph{Syntax with descriptors}\label{syntax-with-descriptors}

\begin{verbatim}
!measurement_shocks
    ShockName, ShockName, ...
    'Description of the shock...' ShockName
\end{verbatim}

\paragraph{Description}\label{description}

The \texttt{!measurement\_shocks} keyword starts a new declaration block
for measurement shocks (i.e.~shocks or errors to measurement equation);
the names of the shocks must be separated by commas, semi-colons, or
line breaks. You can have as many declaration blocks as you wish in any
order in your model file: They all get combined together when you read
the model file in. Each shock must be declared (exactly once).

You can add descriptors to the shocks (enclosed in single or double
quotes, preceding the name of the shock); these will be stored in, and
accessible from, the model object.

\paragraph{Example}\label{example}

\begin{verbatim}
!measurement_shocks
    u1, 'Output measurement error' u2
    u3
\end{verbatim}