%###########################################################################
%
%   Results
%
%###########################################################################
\chapter{Results and Discussion} \label{chap:results}
In this chapter, first we introduce the experimental setup used for this project, then we present the experimental results of performing estimation of the base states for a variety of different gaits on the quadruped Solo12. We compare the estimated states by the EKF estimator with the states measured by motion capturing system (Vicon), as the ground truth. 
%
\section{Experimental Setup} \label{sec:setup}
This section describes components and hardware which are used as the experimental setup (as shown in~\figref{fig:setup}) in this project. For more details, one can refer to \cite{Hardwaregit}.
%
\begin{figure}[!htbp] 
	\begin{center}
		\includegraphics[scale = 0.1]{Bilder/Experimental_Setup.jpg}
		\caption{Experimental setup.}
		\label{fig:setup}
	\end{center}
\end{figure}
%
\subsubsection{Solo12 Quadruped}
The Solo12 quadruped robot, with the total weight of $2.5\: \unit{kg}$, consists of:
\begin{itemize}
	\item A body module and 4 identical 3DoF legs.
	\item A wire connection for power supply and Ethernet communication.
	\item Electronics including: master board, micro drivers, brushless actuator module, inertia measurement unit (IMU).
	\item Vicon object with reflective markers.
\end{itemize}
%
\subsubsection{Motion Capture System - Vicon}
Motion capture is a technology to track the motion of an object or a fixed marker. Its high frequency and accurate information allow closing control loop reliably for high dynamic motions. The frequency of the capture system is $1\: \unit{kHz}$ with max latency of $15 \: \unit{ms}$ (including communication between Vicon workstation to destination PC via network). The motion capture system used for this project is Vicon V-16 as shown in~\figref{fig:setup}.
%
It provides the base position and orientation of the robot with accuracy up to $63\pm 5\: \unit{\mu m}$. The system consists of:
\begin{itemize}
	\item 6 motion capturing cameras around the robot.
	\item Motion capturing software.
	\item Vicon object with markers on the robot.
\end{itemize}
%
\section{Experimental Results} \label{sec:result}
As mentioned in section~\ref{sec:legged-robots}, the final goal of developing legged robot platforms is to deploy them in different environment, in such a way that they can reliably locomote across rough terrain and be stable in the presence of disturbances. For this reason, we inspect our estimation framework with different basic motions, necessary for having robust quadruped locomotion. These motions are mainly, maintaining balance while all feet are in contact and the base of the robot is rotating, trotting forward and backward in order to be able to navigate in the environment, and finally jumping forward and backward in case that the robot needs to jump over some obstacles on the ground.\par
%
We analyze our estimation framework with four different motions:
%
\begin{enumerate}
	\item Moving the base while balancing.
	\item Trotting gait (using diagonal legs is pairs).
	\item Jumping motion.
	\item Trotting gait with EKF.
\end{enumerate}
%
For the first motion, a trajectory is generated using the trajectory optimization framework, and a whole-body controller is used to track the trajectory while satisfying friction cone constraints. For the other motions, we are using the MPC and observer-based controllers introduced in chapter~\ref{chap:controllers}. For the first three motions, we use the motion capturing system to measure the base states and we feed it in our MPC controller.\par
%
We investigate the performance and limitations of EKF framework in each motion, by comparing the graph for the estimated states (position, orientation, and linear velocity of the base) with the ground truth (measurement from Vicon). Finally in the forth scenario we use the proposed observer-based controller introduced in section~\ref{sec:observer} with some modification in order to close the control loop around the estimated states for trotting motion. We show that by considering the robot's base remaining at the same height during this motion, we can use the EKF estimator in an observe-based controller to control the robot locomotion without using any measurement from motion capturing system.
%
\subsection{Moving the Base while Balancing}
The first scenario we consider, is a motion in which we move the base without any contact switching~\figref{fig:base_snapshots}. The main goal of this test is to evaluate the quality of the estimated states in isolation from uncertainties introduced by contact switching, and as well as our contact detection framework as described in the section~\ref{sec:contact}.\par
%
The estimated states are shown in~\figref{fig:moving_base_position} for the base position, in~\figref{fig:moving_base_velocity} for the base linear velocity, and in~\figref{fig:moving_base_orientation} for the base orientation. As expected, we have an accurate estimation for the observable states, linear velocity, roll and pitch angles of the base. Root mean square error (RMSE) values for these states are given in table~\ref{tab:moving_base_rmse}. Considering the unobservability of the position and yaw angle of the base, we can explain the drift seen over the time for these states.
%
\begin{figure}[!htbp]
	\centering
	\includegraphics[height=0.105\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/base_motion/b2.png}%
	%
	\includegraphics[height=0.105\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/base_motion/b3.png}%
	%
	\includegraphics[height=0.105\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/base_motion/b4.png}%
	%
	\includegraphics[height=0.105\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/base_motion/b5.png}%
	%
	\includegraphics[height=0.105\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/base_motion/b6.png}%
	%
	\includegraphics[height=0.105\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/base_motion/b7.png}%
	%
	\caption[]{Snapshots of the first motion scenario.}
	\label{fig:base_snapshots}
	%
