text:
\section*{Appendix}\label{sec:Appendix}

\subsection*{Technical explanation of the investiagtion in section \ref{sec:Error_Lobatto}}
\revsec{We presented the ideas how to reach  \eqref{eq:Delta} from \eqref{eq:continuous_error}.
Applying the interpolation operator together with discrete norms results in\footnote{
Since $\phi \in \P^N$ and if $a\equiv 1$, the volume term is
\begin{equation*}\label{eq:interpolation_foot}
 \est{ \Ip^N(u^k), \partial_\xi \phi^k}=
 \l(  \vec{\Ip^N(u^k)}, \partial_\xi \vec{\phi}^{k,T} \r)_N =\vec{\phi}^k \mat{D}[^T] \mat{M} \vec{\Ip^N(u^k)}
\end{equation*}
and also the terms \eqref{eq:interpolation2}--\eqref{eq:interpolation4} simplify \revPP{and can be brought together}, see
inter alia \cite{kopriva2017error} for details.
}
\begin{align*}
\est{\partial_t \Ip^N(u^k),\phi^k  }=&
\l( \partial_t \vec{\Ip^N(u^k)}, \vec{\phi}^k \r)_N \nonumber \\ &
+\l\{ \est{ \partial_t \Ip^N(u^k), \phi^k }-
 \l( \partial_t \vec{\Ip^N(u^k)}, \vec{\phi}^k \r)_N  \r\}, \tag{\ref{eq:interpolation}}\\
\frac{1}{2}\est{a^k \Ip^N(u^k), \partial_\xi \phi^k}  &=
\frac{1}{2} \l(  \mat{a^k} \vec{\Ip^N(u^k)} , \partial_\xi \vec{\phi}^k\r)_N \nonumber \\ &
+\frac{1}{2} \l\{ \est{a^k \Ip^N(u^k), \partial_\xi \phi^k} -
\l( \mat{a^k} \vec{\Ip^N(u^k)}  , \partial_\xi \vec{\phi}^k\r)_N  \r\},
\stepcounter{equation}\tag{\theequation}\label{eq:interpolation2}\\
\frac{1}{2}\est{a^k   \partial_\xi \Ip^N(u^k),\phi^k}  &=
\frac{1}{2} \l(  \mat{a^k}  \partial_\xi \vec{\Ip^N(u^k) } , \vec{\phi}^k  \r)_N\nonumber \\ &
+\frac{1}{2} \l\{ \est{a^k \partial_\xi \Ip^N(u^k) , \phi^k  } -
\l(  \mat{a^k} \partial_\xi \vec{\Ip^N(u^k)} , \vec{\phi}^k  \r)_N \r\},
\stepcounter{equation}\tag{\theequation}\label{eq:interpolation3}\\
\frac{1}{2}\est{ \Ip^N(u^k) \partial_\xi a^k,  \phi^k }  &=
\frac{1}{2} \l(  \mat{\Ip^N(u^k)} \partial_\xi \vec{a}^k ,\vec{\phi}^k  \r)_N\nonumber \\ &
+\frac{1}{2} \l\{ \est{ \Ip^N(u^k)  \partial_\xi a^k,\phi^k }  -
 \l(  \mat{\Ip^N(u^k)} \partial_\xi \vec{a}^k ,\vec{\phi}^k  \r)_N \r\}.
\stepcounter{equation}\tag{\theequation}\label{eq:interpolation4}
\end{align*}
It is well known \cite[Section~5.4.3]{canuto2006spectral} that the integration error arising from the use of Gauß quadrature (Gauß-Legendre and Gauß-Lobatto-Legendre) decays spectrally
fast. Indeed, for all $\phi \in \P^N$ and $m\geq 1$,
\begin{equation*}
 \l|\est{u,\phi}-(\vec{u},\vec{\phi})_N \r|\leq C N^{-m}|u|_{H^{m,N-1}(-1,1)}||\phi||_{\L^2(-1,1)},
\end{equation*}
where $C$ is a constant independent of $m$ and $u$.
The curly brackets of \eqref{eq:interpolation}, \eqref{eq:interpolation2}--\eqref{eq:interpolation4} have to
be reformulated. Using
\begin{multline}\label{Soblev_approach}
  \est{ \partial_t \Ip^N(u^k), \phi^k }- \l( \partial_t \vec{\Ip^N(u^k)}, \vec{\phi}^k \r)_N
  =
 \est{ \underbrace{ \partial _t \l( \Ip^N(u^k)-P^m_{N-1} \l( \Ip^N(u^k)\r)\r)}_{=:Q(u^k)}, \phi^k }\\
  - \l( \partial_t \l( \vec{\Ip^N(u^k)}-\vec{P^m_{N-1} \l(\Ip^N(u^k)\r) }  \r), \vec{\phi}^k   \r)_N ,
\end{multline}
 where $P^m_{N-1}$ is the orthogonal projection of $u$ onto $\P^{N-1}$ using the inner product of $H^m(e^k)$, gives a
 new formulation for \eqref{eq:interpolation}. \revPP{The projection operator is defined by
 the classical truncated Fourier series $P^{N-1}u=\sum_{k=0}^{N-1} \hat{u}_k \varPhi_k$ up to order $N-1$ where  Sobolev type orthogonal polynomials
 $\{\varPhi_k \} $ are used
 as basis functions in the Hilbert space $H^m(e^k)$.
 The coefficients are calculated using the inner product of $H^m(e^k)$ given by
 \begin{equation*}
  \est{u,v}_m=\sum_{k=0}^m \int_{e_k} \frac{\d^k u}{\d x^k}(x)\frac{\d^k v}{\d x^k}(x) \d x.
 \end{equation*}
For more details about the projection operator and about approximation results, we strongly recommend \cite[Section 5]{canuto2006spectral}
and also \cite{bernardi1989properties, bernardi1992polynomial}.
 }
 An analogous approach as \eqref{Soblev_approach}
 leads to terms with $Q_1$ for \eqref{eq:interpolation2}, $Q_2$ for \eqref{eq:interpolation3}
 and $Q_3$ for \eqref{eq:interpolation4}.
The $Q_j$ measure the projection error of a polynomial of degree $N$ to a polynomial of degree $N-1$.
Since $u$ and $a$ are bounded, also these
values have to be bounded. This values can be introduced and finally one obtains \eqref{eq:Delta}.} \\

