\documentclass[letterpaper,11pt]{article}

%-----------------------------------------------------------
%Margin setup

\setlength{\paperwidth}{8.5in}
\setlength{\paperheight}{11in}
\setlength{\parskip}{1em}
\setlength{\topmargin}{0.0in}
\setlength{\textwidth}{6.5in}
\setlength{\oddsidemargin}{0.0in}
\setlength{\evensidemargin}{0.0in}
%-----------------------------------------------------------
\usepackage{graphicx}
\usepackage{amsmath}
\usepackage{epstopdf}
\usepackage{tikz}
\pagestyle{plain}
\setlength{\tabcolsep}{0in}
\title{Trajectory Integration Feasibility Check}
\author{Brian M. Filarsky}
%-----------------------------------------------------------

\begin{document}

\maketitle

\newpage

\section{Definitions}

\begin{tabular}{p{1cm} l}
   $v$ & Hero Velocity in x direction of body frame with respect to smooth frame ($m/s$) \\
   $a$ & Hero Acceleration in x direction of body frame with respect to smooth frame ($m/s^2$) \\
   $\delta$ & Hero steering angle ($rad$) \\
   $\nu$ & Hero Steering Angle Rate ($rad/s$) \\
   $\kappa$ & Curvature ($1/m$) \\
   $\omega$ & Yaw Rate ($rad/s$) \\
   $d$ & Arc Length ($m$)\\
   $l$ & Vehicle wheel base
\end{tabular}

\newpage

\section{Purpose}

As a part of expanding the scope of checks on the Collision Avoidance System (CAS), a simple car integrator was implemented to check the feasibility of all trajectories.
The Nogo trajectories were found to have errors in Yaw and Position significantly beyond that expected due to floating point rounding.
NoGo trajectories are generated from a Go Trajectory, and in general states are spatially unchanged,
with just the longitudinal/time profile modified, which should presumably lead to a lateral profile identical to the Go Trajectory.
The root cause of the lateral error found with the integrator was that the curvature assumed by the integrator is dependent on the velocity gradient of the arc,
which in general changes between the Go and NoGo trajectories. However, since the tracker only interpolates between positions,
the vehicle does in fact follow the same curvature on both trajectories.

\newpage

\section{Trajectory Integration}

The Car Trajectory consists of a series of states, controls, and times propagating forward into the future. 
There are N states and times associated with N-1 controls; the last state has no associated control. 
These are all measured with respect to Planner Origin which is placed at the point on vehicle with zero lateral velocity.

At each point in the trajectory, the subsequent (n+1) state can be calculated from the current (n) state, 
current control, and the time difference between the two using a simple car model. Velocity 
and steering angle at the next state are found from a simple explicit euler integration in (\ref{eqn:v_next}) 
and (\ref{eqn:steer_next}). Curvature is computed for the two points via (\ref{eqn:kappa_current}), which is 
then used to compute yaw rate at both points (\ref{eqn:omega_current}). The mean velocity over the timestep is 
computed as the mean velocity of the endpoints (\ref{eqn:v_bar}), yaw rate is given the same treatment in (\ref{eqn:omega_bar}). 
Arc length and Yaw are then integrated in (\ref{eqn:d_next}) and (\ref{eqn:psi_next}) using the above mean velocity and yaw rate ---
the trapezoidal rule. 

\begin{align}
   \Delta t &= t_{n+1} - t_n \label{eqn:delta_t} \\
   v_{n+1} &= v_n + a_n \Delta t \label{eqn:v_next} \\
   \delta_{n+1} &= \delta_n + \nu_n \Delta t \label{eqn:steer_next} \\
   \kappa_n &= \frac{\tan{\delta_n}}{l} \label{eqn:kappa_current} \\
   \omega_{n} &= \kappa_{n} v_{n} \label{eqn:omega_current}\\
   \bar{v} &= \frac{v_{n} + v_{n+1}}{2} \label{eqn:v_bar}\\
   \bar{\omega} &= \frac{\omega_{n} + \omega_{n+1}}{2} \label{eqn:omega_bar}\\
   d_{n+1} &= d_n + \bar{v} \Delta t \label{eqn:d_next} \\
   \psi_{n+1} &= \psi_n + \bar{\omega} \Delta \label{eqn:psi_next} t
