\documentclass[12pt]{article}
\usepackage{graphicx}
\usepackage{mathtools}
\usepackage{url}
\topmargin -0.5in
\oddsidemargin -0.0in
\evensidemargin -0.0in
\textheight 9in
\textwidth 6.5in
\begin{document}
%\pagestyle{empty}

\title{Lab \#6 - particle filter}
\author{}
\date{}

\maketitle


In this lab, each student is to develop code to operate a particle filter.
The code can be developed in MATLAB, C, or any high level language.
No graphics display is required, but a plot of results is required.

The filter should be run on the following data:
\url{https://cecas.clemson.edu/ahoover/ece854/labs/magnets-data.txt}.
The three columns of the data are actual position, actual velocity,
and sensor reading.

The data follows a problem outlined in class and in the lecture notes.
The system consists of a 1D position, moving on a line.
The system follows a motion pattern where the position `zig-zags'
back and forth on a line.
The sensor on the system detects a field strength that is the sum
of the distances from two fixed-position magnets.

For this problem, there are two state variables:
\begin{equation}
X_t = \begin{bmatrix}
x_t \\
\dot{x}_t
\end{bmatrix}
\end{equation}
The state transition equation is as follows:
\begin{equation}
f(x_t,a_t) = \begin{bmatrix*}[l]
x_{t+1} = x_t + \dot{x}_t T \\
\dot{x}_{t+1} = \left\{
\begin{array}{l l}
  2 & \quad \text{if $x_t < -20$} \\
  \dot{x}_t + |a_t| & \quad \text{if $-20 \le x_t < 0$} \\
  \dot{x}_t - |a_t| & \quad \text{if $0 \le x_t \le 20$} \\
  -2 & \quad \text{if $x_t > 20$} \\
\end{array} \right.
\end{bmatrix*}
\end{equation}
The velocity equation is a piecewise function that adds or subtracts
a random amount $a_t$ to the current velocity,
depending on the current position.
The values $a_t$ are drawn from a zero-mean Gaussian distribution
$N(0,\sigma_a^2)$.
The data was generated using a value of $\sigma_a = 2^{-4} =  0.0625$.
The goal of the state transition equation is to keep the position
oscillating about zero but between -20 and 20.

For observations, this problem uses a sensor that measures the total
magnetic strength:
\begin{equation}
Y_t = \begin{bmatrix}
y_t
\end{bmatrix}
\end{equation}
Two magnets are placed at $x_{m1}=-10$ and $x_{m2}=10$.
The observation equation for this model is:
\begin{equation}
g(x_t,n_t) = \begin{bmatrix}
y_t = \frac{1}{\sqrt{2 \pi} \sigma_m}
\mathrm{exp} ( \frac{-(x_t - x_{m1})^2}{2 \sigma_m^2} ) +
\frac{1}{\sqrt{2 \pi} \sigma_m}
\mathrm{exp} ( \frac{-(x_t - x_{m2})^2}{2 \sigma_m^2} ) + n_t
\end{bmatrix}
\end{equation}
where $n_t$ is a random sample drawn from $N(0,\sigma_n^2)$
representing measurement noise.
The value $\sigma_m = 4.0$.
The data was generated using a value of $\sigma_n = 2^{-8} = 0.003906$.

When calculating the weight updates, use the prior importance function,
so that:
\begin{equation}
\tilde{w}_t^{(m)} = w_{t-1}^{(m)} \cdot p(y_t | x_t^{(m)})
\end{equation}
The value $p(y_t | x_t^{(m)})$ is determined by the measurement noise.
It should be calculated by taking the ideal measurement of the particle,
and comparing it against the actual measurement, in the model of the
measurement noise.
The ideal measurement of the particle is calculated as follows:
\begin{equation}
g(x_t^{(m)},0) = \begin{bmatrix}
y_t^{(m)} = \frac{1}{\sqrt{2 \pi} \sigma_m}
\mathrm{exp} ( \frac{-(x_t^{(m)} - x_{m1})^2}{2 \sigma_m^2} ) +
\frac{1}{\sqrt{2 \pi} \sigma_m}
\mathrm{exp} ( \frac{-(x_t^{(m)} - x_{m2})^2}{2 \sigma_m^2} )
\end{bmatrix}
\end{equation}
The ideal measurement can then be compared against the actual
measurement in the model of the measurement noise as follows:
\begin{equation}
p(y_t | x_t^{(m)}) = 
\frac{1}{\sqrt{2 \pi} \sigma_n}
\mathrm{exp} ( \frac{-(y_t^{(m)} - y_t)^2}{2 \sigma_n^2} )
\end{equation}

Implement the particle filter using the above equations on the given data.
The number of particles, weight update equation,
resampling method and criteria, and other options of the particle filter,
should be selected as outlined in class.
The results should show a graph plotting the measurements, true
position, and estimated position.
The report should also include at least one plot of the particle
distribution at a single instant in time (e.g. position and weight).
At least one plot should demonstrate resampling.

You must submit your report to Canvas.  This report is due by
midnight of the due date.

\end{document}