\revsec{
Later, in this section the error of the fluxes hase to be calulated.
We obtain for the left and right boundary:
\begin{align*}
 &\text{left:} &-\Eps_L^1 \l( f^{\mathrm{num},1}_L -
 \frac{1}{2}  a_L^1\Eps_L^1 \r)=-\Eps_L^1  \left( \left( a_L^1\frac{0+\Eps_L^1 }{2}-
 \sigma a_L^1 \frac{\Eps_L^1 }{2} \right)-\frac{a_L^1 \Eps_L^1 }{2} \right) \\
 & &= \frac{\sigma a_L^1}{2}\l(\Eps_L^1\r)^2,
 \\
 &\text{right:}&\Eps_R^K \l(f^{\mathrm{num},K}_R -
 \frac{1}{2} a_R^K\Eps^K_R \r)= \Eps^K_R  \l( \l(  a_R^K\frac{0+\Eps^K_R }{2} +
 \frac{1}{2} \sigma  a_R^K \Eps^K_R  \r)-\frac{\Eps^K_R  a_R^K}{2}\r) \\
 & &= \frac{\sigma  a_R^K}{2} \l(\Eps^K_R  \r)^2.
\end{align*}
}

\subsection*{Technical steps of the development in section \ref{sec:Error_Gauss}}
\revsec{
Here, we are presenting the main steps to reach \eqref{eq:R_absch_2}}.
\revsec{
\begin{align*}
 &\frac{\Delta x_k}{2} \est{\partial_t \Ip^N(u^k), \phi^k }
 +\frac{1}{2} \Bigg( a^k \Ip^N(u^k) \phi^k \Bigg|_{-1}^1 -
 \est{a^k \Ip^N(u^k),\partial_\xi \phi^k  }
 + \est{\partial_\xi \Ip^N(u^k), a^k \phi^k } \\
 &+ \revPP{\est{ \partial_\xi a^k , \phi^k \Ip^N(u^k)} }  \Bigg)
 = -\frac{\Delta x_k}{2} \est{\partial_t \epsilon_p^k, \phi^k }-\frac{1}{2} \l(a^k \epsilon_p^k \phi^k \Bigg|_{-1}^1  \r) +\frac{1}{2} \est{a^k \epsilon_p^k, \partial_\xi \phi^k }
  \\ &-\frac{1}{2} \est{\partial_\xi \epsilon_p^k, a^k \phi^k } -
  \frac{1}{2} \est{ \phi^k \epsilon_p^k, \partial_\xi a^k }.
\end{align*}
Integration-by-parts yields
\begin{equation*}
 -\frac{1}{2} \l(a^k \epsilon_p^k \phi^k \Bigg|_{-1}^1 - \est{a^k \epsilon_p^k, \partial_\xi \phi^k } \r)= -\frac{1}{2} \est{ \partial_\xi (a^k \epsilon_p^k), \phi^k }.
\end{equation*}
With \eqref{eq:interpolation},\eqref{eq:interpolation2}-- \eqref{eq:interpolation4}, one obtains
\begin{equation*} \tag{\ref{eq:Error_Gauss_2}}
 \begin{aligned}
   &\quad \frac{\Delta x_k}{2} \l( \partial_t \vec{\Ip^N (u^k)}, \vec{\phi}^k \r)_N +\vec{\phi}^{k,T} \mat{R}[^T] \mat{B} \l( \vecfnumk \l(\Ip^N(u^k)^-, \Ip^N(u^k)^+\r)-
 \frac{1}{2} \l(\mat{R}\vec{a^k} \r)  \cdot
 \l(\mat{R} \vec{u} \r) \r)
 \\&\quad
 + \underbrace{\l( \frac{1}{2}  a^k \Ip^N(u^k)\phi^k\Bigg|_{-1}^1 -
 \vec{\phi}^{k,T} \mat{R}[^T] \mat{B} \l( \vecfnumk \l(\Ip^N(u^k)^-, \Ip^N(u^k)^+\r)-
 \frac{1}{2} \l(\mat{R}\vec{a^k} \r) \cdot
 \l(\mat{R} \vec{u} \r) \r) \r)}_{=:\epsilon_2^k(a^k)}
 \\&\quad
 - \frac{1}{2} \l(  \mat{a^k} \vec{\Ip^N(u^k)} ,  \partial_\xi \vec{\phi}^k \r)_N
  +\frac{1}{2} \l( \partial_\xi \vec{\Ip^N(u^k)}, \mat{a^k} \vec{\phi}^k    \r)_N
  +\frac{1}{2} \l( \partial_\xi \vec{a}^k , \mat{\Ip^N(u^k)}  \vec{\phi}^k \r)_N\\
 &= \frac{\Delta x_k}{2} \est{ \hat{T}^k(u^k) , \phi^k} +\frac{\Delta x_k}{4} \est{Q_1(u^k), \partial_x \phi^k }
 \\&\quad
 +\frac{\Delta x_k}{4}  \Big\{
  \l( \vec{Q(u^k)}, \vec{\phi}^k \r)_N -
   \l( \vec{Q_1(u^k)}, \partial_x \vec{\phi}^k \r)_N +
   \revPP{ \l(  \vec{Q_2(u^k)}, \mat{a}^k \vec{\phi}^k \r)_N } \\ & \quad +
     \l( \partial_x \vec{a}^k, \mat{Q_3(u^k)}\vec{\phi}^k \r)_N \Big \}
 \end{aligned}
\end{equation*}
 with
 \begin{equation*}
 \begin{aligned}
     \hat{T}^(u^k):=& -\Bigg\{\partial_t \epsilon_p^k+\frac{1}{2} \l( \partial_x \l( a^k \epsilon_p^k\r)
     +\epsilon_p^k \partial_x a^k \partial_x \epsilon_p^k \r)\\
 &+\frac{1}{2} \l( Q(u^k) +a^kQ_2(u^k) +Q_3(u^k) \partial_x a^k \r)
 \Bigg\}.
 \end{aligned}
 \end{equation*}}
