%%
%%  chapter07.tex - Obstacle Detection and Planning for Autonomous Vehicles based on Computer Vision Techniques
%%
%%  Copyright 2014 Néstor Morales <nestor@isaatc.ull.es>
%%
%%  This work is licensed under a Creative Commons Attribution 4.0 International License.
%%

\graphicspath{{./images/chapter07/bmps/}{./images/chapter07/vects/}{./images/chapter07/}}

\chapter{Local Planning}\label{ch:chapter07}

By using the methods described in previous chapters, the vehicle is able to detect the obstacles in the surrounding area, and knows how to reach a certain point in the map given its current position. Now we want the vehicle to drive by itself along a trajectory, avoiding the obstacles in the way.
Despite the global path tell us the safest and shortest path to the goal, we can not use it to directly compute the commands the vehicle needs to start driving, as this global path does not model the long term unpredictability of the environment. Due to the presence of dynamic obstacles, this environment is constantly changing. So we need to follow the trajectory using an intermediate mechanism that introduces this short-term information.

\section{Method}\label{ch:chapter07_01}

The way in which we solved this problem is based on the method described in \cite{chu2012local} and also some ideas are taken from \cite{thrun2006stanley}, modified in order to adapt it to our system requirements and the specific characteristics of Verdino. Other changes have been introduced with the aim of improving the behavior of the whole system. Based on these methods, we first transform the current euclidean coordinate system to a new system based on the Frenét space, which is computed as follows: we consider the global path  as the base frame of a curvilinear coordinate system, which will be the path generation space. The directional information of the path is included, so:
\begin{itemize}
 \item The nearest point (where the distance is computed perpendicular to the global path) to the main trajectory will be the origin of the curvilinear coordinate system.
 \item The horizontal axis will be represented by the distance over global path, in its direction.
 \item The vertical axis is represented by the vector perpendicular to the origin point, which is pointing to the left respect to the path direction.
\end{itemize}
With this schema, we can compute easily the trajectories in the curvilinear space (that is, maneuvering information is generated). These are then transformed to the original euclidean space, in which the obstacles information is added by assigning costs to each path.

Inspired in \cite{chu2012local}, we based our cost function in the path smoothness and in the cost of the collision with the obstacles (computed as a function of the distance). This cost is computed by blurring the binary map of the obstacles. For that, we use a exponential function relative to the distance to obstacles and the footprint of the car (See section \ref{ch:chapter07_01_01}).
We also use length and path curvatures as cost functions. Distance to the center of the path is also used, based on the work of \cite{thrun2006stanley}, so we can ensure that the car is able to return to the center of the trajectory. However, as explained in the previous section, our path calculation is different to adapt the costs to the environments in which Verdino will travel. Cost functions will be explained in more detail in section \ref{ch:chapter07_01_04}. Both approaches \citep{chu2012local, thrun2006stanley} use a base frame relative to the global path as tool for an easy and efficient computation of the paths.

