ukf7d_written_solutions.tex

\documentclass{article}
\usepackage{fullpage}
\usepackage{amsmath} % for math stuff
\usepackage{graphicx} % for inserting graphics
\usepackage{xcolor} % for text coloring

\newcommand{\todo}[1]{\textcolor{red}{\textbf{#1}}}

\title{UKF with 7D State Vector}
\author{}
\date{\today}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\begin{document}
\maketitle

\section{Design}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{State Vector}

\begin{equation}
\mathbf{x}_t = \begin{bmatrix}
x \\
y \\
z \\
\dot x \\
\dot y \\
\dot z \\
\psi \end{bmatrix}
\end{equation}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{Prediction Step}

\subsubsection{Control Input}

We define a control input $\mathbf{u}$ populated by linear accelerations from the IMU:

\begin{equation}
\mathbf{u}_t = \begin{bmatrix}
\ddot x^b \\
\ddot y^b \\
\ddot z^b
\end{bmatrix}
\end{equation}

The linear accelerations are in the drone's body frame, so we need to rotate these vectors into the global frame based on the yaw variable that we are tracking and the roll and pitch values from the IMU as well. This transformation will occur in the state transition function.

\subsubsection{State Transition Function}

\begin{equation}
  g(\mathbf{x}_{t-\Delta t}, \mathbf{u}_t, \Delta t) = \todo{?}
\end{equation}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{Measurement Update Step}

\subsubsection{Measurement Vector}

\begin{equation}
\mathbf{z}_t = \begin{bmatrix}
r \\
x \\
y \\
\dot x \\
\dot y \\
\psi_{\text{camera}}
\end{bmatrix}
\end{equation}

\subsubsection{Measurement Function}

\begin{equation}
  h(\mathbf{\bar x}_t) = \todo{?}
\end{equation}

\subsubsection{Measurement Covariance Matrix}

\begin{equation}
  \mathbf{R}_t = \todo{?}
\end{equation}

\end{document}