\revPP{We transposed every term in \eqref{eq:semi_disc_ver_Gauss}  and subtracted
it from equation \eqref{eq:Error_Gauss_2}.}\revsec{
Using $\epsilon_1^k=\Ip^N(u^k)-U^k$ yields
\begin{align*}
 &\frac{\Delta x_k}{2} \l( \partial_t \vec{\epsilon}_1^k, \vec{\phi}^k \r) +
 \vec{\phi}^{k,T} \mat{R}[^T] \mat{B} \l( \vecfnumk \l((\epsilon_1^k)^-, (\epsilon_1^k)^+\r)-
 \frac{1}{2} \l(\mat{R}\vec{a^k} \r) \cdot
 \l(\mat{R} \epsilon_1^k \r) \r)\\
 & +\epsilon_2^k(a^k)- \frac{1}{2} \l(  \mat{a^k} \vec{\epsilon}_1^k ,  \partial_\xi \vec{\phi}^k \r)_N
  +\frac{1}{2} \l( \partial_\xi  \vec{\epsilon}_1^k , \mat{a^k} \vec{\phi}^k    \r)_N
  +\frac{1}{2} \l( \partial_\xi \vec{a}^k , \mat{\epsilon}_1^k   \vec{\phi}^k \r)_N\\
  =& \frac{\Delta x_k}{2} \est{ \hat{T}^k(u^k) , \phi^k} +\frac{\Delta x_k}{4} \est{Q_1(u^k), \partial_x \phi^k }
  +\frac{\Delta x_k}{4}  \Big\{
  \l( \vec{Q(u^k)}, \vec{\phi}^k \r)_N - \\
 &
   \l( \vec{Q_1(u^k)}, \partial_x \vec{\phi}^k \r)_N +
  \revPP{ \l( \vec{Q_2(u^k)}, \mat{a}^k \vec{\phi}^k \r)_N }+
     \l( \partial_x \vec{a}^k, \mat{Q_3(u^k)}\vec{\phi}^k \r)_N \Big \} .
\end{align*}
Putting $\phi^k=\epsilon_1^k$ results in the energy equation similar to \eqref{eq:energy1}:
\begin{align*}
 &\frac{\Delta x_k}{4} \frac{\d }{\d t}||\epsilon_1^k||_N^2 +
 \vec{\epsilon}_1^{k,T} \mat{R}[^T] \mat{B} \l( \vecfnumk \l((\epsilon_1^k)^-, (\epsilon_1^k)^+\r)-
 \frac{1}{2} \l(\mat{R}\vec{a^k} \r) \cdot
 \l(\mat{R} \vec{\epsilon}_1^k \r) \r)\\
 &
 +\epsilon_2^k(a^k)- \frac{1}{2} \l( \mat{a^k} \vec{\epsilon}_1^k , \partial_\xi \vec{\epsilon}_1^k \r)_N
  +\frac{1}{2} \l( \partial_\xi  \vec{\epsilon}_1^k , \mat{a^k} \vec{\epsilon}_1^k    \r)_N
  +\frac{1}{2} \l( \partial_\xi \vec{a}^k , \mat{\epsilon}_1^k   \vec{\epsilon}_1^k \r)_N\\
  =& \frac{\Delta x_k}{2} \est{ \hat{T}^k(u^k) , \epsilon_1^k} +\frac{\Delta x_k}{4} \est{Q_1(u^k), \partial_x \epsilon_1^k } \\
 &+\underbrace{\frac{\Delta x_k}{4}  \Big\{
  \l( \vec{Q(u^k)}, \vec{\epsilon}_1^k \r)_N -
   \l( \vec{Q_1(u^k)}, \partial_x \vec{\epsilon}_1^k \r)_N +
  \revPP{  \l(  \vec{Q_2(u^k)}, \mat{a}^k\vec{\epsilon}_1^k \r)_N }+
     \l( \partial_x \vec{a}^k, \mat{Q_3(u^k)}\vec{\epsilon}_1^k \r)_N \Big \} }_{\hat{Q}^k}.
\end{align*}

Together with \eqref{eq:Deleting_a_terms}, one obtains
\begin{multline}\label{eq:Gauss_error}
  \frac{\Delta x_k}{4} \frac{\d }{\d t}||\epsilon_1^k||_N^2 +
 \vec{\epsilon}_1^{k,T} \mat{R}[^T] \mat{B} \l( \vecfnumk \l((\epsilon_1^k)^-, (\epsilon_1^k)^+\r)-
 \frac{1}{2} \l(\mat{R}\vec{a^k} \r)  \cdot
 \l(\mat{R} \vec{\epsilon}_1^k \r) \r)
 \\+\epsilon_2^k(a^k)
  +\frac{1}{2} \l( \partial_\xi \vec{a}^k , \mat{\epsilon}_1^k   \vec{\epsilon}_1^k \r)_N
  = \frac{\Delta x_k}{2} \est{ \hat{T}^k(u^k) , \epsilon_1^k} +\frac{\Delta x_k}{4} \est{Q_1(u^k), \partial_x \epsilon_1^k }
 +\hat{Q}^k.
\end{multline}
Summing this up over all elements results in
\begin{equation*}
 \begin{aligned}
 &\quad \frac{1}{2}   \frac{\d}{\d t} \sum_{k=1}^K \frac{\Delta x_k}{2}||\epsilon_1^k||_N^2+\sum_{k=1}^K
 \vec{\epsilon}_1^{k,T} \mat{R}[^T] \mat{B}
 \Bigg(\vecfnumk
 \l( (\epsilon_1^k )^{-},(\epsilon_1^k )^{+}\r)  \\&\quad-
 \frac{1}{2 } \l(\mat{R}\vec{a^k} \r)
 \l(\mat{R} \vec{\epsilon}_1^k \r)\Bigg)
 +\frac{1}{2} \sum_{k=1}^K \frac{\Delta x_k}{2}
 \revPP{\l( \partial_x \vec{a}^k , \mat{\epsilon}_1^k  \vec{\epsilon}_1^k  \r)_N }
  +\sum_{k=1}^K\frac{\Delta x_k}{4}
     \epsilon_2^k(a^k)
  \\&=
  \sum_{k=1}^K\frac{\Delta x_k}{2} \est{\hat{T}^k(u^k), \epsilon_1^k}
  + \sum_{k=1}^K\frac{\Delta x_k}{4}  \est{Q_1(u^k), \partial_x \epsilon_1^k}
  + \sum_{k=1}^K  \hat{Q}^k .
 \end{aligned}
\end{equation*}
Applying the same approach like in equations \eqref{eq:T_est}--\eqref{Eq:est_Q_1}
and the fact that $\epsilon_1 \in \P^N$, it is$
||\partial_x \vec{\epsilon}_1^k||_N^2 \leq c_1 N^2   ||\vec{\epsilon}_1^k||_N^2$
and we get finally \eqref{eq:R_absch_2}.}\\