The method, in which a big part of the Robotics Group from Universidad de La Laguna (GRULL) participated, has been successfully tested on Verdino. In section \ref{ch:chapter07_02}, some experimental results are shown. Also, there are a set of videos showing the proper behavior of the vehicle in real situations, which can be seen at \url{http://verdino.webs.ull.es/node/109}, \url{http://verdino.webs.ull.es/node/103}, \url{http://verdino.webs.ull.es/node/105}, and \url{http://verdino.webs.ull.es/node/102}.

This method can be divided in five stages, shown at figure \ref{fig:cp07_pipeline}.
\begin{enumerate}
 \item \emph{Generation of the costmap.} Using the information generated by the sensors or by the methods described in previous chapters, the system constructs a costmap in which costs are related to the distance to obstacles.
 \item \emph{Base frame construction.} Based on the global path constructed in the previous section (see \ref{ch:chapter07_01_02}), we construct the base frame of the curvilinear coordinate system.
 \item \emph{Candidate paths generation.} Candidate paths are generated into the curvilinear space. Then, they are transformed to the euclidean space.
 \item \emph{Selection of the winner path.} Costs for all the paths are assigned, and that with the lowest value is selected.
 \item \emph{Computation of the vehicle commands.} Vehicle speed and steering angles are computed based on the characteristics of the winner path.
\end{enumerate}

\begin{figure*}[h!]
        \centering
        \includegraphics[width=\textwidth]{pipeline}
        \caption{Pipeline of the method described on this chapter.}\label{fig:cp07_pipeline}
\end{figure*}

\subsection{Generation of the costmap}\label{ch:chapter07_01_01}

The costmap maintains information about occupied/free areas in the map in the form of an occupancy grid. It uses sensor data and information from the static map to store and update information about obstacles in the world, which are marked in the map (or cleared , if they are no longer there).

Each cell in the map can have $255$ different cost values:
\begin{itemize}
 \item A value of $255$ means that we do not have information about an specific cell in the map.
 \item $254$ means that a sensor has marked this specific cell as occupied. This is considered as a lethal cell, so the vehicle should never enter there.
 \item The rest of cells are considered as free, but with different cost levels depending on an inflation method relative to the size of the vehicle and its distance to the obstacle.
\end{itemize}

Cost values out from occupied cells decrease with distance using the following expression:

\begin{equation}\label{eq:cp07_costmap_inflation}
 \mathcal{C}(i, j) = \exp (-1.0 \cdot \alpha \cdot ( \|c_{ij}-\vec{o}\| - \rho_{inscribed})) \cdot 253
\end{equation}

In this expression, which is the same we used in section \ref{ch:chapter06_01_01_01} of chapter \ref{ch:chapter06}, $\alpha$ is a scaling factor that allows increasing or decreasing the decay rate of the cost of the obstacle. $\|c_{ij}-\vec{o}\|$ is the distance between the cell $c_{ij} \in \mathcal{C}$ (where $\mathcal{C}$ is the set of cells in the costmap) and the obstacle. Finally, $\rho_{inscribed}$ is the inscribed radius, which is the inner circle of the limits of the car. This radius is depicted in the figure \ref{fig:cp07_costmap_concepts}.

\begin{figure}[p]
\centering
\begin{tabular}{c}
  \begin{subfigure}[b]{\textwidth}
    \centering
    \includegraphics[width=\textwidth]{cost_levels}
    \caption{Cost levels.}
    \label{fig:cp07_cost_levels}
  \end{subfigure}\\
  \begin{subfigure}[b]{\textwidth}
    \centering
    \includegraphics[width=\textwidth]{inscribed_circumscribed}
    \caption{$\rho_{inscribed}$ and $\rho_{circumscribed}$.}
    \label{fig:cp07_inscribed_circumscribed}
  \end{subfigure}
\end{tabular}
\caption{Costmap computation concepts.}\label{fig:cp07_costmap_concepts}
\end{figure}

Despite all of them are free cells, we define four different distance thresholds in order to set different danger levels in the map, depicted in figure \ref{fig:cp07_costmap_concepts}:

\begin{itemize}
 \item $\tau_{lethal}$: There is an obstacle in this cell, so the vehicle is in collision. It is represented by the cost level $254$.
 \item $\tau_{inscribed}$: Cell distance to the nearest obstacle is below $\rho_{inscribed}$. If the center of the vehicle is in this cell, it is also in collision, so areas below this distance threshold should be avoided. Cost level is always $253$.
 \item $\tau_{circumscribed}$: If the vehicle center is on this cell, it is very likely that the car collided with an obstacle, depending on its orientation. A cell with a distance to an obstacle below this threshold should be avoided, but there are still chances of being in one of them without colliding an obstacle.
 \item The rest of cells are assumed to be safe (except from those with \emph{unknown} cost, for which we do not know if they are occupied or not, so we think on them as if they were lethal).
\end{itemize}

In our approach, we just consider those paths passing through cells with a cost below $\tau_{circumscribed}$, which is obtained obtained using the equation \ref{eq:cp07_costmap_inflation} and other cost factors that will be explained later. Paths passing trough the cells over this threshold will be truncated at the last safe point.

For optimization reasons, we do not compute the costmap for the whole map at each iteration. Instead, we just compute the cells in a $40 \times 40\,m$ area centered into the current car position.

For the computation of the costmap and of the costs associated to each cell, we used the \ROS plugin \program{costmap\_2d}\footnote{\url{http://wiki.ros.org/costmap\_2d}}, which implements the functionalities described in this section.

\subsection{Base frame construction}\label{ch:chapter07_01_02}

In this stage, we will define the base frame of the curvilinear coordinate system, so the algorithm will be able to compute the trajectories in this space as if the global plan were a rectilinear trajectory. At this point, we will not care about the presence of obstacles or the restrictions associated to the vehicle's motion model, since we will focus in the generation of trajectories. The geometric relationship between the path in euclidean and curvilinear coordinates is shown at figure \ref{fig:cp07_euclidean_frenet_conversion}.

Before the construction of the base frame, we must find the point we want to use as origin of coordinates. For this purpose, we iterate over the points of the trajectory until we reach the point which is the nearest to the current position of the vehicle. This will be the origin of coordinates of the base frame. If the trajectory is valid (that is, it is different from zero), we can start with the base frame construction.

\begin{figure}[h!]
\centering
\begin{tabular}{cc}
  \begin{subfigure}[b]{0.45\textwidth}
      \centering
      \includegraphics[width=\textwidth, trim=50 30 80 60,clip]{justOneCartesian45}
      \caption{Euclidean space.}
      \label{fig:cp07_justOneCartesian45}
  \end{subfigure} &
  \begin{subfigure}[b]{0.45\textwidth}
    \centering
    \includegraphics[width=\textwidth, trim=50 30 80 60,clip]{justOneFrenet45}
    \caption{Curvilinear space.}
    \label{fig:cp07_justOneFrenet45}
  \end{subfigure}%
\end{tabular}
\caption{Conversion of a trajectory between the Cartesian and Frenét spaces.}\label{fig:cp07_euclidean_frenet_conversion}
\end{figure}

Base frame's arc length ($s$, on the right image) is obtained as the distance of each point along the global plan (represented as a green line) to the origin of coordinates. This distance is represented in the \emph{x-axis} of the curvilinear system. \emph{y-axis}, $q$, represents the perpendicular lateral distance respect to the path. Left side is represented by positive values and the right by the negatives.

For the computation of the transformation between the euclidean and the curvilinear coordinate system, we need to compute path curvature $\kappa$. This value is computed as follows \citep{chu2012local, werling2010optimal, barfoot2004motion}:

\begin{equation}\label{eq:cp07_path_curvature}
 \kappa = {S \over Q} \cdot \left ( \kappa_b \cdot {
 {(1 - q \cdot \kappa_b) \cdot (\partial^2q / \partial s^2) +
 \kappa_b \cdot (\partial q / \partial s )^2
 }
 \over {Q^2} } \right )
\end{equation}

,where

\begin{equation}\label{eq:cp07_path_curvature_s_and_q}
\begin{cases}
S=sign(1 - q \cdot \kappa_b)\\
Q=\sqrt{\left ( { {\partial q} \over {\partial s}} \right ) ^2+ (1 - q \cdot \kappa_b)^2}
\end{cases}
\end{equation}

A generated path will be rejected if $q > {1 \over \kappa_b}$. In this case, the generated path curvature and sense is opposed to that of the base frame. The path violates the non-holonomic condition of the movement of the vehicle, so it is discarded.

We are just able to accept paths in which the lateral offset $q$ is similar or smaller to the curvature radius of the base frame $1 / \kappa_b$. If $q = 1 / \kappa_b$, that means that the path passes through the center of curvature of the base frame. Also, the maximal curvature a path can have in order to be feasible by the vehicle is limited by the maximal steering angle. If this restriction is violated, the corresponding path is rejected. Curvature is directly related to the movement of the vehicle, which can be described through several models. A simplified version  \citep{chu2012local, barfoot2004motion}, which ignores the related physical effects (like inertia or mass), is:

\begin{equation}\label{eq:cp07_simplified_motion_model}
\begin{cases}
\dot{x} = |\vec{v}| \cdot cos(\theta) \\
\dot{y} = |\vec{v}| \cdot sin(\theta) \\
\dot{\theta} = |\vec{v}| \cdot \kappa
\end{cases}
\end{equation}

As these physical effects do not affect to the geometric shape of the path, we can ignore them in the path generation step. At this point, we are neither considering other physical restrictions like the maximal curvature that the vehicle is able to follow or the maximal speed, since it will be done in the next steps. In equation \ref{eq:cp07_simplified_motion_model}, $[\dot{x} ~ \dot{y} ~ \dot{\theta} ]^T$ are the estimated position and orientation of the vehicle and $|\vec{v}|$ is speed magnitude. Considering this simplified model, we assume that the vehicle just have two degrees of freedom, represented by the speed $\vec{v}$ and $\kappa$. As we are just interested into the geometric generation of the path, it is possible to remove the speed of the vehicle $\vec{v}$ from the model. This is done by expressing the movement of the vehicle in terms of the traveled distance. So the following relation is established \citep{chu2012local}:

\begin{equation}\label{eq:cp07_speed_to_distance}
|\vec{v}| = S \cdot Q \cdot { {\partial s} \over {\partial t}}
\end{equation}

If the speed of the vehicle is substituted into the model described in equation \ref{eq:cp07_simplified_motion_model}, the differential equation of the movement can be represented in terms of the base frame's arc length $s$:

\begin{equation}\label{eq:cp07_motion_model_in_distance_terms}
\begin{cases}
{{\partial x} \over {\partial s}} = Q \cdot cos(\theta) \\
{{\partial y} \over {\partial s}} = Q \cdot sin(\theta) \\
{{\partial \theta} \over {\partial s}} = Q \cdot \kappa
\end{cases}
\end{equation}

\subsection{Candidate paths generation}\label{ch:chapter07_01_03}

As seen, path generation is performed in the curvilinear space, without considering the obstacles in the environment. These will be taken into account later, once the tentative trajectories are transformed to the euclidean space.

\subsubsection{Maneuvering paths generation}\label{ch:chapter07_01_03_01}

The curvature of the generated paths is defined by the lateral offset $q$ respect to the base frame. First and second order derivatives of $q$ are needed if we want to compute $\kappa$ (see equations \ref{eq:cp07_path_curvature} and \ref{eq:cp07_path_curvature_s_and_q}), so we need a function dependent on the lateral offset if we want to compute a smooth lateral change.

$q$ can be defined by a sequence of a cubic polynomial and a set of constants \citep{chu2012local}:

\begin{equation}\label{eq:cp07_function_q}
\begin{align*}
q(s) &=
  \begin{cases}
   a \cdot \Delta s^3 + b \cdot \Delta s^2 + c \cdot \Delta s + q_i & \text{if } s_i \le s < s_f \\
   q_f        & \text{if } s_f \le s
  \end{cases}\\
{{\partial q} \over {\partial s}}(s) &=
  \begin{cases}
   3 \cdot a \cdot \Delta s^2 + 2 \cdot b \cdot \Delta s + c & ~~~~~~~\text{if } s_i \le s < s_f \\
   0        & ~~~~~~~\text{if } s_f \le s
  \end{cases}\\
{{\partial^2 q} \over {\partial s^2}}(s) &=
  \begin{cases}
   6 \cdot a \cdot \Delta s + 2 \cdot b & ~~~~~~~~~~~~~~~~~~~~~~~\text{if } s_i \le s < s_f \\
   0        & ~~~~~~~~~~~~~~~~~~~~~~~\text{if } s_f \le s
  \end{cases}
\end{align*}
\end{equation}

, where $\Delta s = s - s_i$.

In figure \ref{fig:cp07_justOneFrenet45}, the components involved in this process are depicted.
\begin{itemize}
 \item The initial lenght $s_i$ is zero, due to the process performed at the beginning of each iteration (section \ref{ch:chapter07_01_02}). Lateral offset $q_i$ is also known, which is the lateral offset respect to the global path's origin.
 \item Angle $\theta$ defines the difference between the vehicle heading angle and the tangent angle of the base frame at the current position.
 \item $s_f$ is a parameter that allows controlling the longitudinal distance needed to reach the offset $q_f$. This distance should be dependent from the speed. However, as the maximal speed of the prototype is not too hight, we can think on $\Delta s_f$ as the distance needed to go from $q_i$ to the biggest $q_f$ at the maximal speed.
 \item The different $q_f$ are computed separately for each path attending to the parameters defined by the user. $s_f$ is also a free parameter.
\end{itemize}

Parameters $s_i$, $q_i$, and $s_f$ are shared by all candidate paths. By modifying the value of $q_f$ we get different tentative trajectories, so the only difference between all them is the lateral offset we want to reach at the end of paths. This lateral offset will give us flexibility in the way in which we avoid obstacles. To do that, we divide the width of the road into as many segments as paths are desired. $q_f$ will be the perpendicular distance between the base frame and the corresponding road width division. Using this technique, the desired number of paths is created. The set of paths should cover the whole road width, ensuring the vehicle is able to avoid the obstacles, if it is possible.

In our tests, we have considered a maximal width of 4\,m (a little bit above the width of the roads in our testing area), and an horizon of 10\,m in the direction of the global path. This horizon gives us a prediction of 2\,seconds at the maximal speed of the vehicle. We evaluate a total of 21 paths, with a distance between them ($\Delta q_f$) of 20\,cm.

\subsubsection{Candidate paths generation}\label{ch:chapter07_01_03_02}

Once we have computed the paths in the curvilinear coordinate system, we transform them so we can work in the euclidean space. In this new space, we will be able to evaluate their associated costs, as those related to the distance to obstacles, smoothness, etc. In figures \ref{fig:cp07_frenet0} and \ref{fig:cp07_frenet45}, we can see two examples of this process.

In figure \ref{fig:cp07_frenet0}, an example in which the vehicle is at a lateral distance of 1\,m respect to the base frame, and there is no difference in the orientation of the vehicle regarding to this base frame is shown. That is, $q_i = 1$, $s_i = 0$ and $\theta = 0$. In the left image, we see that the set of paths is symmetric, being the central path completely parallel to the axis $s$. These trajectories are then projected to the euclidean space, obtaining the paths represented in the right image. There, green line represents the global path, and blue lines are the projected candidate paths. As shown, they fit properly to the global path.

\begin{figure}[h!]
\centering
\begin{tabular}{cc}
  \begin{subfigure}[b]{0.45\textwidth}
    \centering
    \includegraphics[width=\textwidth, trim=50 40 80 60,clip]{frenet0}
    \caption{Curvilinear space.}
    \label{fig:cp07_frenet_space0}
  \end{subfigure} &
  \begin{subfigure}[b]{0.45\textwidth}
    \centering
    \includegraphics[width=\textwidth, trim=50 40 80 60,clip]{cartesian0}
    \caption{Euclidean space.}
    \label{fig:cp07_cartesian0}
  \end{subfigure}%
\end{tabular}
\caption{Example in which the vehicle is oriented parallel to the path.}\label{fig:cp07_frenet0}
\end{figure}

In figure \ref{fig:cp07_frenet45}, a similar example is shown. The only difference is that, this time, the vehicle is oriented $45^\circ$ away from the base frame orientation. This causes the paths in the curvilinear space to move away from the $s$ axis until they start converging. As shown, same effect occurs in the transformed euclidean paths: they move away from the global path but, after a few meters, they start approaching to it.

\begin{figure}[h!]
\centering
\begin{tabular}{cc}
  \begin{subfigure}[b]{0.45\textwidth}
    \centering
    \includegraphics[width=\textwidth, trim=50 40 80 60,clip]{frenet45}
    \caption{Curvilinear space.}
    \label{fig:cp07_frenet_space45}
  \end{subfigure} &
  \begin{subfigure}[b]{0.45\textwidth}
    \centering
    \includegraphics[width=\textwidth, trim=50 40 80 60,clip]{cartesian45}
    \caption{Euclidean space.}
    \label{fig:cp07_cartesian45}
    \end{subfigure}
\end{tabular}
\caption{Example in which the vehicle is rotated with respect to the path.}\label{fig:cp07_frenet45}
\end{figure}

Now the paths are in euclidean coordinates, we can evaluate which is the maximal distance they can reach individually if obstacles are considered. To do that, we iterate over the points in the trajectory, and check the cost associated to each cell $c_{ij}$ containing the point in the costmap computed in section \ref{ch:chapter07_01_01}. If this cost is over the value associated to the threshold $\tau_{circumscribed}$, the path is truncated at this point, as shown in figure \ref{fig:cp07_path_truncation}, where the generated paths are shown together with a colored costmap representation. There, blue means a low value, while the red color is used for the higher costs. Yellow and cyan correspond to cells in lethal and inscribed cells, respectively. The points iterated before reaching this cost will be kept, but it is very unlikely that this will be the winner path, as the occlusion cost will be maximal, and the length of the path shorter (see section \ref{ch:chapter07_01_04}).
As shown, when a path collides with an obstacle, we do not remove the path completely. The reason for that is that there are certain situations in which we can not reach the maximal distance with none of the paths. However, we still want to approach slowly towards the maximal reachable point, with the hope that the obstacles that are blocking the way will disappear in the next iterations. In crowded areas with many pedestrians this is a typical situation: the way is blocked, but when pedestrians see a vehicle that is approaching, they move away. However, if the vehicle reaches a point in which it can not move for a long time, we generate a new trajectory and try to go through a different way.
The problem with this strategy is that one of the colliding paths could win even if there is a path able to go through a clear area. In order to avoid that, we have implemented a weighted cost functions based schema, explained in the next section, that allows a smart selection of the winner path.

\begin{figure}[h!]
\centering
\includegraphics{example15}
\caption{Paths truncation example.}\label{fig:cp07_path_truncation}
\end{figure}

\subsection{Selection of the winner path}\label{ch:chapter07_01_04}

The winner path is selected through the use of a linear combination $J[i]$ of weighted cost functions, related to the following parameters: occlusion, length, distance to the global path, curvature and consistency of the path. $J[i]$ is evaluated as follows:

\begin{equation}\label{eq:cp07_cost_function}
J[i] = \omega_o C_o[i] + \omega_l C_l[i] + \omega_d C_d[i] + \omega_{\kappa} C_{\kappa}[i] + \omega_c C_c[i]
\end{equation}

Here, $i$ is the path index, and $C_o$, $C_l$, $C_d$, $C_{\kappa}$ and $C_c$ are the costs of occlusion, length, distance to the global path, curvature and consistency, respectively. Their relatives $\omega_k$, $k \in \{o, l, d, \kappa, c\}$ are the associate weights that allow to adjust the influence of each of the costs to the final cost value. All these costs are normalized to $1.0$, and

\begin{equation}\label{eq:cp07_weights_sum}
\sum_{{i \in \{o, l, d, \kappa, c\}}} w_i= 1.0
\end{equation}

, so it is easy to determine the proportional influence of each weight. Based on this, the value of $J[i]$ will be always inside the interval $[0\dots1]$.

The following costs will be computed for each candidate path independently from the euclidean space.

\paragraph{Occlusion}\label{ch:chapter07_01_04_00_01}

The occlusion cost is related to the safety of the path. This cost estimates the goodness of a path, being the bests paths those that pass far enough from the obstacles. To do that, we iterate along the path, simulating the footprint of the car at each position. The occlusion cost corresponding to the trajectory point $i$ will be the maximal cost of each of the cells $c_{ij} \in \mathcal{C}$ under the footprint of the car at that position. Based on this, the occlusion cost will be

\begin{equation}\label{eq:cp07_occlusion_cost}
C_o = {{max\{c_i\}} \over 255}, ~~~~~~ i=1 \dots L
\end{equation}

In this expression, $L$ is the length of the current path being evaluated. $max\{c_i\}$ is the maximal value of all the costs, associated to a point in the path. As we saw in section \ref{ch:chapter07_01_01}, the maximal value of each cost is $255$, so we divide the cost by this value, in order to normalize it to $1$.

\paragraph{Length}\label{ch:chapter07_01_04_00_02}

This cost represents the length of the current path. By iterating along the points in the path, we accumulate the distance between them, so we know the real distance traveled in euclidean coordinates. The longer that a path is, the better, as we assume that it means that paths are able to travel a longer distance. However, we are want to minimize the costs, so we must invert the length of each path in the way that longer paths give us smaller values. This is done through the expression:

\begin{equation}\label{eq:cp07_length_cost}
C_l = 1- {{\sum\limits_{i=1}^{L}\|p_i - p_{i - 1}\|} \over {q_{f_{max}} + s_f}}
\end{equation}

Here, $p_i$ is a certain point inside the evaluated path. $q_{f_{max}}$ is the maximal value that a $q_f$ can have for a certain path. Lengths are normalized to a value that a path will never reach. In our case, we decided that the sum of the maximal expected offset for $s$ and for $q$ is good enough (lengths will never trespass this value, while being small enough to allow an easy discrimination between paths). We subtract this cost from $1.0$, in order to make it comparable to the rest of costs (as said, lower values are preferred respect to the higher ones).

\paragraph{Distance to the global path}\label{ch:chapter07_01_04_00_03}

In our implementation, we included information about the average lateral offset respect to the global path. The use of this cost will benefit the choice of those paths that allow coming back to the global path after an occasional obstacle is avoided. It is computed as follows:

\begin{equation}\label{eq:cp07_lateral_cost}
C_d = {{\sum\limits_{i=1}^{L}\|p_i - nearest(p_i, g)\|} \over {L \cdot q_{f_{max}}}}
\end{equation}

, where $nearest(p, g)$ is the nearest point in the global path $g$ to the point $p$. This cost is normalized with respect to the maximal expected offset, $q_{f_{max}}$.

\paragraph{Curvature}\label{ch:chapter07_01_04_00_04}

This cost allows giving priority to the smoother paths. Let $p(x_i, y_i), ~ i=1\cdots L$, be a point in the path. Then,

\begin{equation}\label{eq:cp07_curvature_cost}
C_{\kappa} = max \left \{ {{x_i' \cdot y_i'' - x_i'' \cdot y_i'} \over {(x_i' + y_i')^{3/2}}} \right \}, ~~~~~~ i=1 \dots L
\end{equation}

\paragraph{Consistency}\label{ch:chapter07_01_04_00_05}

This cost avoids the continuous changes in the winner paths between iterations. Once the vehicle starts a maneuver, the idea is keeping this behavior in the following iterations.

This is done through the following expression:

\begin{equation}\label{eq:cp07_consistency_cost}
C_c = {1 \over {s_2 - s_1}} \int \limits_{s_1}^{s_2} l_i ~ ds
\end{equation}

This equation is better understood if we look at figure \ref{fig:cp07_consistency_cost}. There, the lateral cost $l_i(s)$ is the distance between the current and the previous winner path at the same longitudinal position $s$; $s_1$ and $s_2$ are the first and last positions over $s$ for which there are points in common in both trajectories.

\begin{figure}[h!]
  \centering
  \includegraphics[width=\textwidth]{consistency_cost}
  \caption{Representation of the way in which the consistency cost is computed.}\label{fig:cp07_consistency_cost}
\end{figure}

\subsubsection{Selection of the winner path}\label{ch:chapter07_01_04_01}

Once all costs are computed, we just apply the expression described in equation \ref{eq:cp07_cost_function}. In those paths for which it is impossible to advance due to the presence of a nearby obstacle because the car is bad oriented to the global path (meaning that no valid paths can be generated in this situation), the cost will be negative (invalid path).

From all paths, we select that with the smallest cost (winner path $W$). If for any reason there are not valid paths, the vehicle is stopped until the road is clear. If this situation does not change for a while, a new global plan is computed, so the vehicle is able to reach the goal using a different way.

If no valid paths are found because the vehicle has a bad orientation respect to the global plan, a recovery behavior process is started.

\subsection{Computation of the vehicle commands}\label{ch:chapter07_01_05}

The last step required in order to follow the trajectory obtained from the global planner is the computation of the steering angle and speed commands that will be sent to the built-in controller of the vehicle. The way in which this is done is quite simple. Over the winner path, we apply a \ac{PID} controller in which we try to minimize the distance and heading of the vehicle respect to that trajectory. Using this controller and a model of the vehicle, we can compute the proper values, which are finally sent to the vehicle.

\subsection{Recovery behavior}\label{ch:chapter07_01_06}

When the lateral offset of a path is bigger than the radius of curvature of the base frame, paths can not be generated using the approach described before. Because of that, we propose to compute the paths in a different way, so the vehicle can advance towards the global plan until this restriction is satisfied again. The way in which we do that is through the use of a model of the behavior of the vehicle. This model is the same Ackerman model used in \cite{espelosin2013path}.

Four paths are evolved in a parameterized time $t$, considering a low speed. Two of these paths are generated considering a positive speed (forward movement) and the top left and top right steering position, and the other two are similar, but using a negative speed. All these four paths are weighted following the same process described in in \ref{ch:chapter07_01_04}, and the winner is selected. Resulting speed and steering commands for the recovery behavior process will be those used to evolve the winner path.

% \section{Putting all together}\label{ch:chapter08_08}
% \comment{No tengo muy claro si esto debería ir aquí o en las conclusiones}
%
% Using the methods described in this thesis, we are now able to detect the obstacles existing in the environment and track them along the frames. Also, we are able to generate trajectories that can be followed by our vehicle in order to reach a certain point, and we know how to compute the commands that the vehicle need to follow these trajectories. Now it is time to put everything together.
%
% In the images shown at figure \ref{fig:cp07_whole_pipeline}, we can see an integration of some of the methods described in this thesis. These images have been extracted from the videos available at \url{http://youtu.be/08mAOD6bT9w} (for the Stixels based example); and \url{http://youtu.be/SCQ6_GYjbJs} and \url{http://youtu.be/rn4iIafBFZc} (for the Particle Filter based one). In these methods, we have included the obstacles detected using the methods described in chapters \ref{ch:chapter04} and \ref{ch:chapter05}, which are incorporated to the costmap described in section \ref{ch:chapter07_01_01}. Unfortunately, at this time the described implementation of the costmap do not accept temporal information, so we just can include the detected obstacles, but not the information about their future movement that could be obtained thanks to the tracks detected. Also, as we said before, the global planner described in section \ref{ch:chapter06} was discarded as it is not longer needed in this form, but we think on it as a solution for the fast generation of \acp{RNDF}.
%
% \begin{figure*}[h!]
%   \begin{subfigure}[b]{\textwidth}
%     \centering
%     \includegraphics[width=\textwidth, height=0.75\textwidth]{stixels_whole_pipeline}
%     \caption{Integration of the whole pipeline of the application, including the stixel based method, described in chapter \ref{ch:chapter04}.}\label{fig:cp07_stixels_whole_pipeline}
%   \end{subfigure}
%   ~
%   \begin{subfigure}[b]{\textwidth}
%     \centering
%     \includegraphics[width=\textwidth, height=0.75\textwidth]{particle_filter_whole_pipeline}
%     \caption{Integration of the whole pipeline of the application, including the particle filter based method, described in chapter \ref{ch:chapter05}.}\label{fig:cp07_particle_filter_whole_pipeline}
%   \end{subfigure}
%   \caption{Integration of the full pipeline of the application described in this thesis. Frames have been extracted from the videos available at \url{http://youtu.be/08mAOD6bT9w}, \url{http://youtu.be/SCQ6_GYjbJs} and \url{http://youtu.be/rn4iIafBFZc}.}\label{fig:cp07_whole_pipeline}
% \end{figure*}
%
% Anyway, the integration of the method with the planner described in this chapter is quite good. We can see in both images that the planner reacts properly to the objects detected and that, given a pair images, both methods are able to detect the obstacles, allowing a safe trip. In general, both methods are suitable for the evaluated scenarios, being the most limiting features those inherent to the methods, which were described in chapters \ref{ch:chapter04} and \ref{ch:chapter05}.

\section{Summary}\label{ch:chapter07_07}

In this chapter, we have seen a method able to efficiently follow a given trajectory, while avoiding the potential obstacles in the way. The use of a curvilinear coordinate system for the generation of a set of candidate trajectories simplifies the calculations while allows creating soft paths that makes the vehicle to converge to the followed track in a way as smooth as possible. Moreover, these paths are generated with different lateral offsets regarding to the global plan, so we are able to avoid an obstacle even if it is in the middle of the path. Finally, as speed is out of the model used for the generation of the tracks, this generation step is reduced to the computation of a set of parameters that define a third order polynomial. Speed and steering angle are then computed, using a \ac{PID} controller over the winner path, which is chosen based on a cost function.

There are some videos in which the good behavior of this method is available, as those in \url{http://verdino.webs.ull.es/node/109}, \url{http://verdino.webs.ull.es/node/103}, \url{http://verdino.webs.ull.es/node/105} and \url{http://verdino.webs.ull.es/node/102}. Also, the videos available at \url{http://youtu.be/08mAOD6bT9w}, \url{http://youtu.be/rn4iIafBFZc} and \url{http://youtu.be/SCQ6_GYjbJs}, show how the integration of the method with the algorithm described in chapters \ref{ch:chapter04} and \ref{ch:chapter05} is possible (This process is better explained at section \ref{ch:chapter08_08}).

As future work, we can study the inclusion of new cost functions in order to improve the choice of the right path or long-term control strategies, as those used in \cite{werling2010optimal}. Furthermore, the computation of the steering angle based on the winner path could be improved through the use of a predictive controller.

In the next chapter, we will see the results obtained for all the methods described until now, which will give us a clear idea of their performance, and the advantages and disadvantages of using such methods.


% !TEX root = ./Basilisk-CoarseSunSensor-20170803.tex

\section{Test Description and Success Criteria}
The Coarse Sun Sensor Test, test\_coarseSunSensor.py, contains 11 tests. The simulation is set up with only the coarse sun sensor(s) and made-up messages to simulation spacecraft, sun, and eclipse information. The spacecraft is in a convenient attitude relative to the sun and rotates all of the sensors in a full circle past the sun.
\begin{enumerate}
	\item\textbf{Basic Functionality}: A single sensor is run with minimal modifications and compared to a cosine.
	\subitem \textbf{Success Criteria}: The output curve should match a cosine curve.
	\item\textbf{Eclipse}: A single sensor is run with an eclipse simulated and compared to an eclipse factored cosine.
	\subitem \textbf{Success Criteria}: The output curve should match a cosine curve times the eclipse factor input
	\item\textbf{Field of View}: A single sensor is run with a smaller field of view and compared to a clipped cosine.
	\subitem \textbf{Success Criteria}: The output curve should match a cosine curve truncated to zero beyond the field of view input.
	\item\textbf{Kelly Factor}: A single sensor is run with a Kelly factor input and compared to a modified cosine.
	\subitem \textbf{Success Criteria}: The output curve should match a cosine curve modified by the kelly curve equation seen previously in this report
	\item\textbf{Scale Factor}: A single sensor is run with a scale factor and compared to a scaled cosine.
	\subitem \textbf{Success Criteria}: The output curve should match a cosine curve multiplied by the scaleFactor input.
	\item\textbf{Bias}: A single sensor is run with a bias and compared to a modified cosine.
	\subitem \textbf{Success Criteria}: The output curve should match a cosine curve shifted in magnitude by the bias input.
	\item\textbf{Noise}: A single sensor is run with noise and the standard deviation of that noise is compared to the input standard deviation.
	\subitem \textbf{Success Criteria}: Once a clean cosine curve is subtracted from the output, the standard deviation should match the standard deviation input.
	\item\textbf{Albedo}: A single sensor is run with an albedo input and shown to be no different than the standard cosine truth value. This is done because there is some albedo functionality programmed into the module but it should be inactive at this time.
	\subitem \textbf{Success Criteria}: The output curve should match a cosine curve because the albedo input should have no effect.
	\item \textbf{Saturation}: Non-zero minimum saturation and less-than-maximum-output maximum saturation values are input.
	\subitem\textbf{SuccessCriteria}: Output should match generated truth to high accuracy and saturation values should be clearly visible in plot.
	\item\textbf{Sun Distance}: The simulation is run with the spacecraft $2[AU]$ from the sun. The expected result is compared to the output.
	\subitem \textbf{Success Criteria}: Output should match the generated truth to high accuracy.
	\item\textbf{Clean Combined}: All of the inputs above except for noise are run on a single simulation. The expected result is compared to the output.
	\subitem \textbf{Success Criteria}: Output should match the generated truth to high accuracy.
	\item\textbf{Combined}: All of the inputs above are run on a single simulation. The expected result without noise is subtracted from the result. Then, the standard deviation of the noise. is compared to the expected standard deviation.
	\subitem \textbf{Success Criteria}: Once a cosine curve modified by eclipse, field of view, kelly factor, scale factor, bias, and albedo are subtracted from the output, the standard deviation should match the given standard deviation.
	\item\textbf{Constellation}: Two constellations of sensors are set up using various set up methods and simulated with a clean signal. The two constellations are tested to be identical to one another. Constellation P1 is established by directly assigning normal vectors to four sensors. Constellation P2 is established by giving angles that specify the placement of the constellation platform relative to the spacecraft body frame. Then, for constellation 2, the unit direction vector for each sensor is set with an azimuth and an elevation via coarseSunSensor.setUnitDirectionVectorWithPerturbation(). Finally, the fourth sensor in constellation P2 is set up in a different way than the others. It is not assigned platform frame angles but it is given incorrect azimuth and elevation headings which are corrected with "perturbation" inputs. Through all of these tests, constellation set up is verified, including the default platform DCM (identity).
	\subitem \textbf{Success Criteria}: The output curve from constellation P1 should match the output curve from constellation P2.
\end{enumerate}

\section{Test Parameters}

Pytest runs the following cases (numbered as above) when it is called for this test:
	\begin{table}[H]
	\caption{Parameters for each test. Note that relative tolerance is $\frac{truth - output}{truth}$}
	\label{tab:errortol}
	\centering \fontsize{10}{10}\selectfont
	\begin{tabular}{ c | c | c | c | c | c | c | c | c | c | c | c } % Column formatting,
		\hline\hline
		\rot{\textbf{Test}}& \rot{\textbf{useConstellation}}& \rot{\textbf{visibilityFactor}}& \rot{\textbf{fov}}& \rot{\textbf{kelly}}& \rot{\textbf{scaleFactor}}& \rot{\textbf{bias}}& \rot{\textbf{noiseStd}}& \rot{\textbf{albedoValue}}& \rot{\textbf{saturation}}&\rot{\textbf{errTol}}&\rot{\textbf{sunDistInput}}\\
		\hline\hline
		1      & \input{AutoTex/plainUseConstellation}&\input{AutoTex/plainVisibilityFactor}&\input{AutoTex/plainFov}&\input{AutoTex/plainKelly}&\input{AutoTex/plainScaleFactor}&\input{AutoTex/plainBias}&\input{AutoTex/plainNoiseStd}&\input{AutoTex/plainAlbedoValue}&\input{AutoTex/plainMaxSaturation},\input{AutoTex/plainMinSaturation}&\input{AutoTex/plainErrTol}&\input{AutoTex/plainLocation}	   \\ \hline
		2	& \input{AutoTex/eclipseUseConstellation}&\input{AutoTex/eclipseVisibilityFactor}&\input{AutoTex/eclipseFov}&\input{AutoTex/eclipseKelly}&\input{AutoTex/eclipseScaleFactor}&\input{AutoTex/eclipseBias}&\input{AutoTex/eclipseNoiseStd}&\input{AutoTex/eclipseAlbedoValue}&\input{AutoTex/eclipseMaxSaturation},\input{AutoTex/eclipseMinSaturation}&\input{AutoTex/eclipseErrTol}&\input{AutoTex/eclipseLocation}	   \\ \hline
		3	& \input{AutoTex/fieldOfViewUseConstellation}&\input{AutoTex/fieldOfViewVisibilityFactor}&\input{AutoTex/fieldOfViewFov}&\input{AutoTex/fieldOfViewKelly}&\input{AutoTex/fieldOfViewScaleFactor}&\input{AutoTex/fieldOfViewBias}&\input{AutoTex/fieldOfViewNoiseStd}&\input{AutoTex/fieldOfViewAlbedoValue}&\input{AutoTex/fieldOfViewMaxSaturation},\input{AutoTex/fieldOfViewMinSaturation}&\input{AutoTex/fieldOfViewErrTol}&\input{AutoTex/fieldOfViewLocation}	   \\ \hline
		4      & \input{AutoTex/kellyFactorUseConstellation}&\input{AutoTex/kellyFactorVisibilityFactor}&\input{AutoTex/kellyFactorFov}&\input{AutoTex/kellyFactorKelly}&\input{AutoTex/kellyFactorScaleFactor}&\input{AutoTex/kellyFactorBias}&\input{AutoTex/kellyFactorNoiseStd}&\input{AutoTex/kellyFactorAlbedoValue}&\input{AutoTex/kellyFactorMaxSaturation},\input{AutoTex/kellyFactorMinSaturation}&\input{AutoTex/kellyFactorErrTol}&\input{AutoTex/kellyFactorLocation}	   \\ \hline
		5	& \input{AutoTex/scaleFactorUseConstellation}&\input{AutoTex/scaleFactorVisibilityFactor}&\input{AutoTex/scaleFactorFov}&\input{AutoTex/scaleFactorKelly}&\input{AutoTex/scaleFactorScaleFactor}&\input{AutoTex/scaleFactorBias}&\input{AutoTex/scaleFactorNoiseStd}&\input{AutoTex/scaleFactorAlbedoValue}&\input{AutoTex/scaleFactorMaxSaturation},\input{AutoTex/scaleFactorMinSaturation}&\input{AutoTex/scaleFactorErrTol}&\input{AutoTex/scaleFactorLocation}	   \\ \hline
		6 & \input{AutoTex/biasUseConstellation}&\input{AutoTex/biasVisibilityFactor}&\input{AutoTex/biasFov}&\input{AutoTex/biasKelly}&\input{AutoTex/biasScaleFactor}&\input{AutoTex/biasBias}&\input{AutoTex/biasNoiseStd}&\input{AutoTex/biasAlbedoValue}&\input{AutoTex/biasMaxSaturation},\input{AutoTex/biasMinSaturation}&\input{AutoTex/biasErrTol}&\input{AutoTex/biasLocation}	   \\ \hline
		7    & \input{AutoTex/deviationUseConstellation}&\input{AutoTex/deviationVisibilityFactor}&\input{AutoTex/deviationFov}&\input{AutoTex/deviationKelly}&\input{AutoTex/deviationScaleFactor}&\input{AutoTex/deviationBias}&\input{AutoTex/deviationNoiseStd}&\input{AutoTex/deviationAlbedoValue}&\input{AutoTex/deviationMaxSaturation},\input{AutoTex/deviationMinSaturation}&\input{AutoTex/deviationErrTol}&\input{AutoTex/deviationLocation}	   \\ \hline
		8	& \input{AutoTex/albedoUseConstellation}&\input{AutoTex/albedoVisibilityFactor}&\input{AutoTex/albedoFov}&\input{AutoTex/albedoKelly}&\input{AutoTex/albedoScaleFactor}&\input{AutoTex/albedoBias}&\input{AutoTex/albedoNoiseStd}&\input{AutoTex/albedoAlbedoValue}&\input{AutoTex/albedoMaxSaturation},\input{AutoTex/albedoMinSaturation}&\input{AutoTex/albedoErrTol}&\input{AutoTex/albedoLocation}	   \\ \hline
		9	& \input{AutoTex/saturationUseConstellation}&\input{AutoTex/saturationVisibilityFactor}&\input{AutoTex/saturationFov}&\input{AutoTex/saturationKelly}&\input{AutoTex/saturationScaleFactor}&\input{AutoTex/saturationBias}&\input{AutoTex/saturationNoiseStd}&\input{AutoTex/saturationAlbedoValue}&\input{AutoTex/saturationMaxSaturation},\input{AutoTex/saturationMinSaturation}&\input{AutoTex/saturationErrTol}&\input{AutoTex/saturationLocation}	   \\ \hline
		10	& \input{AutoTex/sunDistanceUseConstellation}&\input{AutoTex/sunDistanceVisibilityFactor}&\input{AutoTex/sunDistanceFov}&\input{AutoTex/sunDistanceKelly}&\input{AutoTex/sunDistanceScaleFactor}&\input{AutoTex/sunDistanceBias}&\input{AutoTex/sunDistanceNoiseStd}&\input{AutoTex/sunDistanceAlbedoValue}&\input{AutoTex/sunDistanceMaxSaturation},\input{AutoTex/sunDistanceMinSaturation}&\input{AutoTex/sunDistanceErrTol}&\input{AutoTex/sunDistanceLocation}	   \\ \hline
		11	& \input{AutoTex/cleanCombinedUseConstellation}&\input{AutoTex/cleanCombinedVisibilityFactor}&\input{AutoTex/cleanCombinedFov}&\input{AutoTex/cleanCombinedKelly}&\input{AutoTex/cleanCombinedScaleFactor}&\input{AutoTex/cleanCombinedBias}&\input{AutoTex/cleanCombinedNoiseStd}&\input{AutoTex/cleanCombinedAlbedoValue}&\input{AutoTex/cleanCombinedMaxSaturation},\input{AutoTex/cleanCombinedMinSaturation}&\input{AutoTex/cleanCombinedErrTol}&\input{AutoTex/cleanCombinedLocation}	   \\ \hline
		12	& \input{AutoTex/combinedUseConstellation}&\input{AutoTex/combinedVisibilityFactor}&\input{AutoTex/combinedFov}&\input{AutoTex/combinedKelly}&\input{AutoTex/combinedScaleFactor}&\input{AutoTex/combinedBias}&\input{AutoTex/combinedNoiseStd}&\input{AutoTex/combinedAlbedoValue}&\input{AutoTex/combinedMaxSaturation},\input{AutoTex/combinedMinSaturation}&\input{AutoTex/combinedErrTol}&\input{AutoTex/combinedLocation}	   \\ \hline
		13    & \input{AutoTex/constellationUseConstellation}&\input{AutoTex/constellationVisibilityFactor}&\input{AutoTex/constellationFov}&\input{AutoTex/constellationKelly}&\input{AutoTex/constellationScaleFactor}&\input{AutoTex/constellationBias}&\input{AutoTex/constellationNoiseStd}&\input{AutoTex/constellationAlbedoValue}&\input{AutoTex/constellationMaxSaturation},\input{AutoTex/constellationMinSaturation}&\input{AutoTex/constellationErrTol}&\input{AutoTex/constellationLocation}	   \\ \hline
		\hline
	\end{tabular}
\end{table}
The tolerances above were chosen basically to be machine tolerance which is consistently passable between machines and operating systems. Those tests involving noise have looser tolerances because they are comparing the standard deviation of generated noise with the requested standard deviation. longer runs would make this tolerance tighter, but take large amounts of time for users to run the tests.

\section{Test Results}
The results of each test are shown in the table below. If a test did not pass, an error message is included.

\begin{table}[H]
	\caption{Test results.}
	\label{tab:results}
	\centering \fontsize{10}{10}\selectfont
	\begin{tabular}{ c | c | c } % Column formatting,
		\hline
		\textbf{Test} & \textbf{Pass/Fail} 						   		    			& \textbf{Notes} 									      \\ \hline
		1	   			  	&\input{AutoTex/plainPassedText}      	   			&\input{AutoTex/plainPassFailMsg} 	         	\\ \hline
		2	   			  	&\input{AutoTex/eclipsePassedText}       			&\input{AutoTex/eclipsePassFailMsg} 	       \\ \hline
		3	   			  	&\input{AutoTex/fieldOfViewPassedText}			&\input{AutoTex/fieldOfViewPassFailMsg} 	 \\ \hline
		4	   			  	&\input{AutoTex/kellyFactorPassedText} 			&\input{AutoTex/kellyFactorPassFailMsg} 	   \\ \hline
		5	   			  	&\input{AutoTex/scaleFactorPassedText}			&\input{AutoTex/scaleFactorPassFailMsg} 	 \\ \hline
		6	   			  	&\input{AutoTex/biasPassedText}      	  			&\input{AutoTex/biasPassFailMsg} 	              \\ \hline
		7	   			  	&\input{AutoTex/deviationPassedText}    		&\input{AutoTex/deviationPassFailMsg} 	     	 \\ \hline
		8	   			  	&\input{AutoTex/albedoPassedText}      			&\input{AutoTex/albedoPassFailMsg} 	           		\\ \hline
		9	   			  	&\input{AutoTex/saturationPassedText} 			&\input{AutoTex/saturationPassFailMsg} 	        \\ \hline
		10	   			  	&\input{AutoTex/sunDistancePassedText}      &\input{AutoTex/sunDistancePassFailMsg} 	   \\ \hline
		11	   			  	&\input{AutoTex/cleanCombinedPassedText}    &\input{AutoTex/cleanCombinedPassFailMsg} \\ \hline
		12	   			  	&\input{AutoTex/combinedPassedText}  		&\input{AutoTex/combinedPassFailMsg} 	      	\\ \hline
		13	   			  	&\input{AutoTex/constellationPassedText} 	&\input{AutoTex/constellationPassFailMsg}		\\ \hline

	\end{tabular}
\end{table}

In addition to the tabulated results, test data has been plotted for visual inspection. In Fig. \ref{fig:combinedPlot}, all single coarse sun sensor simulations have been plotted on top of one another. This makes for convenient comparison between the cases. For instance, the scaleFactor case can be seen to peak at 2 rather than 1 whereas the eclipse case peaks at 0.5. Furthermore, the fieldOfView test drops to zero at a value less than $\frac{\pi}{2}$ but it follows the plain case otherwise. The kellyFactor case similarly follows the plain curve, except at the edges. The albedo curve cannot be seen because it lies directly beneath the plain curve. The bias curve is equivalent to the plain curve, but shifted up. Finally, the two curves with noise clearly follow the ordinary curve pattern.

\input{AutoTex/combinedPlot.tex}
\clearpage
The constellation test results are shown in Fig. \ref{fig:constellationPlots}. The results are identical to each other and the test has been successful.

\input{AutoTex/constellationPlots.tex}


\XtoCBlock{DT1}
\label{block:DT1}
\begin{figure}[H]\includegraphics{DT1}\end{figure}

\begin{XtoCtabular}{Inports}
In & Input In(k)\tabularnewline
\hline
\end{XtoCtabular}

\begin{XtoCtabular}{Outports}
Out & Output Out(k)\tabularnewline
\hline
\end{XtoCtabular}

\begin{XtoCtabular}{Mask Parameters}
V & Gain\tabularnewline
\hline
fc & Cut off frequency of low pass filter\tabularnewline
\hline
ts\_fact & Multiplication factor of base sampling time (in integer format)\tabularnewline
\hline
method & Discretization method\tabularnewline
\hline
\end{XtoCtabular}

\subsubsection*{Description:}
First order high pass:

    G(s) = V*s/(s/w + 1)

% include optional documentation file
\InputIfFileExists{\XcHomePath/Library/Control/Doc/DT1_Info.tex}{\vspace{1ex}}{}

\subsubsection*{Implementations:}
\begin{tabular}{l l}
\textbf{FiP8} & 8 Bit Fixed Point Implementation\tabularnewline
\textbf{FiP16} & 16 Bit Fixed Point Implementation\tabularnewline
\textbf{FiP32} & 32 Bit Fixed Point Implementation\tabularnewline
\textbf{Float32} & 32 Bit Floating Point Implementation\tabularnewline
\textbf{Float64} & 64 Bit Floating Point Implementation\tabularnewline
\end{tabular}

\XtoCImplementation{FiP8}
\index{Block ID!3328}
\nopagebreak[0]
% Implementation details
\begin{tabular}{l l}
\textbf{Name} & FiP8 \tabularnewline
\textbf{ID} & 3328 \tabularnewline
\textbf{Revision} & 0.1 \tabularnewline
\textbf{C filename} & DT1\_FiP8.c \tabularnewline
\textbf{H filename} & DT1\_FiP8.h \tabularnewline
\end{tabular}
\vspace{1ex}

8 Bit Fixed Point Implementation

\begin{XtoCtabular}{Controller Parameters}
b0 & \tabularnewline
\hline
b1 & \tabularnewline
\hline
a0 & \tabularnewline
\hline
sfrb & \tabularnewline
\hline
sfra & \tabularnewline
\hline
in\_old & In(k-1)\tabularnewline
\hline
\end{XtoCtabular}

% Implementation data structure
\XtoCDataStruct{Data Structure:}
\begin{lstlisting}
typedef struct {
     uint16        ID;
     int8          *In;
     int8          Out;
     int8          b0;
     int8          b1;
     int8          a0;
     int8          sfrb;
     int8          sfra;
     int8          in_old;
} DT1_FIP8;
\end{lstlisting}

\ifdefined \AddTestReports
\InputIfFileExists{\XcHomePath/Library/Control/Doc/Test_DT1_FiP8.tex}{}{}
\fi
\XtoCImplementation{FiP16}
\index{Block ID!3329}
\nopagebreak[0]
% Implementation details
\begin{tabular}{l l}
\textbf{Name} & FiP16 \tabularnewline
\textbf{ID} & 3329 \tabularnewline
\textbf{Revision} & 0.1 \tabularnewline
\textbf{C filename} & DT1\_FiP16.c \tabularnewline
\textbf{H filename} & DT1\_FiP16.h \tabularnewline
\end{tabular}
\vspace{1ex}

16 Bit Fixed Point Implementation

\begin{XtoCtabular}{Controller Parameters}
b0 & \tabularnewline
\hline
b1 & \tabularnewline
\hline
a0 & \tabularnewline
\hline
sfrb & \tabularnewline
\hline
sfra & \tabularnewline
\hline
in\_old & In(k-1)\tabularnewline
\hline
\end{XtoCtabular}

% Implementation data structure
\XtoCDataStruct{Data Structure:}
\begin{lstlisting}
typedef struct {
     uint16        ID;
     int16         *In;
     int16         Out;
     int16         b0;
     int16         b1;
     int16         a0;
     int8          sfrb;
     int8          sfra;
     int16         in_old;
} DT1_FIP16;
\end{lstlisting}

\ifdefined \AddTestReports
\InputIfFileExists{\XcHomePath/Library/Control/Doc/Test_DT1_FiP16.tex}{}{}
\fi
\XtoCImplementation{FiP32}
\index{Block ID!3330}
\nopagebreak[0]
% Implementation details
\begin{tabular}{l l}
\textbf{Name} & FiP32 \tabularnewline
\textbf{ID} & 3330 \tabularnewline
\textbf{Revision} & 0.1 \tabularnewline
\textbf{C filename} & DT1\_FiP32.c \tabularnewline
\textbf{H filename} & DT1\_FiP32.h \tabularnewline
\end{tabular}
\vspace{1ex}

32 Bit Fixed Point Implementation

\begin{XtoCtabular}{Controller Parameters}
b0 & \tabularnewline
\hline
b1 & \tabularnewline
\hline
a0 & \tabularnewline
\hline
sfrb & \tabularnewline
\hline
sfra & \tabularnewline
\hline
in\_old & In(k-1)\tabularnewline
\hline
\end{XtoCtabular}

% Implementation data structure
\XtoCDataStruct{Data Structure:}
\begin{lstlisting}
typedef struct {
     uint16        ID;
     int32         *In;
     int32         Out;
     int32         b0;
     int32         b1;
     int32         a0;
     int8          sfrb;
     int8          sfra;
     int32         in_old;
} DT1_FIP32;
\end{lstlisting}

\ifdefined \AddTestReports
\InputIfFileExists{\XcHomePath/Library/Control/Doc/Test_DT1_FiP32.tex}{}{}
\fi
\XtoCImplementation{Float32}
\index{Block ID!3331}
\nopagebreak[0]
% Implementation details
\begin{tabular}{l l}
\textbf{Name} & Float32 \tabularnewline
\textbf{ID} & 3331 \tabularnewline
\textbf{Revision} & 0.1 \tabularnewline
\textbf{C filename} & DT1\_Float32.c \tabularnewline
\textbf{H filename} & DT1\_Float32.h \tabularnewline
\end{tabular}
\vspace{1ex}

32 Bit Floating Point Implementation

\begin{XtoCtabular}{Controller Parameters}
b0 & Coefficient b0\tabularnewline
\hline
b1 & Coefficient b1\tabularnewline
\hline
a0 & Coefficient a0\tabularnewline
\hline
in\_old & In(k-1)\tabularnewline
\hline
\end{XtoCtabular}

% Implementation data structure
\XtoCDataStruct{Data Structure:}
\begin{lstlisting}
typedef struct {
     uint16        ID;
     float32       *In;
     float32       Out;
     float32       b0;
     float32       b1;
     float32       a0;
     float32       in_old;
} DT1_FLOAT32;
\end{lstlisting}

\ifdefined \AddTestReports
\InputIfFileExists{\XcHomePath/Library/Control/Doc/Test_DT1_Float32.tex}{}{}
\fi
\XtoCImplementation{Float64}
\index{Block ID!3332}
\nopagebreak[0]
% Implementation details
\begin{tabular}{l l}
\textbf{Name} & Float64 \tabularnewline
\textbf{ID} & 3332 \tabularnewline
\textbf{Revision} & 0.1 \tabularnewline
\textbf{C filename} & DT1\_Float64.c \tabularnewline
\textbf{H filename} & DT1\_Float64.h \tabularnewline
\end{tabular}
\vspace{1ex}

64 Bit Floating Point Implementation

\begin{XtoCtabular}{Controller Parameters}
b0 & Coefficient b0\tabularnewline
\hline
b1 & Coefficient b1\tabularnewline
\hline
a0 & Coefficient a0\tabularnewline
\hline
in\_old & In(k-1)\tabularnewline
\hline
\end{XtoCtabular}

% Implementation data structure
\XtoCDataStruct{Data Structure:}
\begin{lstlisting}
typedef struct {
     uint16        ID;
     float64       *In;
     float64       Out;
     float64       b0;
     float64       b1;
     float64       a0;
     float64       in_old;
} DT1_FLOAT64;
\end{lstlisting}

\ifdefined \AddTestReports
\InputIfFileExists{\XcHomePath/Library/Control/Doc/Test_DT1_Float64.tex}{}{}
\fi


\documentclass[12pt]{article}
\usepackage{graphics}
\newcounter{problem}
\newcommand{\kg}{\mathrm{kg}}
\newcommand{\m}{\mathrm{m}}
\newcommand{\s}{\mathrm{s}}
\newcommand{\mps}{\m\,\s^{-1}}
\newcommand{\hp}{\mathrm{hp}}
\begin{document}
\thispagestyle{empty}

\section*{NYU Physics 1---Problem set 7}

Due Tuesday 2009 November 3 at the beginning of lecture.

\paragraph{Problem~\theproblem:}\refstepcounter{problem}%
A Japanese Shinkansen (bullet train) (of mass $M=10^6~\mathrm{kg}$),
moving at $v_S=300~\mathrm{km\,h^{-1}}$ with respect to the tracks,
hits and collides elastically with a superball (of mass
$m=30~\mathrm{g}$), which is initially at rest.  The front face of the
train is inclined at an angle of $\theta=45~\mathrm{deg}$ to the
horizontal, as shown here in the rest frame of the tracks.\\
\rule{0.1\textwidth}{0pt}
\resizebox{0.8\textwidth}{!}{\includegraphics{../mp/shinkansen.eps}}

\textsl{(a)}~Draw a diagram of the ball--train system, just before the
collision, in the center-of-mass rest frame.  Clearly show the
velocities of the ball and train in this frame.

\textsl{(b)}~Draw a diagram, just after the collision, in the
center-of-mass frame.  Clearly show the velocities.

\textsl{(c)}~Draw a diagram, just after, back in the rest frame of the
tracks.  What is the final speed and direction of the ball,
immediately after the collision, in the rest frame of the tracks?

\paragraph{Problem~\theproblem:}\refstepcounter{problem}%
What is the peak force between two pool balls in a pool shot?
Estimate the momentum transferred to the object ball in a hard pool
shot.  Estimate (or look up) masses and velocities.  For how long are
two pool balls in contact?  The time can be approximated by the length
of time it takes a sound wave to cross a pool ball.  Put it all
together and compare it to the force of gravity or the normal force
from the table on each ball.

\paragraph{Problem~\theproblem:}\refstepcounter{problem}%
\textsl{(a)}~Imagine an ideal car of mass $1000\,\kg$ with a
cross-sectional area $A=3\,\m^2$, drag coefficient $1$ (so the
air-resistance force is exactly $(1/2)\,\rho\,A\,v^2$) producing total
mechanical power of $P=130\,\hp$.  At time $t=0$, the car is traveling
at $5\,\mps$ in the $x$ direction.  Use a spreadsheet to compute the
velocity as a function of time if the driver puts ``petal to the
metal'' for the next $20\,\s$ (with a $0.1\,\s$ time-step) on a flat,
straight, $x$-direction road.  You will have to use the fact that the
force accelerating the car is the power over the velocity (why?) but
subtracting off the air resistance force.  Make a plot of the velocity
of the car as a function of time, with axes clearly labeled.  Note
that this is only a ``first-order'' integration: You only need to keep
track of the velocity, not the position.

\textsl{(b)}~Why did I not have you start at a velocity of $0\,\mps$?
What would limit the acceleration in the first fraction of a second in
this case---for a car starting from rest?  It isn't the power of the
engine!

\end{document}


\subsection{Value Alignment} \label{sec:value_alignment}
AIAs operate autonomously in delegated tasks, with the expectation that they behave according to users' intent frames.
Optimization-based algorithms are arguably among the most common and direct approaches for accomplishing this.
The general idea is to define a \emph{utility function} that normatively governs the AIA's abilities so that desirable behaviors are elicited through maximization of the utility, i.e. such that the AIA behaves rationally in accordance with the user's intent frame.
A utility function describes the `long-term desirability' of taking certain actions in certain conditions, i.e. beyond immediate benefits or penalties, and should coherently reflect user preferences about the state of the world and AIA behaviors ~\cite{Russell2010-wv}.
Such mapping of user intent frames to utility functions has two positive benefits.
Firstly, it ensures that AIA behaviors can themselves be used as assurances: users will tend to trust AIA's more if they are `well-behaved' and acting in accordance with their desired intent than if they are not.
Secondly, an AIA can generate assurances via auxiliary behaviors that help ensure its utility function is aligned with the user's intent frame.  Since it is practically quite challenging to encode user preferences and intent frames into utility functions, the process of \emph{value alignment}\footnote{Value alignment is more commonly known as `AI Alignment' in AI research~\cite{Yudkowsky2001-hb,Bensinger2014-ul}.} leads to many different algorithmic strategies for generating assurances.

Consider a generic decision-making problem where an AIA that must make choice $a \in {\mathcal A}$ given some task state $s \in {\mathcal S}$, with scalar utility function $U_A(a,s)$. %and possible sets of actions and states ${\mathcal A}$ and ${\mathcal S}$.
If a user's true utility is represented by scalar function $U_H(a,s)$, then in the ideal situation the AIA seeks the optimal decision $a^* \in {\mathcal A}$ such that, for any $s \in {\mathcal S}$,
\begin{align*}
    a* = \arg\max_{\mathcal A} U_A(a,s) = \arg \max_{\mathcal A} U_H(a,s).
\end{align*}
Hence, value alignment tries to minimize the difference between the utilities of the AIA and the user. When the utility of the robot $U_A(a,s)$ and the human $U_H(a,s)$ are approximately equivalent (within some tolerance) then the values of the AIA are \emph{aligned} with those of the human. An AIA with aligned values will be considered by users to be more predictable (and thus more competent), because the AIA will be more likely to act in desirable ways.
%
\citet{Bostrom2014-fz} provides a well-known example of an AIA whose value is \emph{not} aligned: an autonomous robot is designed, and deployed, with the intent that it make paper clips. To maximize $U_A(s,a)$, the robot then decides to take over the world in order to maximize its resources and ability to make more paper clips. To reasonable human users, this was clearly \emph{not} the intended behavior; the utilities that the robot used for making decisions did not match those that the human must have had. Therefore the robot's resulting behavior was intrinsically an assurance that reduced trust. On the other hand, if the robot were to try to learn from its mistakes and improve (i.e. make $U_A(s,a)$ closer to $U_h(s,a)$) that could be perceived as an assurance that increases trust---the robot can be `forgiven' for making honest mistakes in trying to optimize an ill-posed/under-specified utility function, as long as it is able to recognize and remedy this.

\subsubsection{Common Approaches:}
There are two algorithmic strategies for value alignment: (i) indirect: approximate $U_H(a,s)$ explicitly via $U_A(a,s)$, and then use this approximation to find $a^*$; (ii) direct: identify $a^* = \arg \max U_H(a,s)$ directly via the use of optimal state-action value functions $Q^*(s,a)$ (which give the utility to be gained if the AIA were to proceed optimally starting from $s$, regardless of its past states or actions).
These strategies closely resemble techniques used for reinforcement learning problems and their variants (especially inverse reinforcement learning); not surprisingly, most value alignment techniques are rooted in this domain.
Value alignment research tends to focus on several different issues \cite{Gordon_Worley2018-xy,Amodei2016-xi}; some of the more directly applicable topics and associated methods that point to useful assurance strategies are described below.
The solutions to these problems are assurances because they afford opportunities for  users to better understand the actual intentions and goals of the AIA, as well as understand how the AIA actually interprets intent frames.

\paragraph{Reward Hacking and Human-Guided Learning}
The reward hacking problem deals with avoiding and removing unintended consequences in AIA behaviors that arise from imperfections in the specification of $U_A(s,a)$ (as in the paper clip-making robot example above).
The most popular solution strategies use some form of offline supervisory human guidance or training data feedback in the utility function learning process.
This approach recognizes the intrinsic difficulty of mapping user preferences to a single scalar utility $U_H(s,a)$ for complex tasks, and leverages sophisticated machine learning and reasoning strategies to identify relevant preferences within $U_A(s,a)$ or $Q^*(s,a)$, depending on the kinds of tasks considered. For instance, one-shot/non-sequential decision-making tasks like image recognition or object perception do not necessarily have dynamical state considerations, but may require potential expansion of the action space for sensible labeling of new object categories.

In the context of sequential decision making problems, \citet{Hadfield-Menell2017-tl}, \citet{Hadfield-Menell2016-ws}, and \citet{Huang2017-lk} all consider variations of the `inverse reward design' problem using inverse reinforcement learning techniques. In these works, discounted cumulative rewards are used to model utility functions $U_A(s,a)$ and $U_H(s,a)$, where the actual reward factors contributing to $U_H(s,a)$ are unknown but can be inferred from user-generated contextual information at design time. Specifically, \cite{Hadfield-Menell2017-tl} notes that reward factors provided by users in limited training contexts serve as `noisy evidence of intent'. Hence, to avoid situations where an AIA trainee demonstrates desirable behaviors in specific training scenarios but later demonstrates undesirable behaviors in novel scenarios, the AIA must be able to reason over the uncertainty in the user's intent in order to fully capture the context in which it was trained.
In a different task setting, \citet{Freitas2006-qo} compared two approaches to discovering `interesting' knowledge from large data sets, based on the idea that human users require assistance from complex systems in order to find useful patterns and other interesting insights. He mentions `user-driven' methods that involve a user suggesting interesting templates or providing general impressions in the form of IF-THEN rules. A subsequent comparison to different `data-driven' methods suggests that the latter are not very effective in practice.
Having said that, user-driven approaches may not fare any better when compared over many users, as each user will likely have different preferences. Other scaled up user-driven approaches, e.g. based on crowd-sourcing~\citet{Chang2017-kl}, can also achieve better accuracy for labeling tasks while also exploring new or ambiguous classes that can be ignored with traditional approaches (especially if training data sets are biased or very limited). \citet{Chang2017-kl} also consider a similar, scaled up, `user-driven' approach called `Revolt' that crowd-sources the labeling of images. It is able to attain high accuracy labeling, while also exploring new or ambiguous classes that might be ignored with traditional approaches.

Some other methods for designing, learning and eliciting appropriate utility functions are also discussed in \cite{Hadfield-Menell2016-ws,Da_Veiga2012-gh,Garcia2015-rs}.
Despite the differences in AIA application contexts, these methods all provide the user with better context for what \emph{should} be known by system, and for how well it can interpolate/extrapolate.
These processes allow users to refine their own intent in complex settings, e.g. to reveal or resolve subtle low-level inconsistencies in desired task requirements that would otherwise lead a rational AIA to undesirable behaviors.

\paragraph{Safe learning and correct-by-construction synthesis:}
In many applications, $U_H(s,a)$ must be safely approximated when certain combinations of $(s,a)$ lead to irreversibly bad consequences.
Hence, as AIAs try to learn what a user's utility is, they must do so in a safe manner.
For instance, humans do not learn about the dangers of heights from falling off of skyscrapers.
Instead we have to do so cautiously over time, and extrapolate from much less drastic experience (i.e. tripping on a curb).
Safe reinforcement learning (safe RL) methods offer formal strategies and assurances for AIAs to learn in similar ways.
Safe RL has been defined as the process of avoiding ``unintended and harmful behavior that [emerges] from machine learning systems''~\cite{Amodei2016-xi}. Two ways to approach safe RL are: (i) modification of the optimality criterion with a safety factor, and (ii) modification of the exploration process through the incorporation of external knowledge~\cite{Garcia2015-rs}.

For example, \citet{Lipton2016-dq} design an `intrinsic fear' RL approach that uses a deep Q-network and a `supervised danger model'. The danger model stores the likelihood of entering a catastrophe state within a `short number of steps'. This model can be learned by detecting catastrophes through experience and can be improved over time. \citet{Curran2016-ij}, in a more specific application, asks how a robot can learn when a task is too risky, and then avoid those situations, or ask for help.
Similarly, \citet{Kahn2017-vy} use Bayesian Deep Neural Nets (using bootstrapping and dropout) to learn about the probability (with uncertainty) of an autonomous vehicle colliding in an environment given its current state, observations, and sequence of controls. Using this model they formulate a `velocity-dependent collision cost' that is used for model-based reinforcement learning. With this approach the vehicle naturally proceeds slowly when there is an elevated risk of collision. This `safety-aware' behavior provides an assurance signal to the user.

Aside from purely learning-based approaches, we can also consider Validation and Verification (V\&V) methods.
Not all practitioners are aware that V\&V techniques can generate soft assurances for users.
This is because V\&V typically refers to the use of formal methods to guarantee the behavior of a system within some set of specifications, which are handed down by a certification authority as requirements to system designers to generate `hard assurances' (formal proofs of the functionality of the system).
Although these `hard assurances' are not primarily designed for user consumption, they could in principle be exposed to and interpreted for users in certain contexts.
A prime example is given by \citet{Raman2013-mz}, who developed a formal way for non-expert users to provide structured natural language task specifications to a robot, such that a `correct-by-construction' controller will be built if the specification is valid.
Otherwise, the robot will provide an explanation about which specification(s) are unrealizable/inconsistent and will cause failure.
In the context of a practical self-driving car application, \citet{Ghosh2016-dl} presents a framework called Trusted Machine Learning (TML) for learning models from dynamically generated data that fit pre-determined `trustworthiness' constraints.
These approaches are promising in that they not only present a way to communicate when and why specified tasks cannot be performed or certain actions cannot be taken, but also provide positive assurances in the form of guaranteed, formally verified, AIA processes for performing desired tasks (plans, models, etc.).
While this directly addresses the competence and predictability components of AIA trust, the `raw' expression of these assurances does not formally account for effects on user trust or TRBs in formulating explanations.

\paragraph{Robustness to context shifts}
How can an AIA determine when the basis and provenance of its approximation to $U_H(s,a)$ or $Q^*(s,a)$ is no longer valid for a particular task?
This problem has attracted much recent attention in the learning literature under the guise of `nonstationary' learning.
Nonstationarity refers to the complex challenge of training a model based on data from one distribution $D$ while taking into account that the test distribution $D^\prime$ will likely shift through time~\cite{Quinonero-Candela2009-fj}.
For instance, in the context of classification problems, \citet{Sugiyama2013-ci} propose using importance sampling Monte Carlo to formally detect events related to `covariate shift' (training and test input data follow different distributions) and `class-balance change' (where the class-prior probabilities are different in training and test phases, but where there is no covariate shift).
Similarly, \citet{Charikar2017-kr} address learning from `untrusted' data, which could be subject to adversarial attack or unknown nonstationarity.

These methods can be more generally adapted and developed beyond learning tasks, in order to evaluate the sensitivity of as-designed AIA's capabilities to possible changes in task context not captured/considered at design time (an example of coping with `unknown unknowns').
If the sensitivities imply a significant deviation in $U_A(s,a)$ or $a^*$ from expected values (i.e. from user intent frame as initially understood), or indicate the presence of new $(s,a)$ pairs that are not accounted for by $U(s,a)$ (e.g. test data that is very far from the training set), then the AIA can inform the user accordingly and thus possibly opt out of performing tasks that are now potentially `out of scope'. This provides direct low-level behavioral assurances about changes in predictability, competence, and situation normality, though these may not be immediately understood by non-expert users.

\subsubsection{Grounding Example:}
In the case of the `VIP Escort' problem (described in Section~\ref{sec:mot_example}), value alignment might be used as an assurance in the following way, starting with the assumptions that:

\begin{itemize}
    \item The UGV has just begun an attempt to escape the road-network
    \item The UGV uses safe RL to learn its escape policy
    \item The operator is able to observe the UGV during its entire escape attempt
\end{itemize}

The operator has used several different UGVs for similar tasks. This newer model uses `safe RL' to learn its policy. When observing the UGV's attempt at escape the operator notices a difference in how the UGV operates. Whereas the older UGV models would sometimes do risky things, this UGV seems to navigate dangerous situations much better.

\paragraph{\textbf{Discussion of Example:}} In this case, safe RL enabled the UGV to treat situations that an operator might classify as `dangerous' with more care. With this integral capability the UGV assures the operator that it is more competent.