# 2D Kalman Filtering #
### Jack Nelson, April 2016###
## Introduction ##
This notebook demonstrates basic Kalman filtering for recursively and probabalistically modeling the state vector of some actor over some period of discrete time. In other words, we model how a robot would use its noisy sensors to develop a probabilistic model of where it is in a 2D grid. 

The concepts behind basic linear Kalman filtering are thoroughly explained as we build actual executable Python code that runs the simulated robot's Kalman filter.

**Note** - There's a bug with Google Chrome where Latex-style math equations have a vertical bar to the right of them. If this bothers you, I suggest you open this notebook in another browser.

## Acknowledgments ##
The notation and theoretical descriptions of the algorithm are taken heavily from Sebastian Thrun and Wolfram Burgard's text *Probabalistic Robotics* (http://www.amazon.com/Probabilistic-Robotics-Intelligent-Autonomous-Agents/dp/0262201623?ie=UTF8&keywords=probabilistic%20robotics&qid=1461459899&ref_=sr_1_1&sr=8-1).



```python
%matplotlib notebook

import matplotlib
import numpy as np
import matplotlib.pyplot as plt
```

Let's setup the initial parameters and the Kalman filter.

First we set the number of iteration steps, the initial position of the robot, and enable or disable movement of the robot. Next we setup the state transition matrices and the initial assumptions of the Kalman filter.

#### Initial State ####
The initial state (position) of the robot is modeled as a Gaussian with mean $x_0$ and covariance $\Sigma_0$.

#### State Transition Probability ###
The state transition probability $p(x_t\ |\ u_t,\ x_t-1)$ is a linear Gaussian function of the previous state, the current control input, and Gaussian noise:

\begin{equation}
x_t = A_tx_{t-1} + B_tu_t + \epsilon_t
\end{equation}

where $ A_t $ is an $n x n$ state transition matrix where $n$ is the dimension of the state vector $x_t$, which in this case is 2, and $B_t$ is an $n x m$ control matrix where $m$ is the dimension of the control vector $u_t$. $\epsilon_t$ is an n-dimensional Gaussian random vector that models the uncertainty introduced by the state transition. It's mean is zero and its covariance is $R_t$.

The state transition equation represents the robot's internal model (belief) of its physical system and its state in that system at each time step. $A_t$ models how the system changes over time and is diagonal (that is it's zero everywhere but the diagonal) if the system is not cross-dependent. For example, in a system where our robot is sitting on a perfectly flat table with no external forces, $A_t$ would be zero everywhere because the robot would not be moving unless a command was sent to its actuators to move it.

$B_t$ models how our command inputs map to actual outcomes in our robot's state. Once again, this matrix is diagonal if there are no cross-dependencies of outcomes on control inputs, i.e. if each "axis" of the state vector has its own actuator. 

#### Measurement Probability ####
The measurement probability is a linear Gaussian function of the current state $x_t$ and Gaussian noise:

$$ z_t = C_tx_t + \delta_t $$

where $C_t$ is a matrix of size $kxn$ where $k$ is the dimension of the measurement vector $z_t$. The vector $\delta_t$ is the measurement noise modeled by a multivariate Gaussian with zero mean and covariance $Q_t$.

The measurement probability function models our robot's sensors perception of the robot's state and returns what the sensors believe the mean of the robot's state is. $C_t$ models the sensors' dependency on the state of the robot. $C_t$ should usually be an $n\ x\ k$ identity matrix.


```python
# setup simulation parameters
n_iter = 100 # number of iterations
n = 2 # state vector dimension
m = 2 # control vector dimension
k = 2 # measurement vector dimension

# initial state Gaussian
mu_0 = np.array([[-0.5],[0.5]]) #Initial state vector
Sigma_0 = np.array([[1,1],[1,1]]) #Initial state covariance



# covariance matrices
R_t = np.array([[0.1, 0.1], 
              [0.1, 0.1]])# state transition noise covariance
Q_t = np.array([[0.5, 0.5],
              [0.5, 0.5]]) # measurement noise covariance

# initialize arrays to hold state, control, and measurement values (to plot later)
mu = np.zeros((n_iter, n))	# array of actual position values
u = np.zeros((n_iter, m))    # array of control inputs
z = np.zeros((n_iter, k))	# measurement array

# initialize the posterior Gaussian
mu_bar = np.zeros((n_iter, n))	# posterior estimates of x
Sigma_bar = np.zeros((n_iter,n,n))	# posterior error estimates (covariance)

K = np.zeros((n,n))	# Kalman gain or blending factor

# initial position
mu[0] = mu_0.T
```