\end{align}

The vehicle pose is then integrated forward assuming a constant velocity over a constant curvature arc,
 using the mean velocity and yaw rate calculated in (\ref{eqn:v_bar}) and (\ref{eqn:omega_bar}). 
 Of note, while velocity evolves linearly over each timestep, yaw rate evolves quadratically over 
 time with some additional nonlinearity due to the tangent function. Trapezoidal integration is precise for velocity, 
 but an approximation for yaw rate.

%  \begin{align}
%     \mathbf{v} &= \begin{pmatrix} \bar{\omega} \\\bar{v} \\ 0 \end{pmatrix} \\
%    R_{n \rightarrow n+1} &= \exp(\mathbf{v} \Delta t) \\
%    X_{n+1} &= X_n R_{n \rightarrow n+1} \\
% \end{align}

\newpage

\section{NoGo Generation}

In CAS, NoGo trajectories are built using the Go Trajectory. 
Other than a point added at the start of deceleration and the stop of the trajectory, 
the points are spatially unchanged from the Go trajectory. 
The longitudinal profile is changed by setting the deceleration profile, 
then recomputing velocity, time, yaw rate, and steering angle rate at each point until hero stops. 
Values changed by the NoGo generator are marked with a tilde.

\begin{align}
   \Delta d &= d_{n} - d_{n-1} \\
   \tilde{a}_{n-1} &\stackrel{def}{=} a_{cmd} \\
   \tilde{v}_{n} &= \sqrt{\tilde{v}_{n-1}^2 + 2 \tilde{a}_{n-1} \Delta d} \\
   \tilde{\omega}_n &= \kappa_{n} \tilde{v}_{n} \\
   \Delta \tilde{t} &= \frac{\tilde{v}_{n} - \tilde{v}_{n-1}}{\tilde{a}_{n-1}} \\
   \tilde{t}_{n} &= \tilde{t}_{n-1} + \Delta \tilde{t} \\
   \tilde{\nu}_{n-1} &= \frac{\delta_n - \delta_{n-1}}{\Delta \tilde{t}}
\end{align}

\newpage

\section{Feasibility Integration Check}

The CAS Feasibility check runs a bicycle model integrator over the states and controls as a part of ensuring a feasible trajectory. 
Plugging the values output from the NoGo trajectory into the integrator above, the values at each control and state are integrated 
to ensure they match the next state. Specifically, integrated velocity (\ref{eqn:check_v}), steering angle (\ref{eqn:check_delta}),
yaw rate (\ref{eqn:check_omega}), arc length (\ref{eqn:check_s}), and yaw (\ref{eqn:check_psi}) are checked.

\begin{align}
   \tilde{v}_{n+1} &\stackrel{?}{=} \tilde{v}_n + \tilde{a}_n \Delta \tilde{t} \label{eqn:check_v} \\
   \delta_{n+1} &\stackrel{?}{=} \delta_n + \tilde{\nu}_n \Delta \tilde{t} \label{eqn:check_delta} \\
   \tilde{\omega}_{n} &\stackrel{?}{=} \kappa_{n} \tilde{v}_{n} \label{eqn:check_omega} \\
   d_{n+1} &\stackrel{?}{=} d_n + \tilde{\bar{v}} \Delta \tilde{t} \label{eqn:check_s} \\
   \psi_{n+1} &\stackrel{?}{=} \psi_n + \tilde{\bar{\omega}} \Delta \tilde{t} \label{eqn:check_psi} 
\end{align}

For velocity, it is straightforward to replace the next state velocity with the equation used in the NoGo trajectory
and prove that the new derivatives match the Taylor expansion for arc length in (\ref{eqn:check_v_first}) - (\ref{eqn:check_v_final}).

\begin{align}
   \tilde{v}_{n+1} &\stackrel{?}{=} \tilde{v}_n + \tilde{a}_n \Delta \tilde{t} \label{eqn:check_v_first} \\
   \sqrt{\tilde{v}_{n}^2 + 2 \tilde{a}_{n} \Delta d} &\stackrel{?}{=} \tilde{v}_n + \tilde{a}_n \Delta \tilde{t} \\
   \tilde{v}_{n}^2 + 2 \tilde{a}_{n} \Delta d &\stackrel{?}{=} (\tilde{v}_n + \tilde{a}_n \Delta \tilde{t})^2 \\
   \tilde{v}_{n}^2 + 2 \tilde{a}_{n} \Delta d &\stackrel{?}{=} \tilde{a}_n^2 \Delta \tilde{t}^2 + 2 \tilde{a}_n \Delta \tilde{t} \tilde{v}_n + \tilde{v}_n^2 \\
   \Delta d &= \frac{1}{2} \tilde{a}_n \Delta \tilde{t}^2 + \Delta \tilde{t} \tilde{v}_n \label{eqn:check_v_final} 
