The purpose of this lab is to give you an idea of what the Kalman Filter does and under what conditions it works well (HINT: it doesn't work perfectly in every situation). In addition to tracking enemy tanks, the Kalman Filter will help you compensate for sensor noise, which is introduced in this lab (and will be present in the final tournament). You will be required to do the following:

- Code up a few simple “clay pigeon” to aim at (two will meet the assumptions of the Kalman filter, one will not)
- Code up the multivariate Kalman Filter update equations
- Draw the filter's output for your “clay pigeons”

Note that the numbers provided below are just a starting point. **I expect you to have to adjust them to get this to work, that will be the hard part**.

To pass off this lab, you will:

- Submit all of your code electronically to the ta.
- Turn in a declaration of time spent by each lab partner
- Demonstrate to your code to the TA, and be able to discuss the item discussed below, that would have been in the write-up. You must show the TA some graphs and explain them.

Your discussion with the TA should include information about what kinds of transition and covariance matrices you used and how it affected performance. Do meaningful experiments that test the abilities of the filter, and try to make meaningful and insightful observations. Discuss why it works better or worse in various circumstances and what you might do in the Tournament based on your observations in this lab.

The Kalman Filter makes a number of assumptions about the path that the tracked object is following. Your first task is to code several “Clay Pigeon” that conforms to these assumptions. You will make 2 Conforming Clay Pigeons which behave in the following ways:

- “Sitting Duck”– Just sit there
- Constant x and y velocity (a straight line)

Your second task is to build a Clay Pigeon that violates the Kalman assumptions in some way (your choice). For fun try to make it as hard to hit as you can. Skill in fooling the Kalman Filter will be an asset when you are competing in the final tournament. I hope it also teaches you something about the Kalman Filter!

Your Kalman Agent must also plot the density of the output of the Kalman Filter (see GnuplotHelp). You will need to plot your filtered estimate of the current location of the clay pigeons and the projected locations. Please code up the plot early rather than at the end of the project; it is a great debugging tool and will really help you understand what is happening with the Kalman Filter.

Use the empty world (remove all obstacles). Both teams should be run with –[color]-tanks=1 (1 player on each team). Your agent should be run with noise by using –default-posnoise=5 (Please comment on the discussion page or talk/email me if you have trouble with this much noise, try various values). Your agent may rotate but may not move. You should successfully track and reliably (maybe not perfectly) shoot the Conforming Clay Pigeons. There may also be instances where the random starting position of the enemy puts it out of range of your tank for an extended period of time.

Part of your task is to tune your Kalman agent to do as well as possible on your Wild Pigeon. In your writeup, explaining exactly why you had difficulty and what you tried to do about it. Communicate the creative efforts you used to mislead the Kalman Filter and what you did to try to overcome these problems.

To accomplish this lab, it is helpful to understand the “physics” used by the enemy agent. We will represent these physics using matrices as done in the class discussions. You will want to play with the values in these matrices, especially $\Sigma_x$ and $\Sigma_z$, and we encourage you to do so in order to better understand how the Kalman Filter works.

Initially, your clay pigeons will be at some unknown position on the playing field, and the velocity and acceleration will both be zero. You can use that information to create your initial estimates of the mean and covariance. The physics are based on the six values in our state vector (in this order, represented as a column vector):

$X_t =
\begin{bmatrix}
x_t

\dot{x}_t

\ddot{x}_t

y_t

\dot{y}_t

\ddot{y}_t
\end{bmatrix}$

where $x$ and $y$ are the $(x, y)$ position of the enemy agent, $\dot{x}$ is the x component of the agent's velocity, $\ddot{x}$ is the x component of the agent's acceleration, and etc. Note that we use $X_t$ to represent the entire observation at time $t$.

Given this state vector, the Kalman Filter will produce a mean estimate for this vector $\mu$ and a covariance matrix for this vector $\Sigma$. So, your initial estimates of the mean and covariance could look like these:

$\mu_0 =
\begin{bmatrix}
0

0

0

0

0

0
\end{bmatrix}$

which means that you think the agent begins at the origin with no velocity and no acceleration, and

$\Sigma_0 =
\begin{bmatrix}
100 & 0 & 0 & 0 & 0 & 0

0 & 0.1 & 0 & 0 & 0 & 0

0 & 0 & 0.1 & 0 & 0 & 0

0 & 0 & 0 & 100 & 0 & 0

0 & 0 & 0 & 0 & 0.1 & 0

0 & 0 & 0 & 0 & 0 & 0.1
\end{bmatrix}$

which means that you are pretty sure that the agent is not accelerating or going anywhere, but that you are pretty unsure exactly where the agent is.

Once every time period ($\Delta t$), the enemy agent will update its state $X$ as follows:

$X_t \sim N(FX_t, \Sigma_x)\,$

In other words, the enemy agent applies the system transition matrix $F$ to its previous state and then adds noise, which is drawn from some distribution. Since the initial state and all subsequent states are random variables, these variables are capitalized to be consistent with our notes in class. The $F$ matrix used in this lab is precisely the one that we derived in class using Newton's laws of motion (with one exception):

$F =
\begin{bmatrix}
1 & \Delta t & \Delta t^2/2 & 0 & 0 & 0

0 & 1 & \Delta t & 0 & 0 & 0

0 & -c & 1 & 0 & 0 & 0

0 & 0 & 0 & 1 & \Delta t & \Delta t^2/2

0 & 0 & 0 & 0 & 1 & \Delta t

0 & 0 & 0 & 0 & -c & 1
\end{bmatrix}$

where the $c$ indicates that we have a linear friction force working against this agent. If in this lab, you re-compute every half-second then $\Delta t = 0.5$. Try setting the friction coefficient $c$ to $0.1$ to start out with. Sometimes it works better without it ($C=0$).

Each $X_t$ is a sample drawn from a multivariate normal distribution. In reality, only $x$ and $y$ accelerations have noise (with a standard deviation of $0.5$), but since there are some other influences on the behavior of the agent (such as being pushed away from walls) you will want play with the covariance matrix. A good place to start is with a covariance matrix that allows acceleration to vary from the model more than velocity or position, like the following:

$\Sigma_x =
\begin{bmatrix}
0.1 & 0 & 0 & 0 & 0 & 0

0 & 0.1 & 0 & 0 & 0 & 0

0 & 0 & 100 & 0 & 0 & 0

0 & 0 & 0 & 0.1 & 0 & 0

0 & 0 & 0 & 0 & 0.1 & 0

0 & 0 & 0 & 0 & 0 & 100
\end{bmatrix}$

The noisy measurements of the enemy position will have zero-mean Gaussian noise with a standard deviation of 5 units in each dimension. The sensor model is as follows:

$Z_t \sim N(HX_t, \Sigma_z)\,$

In this equation, $X_t$ is a random variable representing the true (unknown) state, and $Z_t$ is a random variable representing the noisy and limited observations provided by the server. Each observation from the server is a sample from $Z_t$ and is encoded as a 2-dimensional vector. These samples are used to perform inference about $X_t$.

**NOTE:** As I recall, while you get $x$ and $y$ positions and $\dot{x}$ and $\dot{y}$ velocities for your own tank, you only get $x$ and $y$ for other tanks.

The observation matrix, $H$, selects out the two “position” values from the state vector. It looks like this:

$H =
\begin{bmatrix}
1 & 0 & 0 & 0 & 0 & 0

0 & 0 & 0 & 1 & 0 & 0
\end{bmatrix}$

Since these measurements are corrupted by noise, it is important to know the covariance matrix of this noise. Since the standard deviation of the $x$ and $y$ position noise is 5 and since these two noises are uncorrelated, the covariance matrix is given by:

$\Sigma_z =
\begin{bmatrix}
25 & 0

0 & 25
\end{bmatrix}$

**NOTE:** I may change this, make it a parameter!

- You are more than welcome to implement your own matrix manipulation code. However, you are encouraged to spend your precious time on more important things. Find a reputable source and download a matrix package. If you are using python, I think all of the needed matrix operators are available in numpy. I think numpy is on the lab machines.
- Here are the three Kalman update equations (NOTE: The third equation is wrong in the second edition of the book! It was fixed for the third edition):

$K_{t+1} = (F \Sigma_t F^T + \Sigma_x) H^T (H(F \Sigma_t F^T + \Sigma_x) H^T + \Sigma_z)^{-1}$

$\mu_{t+1} = F\mu_t + K_{t+1}(z_{t+1} - HF\mu_t)$

$\Sigma_{t+1} = (I - K_{t+1} H)(F \Sigma_t F^T + \Sigma_x)$

- Be careful not to get confused with the different $\Sigma$ matrices: $\Sigma_x$, $\Sigma_z$ and the various $\Sigma_t$ matrices (one for each time step t).
- Note that these four matrices are constants, so can be initialized once: $F$,$\Sigma_x$, $H$, $\Sigma_z$.
- Note also that since $H$ and $F$ are constant, $H^T$ and $F^T$ are also constant, and can be precomputed just once.
- Initialize your $\mu_0$ and $\Sigma_0$ matrices at the start of each run.
- Don't be too scared by all the subscripts in the Kalman equations. Just think of $t$ as “the last time” and $t + 1$ as “this time.”
- Note also that the expression $(F\Sigma_tF^T + \Sigma_x)$ occurs three times in the equations, so you may save some time by calculating that first.
- To apply predictions into the future, you don't make additional observations, so you shouldn't use the full equations. Instead, use this equation:

$\mu_{t+1} = F\mu_t$

Thanks to Chris Monson, Andrew McNabb, David Wingate and Kirt Lillywhite