### Recursive Kalman Filter Algorithm ###
Now that the filter has been set up, we can define a function to actually perform the filter, then recursively apply the filter over the `n_iter` number of time steps.

#### Filter Inputs and Outputs ####
The recursive Kalman filter takes as inputs the previous time-step's state probability in the form of the the mean $\mu_{t-1}$ and the covariance $\Sigma_{t-1}$, as well as the control input at that time step $u_t$ and the measurement at that step $z_t$. The filter returns the state probability at that time step as the mean $\mu_t$ and covariance $\Sigma_t$. The mean and covariance returned at time $t$ then becomes the input to the filter at time $t+1$, the filter is applied again, and so on and so forth.

#### Calculating the A Priori Belief (Prediciton Step) ####
The filter starts by making a predicted belief of the state probability at time $t$, known as the a priori belief (before we incorporate sensor measurements), from the information provided about the state probability at time $t-1$ and the control input to the state transition function from $t-1$ to $t$, or in other words how the robot thinks it moved during the last time step. As a reminder, the state transition function connects the state at $t-1$ to the current state at $t$ by:

$$ x_t = A_tx_{t-1} + B_tu_t + \epsilon_t $$

where $\epsilon_t$ is a Gaussian random variable representing noise in the transition process.

The Kalman filter predicts the mean and covariance of the state at $t$ as follows:

$$ \bar{\mu_t} = A_t\mu_{t-1} + B_tu_t $$
$$ \bar{\Sigma_t} = A_t\Sigma_{t-1}A_t^T + R_t $$

Where the predicted belief of the state is represented by the predicted mean $\bar{\mu_t}$ and the predicted covariance $\bar{\Sigma_t}$, which describes a standard normal Gaussian probability distribution of the current predicted state, which in this case is the x-y location of a robot in a 2D grid.

In the first equation, the mean update, the $A_t$ and $B_t$ are the state transition and control matrices, respectively. $A_t$ effectively describes how the system changes on its own from time $t-1$ to $t$, while $B_t$ describes how a control input $u_t$ affects the state of the system from $t-1$ to $t$.

In the second equation, the covariance update considers the fact that states depend on previous states through the linear matrix $A_t$, which is multiplied twice into the covariance, since the covariance is a quadratic matrix.

#### Calculating the Kalman Gain ####
The next part of the recursive filter algorithm calculates the Kalman gain $K_t$, which specifies the degree to which the measurement $z_t$ is to be incorporated into the new state estimate.

$$ K_t = \frac{\bar{\Sigma_t}C_t^T}{ C_t\bar{\Sigma_t}C_t^T + Q_t} $$

As a reminder, the measurement $z_t$ is given by:

$$z_t = C_tx_t + \delta_t$$

where $\delta_t$ is a multivariate Gaussian with zero mean and covariance $Q_t$, so $Q_t$ describes the "spread" of the noise.

#### Calculating the Posterior Belief (Measurement Update Step) ####
With the Kalman gain calculated, we now go about calculating the posterior belief and thus updating our robot's internal belief as to where it actually is. Once again, this "belief" takes the form of a normal Gaussian distribution centered around the robot's "best guess" as to where it is, that is the mean $\mu_t$, with covariance $\Sigma_t$.

Both the mean and covariance at time $t$ are updated from the robot's prediction of where it is after the last time step and its sensor observation(s) of its location. The magnitude of the Kalman gain weights how much the new observations are incorporated into the robot's belief. The mean and covariance are updated as follows:

$$ \mu_t = \bar{\mu_t} + K_t(z_t - C_t\bar{\mu_t}) $$
$$ \Sigma_t = (I - K_tC_t)\bar{\Sigma_t} $$

where $I$ is the identity matrix. In the first equation, the calculation of the posterior mean $\mu_t$, the robot adds the difference between its measured location and its a priori predicted location, weighted by the Kalman gain, to its the a priori predicted location. The difference between the measurement $z_t$ and the expected measurement $C_t\bar{\mu_t}$ is termed the *innovation*.

In the second equation the posterior covariance $\Sigma_t$ is calculated. **TODO - Add more info about the posterior covariance**.

Now let's put it all together. First the prior belief update:

$$ \bar{\mu_t} = A_t\mu_{t-1} + B_tu_t $$
$$ \bar{\Sigma_t} = A_t\Sigma_{t-1}A_t^T + R_t $$

Then the Kalman gain:
$$ K_t = \frac{\bar{\Sigma_t}C_t^T}{ C_t\bar{\Sigma_t}C_t^T + Q_t} $$

And finally the posterior belief update:
$$ \mu_t = \bar{\mu_t} + K_t(z_t - C_t\bar{\mu_t}) $$
$$ \Sigma_t = (I - K_tC_t)\bar{\Sigma_t} $$

This algorithm is applied recursively, meaning the outputs of the filter at time $t-1$ form the inputs (along with a measurement and control input) to the filter at time $t$, and so on and so forth over and over again.

As a Python function, it looks like this:


```python
def kalman_filter(mu_minus, Sigma_minus, u_t, z_t):
    # a priori belief (prediction)
    mu_bar = np.matmul(A_t, mu_minus) + np.matmul(B_t, u_t)
    Sigma_bar = A_t * Sigma_minus * A_t.T + R_t
    
    # Kalman gain
    K_t = (Sigma_bar * C_t.T) / (C_t * Sigma_bar * C_t.T + Q_t)
    
    # posterior belief update (measurement update)
    mu_t = mu_bar + K_t*(z_t - C_t*mu_bar)
    Sigma_t = (np.identity((n)) - K_t*C_t) * Sigma_bar
    
    return mu_t, Sigma_t
```

The Kalman filter described above alternates between predicting the robot's movement (the prediciton step) and using its sensors to ascertain its actual position and integrating that information into its internal model of its location (the measurement update). The prediction step increases the robot's uncertainty of where it is, while the measurement step decreases its uncertainty.

#### Summary of vectors and matrices ####
Before we go any further, let's list some of the vectors and matrices we've been using.

** indices **
- n - spatial dimension
- m - control dimension
- k - measurement dimension

**vectors**
- $x_t$ - $(n\ x\ 1)$ posterior state vector
- $u_t$ - $(m\ x\ 1)$ control vector
- $z_t$ - $(k\ x\ 1)$ measurement vector

- $\mu_t$ - $(n\ x\ 1)$ posterior state vector mean
- $\bar{\mu_t}$ - $(n\ x\ 1)$ a priori (prediction) state vector mean
- $\epsilon_t$ -$(n\ x\ 1)$ state transition uncertainty (Gaussian random vector)
- $\delta_t$ - $(k\ x\ 1)$ measurement noise vector (Gaussian random vector)

** matrices **
-  $ \Sigma_t$ - $(n\ x\ n)$ posterior state covariance matrix.
- $\bar{\Sigma_t}$ - $(n\ x\ n)$ a priori (prediction) state vector covariance.
- $ A_t$ - $(n\ x\ n)$ state transition matrix.
- $ B_t$ - $(m\ x\ n)$ control matrix.
- $ C_t$ - $(k\ x\ n)$ sensor measurement matrix.
- $ K_t$ - $(n\ x\ n)$ Kalman gain matrix.
- $ Q_t$ - $(n\ x\ n)$ sensor measurement covariance.

