<html><head><meta name="color-scheme" content="light dark"></head><body><pre style="word-wrap: break-word; white-space: pre-wrap;">\documentclass[onecolumn,letterpaper,11pt]{article}

%===================================================================
\input{common_latex/common_latex_packages}
\input{common_latex/common_latex_definitions}
\input{common_latex/common_latex_help_format}
%===================================================================

\begin{document}

\begin{labcover}
\labtitle{uSimMarine: Basic Vehicle Simulation}
\vspace{0.1in}
\labdate{June 2018}
\vspace{0.1in}
\labauth{Michael Benjamin, mikerb@mit.edu}
\labauth{Department of Mechanical Engineering}
\labauth{MIT, Cambridge MA 02139}
\labsrc{project-pavlab/appdocs/app\_usimmarine} 
\labtoc{small}
\end{labcover}

\wikidoc{file=IvPTools.USimMarine}
\wikidoc{bugz=uSimMarine}
\wikidoc{auth=mikerb@mit.edu}

%=================================================================
\section{Overview}
%=================================================================
\label{sec_util_usimmarine}
\index{uSimMarine}

The \app{uSimMarine} application is a simple 3D vehicle simulator that
updates vehicle state, position and trajectory, based on the present
actuator values and prior vehicle state. The typical usage scenario
has a single instance of \app{uSimMarine} associated with each
simulated vehicle, as shown in Figure \ref{fig_usm_topology}.

\begin{figure}[ht]
  \centering 
  \includegraphics[width=0.82 \textwidth]{figures/usm_topology.png}
  \caption{{\bf Typical \aapp{uSimMarine} Usage:} In an N-vehicle
    simulation, an instance if \aapp{uSimMarine} is used for each
    vehicle. Each simulated vehicle typically has its own dedicated
    MOOS community. The IvP Helm (\aapp{pHelmIvP}) publishes
    high-level control decisions. The PID controller
    (\vvar{pMarinePID}) converts the high-level control decisions to
    low-level actuator decisions.  Finally the simulator
    (\aapp{uSimMarine}) reads the low-level actuator postings to
    produce a new vehicle position.}
\label{fig_usm_topology}
\end{figure}

This style of simulation can be contrasted with simulators that
simulate a comprehensive set of aspects of the simulation, including
multiple vehicles, and aspects of the environment and
communications. The \app{uSimMarine} simulator simply focuses on a
single vehicle.  It subscribes for the vehicle navigation state
variables:

\vspace{0.1in}
\begin{packed_itemize}
\item \mvar{NAV\_X}, \mvar{NAV\_Y}, \mvar{NAV\_SPEED}, \mvar{NAV\_HEADING}, 
and \mvar{NAV\_DEPTH}
\end{packed_itemize}
\vspace{0.05in}

\noindent
as well as the actuator values

\vspace{0.05in}
\begin{packed_itemize}
\item \mvar{DESIRED\_RUDDER}, \mvar{DESIRED\_THRUST}, and \mvar{DESIRED\_ELEVATOR}.
\end{packed_itemize}

\vspace{0.1in}

\noindent
The simulator accommodates a notion of external drifts applied
to the vehicle to crudely simulate current or wind. These drifts may
be set statically or may be changing dynamically by other MOOS
processes. The simulator also may be configured with a simple
geo-referenced data structure representing a field of water currents.

\pskip

Under typical UUV payload autonomy operation, the simulator and
\app{pMarinePID} MOOS modules would not be present. The vehicle's
native controller would handle the role of \app{pMarinePID}, and the
vehicle's native navigation system (and the vehicle itself) would
handle the role of the simulator.

%=================================================================
\section{Configuration Parameters for uSimMarine}
%=================================================================
\index{uSimMarine!Configuration Parameters}
\index{Configuration Parameters!uSimMarine}

The following parameters are defined for \app{uSimMarine}. A more
detailed description is provided in other parts of this
section. Parameters having default values are indicated so in
parentheses below.