\end{align}

Steering angle trivially matches in (\ref{eqn:check_delta_first}) - (\ref{eqn:check_delta_final}).

\begin{align}
   \delta_{n+1} &\stackrel{?}{=} \delta_n + \tilde{\nu}_n \Delta \tilde{t} \label{eqn:check_delta_first} \\
   \delta_{n+1} &\stackrel{?}{=} \delta_n + (\frac{\delta_{n+1} - \delta_n}{\Delta \tilde{t}}) \Delta \tilde{t} \\
   \delta_{n+1} &= \delta_{n+1} \label{eqn:check_delta_final} 
\end{align}

Yaw rate matches on inspection, and arc length reduces precisely in (\ref{eqn:check_s_first}) - (\ref{eqn:check_s_final}).

\begin{align}
   d_{n+1} &\stackrel{?}{=} d_n + \tilde{\bar{v}} \Delta \tilde{t} \label{eqn:check_s_first} \\
   d_{n+1} &\stackrel{?}{=} d_n + \frac{\tilde{v}_{n} + \sqrt{\tilde{v}_{n}^2 + 2 \tilde{a}_{n} \Delta d}}{2} \frac{\sqrt{\tilde{v}_{n}^2 + 2 \tilde{a}_{n} \Delta d} - \tilde{v}_{n}}{\tilde{a}_{n}} \\
   d_{n+1} &\stackrel{?}{=} d_n + \frac{\tilde{v}_{n}^2 + 2 \tilde{a}_{n} \Delta d - \tilde{v}_{n}^2}{2\tilde{a}_{n}} \\
   d_{n+1} &= d_n + \Delta d \label{eqn:check_s_final}
\end{align}

On the other hand, the approximation used for Yaw does not hold. Manipulating the terms in (\ref{eqn:check_psi_first}) -
 (\ref{eqn:check_psi_intermediate1}) makes it clear that mean yaw rate needs to scale inversely with time. 
 However, substituting in those terms in (\ref{eqn:check_psi_intermediate2}) and (\ref{eqn:check_psi_final}) makes it clear that mean yaw rate is weighted towards
 the curvature term with the higher velocity - it does not simply scale with average velocity the way time does. 

\begin{align}
   \psi_{n+1} &\stackrel{?}{=} \psi_n + \tilde{\bar{\omega}} \Delta \tilde{t} \label{eqn:check_psi_first}\\
   \psi_{n+1} - \psi_n &\stackrel{?}{=} \tilde{\bar{\omega}} \Delta \tilde{t} \\
   \bar{\omega} \Delta t &\stackrel{?}{=} \tilde{\bar{\omega}} \Delta \tilde{t} \\
   \frac{\bar{\omega}}{\tilde{\bar{\omega}}}  &\stackrel{?}{=} \frac{\Delta \tilde{t}}{\Delta t} \label{eqn:check_psi_intermediate1} \\
   \frac{\bar{\omega}}{\tilde{\bar{\omega}}}  &\stackrel{?}{=} \frac{\frac{\Delta d}{\tilde{\bar{v}}}}{\frac{\Delta d}{\bar{v}}} \label{eqn:check_psi_intermediate2} \\
   \frac{\kappa_{n} v_{n} + \kappa_{n+1} v_{n+1}}{\kappa_{n} \tilde{v}_{n} + \kappa_{n+1} \tilde{v}_{n+1}} &\neq \frac{v_n + v_{n+1}}{\tilde{v}_n + \tilde{v}_{n+1}} \label{eqn:check_psi_final}
\end{align}

\newpage

\section{Yaw Approximation}