\subsubsection*{Calculating the fluxes from Table \ref{Table:error_flux}}
\revsec{
\begin{itemize}
\item Split central flux $\fnum(u_-, u_+)= \frac{a_-u_-+a_+u_+}{2}$:
One obtains
\begin{align*}
 &\frac{1}{2}\l(a_R^{k-1}\Eps^{k-1}_R+a_L^{k}\Eps^k_L \r) \l(\Eps^{k-1}_R-\Eps_L^k \r)-\frac{1}{2} \l( a_R^{k-1}\l( \Eps_R^{k-1 } \r)^2    -a_L^k \l(\Eps^k_L \r)^2 \r)\\
 =& \frac{1}{2} \l(a_R^{k-1} \left(\Eps_R^k\right)^2-a_R^{k-1}\Eps_L^k \Eps^{k-1}_R + a_L^k \Eps^k_L \Eps^{k-1}_R-a_L^k \l( \Eps_L^k \r)^2 \r) \\
 &-\frac{1}{2} \l( a_R^{k-1}\l( \Eps_R^{k-1 } \r)^2    -a_L^k \l(\Eps^k_L \r)^2 \r)
 = \frac{1}{2} \Eps_L^k \Eps_R^{k-1} \l(a_L^k-a_R^{k-1} \r) = 0
\end{align*}
and
\begin{align*}
 &\text{ left:}& -\Eps_L^1 \l( f^{\mathrm{num},1}_L-\frac{1}{2}a_L^1\Eps_L^1 \r)=-\Eps_L^1 \l( \frac{a^1_L}{2}\Eps_L^1-\frac{a_L^1}{2} \Eps_L^1 \r)=0, \\
 &\text{ right:}& \Eps^K_R  \l( f^{\mathrm{num},K}_R -\frac{1}{2}a_R^K \Eps^K_R \r)=\frac{1}{2}
 \l(\Eps_R^K \r)^2 \l(a^K_R-a_R^K \r) =0 .
\end{align*}
\item Edge based upwind flux $\fnum(u_-,u_-)=a(x)u_-$:
It is
\begin{align*}
 &a^{k-1}(x_R)\Eps^{k-1}_R \l( \Eps^{k-1}_R-\Eps_L^k \r)-\frac{1}{2} \l( a_R^{k-1}\l( \Eps_R^{k-1 } \r)^2    -a_L^k \l(\Eps^k_L \r)^2 \r)\\
  =& a^{k-1}(x_R) \left(\Eps_R^{k-1}\right)^2-a^{k-1}(x_R)\Eps_L^k \Eps^{k-1}_R
 -\frac{1}{2} \l( a_R^{k-1}\l( \Eps_R^{k-1 } \r)^2 -a_L^k \l(\Eps^k_L \r)^2 \r) \\
 =&\l( \Eps_R^{k-1 } \r)^2 \l( a^{k-1}(x_R) -\frac{1}{2} a_R^{k-1} \r) +\frac{\Eps^k_L}{2} \l(a_L^k \Eps_L^k-a^{k-1}_R \Eps_R^{k-1} \r)
 \\ =& \frac{1}{2} a_R^{k-1} \jump{E_R^{k-1}}^2
\end{align*}
and
\begin{align*}
 &\text{ left:}& -\Eps_L^1 \l( f^{\mathrm{num},1}_L-\frac{1}{2}a_L^1\Eps_L^1 \r)&=\frac{1}{2} \l(\Eps^1_L \r)a_L^1, \\
 &\text{ right:}& \Eps^K_R  \l( f^{\mathrm{num},K}_R -\frac{1}{2}a_R^K \Eps^K_R \r)&=\l(\Eps^k_R \r)^2 \l(a^K(x_R)-\frac{a_R^{K}}{2} \r)
 & & = \l(\Eps^k_R \r)^2 \l(\frac{a_R^{K}}{2} \r).
\end{align*}
\item Split upwind flux $\fnum(u_-,u_-)=a_-u_-$:
It is
\begin{align*}
 &a^{k-1}_R\Eps^{k-1}_R \l( \Eps^{k-1}_R-\Eps_L^k \r)-\frac{1}{2} \l( a_R^{k-1}\l( \Eps_R^{k-1 } \r)^2    -a_L^k \l(\Eps^k_L \r)^2 \r)\\
  =& \frac{1}{2}\l( a^{k-1}_R\l(\Eps_R^{k-1}\right)^2-2a^{k-1}_R\Eps_L^k \Eps^{k-1}_R +a_L^k \l(\Eps^k_L\r)^2 \r)
  = \frac{1}{2} a_R^{k-1} \jump{E_R^{k-1}}^2,
\end{align*}
where we used in the last step the assumption about the exactness of the interpolation and the continuity of $a$.
At the boundaries we get
\begin{align*}
 &\text{ left:}& \frac{a_L^1 }{2}  \l(\Eps^1_L\r)^2,\\
 &\text{ right:}&  \frac{a_R^K }{2} \l(\Eps^K_R\r)^2.
\end{align*}
\end{itemize}
}

config: arxiv


TITLE: Similar Matrices Formula
QUESTION [0 upvotes]: I was confused by the formula for similar matrices. The book says $A = S^{-1} B S$ if $A$ and $B$ are similar. But I'm wondering if $S B S^{-1}$ makes a difference at all? Which formula should I stick to in general?

REPLY [3 votes]: $S^{-1}BS=(S^{-1})B(S^{-1})^{-1}$, which is of the form $PBP^{-1}$, so without loss of generality we can assume $S^{-1}BS$.


text:
\section{MOTION PLANNING PROBLEM DESCRIPTION}
\label{sec:prob_description}