\vspace{0.15in}
\begin{listing}{Configuration Parameters for uSimMarine}
\label{lst_usm_params}
\renewcommand{\arraystretch}{1.1}
\begin{longtable}{rp{0.72\textwidth}} 

  \pvar{buoyancy\_rate}: &amp; Rate, in meters per second, at which
  vehicle floats to surface at zero speed. The default is zero.
  Section \ref{sec_usm_depth}. \\

  \pvar{current\_field}: &amp; A file containing the specification of a
  current field. \\

  \pvar{current\_field\_active}: &amp; If true, simulator uses the
  current field if specified. \\

  \pvar{default\_water\_depth}: &amp; Default value for local water depth
  for calculating altitude (0). \\
    
  \pvar{drift\_vector}: &amp; A pair of external drift values, direction
  and magnitude. \\
  
  \pvar{rotate\_speed}: &amp; An external rotational speed in degrees per
  second (0). \\
  
  \pvar{drift\_x}: &amp; An external drift value applied in the $x$
  direction (0).  \\
  
  \pvar{drift\_y}: &amp; An external drift value applied in the $y$
  direction (0). \\
  
  \pvar{max\_acceleration}: &amp; Maximum rate of vehicle acceleration in $m/s^2$
  (0.5). \\
  
  \pvar{max\_deceleration}: &amp; Maximum rate of vehicle deceleration in $m/s^2$
  (0.5). \\
  
  \pvar{max\_depth\_rate}: &amp; Maximum rate of vehicle depth change,
  meters per second. The defauls is 0.5. Section
  \ref{sec_usm_depth}. \\

  \pvar{max\_depth\_rate\_speed}: &amp; Vehicle speed at which max depth
  rate is achievable (2.5). Section \ref{sec_usm_depth}. \\

  \pvar{prefix}: &amp; Prefix of MOOS variables published.  The default is
  \var{USM\_}. \\
  
  \pvar{sim\_pause}: &amp; If true, the simulation is paused. The default is
  false. \\
  
  \pvar{start\_depth}: &amp; Initial vehicle depth in meters. The default
  is zero. Section \ref{sec_usm_initial_vals}. \\
  
  \pvar{start\_heading}: &amp; Initial vehicle heading in degrees.
  The default is zero. Section \ref{sec_usm_initial_vals}. \\
  
  \pvar{start\_pos}: &amp; A full starting position and trajectory
  specification. Section \ref{sec_usm_initial_vals}. \\
  
  \pvar{start\_speed}: &amp; Initial vehicle speed in meters per second.
  The default is zero. Section \ref{sec_usm_initial_vals}. \\
  
  \pvar{start\_x}: &amp; Initial vehicle $x$ position in local coordinates.
  The default is zero. Section \ref{sec_usm_initial_vals}. \\
  
  \pvar{start\_y}: &amp; Initial vehicle $y$ position in local coordinates.
  The default is zero. Section \ref{sec_usm_initial_vals}. \\
  
  \pvar{thrust\_factor}: &amp; A scalar correlation between thrust and
  speed. The default is 20. \\
  
  \pvar{thrust\_map}: &amp; A mapping between thrust and speed values. 
  Section \ref{sec_thrust_map}. \\
  
  \pvar{thrust\_reflect}: &amp; If true, negative thrust is simply
  opposite positive thrust. The default is false.  Section
  \ref{sec_thrust_map}. \\
 
  \pvar{turn\_loss}: &amp; A range $[0, 1]$ affecting speed lost during a turn.
  The default is 0.85. \\
  
  \pvar{turn\_rate}: &amp; A range $[0, 100]$ affecting vehicle turn radius, 
  e.g., 0 is an infinite turn radius. The default is 70. \\
  
\end{longtable}
\end{listing}


%================================================================
\subsection*{An Example MOOS Configuration Block}

An example MOOS configuration block is provided in Listing
\ref{lst_usm_econfig} below. This can also be obtained from a terminal
window with:

\vspace{0.1in}
\begin{consoleverb}
 $ uSimMarine --example or -e
\end{consoleverb}
\vspace{0.1in}

\vspace{0.25in}
\begin{listing}{Example configuration of the uSimMarine application}
\label{lst_usm_econfig}
\footnotesize
\begin{verbatim}
   1  =============================================================== 
   2  uSimMarine Example MOOS Configuration                           
   3  =============================================================== 
   4                                                                  
   5  ProcessConfig = uSimMarine                                      
   6  {                                                               
   7    AppTick   = 4                                                 
   8    CommsTick = 4                                                 
   9                                                                  
  10    start_x       = 0                                             
  11    start_y       = 0                                             
  12    start_heading = 0                                             
  13    start_speed   = 0                                             
  14    start_depth   = 0                                             
  15    start_pos     = x=0, y=0, speed=0, heading=0, depth=0         
  16                                                                  
  17    drift_x       = 0                                             
  18    drift_y       = 0                                             
  19    rotate_speed  = 0                                             
  20    drift_vector  = 0,0      // heading, magnitude             
  21                                                                  
  22    buoyancy_rate        = 0.025 // meters/sec                 
  23    max_acceleration     = 0     // meters/sec^2               
  24    max_deceleration     = 0.5   // meters/sec^2               
  25    max_depth_rate       = 0.5   // meters/sec                 
  26    max_depth_rate_speed = 2.0   // meters/sec                 
  27                                                                  
  28    sim_pause            = false // or {true}                  
  29    dual_state           = false // or {true}                  
  30    thrust_reflect       = false // or {true}                  
  31    thrust_factor        = 20    // range [0,inf)              
  32    turn_rate            = 70    // range [0,100]              
  33    thrust_map           = 0:0, 20:1, 40:2, 60:3, 80:5, 100:5     
  34  }                                                              
\end{verbatim}
\normalsize
\end{listing}
 
%================================================================
\section{Publications and Subscriptions for uSimMarine}
%================================================================
\index{uSimMarine!Publications and Subscriptions}
\index{Publications and Subscriptions!uSimMarine}
\label{sec_usm_interface}

The interface for \app{uSimMarine}, in terms of publications and
subscriptions, is described below. This same information may also be
obtained from the terminal with:

\vspace{0.1in}
\begin{consoleverb}
 $ usimMarine --interface or -i
\end{consoleverb}
\vspace{0.05in}

%================================================================
\subsection{Variables Published by uSimMarine}
\label{sec_umv_pubs}

The primary output of \app{uSimMarine} to the MOOSDB is the full
specification of the updated vehicle position and trajectory, along
with a few other pieces of information:

\begin{packed_itemize}[1]
\item \mvar{APPCAST}: Contains an appcast report identical to the
  terminal output. Appcasts are posted only after an appcast request
  is received from an appcast viewing utility. 

\item \mvar{BUOYANCY\_REPORT}:
\item \mvar{TRIM\_REPORT}:

\item \mvar{USM\_ALTITUDE}: The updated vehicle altitude in meters if
  water depth known.
\item \mvar{USM\_DEPTH}: The updated vehicle depth in meters. Section
  \ref{sec_usm_depth}.
\item \mvar{USM\_DRIFT\_SUMMARY}: A summary of the current total
  external drift.
\item \mvar{USM\_HEADING}: The updated vehicle heading in degrees.
\item \mvar{USM\_HEADING\_OVER\_GROUND}: The updated vehicle heading
  over ground.