The mean yaw with respect to time over the integration step is precisely given by (\ref{eqn:mean_omega_precise}). 
The linear mean is given in (\ref{eqn:mean_omega_linear}); this is the current method in use. 
The quadratic mean is given in (\ref{eqn:mean_omega_quadratic}); this is the most accurate --- its only assumption is that the tan function is linear over the timestep.
The mean curvature is given in (\ref{eqn:mean_curvature_omega}). Derivations for all three are given in the Appendix.

Figure \ref{fig:go_yaw_rate_time} highlights the difference between the assumed constant-acceleration / constant-steering-angle-rate over the timestep,
versus the constant-velocity/constant-curvature integration used in the integration.
The first uses 2.5ms timesteps, taking into account variable velocity and curvature.
All of the others assume a constant yaw rate through the timestep, 
with each calculating that constant value from one of the three ways mentioned. 
Figure \ref{fig:go_yaw} demonstrates the evolution of yaw over a timestep using each of these methods. 
Figure \ref{fig:go_pos} demonstrates the evolution of position over a timestep using each of these methods (starting at bottom-right, moving towards top-left in time). 
The difference in yaw calculations between the three approximations is imperceptible, but the position does diverge over time due to the constant-curvature 
approximation of the integration.

\begin{align}
   \bar{\omega} &= \frac{1}{\Delta t}\int_{0}^{\Delta t} \kappa(t) v(t) dt \label{eqn:mean_omega_precise} \\
   \bar{\omega}_{1} &\approx  \frac{1}{2} (\kappa_n v_n + \kappa_{n+1} v_{n+1}) \label{eqn:mean_omega_linear} \\
   \bar{\omega}_{2} &\approx  \frac{1}{3} (\kappa_n v_n + \kappa_{n+1} v_{n+1}) + \frac{1}{6}(\kappa_n v_{n+1} + \kappa_{n+1} v_n) \label{eqn:mean_omega_quadratic} \\
   \bar{\omega}_{3} &\approx  \frac{1}{4} (\kappa_n + \kappa_{n+1}) (v_n + v_{n+1}) \label{eqn:mean_curvature_omega}
\end{align}

\begin{figure}
   \begin{tikzpicture}
      \draw (0, 0) node[inner sep=0] {\includegraphics[width=\textwidth]{figures/go_yaw_rate_time.eps}};
      \draw (-3.0, -1) node {$v_0 = 15 m/s$};
      \draw (-3.0, -1.75) node {$a = 2 m/s^2$};
      \draw (-3.0, -2.5) node {$\delta = 0.05 rad$};
      \draw (-3.0, -3.25) node {$\nu = -0.2 rad/sec$};
   \end{tikzpicture}
\caption{Yaw Rate over 250 ms} \label{fig:go_yaw_rate_time}
\end{figure}

\begin{figure}
   \begin{tikzpicture}
      \draw (0, 0) node[inner sep=0] {\includegraphics[width=\textwidth]{figures/go_yaw.eps}};
      \draw (3.0, -1) node {$v_0 = 15 m/s$};
      \draw (3.0, -1.75) node {$a = 2 m/s^2$};
      \draw (3.0, -2.5) node {$\delta = 0.05 rad$};
      \draw (3.0, -3.25) node {$\nu = -0.2 rad/sec$};
   \end{tikzpicture}
\caption{Yaw over 250 ms} \label{fig:go_yaw}
\end{figure}

\begin{figure}
   \begin{tikzpicture}
      \draw (0, 0) node[inner sep=0] {\includegraphics[width=\textwidth]{figures/go_pos.eps}};
      \draw (-3.0, -1) node {$v_0 = 15 m/s$};
      \draw (-3.0, -1.75) node {$a = 2 m/s^2$};
      \draw (-3.0, -2.5) node {$\delta = 0.05 rad$};
      \draw (-3.0, -3.25) node {$\nu = -0.2 rad/sec$};
   \end{tikzpicture}
\caption{Position over 250 ms} \label{fig:go_pos}
\end{figure}

\newpage

\section{Yaw Rate and the NoGo}

Fundamentally assuming constant-acceleration/constant-steering-angle-rate arcs between
points is mathematically incompatible with later changing the velocity, while keeping
the same constraint and the same arc. Figure \ref{fig:nogo_steer_time} shows the steering
angle over time of a go trajectory and a nogo trajectory using this approach - linearly
changing steering angle between the same two points over the timestep. 
Figure \ref{fig:nogo_steer_dist} shows steering angle of the same trajectories as a function
of arc-length --- the steering angle changes spatially between the two due to the different
velocity profiles of the two trajectories.