This paper addresses robotic systems described by nonlinear and time-invariant differential equations with time $t \in \mathbb{R}$, state trajectory $\fundef{\x}{\mathbb{R}}{\xset}$ and control trajectory $\fundef{\u}{\mathbb{R}}{\uset}$:
\begin{equation}
\xcdot{t} = \f\big(\xc{t},\uc{t}\big).
\label{eq:system_dynamics_cont}
\end{equation}
The mapping $\fundef{\f}{\xset \times \uset}{\mathbb{R}^p}$ with $p=\dim(\xset)$ is continuous and Lipschitz in its first argument.
The state and control spaces, $\xset$ and $\uset$ respectively, do not have to be exclusively Euclidean spaces, as discussed later.
System~\eqref{eq:system_dynamics_cont} is also subject to state and input constraint sets originating from internal robot constraints, but also from the dynamic environment in which the robot operates.
Therefore, state constraints $\xc{t} \in \rxset(t) \subseteq \xset$ and input constraints $\uc{t} \in \ruset\big( \xc{t}, t \big) \subseteq \uset$ are time-variant.

\subsection{Local Planning via Optimal Control}

The planning task is to guide the robotic system~\eqref{eq:system_dynamics_cont} from $\xs \in \xset$ at time $\ts$ to an intermediate or ultimate goal set $\xfset(\ts) \subset \xset$ within time $\tf \in I$ while minimizing an objective function and adhering to constraints.
Hereby, $I \subseteq \mathbb{R}$ denotes a predefined time interval containing~$\ts$.
With running cost $\fundef{\ell}{\xset \times \uset}{\realposzero}$ and terminal cost $\fundef{\Jf}{\xset}{\realposzero}$, the \OCP{} is given as follows:

\begin{gather}

	\underset{\uc{t},\, \xc{t},\, \tf}{\min} \quad \Jf\big(\xc{\tf}\big) + \int_{t=\ts}^{\tf} \ell\big(\xc{t}, \uc{t}\big)\,\dt
	\label{eq:ocp} \\%[-2ex]
	\hspace{-6.8cm}\text{subject to} \nonumber \\
	\begin{align*}
&\xc{\ts} = \xs,   \quad \xcdot{t} = \f\big( \xc{t}, \uc{t} \big),  \quad \xc{\tf} \in \xfset(\ts), \\
&\xc{t} \in \rxset(t), \ \uc{t} \in \ruset\big(\xc{t}, t\big), \ \ucdot{t} \in \rudset, \ \tf \in I.
	\end{align*}
\end{gather}
State and control constraints are included as described before.
In addition, the control derivative $\ucdot{t}$ is restricted to a given set $\rudset$.
Note, the same result can be achieved by augmenting the state space with integrator dynamics.
However, in robot applications the system is often described by a kinematic model with velocities as input, and therefore such a constraint (i.e. acceleration limits) eliminates the need to increase the number of optimization parameters.
\OCPc{}~\eqref{eq:ocp} is generic at this point and includes the most common objectives like minimizing time or minimizing control error and effort (quadratic form). These are detailed in~\refsec{sec:realization}.
Note that potential fields for increasing the distances to obstacles or attraction terms for approaching waypoints may also be included in $\ell(\cdot)$.

\subsection{Feedback Control}

An MPC-based local planner provides control actions directly to the robot or to a cascaded low-level controller.
In practice, \eqref{eq:ocp} can only be solved at discrete time instances and hence the control law is defined according to the grid
$t_0 < t_1 < \dotsc < t_n < \dotsc < \infty$ with \mbox{$n \in \naturalzero$} and $t_{n} \in \realposzero$.
The control law $\fundef{\mulaw}{\xset}{\uset}$ for $t \in [t_n, t_{n+1})$ is given by:
\begin{equation}
\mulaw\big(\xc{t}\big) \mdef \uucstar{t} \big\vert_{\xs=\xc{t_n}, t_s = t_n}.
\label{eq:mulaw}
\end{equation}
Hereby, $\uucstar{t}$ denotes the resulting optimal control trajectory from \OCP{}~\eqref{eq:ocp} with substitutions $\xs=\xc{t_n}$ and $t_s = t_n$.
The current state $\xc{t_n}$ is either directly measurable or estimated by a state observer.

\section{NUMERICAL REALIZATION}
\label{sec:realization}

This section transforms the generic motion planning problem into a numerically tractable realization.

\subsection{Alternative State Spaces}
\label{sec:alternative_ss}

Common MPC formulations usually assume that the state space is Euclidean (resp. the real \textit{n}-space).
But especially in robotics, state spaces often include rotational components which span over the non-Euclidean rotation groups \textit{SO(2)} or \textit{SO(3)}.
A possible approach is to provide an over-parameterized formulation with constraints.
However, local derivative-based optimization schemes apply local increments $\zstar = \z + \deltaz$ with parameter \mbox{$\z \in \mathbb{R}^M$} and increment $\deltaz \in \mathbb{R}^M$ over an $M$-dimensional Euclidean parameter space in each solver iteration~\cite{nocedal2006_book} without maintaining any of these constraints.
To overcome these difficulties, we adapt and extend the idea of alternative parameterizations from graph-based SLAM \cite{kuemmerle2011},
which we have already successfully applied in simplified form in the TEB implementation.
The idea follows the observation that $\deltaz$ is usually a small perturbation around $\z$ and hence far from singularities.
Therefore, $\deltaz$ represents a minimal representation computed in the local Euclidean surroundings of $\z$.
A nonlinear increment operator then converts and applies $\deltaz$ to the correct space.
Due to the limited scope of this paper, we restrict ourselves to 2D rotation groups \textit{SO(2)} which applies to most robot navigation scenarios.
In this case, even~$\z$ does not need to be over-parameterized but the nonlinear increment operator still maintains proper 2D rotations.

Consider a state $\x \in \xset$. An increment $\dx \in \mathbb{R}^p$ is then applied by the nonlinear increment operator $\fundef{\boxplus}{\xset \times \mathbb{R}^p}{\xset}$:
\begin{equation}
\x^* = \x \boxplus \dx.
\end{equation}
Hereby, $\mathbb{R}^p$ is a local Euclidean space.
To properly evaluate the system dynamics in the alternative state space, the difference $\xd{2} - \xd{1}$ for $\xd{1},\xd{2} \in \xset$ must be embedded into $\xset$.
This is achieved by defining a nonlinear difference operator $\fundef{\boxminus}{\xset \times \xset}{\xset}$ such that $\delta \x = \xd{2} \boxminus \xd{1}$.
Note that no singularities occur for rotational components in \textit{SO(2)} and the operator defines the shortest angular distance between two states without discontinuities.
In this work we apply these operators only to states, but the extension to alternative control input spaces is done in exactly the same way.