### A Simple Example - Robot on a Flat Table ###
Now let's apply our Kalman filter theory to a working example. We want to model a robot sitting on a table at rest initially. The robot can move in the x and y direction, and has a suite of ultrasound sensors that sense its distance from walls that form the x and y axes.

#### Modeling the robot's state ####
We start by setting up our state transition model. Our robot is sitting on a flat table with no external forces acting on it (we assume), so from one time step to another, unless we command it to, it should stay still at $(x_0, y_0)$. Our state transition matrix $A_t$ is then zero, and the state transitions of the robot are completely determined by our command inputs and the random Gaussian noise of our model.

We'll assume our control inputs are normalized floats between 0 and 1 for each axis of the table. Our control matrix then maps the magnitude of our inputs to movement outputs that correspond to how the robot actually moves on the table. That is, a magnitude 1.0 input maps to the largest magnitude position change the robot can achieve in one time step. Once again, we'll assume our robot's movement has no cross-dependency on control inputs so our control matrix $B_t$ can be diagonal.

Our ultrasound "ping" sensors which measure our robot's distance from the x and y axes (walls) return float values that directly correspond to distance and are not cross-dependent, so $C_t$ can be the $(n\ x\ k)$ identity matrix since the measurement values do not need to be scaled. 

Ping sensors tend to be quite noisy and unreliable, as you probably know if you've ever worked with them. Modeling the full characteristics of ping sensor measurements is beyond the scope of this example. Our model will assume that their measurement values are at least roughly centered about the robot's actual distance from each wall, but have a standard deviation of 0.2. We'll assume our sensor noise is not cross-dependent so $Q_t$, the sensor noise covariance matrix, can be diagonal.

In Python, our robot's state transition and sensor measurement models look like this:


```python
# transition matrices
u_max = 1.0
A_t = np.zeros((n,n)) # state transition matrix
B_t = np.array([[u_max, 0.0],
                [0.0, u_max]]) # control input transformation
C_t = np.identity(n) # measurement transformation (identity matrix)
```

We now setup our simulation similarly to the 1-D case. We'll loop from 1 to `n_iter`. During each iteration, we'll generate 2-D control inputs to our robot, move our robot according to our state transition model, generate a measurement of the new state, then pass this information to our Kalman filter.


```python
T = 0.5 # random control input gain

for k in range(1, n_iter):
    # generate control inputs
    u[k] = [np.random.randn()*T for i in range(n)] # random walk movement
    
    # move the robot
    epsilon_t = [np.random.randn() for i in range(n)] # Gaussian movement noise
    mu[k] = np.matmul(A_t, mu[k-1]) + np.matmul(B_t,u[k]) + epsilon_t
    
    # generate a sensor measurement about the new state x[k]. Noise
    # is added to the measurement by drawing from a multivariate gaussian
    # distribution with mean centered around the robot's current location
    z[k] = np.matmul(C_t, mu[k]) + np.random.multivariate_normal(mu[k], Q_t)
    
    #apply the kalman filter
    mu_bar[k], Sigma_bar[k] = kalman_filter(mu_bar[k-1], Sigma_bar[k-1], u[k], z[k])
    print("%1.3f, %1.3f" %(xhat[k], Vhat[k]))
```

## Links and Resources ##
- Kalman Filtering Example from the SciPy Cookbook (http://scipy-cookbook.readthedocs.io/items/KalmanFiltering.html)


```python

```

71258b9c59643b857cd8247b26d8cf603e063121
ipynb
Jupyter Notebook
0.2 - 2D Kalman.ipynb
jwnelson/Kalman
2417351d994f0cd869c4e44727886ee096260433
MIT
2021-04-08T05:17:31.000Z
2021-04-08T05:17:31.000Z
0.2 - 2D Kalman.ipynb
jwnelson/Kalman
2417351d994f0cd869c4e44727886ee096260433
MIT
0.2 - 2D Kalman.ipynb
jwnelson/Kalman
2417351d994f0cd869c4e44727886ee096260433
MIT
2016-09-30T17:51:40.000Z
2016-09-30T17:51:40.000Z
Qwen/Qwen-72B
1. YES
2. YES