\end{figure}	
%
\begin{figure}[!htbp]
	\scalebox{0.54}{\input{Bilder/base_motion/pgf_files/base_position.pgf}}
	\caption{Motion \#1: Estimation of the base position.}
	\label{fig:moving_base_position}
\end{figure}
%
\begin{figure}[!htbp]
	\centering
	\scalebox{0.54}{\input{Bilder/base_motion/pgf_files/base_velocity.pgf}}
	\caption{Motion \#1: Estimation of the base linear velocity.}
	\label{fig:moving_base_velocity}
	\vspace{5mm}
	\scalebox{0.54}{\input{Bilder/base_motion/pgf_files/base_orientation.pgf}}
	\caption{Motion \#1: Estimation of the base orientation.}
	\label{fig:moving_base_orientation}
\end{figure}
%
\begin{table}[!htbp]
	\caption{Motion \#1: RMSE values.}\vspace{6pt}
	\label{tab:moving_base_rmse}
	\centering
	\begin{tabular}{p{2cm}p{1.5cm}p{1.5cm}p{1.5cm}p{1.5cm}p{1.5cm}}
		\hline
		\hline
		&  \multicolumn{3}{c}{Linear velocity $(\unit{m/s})$}  & \multicolumn{2}{c}{Rotation $(\unit{rad})$} \\
		& $V_x$ & $V_y$ & $V_z$ & Roll & Pitch\\
		\hline
		\hline
		RMSE & $0.0163$ & $0.0147$ &  $0.0170$ & $0.0021$ & $0.0104$\\
		\hline
		\hline
	\end{tabular}
\end{table}
%
\subsection{Trotting Gait}
In the second scenario, we consider a more complex motion involving intermittent contact switching,~\figref{fig:trot_snapshots}. In this test a desired velocity in the x-y direction, by using a joystick, is given to the nonlinear MPC controller as an input command; and we move the robot a few meters around for one minute.\par
%
Although we have discontinuities caused by the contact switching in this motion, the magnitude of these discontinuities for the estimated linear velocity~\figref{fig:trot_velocity}, roll and pitch angles~\figref{fig:trot_orientation}, of the base are comparable to the Vicon measurements, indicating the accurate estimation of the observable states with small RMSE values as shown in table~\ref{tab:trot_rmse}.\par
%
\begin{figure}[!htbp]
	\centering
	\includegraphics[height=0.10\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/trot/t1.png}%
	%
	\includegraphics[height=0.10\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/trot/t2.png}%
	%
	\includegraphics[height=0.10\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/trot/t3.png}%
	%
	\includegraphics[height=0.10\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/trot/t4.png}%
	%
	\includegraphics[height=0.10\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/trot/t5.png}%
	%
	\includegraphics[height=0.10\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/trot/t6.png}%
	%
	\caption[]{Snapshots of the second motion scenario (trotting).}
	\label{fig:trot_snapshots}
\end{figure}
%
\begin{figure}[!htbp]
	\scalebox{0.54}{\input{Bilder/trot/pgf_files/base_position.pgf}}
	\caption{Motion \#2: Estimation of the base position.}
	\label{fig:trot_position}
	\vspace{5mm}
	%
	\scalebox{0.54}{\input{Bilder/trot/pgf_files/base_velocity.pgf}}
	\caption{Motion \#2: Estimation of the base linear velocity.}
	\label{fig:trot_velocity}
\end{figure}
%
The drift in the position~\figref{fig:trot_position}, and yaw angle~\figref{fig:trot_orientation}, are explainable by the unobservability of these states. Notable sources for the drift are the inaccurate leg kinematics, the fault-prone contact detection and the noise introduced in the estimation framework at each contact switching.\par
%
Contact detection is performed, based on \eqref{eq:contact_detection}, by projecting the joint torques of each leg through the jacobian into the space of end effector forces, and applying a force threshold to determine whether each foot is in contact. The estimated end effector force for the front-left (FL) foot along with the detected contact for this foot are shown in~\figref{fig:trot_force}. In order to avoid outliers in the estimated contact, we use higher force thresholds (red and green lines in the graph) for contact detection in trotting motion, which starts after 5 $\unit{sec}$, comparing to the case when the robot is stationary (from 0 to 5 $\unit{sec}$). In the contact estimation the value 1 indicates that the the foot is in contact with the ground.
%
\begin{figure}[!htbp]
	\centering
	\scalebox{0.54}{\input{Bilder/trot/pgf_files/base_orientation.pgf}}
	\caption{Motion \#2: Estimation of the base orientation.}
	\label{fig:trot_orientation}
\end{figure}
%
\begin{table}[!htbp]
	\caption{Motion \#2: RMSE values.}\vspace{6pt}
	\label{tab:trot_rmse}
	\centering
	\begin{tabular}{p{2cm}p{1.5cm}p{1.5cm}p{1.5cm}p{1.5cm}p{1.5cm}}
		\hline
		\hline
		&  \multicolumn{3}{c}{Linear velocity $(\unit{m/s})$}  & \multicolumn{2}{c}{Rotation $(\unit{rad})$} \\
		& $V_x$ & $V_y$ & $V_z$ & Roll & Pitch\\
		\hline
		\hline
		RMSE & $0.0456$ & $0.0266$ &  $0.0557$ & $0.0067$ & $0.0071$\\
		\hline
		\hline
	\end{tabular}
\end{table}
%
\begin{figure}[!htbp]
	\centering
	\scalebox{0.54}{\input{Bilder/trot/pgf_files/FL_force_cnt.pgf}}
	\caption{Motion \#2: Force and contact estimation for front left end effector.}
	\label{fig:trot_force}
\end{figure}
%
\subsection{Jumping Motion}
The third motion we consider, is a jump with commanded velocity to move the robot in x-y direction,~\figref{fig:jump_snapshots}. This motion is especially challenging, as there is considerable impact during landing that could be problematic if a direct force/torque sensor at the end-effector were used for contact detection. However, since we are instead using torque measurements in the proposed estimator, we benefit from the fact that Solo12’s structural and drive system damping filters out the effect of the impact from measured torque considerably. As demonstrated by the results for observable states, shown in~\figref{fig:jump_velocity} and~\figref{fig:jump_orientation}, our proposed estimator is performing accurate with small RMSE values in table~\ref{tab:jump_rmse}, and is not affected by the impact during landing.
The drift in the position~\figref{fig:jump_position}, is more affected by the noise introduced to the estimation due to impact.
%
\begin{figure}[!htbp]
	\centering
	\includegraphics[height=0.136\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/jump/j1.png}%
	%
	\includegraphics[height=0.136\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/jump/j2.png}%
	%
	\includegraphics[height=0.136\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/jump/j3.png}%
	%
	\includegraphics[height=0.136\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/jump/j4.png}%
	%
	\includegraphics[height=0.136\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/jump/j5.png}%
	%
	\includegraphics[height=0.136\linewidth, trim={0cm 0cm 0cm 0cm}, clip]{Bilder/jump/j6.png}%
	%
	\caption[]{Snapshots of the third motion scenario (jump).}
	\label{fig:jump_snapshots}
\end{figure}
%
\begin{figure}[!htbp]
	\scalebox{0.54}{\input{Bilder/jump/pgf_files/base_position.pgf}}
	\caption{Motion \#3: Estimation of the base position.}
	\label{fig:jump_position}
\end{figure}
%
\begin{figure}[!htbp]
	\centering
	\scalebox{0.54}{\input{Bilder/jump/pgf_files/base_velocity.pgf}}
	\caption{Motion \#3: Estimation of the base linear velocity.}
	\label{fig:jump_velocity}
	\vspace{5mm}
	%
	\scalebox{0.54}{\input{Bilder/jump/pgf_files/base_orientation.pgf}}
	\caption{Motion \#3: Estimation of the base orientation.}
	\label{fig:jump_orientation}
\end{figure}
%
\begin{table}[!htbp]
	\caption{Motion \#3: RMSE values.}\vspace{6pt}
	\label{tab:jump_rmse}
	\centering
	\begin{tabular}{p{2cm}p{1.5cm}p{1.5cm}p{1.5cm}p{1.5cm}p{1.5cm}}
		\hline
		\hline
		&  \multicolumn{3}{c}{Linear velocity $(\unit{m/s})$}  & \multicolumn{2}{c}{Rotation $(\unit{rad})$} \\
		& $V_x$ & $V_y$ & $V_z$ & Roll & Pitch\\
		\hline
		\hline
		RMSE & $0.0370$ & $0.0272$ &  $0.1125$ & $0.0036$ & $0.0094$\\
		\hline
		\hline
	\end{tabular}
\end{table}
%
\subsection{Trotting Gait with EKF}
As mentioned in section~\ref{sec:observer}, the unobservability of the base position and yaw angle in the EKF is not affecting the robot's locomotion at low level control. Therefore, we only need relative position of the base with respect to end effectors in order to control the whole body motion planning of the robot. On the other hand, the global position (with respect to a fixed world frame), and the yaw angle of the base are needed for the task of localization, navigation and controlling robot's motion at high-level.\par
%
Considering significant drift in the z-direction of the base position estimated by the EKF, we correct this value by a constant. We assume that the robot's base remains flat and at the same height during the motion. Therefore, we substitute the value of the base position in z-direction estimated by the EKF  by $p_z = 0.22\:[\unit{cm}]$.\par
%
By this modification, we rerun the trotting gait with the observer-based controller,and as expected the low-level control is performing same as before handling the task of locomotion perfectly. The results for the base position~\figref{fig:trot_ekf_position}, linear velocity~\figref{fig:tror_ekf_velocity}, and orientation \figref{fig:trot_ekf_orientation} are consistent with the results we had in trotting motion with Vicon cameras.
%
\begin{figure}[!htbp]
	\scalebox{0.54}{\input{Bilder/trot_ekf/pgf_files/base_position.pgf}}
	\caption{Motion \#4: Estimation of the base position.}
	\label{fig:trot_ekf_position}
\end{figure}
%
\begin{figure}[!htbp]
	\scalebox{0.54}{\input{Bilder/trot_ekf/pgf_files/base_velocity.pgf}}
	\caption{Motion \#4: Estimation of the base linear velocity.}
	\label{fig:tror_ekf_velocity}
\end{figure}
%
\begin{figure}[!htbp]
	\centering
	\scalebox{0.54}{\input{Bilder/trot_ekf/pgf_files/base_orientation.pgf}}
	\caption{Motion \#4: Estimation of the base orientation.}
	\label{fig:trot_ekf_orientation}
\end{figure}
%
\begin{table}[!htbp]
	\caption{Motion \#4: RMSE values.}\vspace{6pt}
	\label{tab:trot_ekf_rmse}
	\centering
	\begin{tabular}{p{2cm}p{1.5cm}p{1.5cm}p{1.5cm}p{1.5cm}p{1.5cm}}
		\hline
		\hline
		&  \multicolumn{3}{c}{Linear velocity $(\unit{m/s})$}  & \multicolumn{2}{c}{Rotation $(\unit{rad})$} \\
		& $V_x$ & $V_y$ & $V_z$ & Roll & Pitch\\
		\hline
		\hline
		RMSE & $0.0264$ & $0.0230$ &  $0.0439$ & $0.0071$ & $0.0094$\\
		\hline
		\hline
	\end{tabular}
\end{table}
%
\section{Discussion} \label{sec:discussion}
Based on the experiments performed in section \ref{sec:result}, we can summarize the results as follow:
\begin{itemize}
	\item In all motions, the EKF estimation for the observable states (linear velocity, roll and pitch angles of the base), is accurate enough for the task of local stabilization of force controlled legged robot, and this can be verified by the small RMSE values.
	\item The unobservable states (base position and yaw angle) estimated by the EKF are drifting over the time, and this divergence is noticeable for more dynamic motion. This can be explained by the fact that in highly dynamic motions the intermittent contact switching of the legs introduces noise to the estimation framework, which affects the performance of the EKF. 
	\item Since the global position and yaw angle of the base are only important for the task of navigation and localization, with some modification the EKF can be successfully used in an observer-based controller in order to handle the motion planning of the robot in trotting gait.
\end{itemize}