\begin{example} \label{ex:se2}
	As an example consider the special Euclidean group~2, i.e. $\xset = \mtext{\textit{SE(2)}} = \mathbb{R}^2 \times \mtext{\textit{SO(2)}}$  that is defined in terms of a 2D translation in $\mathbb{R}^2$ and a rotational component.
	The state vector is defined as $\x = ( x, y, \theta )^\transpose \in \xset$ with $\theta \in [-\pi,\pi)$. Let $\operatorname{normAngle}(\varphi)$ define a function that normalizes an angle $\varphi\in\mathbb{R}$ to the interval $[-\pi,\pi)$.
	Then operator~$\boxplus$ is specified as:
	\begin{equation}
	\x \boxplus \dx \mdef \big(x+\Delta x, y + \Delta y, \operatorname{normAngle}(\theta + \Delta \theta)\big)^\transpose.
	\end{equation}
	Accordingly, the difference operator is: $\xd{2} \boxminus \xd{1} \mdef \xd{2} \boxplus -\xd{1}$.
\end{example}

\subsection{Direct Transcription}

This section applies direct transcription~\cite{betts2010_book} in combination with the previously defined increment and difference operators to convert \OCP{}~\eqref{eq:ocp} to an NLP.
The time interval $[\ts, \tf]$ of the planning horizon is now discretized according to the following grid:
$\ts = t_0 \leq t_1 \leq \dotsc \leq t_k \leq \dotsc \leq t_N = \tf$ with $t_k \in I$, $k=0,1,\dotsc,N$ and $N\in\naturalpos$.
Note that index $k$ indicates that the context belongs to prediction rather than closed-loop control.
Furthermore, $\vardt_k = t_{k+1} - t_k$ denotes the time interval for an individual grid partition.
In this paper, direct transcription relies on a piecewise constant control trajectory with respect to the temporal grid, i.e.
$\uc{t} \mdef \uud{k} = \mtext{constant}$ for $t \in [t_k, t_k+\vardt_k)$ for $k=0,1,\dotsc,N-1$.
The states at grid points~$t_k$ are denoted as $\xuc{t_k} \mdef \xud{k}$ for $k=0,1,\dotsc,N$.

Several methods exist to discretize the boundary value problem in \OCP{}~\eqref{eq:ocp} induced by system~\eqref{eq:system_dynamics_cont}.
Established methods are multiple shooting and collocation~\cite{betts2010_book}.
Whereas multiple shooting usually applies explicit integration schemes, collocation mainly refers to implicit schemes.
In the following, we utilize low order collocation via finite-differences as they provide a reasonable trade-off between computational resources and accuracy
and they are directly suitable for the nonlinear difference operator $\boxminus$.

The system dynamics error on the $k^\mtext{th}$ grid partition is approximated by a basis function
$\boldsymbol{\phi}( \xud{k+1} , \xud{k}, \uud{k}, \vardt_k )  = (\xud{k+1}\boxminus\xud{k})\vardt_k^{-1} - \boldsymbol{\xi}(\xud{k+1} , \xud{k}, \uud{k})$
with a suitable finite difference kernel $\boldsymbol{\xi}(\cdot)$,
 i.e. either forward differences (explicit, first order, similar to multiple shooting with forward Euler and full discretization):
\begin{equation}
\boldsymbol{\xi}(\xud{k+1} , \xud{k}, \uud{k}) \mdef \f(\xud{k}, \uud{k}), \label{eq:fwd_diff}
\end{equation}
or Crank-Nicolson differences (implicit, second order):
\begin{equation}
\boldsymbol{\xi}(\xud{k+1} , \xud{k}, \uud{k}) \mdef 0.5\big(\f(\xud{k}, \uud{k}) + \f(\xud{k+1}, \uud{k})\big).
\label{eq:crank_nic}
\end{equation}
By further approximating the integral cost in~\eqref{eq:ocp} by the right Riemann sum and $\ucdot{t}$ by forward differences, the resulting NLP is given as follows:
\begin{gather}

	\underset{ \substack{ \uud{0}, \uud{1}, \dotsc, \uud{N-1}, \\ \xud{0}, \xud{1}, \dotsc, \xud{N}, \\  \vardt_0, \vardt_1, \dotsc, \vardt_{N-1}} }{\min}\ \Jf\big(\xud{N}\big) + \sum_{k=0}^{N-1} \ell(\xud{k}, \uud{k}) \vardt_k
	\label{eq:local_uniform_nlp} \\
	\hspace{-6.8cm}\text{subject to} \nonumber \\
	\begin{align*}
		&\xud{0} = \xs, \ \xud{N} \in \xfset(\ts),\ \xud{k} \in \rxset(k \vardt_k), \ \uud{k} \in \ruset\big( \xud{k}, k \vardt_k \big),\\
		& (\uud{k+1}-\uud{k})\vardt_k^{-1} \in \rudset, \  (\uud{0}-\uud{\mtext{p}})\vardt_{\mtext{p}}^{-1} \in \rudset,\\
		&\vardtmin \leq \vardt_0 \leq \vardtmax,\quad \vardt_k = \vardt_{k+1},\\
		& \boldsymbol{\phi}( \xud{k+1} , \xud{k}, \uud{k}, \vardt_k ) = \nullvec,\quad k=0,1,\dotsc,N-1.
	\end{align*}
\end{gather}
The limitation of the first control w.r.t. its derivative at $t_0$ requires the specification of the previous control input $\uud{\mtext{p}} \in \uset$ and the time elapsed since $\uud{\mtext{p}}$, i.e. $\vardt_{\mtext{p}}=t_{n}-t_{n-1}$.
In the very first closed-loop step~\eqref{eq:mulaw}, $\uud{\mtext{p}}$ is set to zero or some control for which $\f(\xs, \uud{\mtext{p}})=\nullvec$ holds. The same holds for the last control $\uud{N}$ which is not subject to optimization but provides the
final boundary value for the control derivative. This way, a robot with velocity input always plans to stop at the end of the horizon for safety reasons.
Note that this is not conventional in receding-horizon \MPC{} as this inherently leads to differing open-loop and closed-loop solutions and might affect convergence to $\xfset(\ts)$.
However, with terminal equality conditions as in the time-optimal variant shown later, this does not lead to changes in terms of convergence due to the optimality principle.
Condition $\tf \in I$ in~\eqref{eq:ocp} has been replaced by $\vardtmin \leq \vardt_0 \leq \vardtmax$ with bounds $\vardtmin,\vardtmax \in \realposzero$.
Furthermore, condition $\vardt_k = \vardt_{k+1}$ ensures uniformity between all time intervals.
Notice that for the set evaluations $t_k = k\vardt_0 = k\vardt_k$ holds due to the uniform grid and choosing $k\vardt_k$ preserves the local structure of the optimization problem.