The issue shown in (\ref{eqn:check_psi_final}) with the current method of integrating yaw
falls apart with the nogo for the same reason --- it is an approximation of a changing
steering angle and velocity over the integration step, and also has a different result 
for different velocity profiles.

The only way to avoid this issue would be to define the curvature between the points either explicitly as part of the trajectory,
or in a way that is velocity independent. As an example, equation \ref{eqn:mean_curvature_omega} does just this;
the mean yaw rate is defined as the mean curvature times the mean velocity --- curvature is independent
of velocity. This would remedy the mathematical inconsistency with the NoGo Trajectory as shown 
in (\ref{eqn:mean_curvature_int_start}) and (\ref{eqn:mean_curvature_int_final}), 
which continue from (\ref{eqn:check_psi_intermediate2}) using (\ref{eqn:mean_curvature_omega}).

\begin{align}
   \frac{\bar{\omega}}{\tilde{\bar{\omega}}}  &\stackrel{?}{=} \frac{\frac{\Delta d}{\tilde{\bar{v}}}}{\frac{\Delta d}{\bar{v}}} \label{eqn:mean_curvature_int_start}\\
   \frac{(\kappa_{n} + \kappa_{n+1})(v_{n} + v_{n+1})}{(\kappa_{n} + \kappa_{n+1})(\tilde{v}_{n} + \tilde{v}_{n+1})} &= \frac{v_n + v_{n+1}}{\tilde{v}_n + \tilde{v}_{n+1}} \label{eqn:mean_curvature_int_final}
\end{align}

\begin{figure}
   \begin{tikzpicture}
      \draw (0, 0) node[inner sep=0] {\includegraphics[width=\textwidth]{figures/nogo_steer_time.eps}};
      \draw (-3.0, -1) node {$v_0 = 15 m/s$};
      \draw (-3.0, -1.75) node {$a = 2 m/s^2$};
      \draw (-3.0, -2.5) node {$\delta = 0.05 rad$};
      \draw (-3.0, -3.25) node {$\nu = -0.2 rad/sec$};
      \draw (-1.5, 3.25) node {$v_0 = 4 m/s$};
      \draw (-1.5, 2.5) node {$a = -2 m/s^2$};
   \end{tikzpicture}
\caption{Steering Angle over Time} \label{fig:nogo_steer_time}
\end{figure}

\begin{figure}
   \begin{tikzpicture}
      \draw (0, 0) node[inner sep=0] {\includegraphics[width=\textwidth]{figures/nogo_steer_dist.eps}};
      \draw (-2.0, 0.75) node {$v_0 = 15 m/s$};
      \draw (-2.0, 0) node {$a = 2 m/s^2$};
      \draw (-2.0, -.75) node {$\delta = 0.05 rad$};
      \draw (-2.0, -1.5) node {$\nu = -0.2 rad/sec$};
      \draw (1, 3.25) node {$v_0 = 4 m/s$};
      \draw (1, 2.5) node {$a = -2 m/s^2$};
   \end{tikzpicture}
\caption{Steering Angle over Arc Length} \label{fig:nogo_steer_dist}
\end{figure}

\newpage

\section{Conclusions}
 
The trajectory is tracked by interpolating between states, which assumes a constant curvature arc between the states. 
Despite the fact that the NoGo Trajectory doesn't match the integration check perfectly, it is still trackable, and follows
the same arc as the go trajectory. Therefore, no immediate issues need to be addressed, but it would be beneficial to 
address the structure of the trajectory, making the intended curvature between two points more explicit, and removing
information that is unused outside of planner.

\newpage

\appendix{Appendix}

The current approximation for mean yaw rate is simply the trapezoidal rule.

\begin{align}
   \bar{\omega} &= \frac{1}{\Delta t}\int_{0}^{\Delta t} \kappa(t) v(t) dt \\
   \bar{\omega}_{1} &\approx  \frac{1}{2} (\kappa_n v_n + \kappa_{n+1} v_{n+1})
\end{align}