\item \mvar{USM\_LAT}: The updated vehicle latitude position.
\item \mvar{USM\_LONG}: The updated vehicle longitude position.
\item \mvar{USM\_RESET\_COUNT}: The number of time the simulator has been reset.
\item \mvar{USM\_SPEED}: The updated vehicle speed in meters per
  second.
\item \mvar{USM\_SPEED\_OVER\_GROUND}: The updated speed over ground.
\item \mvar{USM\_X}: The updated vehicle $x$ position in local
  coordinates.
\item \mvar{USM\_Y}: The updated vehicle $y$ position in local
  coordinates.
\item \mvar{USM\_YAW}: The updated vehicle yaw in radians.
\end{packed_itemize}

\noindent
An example \mvar{USM\_DRIFT\_SUMMARY} string: \var{"ang=90, mag=1.5,
  xmag=90, ymag=0"}.

%================================================================
\subsection{Variables Subscribed for by uSimMarine}
\label{sec_umv_subs}

The \app{uSimMarine} application will subscribe for the following
MOOS variables:

\begin{packed_itemize}[1]
\item \mvar{APPCAST\_REQ}: A request to generate and post a new
  apppcast report, with reporting criteria, and expiration. See the
  documentation on Appcasting.

\item \mvar{DESIRED\_THRUST}: The thruster actuator setting, $[-100,100]$.
\item \mvar{DESIRED\_RUDDER}: The rudder actuator setting, $[-100,100]$.  
\item \mvar{DESIRED\_ELEVATOR}: The depth elevator setting, $[-100,100]$.
\item \mvar{USM\_SIM\_PAUSED}: Simulation pause request, either
  \var{true} or \var{false}.
\item \mvar{USM\_CURRENT\_FIELD}: If \var{true}, a configured current
  field is active.
\item \mvar{USM\_BUOYANCY\_RATE}: Dynamically set the zero-speed float
  rate.
\item \mvar{ROTATE\_SPEED}: Dynamically set the external rotational
  speed.
\item \mvar{DRIFT\_X}: Dynamically set the external drift in the $x$
  direction.
\item \mvar{DRIFT\_Y}: Dynamically set the external drift in the
  $y$ direction.
\item \mvar{DRIFT\_VECTOR}: Dynamically set the external drift
  direction and magnitude.
\item \mvar{DRIFT\_VECTOR\_ADD}: Dynamically modify the external
  drift vector.
\item \mvar{DRIFT\_VECTOR\_MULT}: Dynamically modify the external
  drift vector magnitude.
\item \mvar{USM\_RESET}: Reset the simulator with a new position,
  heading, speed and depth.
\item \mvar{WATER\_DEPTH}: Water depth at the present vehicle
  position.
\end{packed_itemize}


\noindent
Each iteration, after noting the changes in the navigation and
actuator values, it posts a new set of navigation state variables in
the form of \mvar{USM\_X}, \mvar{USM\_Y}, \mvar{USM\_SPEED},
\mvar{USM\_HEADING}, \mvar{USM\_DEPTH}. 

%================================================================
\subsection{Command Line Usage of uSimMarine}
\label{sec_usm_cmdline}
\index{Command Line Usage!uSimMarine}
\index{uSimMarine!Command Line Usage}
\index{Configuration Parameters!uSimMarine}
\index{uSimMarine!Configuration Parameters}

The \app{uSimMarine} application is typically launched as a part
of a batch of processes by pAntler, but may also be launched from the
command line by the user. The basic command line usage for the
\app{uSimMarine} application is the following:

\vspace{0.1in}
\begin{listing}{Command line usage for the \app{uSimMarine} application}
\label{lst_usm_usage}
\footnotesize
\begin{verbatim}
 1    Usage: uSimMarine file.moos [OPTIONS]                     
 2                                                              
 3    Options:                                                  
 4      --alias=&lt;ProcessName&gt;                                   
 5          Launch uSimMarine with the given process name       
 6          rather than uSimMarine.                             
 7      --example, -e                                           
 8          Display example MOOS configuration block.           
 9      --help, -h                                              
10          Display this help message.                          
11      --version,-v                                            
12          Display the release version of uSimMarine.       
\end{verbatim}
\normalsize
\end{listing}

%================================================================
\section{Setting the Initial Vehicle Position, Pose and Trajectory}
%================================================================
\label{sec_usm_initial_vals}
\index{uSimMarine!Initial Vehicle Position and Pose}

The simulator is typically configured with a vehicle starting position,
pose and trajectory given by the following five configuration parameters:

\begin{packed_itemize}
\item \pvar{start\_x}
\item \pvar{start\_y}
\item \pvar{start\_heading}
\item \pvar{start\_speed}
\item \pvar{start\_depth}
\end{packed_itemize}

\noindent
The position is specified in local coordinates in relation to a 
local datum, or $(0,0)$ position. This datum is specified in the 
\var{.moos} file at the global level. The heading is specified in 
degrees and corresponds to the direction the vehicle is pointing.
The initial speed and depth by default are zero, and are often
left unspecified in configuration. Alternatively, the same five
parameters may be set with the \pvar{start\_pos} parameter as
follows:

\prevblue
\begin{verbatim}
   start_pos = x=100, y=150, speed=0, heading=45, depth=0
\end{verbatim}
\postverb

\noindent
The simulator can also be reset at any point during its operation, 
by posting to the MOOS variable \mvar{USM\_RESET}. A posting of the
following form will reset the same five parameters as above:

\index{uSimMarine!Resetting}
\vspace{0.1in}
   \msvar{USM\_RESET = x=200, y=250, speed=0.4, heading=135, depth=10}
\vspace{0.1in}

\noindent
This has been useful in cases where the objective is to observe the
behavior of a vehicle from several different starting positions, and
an external MOOS script, e.g., \app{uTimerScript}, is used to reset
the simulator from each of the desired starting states.

%================================================================
\section{Propagating the Vehicle Speed, Heading, Position and Depth}
%================================================================
\index{uSimMarine!Propagating Vehicle Position and Pose}

The vehicle position is updated on each iteration of the
\app{uSimMarine} application, based on (a) the previous vehicle state,
(b) the elapsed time since the last update, $\Delta T$, (c) the
current actuator values, \mvar{DESIRED\_RUDDER}, \mvar{DESIRED\_THRUST},
and \mvar{DESIRED\_ELEVATOR}, and (d) several parameter settings
describing the vehicle model.

\pskip

For simplicity, this simulator updates the vehicle speed, heading,
position and depth in sequence, in this order. For example, the
position is updated after the heading is updated, and the new position
update is made as if the new heading were the vehicle heading for the
entire $\Delta T$. The error introduced by this simplification is
mitigated by running \app{uSimMarine} with a fairly high MOOS AppTick
value keeping the value of $\Delta T$ sufficiently small.

%================================================================
\subsection{Propagating the Vehicle Speed}

The vehicle speed is propagated primarily based on the current value
of thrust, which is presumably refreshed upon each iteration by
reading the incoming mail on the MOOS variable \mvar{DESIRED\_THRUST}.
To simulate a small speed penalty when the vehicle is conducting a
turn through the water, the new thrust value may also be affected by
the current rudder value, referenced by the incoming MOOS variable
\mvar{DESIRED\_RUDDER}. The newly calculated speed is also dependent on
the previously noted speed noted by the incoming MOOS variable
\mvar{NAV\_SPEED}, and the settings to the two configuration parameters
\mvar{MAX\_ACCELERATION} and \mvar{MAX\_DECELERATION}.

\vspace{0.1in}
\noindent
The algorithm for updating the vehicle speed proceeds as:

\begin{packed_enumerate}
\item Calculate $v_{i \var{(RAW)}}$, the new raw speed based on the thrust.
\item Calculate $v_{i{\var{(TURN)}}}$, an adjusted and potentially
  lower speed, based on the raw speed, $v_{i \var{(RAW)}}$, and the
  current rudder angle, \var{DESIRED\_RUDDER}.
\item Calculate $v_{i \var{(FINAL)}}$, an adjusted and potentially
  lower speed based on $v_{i{\var{(TURN)}}}$, compared to the prior
  speed.  If the magnitude of change violates either the max
  acceleration or max deceleration settings, then the new speed is
  clipped appropriately.
\item Set the new speed to be $v_{i \var{(FINAL)}}$, and use this new 
  speed in the later updates on heading, position and depth.
\end{packed_enumerate}

\noindent 
{\em Step 1}: In the first step, the new speed is calculated by the
current value of thrust. In this case the {\em thrust map} is
consulted, which is a mapping from possible thrust values to speed
values. The thrust map is configured with the \var{THRUST\_MAP}
configuration parameter, and is described in detail in Section
\ref{sec_thrust_map}.


$$ v_{i \hvvvar{(RAW)}} = \hvvar{THRUST\_MAP}(\hvvar{DESIRED\_THRUST}) $$

\noindent
{\em Step 2}: In the second step, the calculated speed is potentially
reduced depending on the degree to which the vehicle is turning, as
indicated by the current value of the MOOS variable
\mvar{DESIRED\_RUDDER}.  If it is not turning, it is not diminished at
all. The adjusted speed value is set according to:

$$ v_{i_{\hvvvar{(TURN)}}} = v_{i \hvvvar{(RAW)}} *  (1 - ( \frac{| \hvvar{RUDDER} |}{100} * \hvvar{TURN\_LOSS} )) $$

\vspace{0.1in}
\noindent
The configuration parameter \pvar{turn\_loss} is a value in the range
of $[0,1]$.  When set to zero, there is no speed lost in any
turn. When set to 1, there is a 100\% speed loss when there is a
maximum rudder. The default value is 0.85.

\pskip

\noindent
{\em Step 3}: In the last step, the candidate new speed,
$v_{i_{\hvvvar{(TURN)}}}$, is compared with the incoming vehicle
speed, $v_{i-1}$. The elapsed time since the previous simulator
iteration, $\Delta T$, is used to calculate the acceleration or
deceleration implied by the new speed. If the change in speed violates
either the \pvar{min\_acceleration}, or \var{max\_acceleration}
parameters, the speed is adjusted as follows:


\begin{align*}
  v_{i_{\hvvvar{(FINAL)}}} &amp;= 
  \begin{cases}
    v_{i-1} + (\hvvar{MAX\_ACCELERATION} * \Delta T)  &amp; \frac{(v_{i_{\hvvvar{(TURN)}}} \; - \; v_{i-1})} {\Delta T} &gt; \hvvar{MAX\_ACCELERATION}, \\
    v_{i-1} - (\hvvar{MAX\_DECELERATION} * \Delta T)  &amp; \frac{(v_{i-1} \; - \; v_{i_{\hvvvar{(TURN)}}})} {\Delta T} &gt; \hvvar{MAX\_DECELERATION}, \\ 
    v_{i_{\hvvvar{(TURN)}}} &amp; \text{otherwise}.
  \end{cases} \\
\end{align*}

\vspace{0.04in}
\noindent
{\em Step 4}: The final speed from the previous step is posted by 
the simulator as \mvar{USM\_SPEED}, and is used the calculations of
position and depth, described next.

%================================================================
\subsection{Propagating the Vehicle Heading}

The vehicle heading is propagated primarily based on the current
\var{RUDDER} value which is refreshed upon each iteration by reading
the incoming mail on the MOOS variable \mvar{DESIRED\_RUDDER}, and
the elapsed time since the simulator previously updated the vehicle
state, $\Delta T$. The change in heading my also be influenced by the 
\var{THRUST} value from the MOOS variable \mvar{DESIRED\_THRUST}, and
may also factor an external rotational speed.

\vspace{0.1in}
\noindent
The algorithm for updating the new vehicle heading proceeds as:

\begin{packed_enumerate}
\item Calculate $\Delta \theta_{i \var{(RAW)}}$, the new raw change in 
  heading influenced only by the current rudder value.

\item Calculate $\Delta \theta_{i{\var{(THRUST)}}}$, an adjusted
  change in heading, based on the raw change in heading, $\Delta
  \theta_{i \var{(RAW)}}$, and the current \var{THRUST} value.

\item Calculate $\Delta \theta_{i{\var{(EXTERNAL)}}}$, an adjusted
  change in heading considering external rotational speed.

\item Calculate $\theta_{i}$, the final new heading based on the
  calculated change in heading and the previous heading, and converted
  to the range of $[0,359]$.
\end{packed_enumerate}


\noindent 
{\em Step 1}: In the first step, the new heading is calculated by the
current \var{RUDDER} value:

$$ \Delta \theta_{i \hvvvar{(RAW)}} = \hvar{RUDDER} * \frac{\hvar{TURN\_RATE}}{100} * \Delta T $$

\vspace{0.08in}
\noindent
The \var{TURN\_RATE} is an \app{uSimMarine} configuration parameter
with the allowable range of $[0,100]$. The default value of this
parameter is 70, chosen in part to be consistent with the
performance of the simulator prior to this parameter being exposed to
configuration. A value of $0$ would result in the vehicle never turning, 
regardless of the rudder value.

\vspace{0.08in}
\noindent
{\em Step 2}: In the second step the influence of the current vehicle
thrust (from the MOOS variable \mvar{DESIRED\_THRUST}) may be applied
to the change in heading. The magnitude of the change of heading is
adjusted to be greater when the thrust is greater than 50\% and less
when the thrust is less than 50\%.

$$ \Delta \theta_{i \hvvvar{(THRUST)}} = \theta_{i \hvvvar{(RAW)}} * (1 + \frac{|\hvvar{THRUST}|-50}{50}) $$ 

\vspace{0.08in}
\noindent
The direction in heading change is then potentially altered based on
the sign of the \var{THRUST}:

\begin{align*}
  \Delta \theta_{i \hvvvar{(THRUST)}} &amp; =
  \begin{cases}
    - \; \Delta \theta_{i \hvvvar{(THRUST)}} &amp; \hvvar{THRUST} &lt; 0, \\
    \Delta \theta_{i \hvvvar{(THRUST)}} &amp; \text{otherwise.} \\
  \end{cases} \\
\end{align*}

\noindent
{\em Step 3}: In the third step, the change in heading may be further
influenced by an external rotational speed. This speed, if present,
would be read at the outset of the simulator iteration from either the
configuration parameter \pvar{rotate\_speed}, or dynamically from the
MOOS variable \mvar{ROTATE\_SPEED}.  The updated value is calculated
as follows:

$$ \Delta \theta_{i \hvvvar{(EXTERNAL)}} = \theta_{i \hvvvar{(THRUST)}} + (\hvvar{ROTATE\_SPEED} * \Delta T) $$

\vspace{0.08in}
\noindent
{\em Step 4}: In final step, the final new heading is set based on the 
previous heading and the change in heading calculated in the previous
three steps. If needed, the value of the new heading is converted to
its equivalent heading in the range $[0, 359]$.

$$ \theta_{i} = \hvar{heading360}(\theta_{i-1} + \Delta \theta_{i \hvvvar{(EXTERNAL)}}) $$

\vspace{0.08in}
\noindent
The simulator then posts this value to the MOOSDB as \mvar{USM\_HEADING}.

%================================================================
\subsection{Propagating the Vehicle Position}
\label{sec_usm_prop_vpos}

The vehicle position is propagated primarily based on the newly calculated
vehicle heading and speed, the previous vehicle position, and the elapsed
time since updating the previous vehicle position, $\Delta T$. 

\vspace{0.1in}
\noindent
The algorithm for updating the new vehicle position proceeds as:

\begin{packed_enumerate}
\item Calculate the vehicle heading and speed used for updating the
  new vehicle position, with the heading converted into radians.

\item Calculate the new positions, $x_i$ and $y_i$, based on the 
  heading, speed and elapsed time.

\item Calculate a possibly revised new position, factoring in any 
  external drift.

\end{packed_enumerate}

\noindent 
{\em Step 1}: In the first step, the heading value, $\bar{\theta}$,
and speed value, $\bar{v}$ used for calculating the new vehicle
position is set averaging the newly calculated values with their
prior values:

\begin{equation}
\bar{v} = \frac{(v_i + v_{i-1})}{2}
\label{eq_avg_spd}
\end{equation}


$$ \bar{\theta} = \text{atan2}{(s, c)} $$ 

\noindent 
where $s$ and $c$ are given by:

$$ s = sin(\theta_{i-1} \pi/180) + sin(\theta_{i} \pi/180) $$
$$ c = cos(\theta_{i-1} \pi/180) + cos(\theta_{i} \pi/180) $$

\noindent
The above calculation of the heading average handles the issue of 
angle wrap, i.e., the average of $359$ and $1$ is zero, not $180$.

\vspace{0.1in}
\noindent
{\em Step 2}: The vehicle $x$ and $y$ position is updated by the
following two equations:

$$x_i = x_{i-1} + sin(\bar{\theta}) * \bar{v} * \Delta \vvar{T}$$
$$y_i = y_{i-1} + cos(\bar{\theta}) * \bar{v} * \Delta \vvar{T}$$

\vspace{0.07in}
\noindent
The above is calculated keeping in mind the difference in convention
used in marine navigation where zero degrees is due North and 90
degrees is due East. That is, the mapping is as follows from marine to
traditional trigonometric convention: $0^{\circ} \rightarrow
90^{\circ}$, $90^{\circ} \rightarrow 0^{\circ}$, $180^{\circ}
\rightarrow 270^{\circ}$, $270^{\circ} \rightarrow 180^{\circ}$.

\vspace{0.1in}
\noindent 
{\em Step 3}: The final step adjusts the $x$, and $y$ position from
above, taking into consideration any external drift that may be
present.  This drift includes both the drift that may be directed from
the incoming MOOS variables as described in Section
\ref{sec_usm_drift}. The drift components below are also a misnomer
since they are provided in units of meters per second.

\begin{equation}
x_i = x_{i} + \hvvar{EXTERNAL\_DRIFT\_X} * \Delta \vvar{T} 
\label{eq_xi}
\end{equation}
\begin{equation}
y_i = y_{i} + \hvvar{EXTERNAL\_DRIFT\_Y} * \Delta \vvar{T}
\label{eq_yi}
\end{equation}

%================================================================
\subsection{Propagating the Vehicle Depth}
\label{sec_usm_depth}
\index{Depth Simulation} 
\index{uSimMarine!Depth Simulation}

Depth change in \app{uSimMarine} is simulated based on a few input
parameters. The primary parameter that changes from one iteration to
the next is the \var{ELEVATOR} actuator value, from the MOOS variable
\mvar{DESIRED\_ELEVATOR}. On any given iteration the new vehicle depth,
$z_i$, is determined by:

$$z_i = z_{i-1} + (\dot{z}_{i} * \Delta{t}) $$

\vspace{0.05in}
\noindent
The new vehicle depth is altered by the {\em depth change rate},
$\dot{z}_i$, applied to the elapsed time, $\Delta{t}$, which is
roughly equivalent to the \pvar{apptick} interval set in the
\app{uSimMarine} configuration block.  The depth change rate on the
current iteration is determined by the vehicle speed and the
\var{ELEVATOR} actuator value, and by the following three
vehicle-specific simulator configuration parameters that allow for
some variation in simulating the physical properties of the
vehicle. The \pvar{buoyancy\_rate}, for simplicity, is given in meters
per second where positive values represent a positively buoyant
vehicle. The \pvar{max\_depth\_rate}, and
\pvar{max\_depth\_rate\_speed} parameters determine the function(s)
shown in Figure \ref{fig_usm_depth}. The vehicle will have a higher
depth change rate at higher speeds, up to some maximum speed where the
speed no longer affects the depth change rate. The actual depth change
rate then depends on the elevator and vehicle speed.

\index{Buoyancy}

\vspace{0.15in}
\begin{figure}[H]
  \centering 
  \includegraphics[width=0.69\textwidth]{figures/usm_depth.png}
  \caption{The relationship between the rate of depth change rate,
    given a current vehicle speed. Different elevator settings
    determine unique curves as shown.}
\label{fig_usm_depth}
\end{figure}
\vspace{0.15in}

\noindent
The value of the depth change rate, $\dot{v}_i$, is determined as follows:

\vspace{0.1in}
\begin{equation}
\dot{z}_{i} = (\frac{\bar{v}}{\hvvar{MAX\_DEPTH\_RATE\_SPEED}})^2 * \; \frac{\hvvar{ELEVATOR}}{100} \; * \; \hvvar{MAX\_DEPTH\_RATE} + \hvvar{BUOYANCY\_RATE}
\label{eq_zidot}
\end{equation}
\vspace{0.1in}

\noindent
Both fraction components in the above equation are clipped to $[-1,
  1]$.  When the vehicle is in reverse thrust and has a negative
speed, this equation still holds. However, a vehicle would likely not
have a depth change rate curve symmetric between positive and negative
vehicle speeds.  By default the value of \pvar{buoyancy\_rate} is set
to $0.025$, slightly positively buoyant, \pvar{max\_depth\_rate} is
set to $0.5$, and \pvar{max\_depth\_rate\_speed} is set to $2.0$. The
prevailing buoyancy rate may be dynamically adjusted by a separate
MOOS application publishing to the variable \mvar{BUOYANCY\_RATE}.


%================================================================
\section{Propagating the Vehicle Altitude}
%================================================================
\index{uSimMarine!Propagating Vehicle Altitude}

The vehicle altitude is base solely on the current vehicle depth and the 
depth of the water at the current vehicle position. If nothing is known
about the water depth, then \mvar{USM\_ALTITUDE} is not published. The 
simulator may be configured with a default water depth:

\vspace{0.1in}
   \pvar{default\_water\_depth} \psvar{ = 100}
\vspace{0.1in}

\noindent
This will allow the simulator to produce some altitude information if
needed for testing consumers of \mvar{USM\_ALTITUDE}
information. Furthermore, the simulator subscribes for water depth
information in the variable \mvar{USM\_WATER\_DEPTH} which could
conceivably be produced by another MOOS application with access to
bathymetry data and the vehicle's navigation position.


%================================================================
\section{Simulation of External Drift}
%================================================================
\label{sec_usm_drift}

When the simulator updates the vehicle position as in equations
provided in Section \ref{sec_usm_prop_vpos}, it factors a possible
external drift in the $x$ and $y$ directions, in the term
\var{EXTERNAL\_DRIFT\_X}, and \var{EXTERNAL\_DRIFT\_X}
respectively. The external drift may have two distinct components; a
drift applied generally, and a drift applied due to a current field
configured with an external file correlating drift vectors to local
$x$ and $y$ positions.  These drifts may be set in one of three ways
discussed next.

%================================================================
\subsection{External X-Y Drift from Initial Simulator Configuration}
\label{sec_usm_drift_internal}

An external drift may be configured upon startup by either specifying
explicitly the drift in the $x$ and $y$ direction, or by specifying a
drift magnitude and direction.  Figure \ref{fig_usm_drift} shows two
external drifts each with the appropriate configuration using either
the \pvar{drift\_x} and \pvar{drift\_y} parameters or the single
\pvar{drift\_vector} parameter:

\vspace{0.15in}
\begin{figure}[H]
  \centering 
  \includegraphics[width=0.8\textwidth]{figures/usm_drift.png}
  \caption{{\bf External Drift Vectors}: Two drift vectors each
    configured with either the \pvar{drift\_x} and \pvar{drift\_y}
    configuration parameters or their equivalent single
    \pvar{drift\_vector} parameter.}
\label{fig_usm_drift}
\end{figure}

\noindent
If, for some reason, the user mistakenly configures the simulator with
both configuration styles, the configuration appearing last in the
configuration block will be the prevailing configuration.  If
\app{uSimMarine} is configured with these parameters, these external
drifts will be applied on the very first iteration and all later
iterations unless changed dynamically, as discussed next.

%================================================================
\subsection{External X-Y Drift Received from Other MOOS Applications}
\label{sec_usm_drift_external}

External drifts may be adjusted dynamically by other MOOS applications
based on any criteria wished by the user and developer. The
\app{uSimMarine} application registers for the following MOOS
variables in this regard: \mvar{DRIFT\_X}, \mvar{DRIFT\_Y},
\mvar{DRIFT\_VECTOR}, \mvar{DRIFT\_VECTOR\_ADD},
\mvar{DRIFT\_VECTOR\_MULT}. The first three variables simply
override the previously prevailing drift, set by either the initial
configuration or the last received mail concerning the drift.

\pskip

By posting to the \mvar{USM\_DRIFT\_VECTOR\_MULT} variable the {\em
  magnitude} of the prevailing vector may be modified with a single
multiplier such as:

\prevgreen
\begin{verbatim}
  DRIFT_VECTOR_MULT = 2
  DRIFT_VECTOR_MULT = -1
\end{verbatim}
\postverb

\noindent
The first MOOS posting above would double the size of the prevailing
drift vector, and the second example would reverse the direction of
the vector. The \mvar{DRIFT\_VECTOR\_ADD} variable describes a
drift vector to be {\em added} to the prevailing drift vector. For
example, consider the prevailing drift vector shown on the left in
Figure \ref{fig_usm_drift}, with the following MOOS mail received by
the simulator:

\prevgreen
\begin{verbatim}
  DRIFT_VECTOR_ADD = "262.47, 15.796"
\end{verbatim}
\postverb

\noindent
The resulting drift vector would be the vector shown on the right in
Figure \ref{fig_usm_drift}. This interface opens the door for the scripting
changes to the drift vector like the one below, that crudely simulate
a gust of wind in a given direction that builds up to a certain 
magnitude and dies back down to a net zero drift.

\prevgreen
\begin{verbatim}
    DRIFT_VECTOR_ADD = 137, 0.25
    DRIFT_VECTOR_ADD = 137, 0.25
    DRIFT_VECTOR_ADD = 137, 0.25
    DRIFT_VECTOR_ADD = 137, 0.25
    DRIFT_VECTOR_ADD = 137, 0.25
    DRIFT_VECTOR_ADD = 137, -0.25
    DRIFT_VECTOR_ADD = 137, -0.25
    DRIFT_VECTOR_ADD = 137, -0.25
    DRIFT_VECTOR_ADD = 137, -0.25
    DRIFT_VECTOR_ADD = 137, -0.25
\end{verbatim}
\postverb

\noindent
The above style script was described in the documentation for
\app{uTimerScript}, where the \app{uTimerScript} utility was used to
simulate wind gusts in random directions with random magnitude. The
\msvar{DRIFT\_*} interface may also be used by any third party MOOS
application simulating things such as ocean or wind currents.

%================================================================
\newpage
\section{The ThrustMap Data Structure}
%================================================================
\label{sec_thrust_map}
\index{Thrust Map}
\index{uSimMarine!Thrust Map}

A {\em thrust map} is a data structure that may be used to simulate a
non-linear relationship between thrust and speed.  This is configured
in the \app{uSimMarine} configuration block with the \pvar{thrust\_map}
parameter containing a comma-separated list of colon-separated
pairs. Each element in the comma-separated list is a single mapping
component. In each component, the value to the left of the colon is a
thrust value, and the other value is a corresponding speed. The
following is an example mapping given in string form, and rendered in
Figure \ref{fig_usm_tmap}.

\prevblue
\begin{verbatim}
    thrust_map = "-100:-3.5, -75:-3.2, -10:-2, 20:2.4, 50:4.2, 80:4.8, 100:5"
\end{verbatim}
\postverb

\vspace{0.15in}
\begin{figure}[H]
  \centering 
  \includegraphics[width=0.85\textwidth]{figures/usm_thrust_map.png}
  \caption{{\bf A Thrust Map}: The example thrust map was defined by
    seven mapping points in the string "-100:-3.5, -75:-3.2, -10:-2,
    20:2.4, 50:4.2, 80:4.8, 100:5".}
\label{fig_usm_tmap}
\end{figure}

%================================================================
\subsection{Automatic Pruning of Invalid Configuration Pairs}

The thrust map has an immutable domain of $[-100, 100]$, indicating
100\% forward and reverse thrust.  Mapping pairs given outside this
domain will simply be ignored.  The thrust mapping must also be
monotonically increasing. This follows the intuition that more
positive thrust will not result in the vehicle going slower, and
likewise for negative thrust. Since the map is configured with a
sequence of pairs as above, a pair that would result in a non-monotonic
map is discarded. All maps are created as if they had the pair
\var{0:0} given explicitly. Any pair provided in configuration with
zero as the thrust value will ignored; zero thrust always means zero
speed. Therefore, the following map configurations would all be
equivalent to the map configuration above and shown in Figure
\ref{fig_usm_tmap}:

\prevblue
\begin{verbatim}
  thrust_map = -120:-5, -100:-3.5, -75:-3.2, -10:-2, 20:2.4, 50:4.2, 80:4.8, 100:5.0, 120:6
  thrust_map = -100:-3.5, -75:-3.2, -10:-2, 20:2.4, 50:4.2, 80:4.8, 90:4, 100:5.0
  thrust_map = -100:-3.5, -75:-3.2, -10:-2, 0:0, 20:2.4, 50:4.2, 80:4.8, 100:5.0
  thrust_map = -100:-3.5, -75:-3.2, -10:-2, 0:1, 20:2.4, 50:4.2, 80:4.8, 100:5.0
\end{verbatim}
\postverb

\noindent
In the first case, the pairs \var{"-120:-5"} and \var{"120:6"} would
be ignored since they are outside the $[-100,100]$ domain. In the
second case, the pair \var{"90:4"} would be ignored since its
inclusion would entail a non-monotonic mapping given the previous pair
of \var{"80:4.8"}. In the third case, the pair \var{"0:0"} would be
effectively ignored since it is implied in all map configurations
anyway. In the fourth case, the pair \var{"0:1"} would be ignored
since a mapping from a non-zero speed to zero thrust is not permitted.

%================================================================
\subsection{Automatic Inclusion of Implied Configuration Pairs}

Since the domain $[-100,100]$ is immutable, the thrust map is altered
a bit automatically when or if the user provides a configuration
without explicit mappings for the thrust values of $-100$ or $100$. In
this case, the missing mapping becomes an implied mapping. The mapping
\var{100:v} is added where \var{v} is the speed value of the closest
point. For example, the following two configurations are equivalent:

\prevblue
\begin{verbatim}
    thrust_map = -75:-3.2, -10:-2, 20:2.4, 50:4.2, 80:4.8
    thrust_map = -100:-3.2, -75:-3.2, -10:-2, 20:2.4, 50:4.2, 80:4.8, 100:4.8
\end{verbatim}
\postverb[0in]

%================================================================
\subsection{A Shortcut for Specifying the Negative Thrust Mapping}

For convenience, the mapping of positive thrust values to speed values
can be used in reverse for negative thrust values. This is done by
configuring \app{uSimMarine} with \psvar{thrust\_reflect=true}, which is
false by default.  If \pvar{thrust\_reflect} is false, then a speed of
zero is mapped to all negative thrust values. If \pvar{thrust\_reflect}
is true, but the user nevertheless provides a mapping for a negative
thrust in a thrust map, then the \pvar{thrust\_reflect} directive is
simply ignored and the thrust map is used instead. For example, the
following two configurations are equivalent:

\prevblue
\begin{verbatim}
    thrust_map = -100:-5, -80:-4.8, -50:-4.2, -20:-2.4, 20:2.4, 50:4.2, 80:4.8, 100:5
\end{verbatim}
\postverb

\noindent
and

\prevblue
\begin{verbatim}
    thrust_map = 20:2.4, 50:4.2, 80:4.8, 100:5
    thrust_reflect = true
\end{verbatim}
\postverb[0in]

%================================================================
\subsection{The Inverse Mapping - From Speed To Thrust}

Since a thrust map only permits configurations resulting in a
non-monotonic function, the inverse also holds (almost) as a valid
mapping from speed to thrust. We say "almost" because there is
ambiguity in cases where there is one or more plateau in the thrust
map as in:

\prevblue
\begin{verbatim}
    thrust_map = -75:-3.2, -10:-2, 20:2.4, 50:4.2, 80:4.8
\end{verbatim}
\postverb

\noindent
In this case a speed of $4.8$ maps to any thrust in the range
$[80,100]$. To remove such ambiguity, the thrust map, as implemented
in a C++ class with methods, returns the lowest magnitude thrust in
such cases. A speed of $4.8$ (or $5$ for that matter), would return a
thrust value of $80$. A speed of $-3.2$ would return a thrust value of
$-75$. The motivation for this way of disambiguation is that if a
thrust value of $80$ and $100$, both result in the same speed, one
would always choose the setting that conserves less energy.
Reverse mappings are not used by the \app{uSimMarine} application, but
may be of use in applications responsible for posting a desired thrust
given a desired speed, as with the \app{pMarinePID} application.

%================================================================
\subsection{Default Behavior of an Empty or Unspecified ThrustMap}

If \app{uSimMarine} is configured without an explicit
\pvar{thrust\_map} or \pvar{thrust\_reflect} configuration, the default
behavior is governed as if the following two lines were actually
included in the \app{uSimMarine} configuration block:

\prevblue
\begin{verbatim}
    thrust_map     = 100:5
    thrust_reflect = false
\end{verbatim}
\postverb

\noindent
The default thrust map is rendered in Figure \ref{fig_usm_tmap_default}.

\vspace{0.1in}
\begin{figure}[H]
  \centering 
  \includegraphics[width=0.8\textwidth]{figures/usm_thrust_map_default.png}
  \caption{{\bf The Default Thrust Map}: This thrust map is used if no
    explicit configuration is provided.}
\label{fig_usm_tmap_default}
\end{figure}
\vspace{0.05in}

\noindent
This default configuration was chosen for its reasonableness, and to be
consistent with the behavior of prior versions of \app{uSimMarine} where
the user did not have the ability to configure a thrust map.

\end{document}

</pre></body></html>