\begin{rem}
	NLP~\eqref{eq:local_uniform_nlp} is derived w.r.t. individual optimization parameters for each time interval but with constraints enforcing uniformity (local uniform grid approach).
	Another formulation, the global uniform grid, is obtained by replacing all $\vardt_k$ with a single time parameter $\vardt$ and omitting constraints $\vardt_k = \vardt_{k+1}$.
	The optimal solution is identical, but the structure of the optimization problem differs slightly. For more details refer to~\cite{roesmann2020_stability}.
\end{rem}

\subsection{Receding-Horizon Quadratic-Form MPC}
\label{sec:quad_form_mpc}

The most common \MPC{} realization is defined in terms of a quadratic-form objective to minimize the quadratic control error and effort.
To account for the alternative state space representations defined in \refsec{sec:alternative_ss}, the control error metric must be adjusted.
Common metrics for rotation groups are provided in~\cite{lavalle2006}, but in this case we include the nonlinear difference operator in the quadratic form.
The terminal and running costs are given as follows:
\begin{align}
\Jf(\xud{N}) &= (\xud{N} \boxminus \xf)^\transpose \Qf (\xud{N} \boxminus \xf), \label{eq:quad:jf} \\
 \ell(\xud{k}, \uud{k}) &= (\xud{k} \boxminus \xf)^\transpose \Q (\xud{k} \boxminus \xf) + \uud{k}^\transpose \R \uud{k}.  \label{eq:quad:ell}
\end{align}
Hereby, $\Q,\Qf \in \mathbb{R}^{p\times p}$ and $\R \in \mathbb{R}^{q\times q}$ denote weighting matrices with state dimension $p = \dim(\xset)$ and input dimension $q = \dim(\uset)$, respectively.
Furthermore, $\xf\in \xfset(t_n)$ represents the current intermediate goal state.
In case the full goal state is not available, it is possible to either choose additional states to complete a steady state or to set related components in $\Q$ and $\Qf$ to zero.
Note that $\xf$ could also be replaced by a time-dependent reference $\xf(t)$ during optimization for tracking control.
In this \MPC{} realization, the horizon length resp. final time~$\tf$ is fixed to $ I = \{ N \vardtref \}$ which implies $\vardtmin = \vardtmax = \vardt_k = \vardtref$ with a predefined grid resolution $\vardtref\in\realpos$.
Therefore the optimization w.r.t. time becomes obsolete and thus the optimization parameters $\vardt_k$ with $k=0,1,\dots,N-1$ as well as temporal constraints are excluded to reduce the dimensions of the optimization problem.

Stability and recursive feasibility conditions for \MPC{} usually rely on proper terminal conditions $\Jf(\cdot)$ resp.~$\Qf$ and $\xfset(t_n)$ to, e.g., approximate an infinite horizon.
The interested reader is referred to~\cite{gruene2017_book} and notice that a sampled-data discrete-time representation follows by setting $\vardtref=1$.
For (output) path-following MPC, \cite{faulwasser2016_tac} provides terminal conditions and conditions on the reference path under which stability is guaranteed.
However, as a suitable terminal region is difficult to determine, most practical approaches usually relay on sufficiently long horizon lengths to ensure stability and feasibility and choose $\xfset(t_n)\mdef\xset$.
For static environments and a class of robotic systems, i.e. differential drive robots, \cite{mehrez2017} guarantees stability for shorter receding horizons.

\subsection{Time-Optimal MPC}
\label{sec:to_mpc}

Another interesting realization of NLP~\eqref{eq:local_uniform_nlp} is a pure time-optimal MPC as addressed for real spaces in~\cite{roesmann2020_stability}.
By setting $\Jf(\xud{N}) \mdef 0$ and $\ell(\xud{k}, \uud{k}) \mdef 1$ in \eqref{eq:local_uniform_nlp}, the resulting state trajectory reaches $\xfset(t_n)$ in minimum time.
$\vardtmax$ is set to a worst case accuracy for the system dynamics error or is even omitted ($\vardtmax = \infty$) as the grid adaptation scheme described below better controls the accuracy in changing environments.
For example, approaching obstacles extend the trajectory (longer traveling times for detours) while obstacles moving away contract the trajectory due to time-optimality.
Similar as for the TEB approach~\cite{roesmann2017}, the temporal resolution $\vardt_k$ is adapted during closed-loop control w.r.t. a given reference $\vardtref \in \realpos$ and hysteresis $\vardthyst\in \realpos$:
	\begin{align*}
	N_{n+1} &=
	\begin{dcases}
		N_{n}+1 & \vardtstar_{n} > \vardtref + \vardthyst \\
		\max (N_{n}-1,\Nmin)    & \vardtstar_{n} < \vardtref - \vardthyst
	\end{dcases}.
\end{align*}
The grid size at closed-loop time $t_n$ is denoted by $N_n$ with initial size $N_0 = N \geq \Nmin$ and safe guard $\Nmin \in \naturalpos$.
$\vardtstar_{n}$ denotes the time interval obtained from the solution of NLP~\eqref{eq:local_uniform_nlp} at closed-loop time $t_n$.
Note that this linear search lowers the changes between subsequent solver calls (numerical robustness), but requires some iterations to react on highly dynamic environments.
In practice, changing the grid size one by one pointed out to be sufficient as the control rate is much faster than the changes in the environment. If this is not the case,
grid adaptation may change $N_n$ further or, alternatively,  an inner loop terminates NLP~\eqref{eq:local_uniform_nlp} prior to convergence and an outer loop adapts the grid size.

For time-optimal control, the terminal condition is often set to the intermediate goal state $\xf$, i.e. $\xfset=\{ \xf \}$.
Closed-loop convergence and recursive feasibility results are provided in~\cite{roesmann2020_stability}.

\subsection{Obstacle Avoidance Constraints}

