\documentclass{article}
\usepackage[margin=1in]{geometry}
\usepackage{amsmath}
\usepackage{microtype}
\usepackage{parskip}
\usepackage[colorinlistoftodos]{todonotes}
\usepackage{titling}
\setlength{\droptitle}{-7em}

\title{Errors in Curve Fitting}
\author{Lakshay Garg}

\begin{document}
\maketitle

The curve fitting logic replaces spirals that are very close to a
straight line or an arc with straight line/arc to prevent numerical
issues in degenerate geometries. This section derives the errors
introduced by these replacements.

\section{Approximating spiral with straight line}

A spiral is replaced by a straight line when the absolute value of
curvature at all points on the spiral is $\le 10^{-6} m^{-1}$. The
line approximation for a spiral is given by:

\[
  l(s) = c(s_1) + (s - s_1)\,t(s_1)
\]
\todo[inline,size=\small]{Note: the Curve fitter uses this approximation and not the one mentioned above $c(s_1) + (s - s_1)\,\dfrac{c(s_2) - c(s_1)}{\lVert c(s_2) - c(s_1)\rVert}$}

\subsection{Distance error}

\begin{align*}
  e &= \lVert c(s_2) - l(s_2) \rVert \\
    &= \lVert c(s_2) - (c(s_1) + (s_2 - s_1)\,t(s_1)) \rVert \\
    &= \lVert (c(s_2) - c(s_1)) - (s_2 - s_1)\,t(s_1) \rVert \\
\end{align*}

Since $s_1$ and $s_2$ are close, we can replace $c(s_2)$ with a Taylor
series approximation around $s_1$

\begin{align*}
  c(s_2) &= c(s_1) + (s_2 - s_1)\, t(s_1) + s_1 (s_2 - s_1)^2\, n(s_1) \\
  (c(s_2) - c(s_1)) - (s_2 - s_1)\, t(s_1) &= s_1 (s_2 - s_1)^2\, n(s_1) \\
  e &= \lVert s_1 (s_2 - s_1)^2\, n(s_1) \rVert \\
  e &= \lVert s_1 (s_2 - s_1)^2 \rVert \\
\end{align*}

With the thresholds we use for performing the geometry conversion,
this error is $\approx 10^{-18} m$ in the worst case.

\subsection{Tangent angle error}

The tangent angle for the spiral at $s$ is $s^2$ and the tangent
angle for the line at $s$ is $s_1^2$. The error in angle is

\begin{align*}
  e &= s^2 - s_1^2 \\
    & \le s_2^2 - s_1^2 \\
\end{align*}

The error in angle is $s_2^2 - s_1^2$. This is
$\approx 10^{-12}\, \mathrm{rad}$ with our current thresholds

\section{Approximating spiral with arc}

A spiral is replaced by an arc when the absolute value of
curvature change for the spiral is $\le 10^{-6} m^{-1}$. The
arc approximation for a spiral is given by:

\begin{align*}
  a(s) &= c(s_1) - \frac{n(s) - n(s_1)}{s_1 + s_2}
\end{align*}

\subsection{Distance error}

\begin{align*}
  e &= \lVert c(s) - a(s) \rVert \\
    &= \lVert c(s) - \left( c(s_1) - \frac{n(s) - n(s_1)}{s_1 + s_2} \right) \rVert \\
    &= \lVert \left( c(s) + \frac{n(s)}{s_1 + s_2} \right) - \left( c(s_1) + \frac{n(s_1)}{s_1 + s_2} \right) \rVert
\end{align*}

Let $f(s) = c(s) + \frac{n(s)}{s_1 + s_2}$. Since $s_1 \le s \le s_2$ and $|s_2 - s_1|$
is a small number, we use a Taylor series expansion around $s_1$

\begin{align*}
  f(s) &= c(s) + \frac{n(s)}{s_1 + s_2} \\
  f'(s) &= t(s) - \frac{2\, s\, t(s)}{s_1 + s_2} \\
        &= \left( 1 - \frac{2\, s}{s_1 + s_2} \right) t(s) \\
  f(s)  &= f(s_1) + f'(s_1)\,(s - s_1) + \mathcal{O}(\Delta s^2) \\
  f(s) - f(s_1) &= f'(s_1)\, (s - s_1) \\
  \lVert f(s) - f(s_1) \rVert &= \lVert f'(s_1)\, (s - s_1) \rVert \\
  e &= \lVert \left( 1 - \frac{2\, s_1}{s_1 + s_2} \right) (s - s_1)\, t(s_1) \rVert \\
    &= \left( 1 - \frac{2\, s_1}{s_1 + s_2} \right) (s - s_1) \\
    &= \frac{s_2 - s_1}{s_2 + s_1} (s - s_1) \\
\end{align*}

With our current thresholds, this value $\approx 10^{-6}m$ in the worst case

\subsection{Tangent angle error}

\begin{align*}
  a'(s) &= \frac{2\, s}{s_1 + s_2} t(s) \\
  a'_{\mathrm{norm}}(s) &= t(s)
\end{align*}

The slope of the arc approximation is same as the slope of the clothoid at
every value of $s$, i.e, the angle error is zero.

\end{document}