The second option is to assume curvature and velocity both vary linearly with time. 
The integral can then be rewritten in terms of curvature and velocity at the beginning of the timestep and their derivatives in (\ref{eqn:omega_quadratic_approx}).
The integral is solved in the steps through (\ref{eqn:omega_quadratic_integrated}).
Substituting in estimates for the derivatives (\ref{eqn:kappa_deriv}) and (\ref{eqn:velocity_deriv}) in (\ref{eqn:sub_derivs}) 
and working through, we arrive at the second estimate.

\begin{align}
   \bar{\omega} &= \frac{1}{\Delta t}\int_{0}^{\Delta t} \kappa(t) v(t) dt \\
    &\approx \frac{1}{\Delta t}\int_{0}^{\Delta t} (\kappa_0 + \dot{\kappa}_0 t) (v_0 + a_0 t) dt \label{eqn:omega_quadratic_approx} \\
    &\approx \frac{1}{\Delta t}\int_{0}^{\Delta t} \kappa_0 v_0 + \dot{\kappa}_0 v_0 t + \kappa_0 a_0 t + \dot{\kappa}_0 a_0 t^2 dt \\
    &\approx \frac{1}{\Delta t} \left(\kappa_0 v_0 t + \frac{1}{2}\dot{\kappa}_0 v_0 t^2 + \frac{1}{2}\kappa_0 a_0 t^2 + \frac{1}{3}\dot{\kappa}_0 a_0 t^3 \right) \big|_{0}^{\Delta t} \\
    &\approx \frac{1}{\Delta t} \left(\kappa_0 v_0 \Delta t + \frac{1}{2}\dot{\kappa}_0 v_0 \Delta t^2 + \frac{1}{2}\kappa_0 a_0 \Delta t^2 + \frac{1}{3}\dot{\kappa}_0 a_0 \Delta t^3 \right) \\
    &\approx \kappa_0 v_0 + \frac{1}{2}\dot{\kappa}_0 v_0 \Delta t + \frac{1}{2}\kappa_0 a_0 \Delta t + \frac{1}{3}\dot{\kappa}_0 a_0 \Delta t^2 \label{eqn:omega_quadratic_integrated} \\
    \dot{\kappa}_0 &= \frac{\kappa_{n+1} - \kappa_{n}}{\Delta t} \label{eqn:kappa_deriv} \\
    a_0 &= \frac{v_{n+1} - v_{n}}{\Delta t} \label{eqn:velocity_deriv} \\
    &\approx \kappa_n v_n + \frac{1}{2}(\kappa_{n+1} - \kappa_{n}) v_n + \frac{1}{2}\kappa_n (v_{n+1} - v_{n}) + \frac{1}{3}(\kappa_{n+1} - \kappa_{n}) (v_{n+1} - v_{n}) \label{eqn:sub_derivs} \\
    &\approx \kappa_n v_n + \frac{1}{2}\kappa_{n+1} v_n - \frac{1}{2} \kappa_{n} v_n + \frac{1}{2}\kappa_n v_{n+1} - \frac{1}{2}\kappa_n v_{n} \\
    &+ \frac{1}{3} \kappa_{n+1} v_{n+1} - \frac{1}{3} \kappa_{n} v_{n+1} - \frac{1}{3} v_{n} \kappa_{n+1} + \frac{1}{3} \kappa_n v_n \\
   \bar{\omega}_{2} &\approx  \frac{1}{3} (\kappa_n v_n + \kappa_{n+1} v_{n+1}) + \frac{1}{6}(\kappa_n v_{n+1} + \kappa_{n+1} v_n)
\end{align}

The third option is to define the arc as having the mean curvature of the endpoints, then assume that we will follow that arc at the mean velocity. 
Applying the trapezoidal rule to each gives the third estimate.

\begin{align}
   \bar{\omega} &= \left(\frac{1}{\Delta t}\int_{0}^{\Delta t} \kappa(t)  dt \right) \left( \frac{1}{\Delta t} \int_{0}^{\Delta t} v(t) dt\right) \\
   &\approx  \left(\frac{1}{2} (\kappa_n + \kappa_{n+1}) \right) \left(  \frac{1}{2} (v_n + v_{n+1}) \right) \\
   \bar{\omega}_{3} &\approx  \frac{1}{4} (\kappa_n + \kappa_{n+1}) (v_n + v_{n+1})
\end{align}

\end{document}