In this work, obstacles are defined as simply connected regions $\mathcal{O}_l(t), l =1,\dotsc, L$ with $L$ as the number of obstacles.
The time-dependency explicitly accounts for dynamic obstacles.
Furthermore, let $\mathcal{R}(\x)$ denote a geometric robot collision model dependent on the state $\x \in \xset$, e.g. position and orientation.
Usually these sets are embedded in \textit{SE(2)} or \textit{SE(3)}.
The minimum distance $\fundef{d_l}{\xset \times \mathbb{R}}{\realposzero}$  between the robot and obstacle~$l$ is then determined by:
\begin{equation}
	d_l(\x,t) = \min_{p_1 \in \mathcal{R}(\x), p_2 \in \mathcal{O}_l(t)} \ \lVert p_2 - p_1 \rVert_2.
\end{equation}
By specifying a minimum separation to obstacles $\dmin\in \realposzero$, the collision-free state space $\rxoset(t)$ is given as follows:
\begin{equation}
\rxoset(t) = \{ \x \in \xset \mid d_l(\x,t) \geq \dmin, l=1,2,\dotsc,L \}.
\end{equation}
Furthermore, let the robot's internal state constraints are described by set $\rxiset(t)\in \xset$, then $\rxset(t)$ in \eqref{eq:ocp} and \eqref{eq:local_uniform_nlp} is given by $\rxset(t) = \rxoset(t) \cap \rxiset(t)$.
Note that there exist techniques in the literature for computationally efficient optimization-based obstacle avoidance, e.g.~\cite{zhang2017optimizationbased}.

\subsection{Solution of the Nonlinear Program}
\label{sec:optim}

For NLP~\eqref{eq:local_uniform_nlp} general necessary and sufficient optimality conditions apply~\cite{nocedal2006_book}.
Any practical implementation replaces constraint sets $\rxset,\ruset,\rudset$ and $\xfset$ by algebraic equality and inequality functions.
The NLP is solved by standard local constrained optimization techniques, such as interior point methods or sequential quadratic programming.
For the time-optimal formulation even sequential linear programming can be applied.
Warm-starting the NLPs in subsequent closed-loop steps with the previous solution significantly increases performance.
To comply with the alternative parameterizations (\refsec{sec:alternative_ss}), the solvers need to be modified in a few places.
Let $\mathbf{z} = (\mathbf{u}_0, \mathbf{x}_0, \vardt_0, \dotsc, \mathbf{u}_{N-1}, \mathbf{x}_{N-1}, \vardt_{N-1}, \mathbf{x}_N)$ define the optimization parameter vector of~\eqref{eq:local_uniform_nlp}.
Local solutions $\mathbf{z}^* = \mathbf{z} \boxplus \Delta \mathbf{z}$ after each solver step follow by incrementing components as follows: $\mathbf{u}_k + \Delta \mathbf{u}_k$, $\vardt_{k} + \Delta \vardt_k$ and \mbox{$\mathbf{x}_k \boxplus \Delta \mathbf{x}_k$}.
Also the calculation of Jacobians and gradients w.r.t. $\mathbf{x}$ requires to apply proper increments. E.g. the Jacobian of some vector valued state-dependent function $\mathbf{g}(\cdot)$ evaluated at $\bar{\mathbf{x}}$ is given by:
\begin{equation}
\jacobwrt{\mathbf{g}}{\mathbf{x}}(\bar{\mathbf{x}}) = \frac{\partial \mathbf{g}( \bar{\mathbf{x}} \boxplus \Delta \mathbf{x} ) }{ \partial\Delta \mathbf{x} } \bigg|_{\Delta \mathbf{x}=\nullvec} \label{eq:jacob_state}
\end{equation}
Since second-order derivatives can be calculated numerically using quasi-Newton methods or finite differences from first-order information, no dedicated increment is required.
\begin{rem}
Note that the nonlinear increment in~\eqref{eq:jacob_state} can be omitted for \textit{SO(2)} components in case $\mathbf{g}$ inherently normalizes the components.
For example, consider the collocation constraint $\mathbf{g} \mdef \boldsymbol{\phi}(\cdot)$:
Here $\xud{k+1}\boxminus\xud{k}$ also applies to unnormalized components and the robot system dynamics $\f(\cdot)$ often considers rotational components only within trigonometric functions.
The same applies to~\eqref{eq:quad:jf},~\eqref{eq:quad:ell} and standard constraints including obstacle avoidance.
\end{rem}

The structure of NLP~\eqref{eq:local_uniform_nlp} is sparse and utilizing sparse algebra for efficient optimization is crucial.
While established NLP solvers like IPOPT~\cite{waechter2006_matprog} already support sparse algebra,
it is possible to further speed up optimization by computing derivative information only for structured non-zeros.
Similar to the TEB, NLP~\eqref{eq:local_uniform_nlp} is represented as a hypergraph with cost functions and constraints as hyperedges and states, controls and time invervals as vertices.
This facilitates the computation of sparse finite differences by iterating the edge set.
A major advantage of the hypergraph is its simple algorithmic reconfiguration at negligible overhead.
This is crucial for real-time control as the NLP dimensions change during runtime either due to changing environments or grid adaptation.
Refer to~\cite{roesmann2018_aim} for general performance results and a detailed description on how to formulate NLPs in MPC as hypergraph.

 \subsection{Local Planning in Distinctive Topologies}
 \label{sec:topologies}

Solving NLP~\eqref{eq:local_uniform_nlp} as described before leads to locally optimal solutions.
Many local minima are caused by the presence of obstacles that imply non-convexity.
Identifying these local minima in advance coincides with exploring and analyzing distinctive topologies between start and goal poses.
Due to the limited scope of this paper, we cannot provide a detailed description, but would like to point out that distinctive topologies (homotopy classes)
can be managed during runtime analogous to~\cite{roesmann2017} for $\xfset=\{\xf\}$.
Several NLPs~\eqref{eq:local_uniform_nlp}  with state trajectories initialized for each active class are solved in parallel (multi-threaded).
For dynamic obstacles and thus ($x$-$y$-$t$)  spaces, the equivalence relation that identifies those classes is changed to a 3D variant according to~\cite{bhattacharya2011}.
And for evaluating this relation, sampled roadmaps in~\cite{roesmann2017} are augmented with temporal profiles (maximum velocities).