Estimation: Introduction by Example

V. Hunter Adams (vha3)

Click the button below to show/hide code
In [1]:
from IPython.display import Latex
import matplotlib.pyplot as plt
%matplotlib inline
import numpy
import math
import scipy.misc
from numpy.linalg import pinv
from numpy.linalg import inv
from scipy.stats import norm
from scipy import integrate
from scipy.integrate import odeint
from IPython.display import Image

In This Document

This document introduces some of the most common estimation techniques via example. We start with particle filters, and work our way to linear Kalman filters.

Resources

Derivations for the EKF and ideas for explaining things come from Tucker McClure's website (http://www.anuncommonlab.com/articles/how-kalman-filters-work/). This is a fantastic resource, the only one of its kind that I've ever seen. You should read it and, if you like it, let Tucker know (he's a Cornell grad and a super nice guy).

Other resources for the other filters include lecture notes from Mark Psiaki's 6760 class, the book Applied Optimal Estimation by Gelb, and Optimal Control and Estimation by Stengel, and Estimation with Applications to Tracking and Navigatoin by Bar-Shalom.

Introduction

Estimation is the practice of using noisy, limited, imperfect data to get the best possible estimate of some quantity of interest. Many of these techniques were born for aerospace and robotics applications and these are the most common places to find them in use, but they are general enough to be usable in a variety of other fields of study. All that we require is some model for the evolution of our system (an equation of motion), measurements of some quantity that is correlated with the quantity that we are trying to estimate, and an understanding of the noise present in our sensors and in our system. These are well understood in an aerospace/robotics context, but you can imagine modeling biological, oceanographic, geologic, or other systems and applying the same techniques.

Estimation is, in many ways, the mirror image of control. You can arrive at optimal estimation algorithms using precisely the same strategies used to arrive at optimal control methods. Your cost function, rather than minimizing the difference between a current and a desired state, instead minimizes the difference between the true and estimated state. We'll see this when we discuss the Kalman filter. Familiarity with control will make derivation of estimation algorithms easier, and vice versa.

In aerospace, the most common use for these estimation algorithms is to estimate the state of a spacecraft (quaternion, angular velocity, position, velocity) from sensors that are unable to detect these quantities directly, or that do so with a lot of noise. If you want to employ state feedback, then you're going to need to implement an estimation algorithm.

NOTE!

The code in this document is written for clarity, not for efficiency. There are plenty of opportunities to optimize.

Also, every one of these filters could be made to perform better (in particular the nonlinear ones). These are just to provide some insight into how to construct these things.

Dynamic System in Question

The point of this document is not dynamics, so I want to choose an example dynamical system that is as simple as possible while still illustrating the properties of the various estimators. Let us consider a ballistic projectile under the influence of drag and buffeting.

Equations of Motion

Consider the Lagrangian

\begin{align} \mathcal{L} &= T - V\\ &= \text{Kinetic Energy} - \text{Potential Energy} \end{align}

We'll use a set of cartesian coordinates with origin at the location of the cannon. In these coordinates, it is easy to write down the expressions for the kinetic and potential energy:

\begin{align} T &= \frac{1}{2}m\dot{x}^2 + \frac{1}{2}m\dot{y}^2 \end{align}
\begin{align} V &= mgy \end{align}

With these, we can write down the Lagrangian:

\begin{align} \mathcal{L} &= \frac{1}{2}m\dot{x}^2 + \frac{1}{2}m\dot{y}^2 - mgy \end{align}

Applying the Euler-Lagrange equation gives us two coupled differential equations that describe the motion of the spacecraft:

\begin{align} \frac{\partial{\mathcal{L}}}{\partial{q}} - \frac{d}{dt}\frac{\partial{\mathcal{L}}}{\partial{\dot{q}}} + Q_i = 0 \end{align}

In the above expression, $Q_i$ are the generalized forces acting on each generalized coordinate. In this case, we have only drag and buffeting (a random acceleration):

\begin{align} F_d &= -c\dot{x} \hat{x} - c\dot{y}\hat{y} \end{align}
\begin{align} F_b &= m\mathcal{N}(0,\sigma_{ax})\hat{x} + m\mathcal{N}(0,\sigma_{ay})\hat{y} \end{align}

Solving:

\begin{align} m\ddot{x} + c\dot{x} + m\mathcal{N}(0,\sigma_{ax}) &= 0 && m\ddot{y} + c\dot{y} - mg + m\mathcal{N}(0,\sigma_{ay}) = 0 \end{align}

State-Space Model

Get these differential equations into a state-space representation:

\begin{align} q_1 &= x && \dot{q_1} = \dot{x} = q_2\\ q_2 &= \dot{x} && \dot{q_2} = \ddot{x} = -\frac{c}{m}\dot{x} + \mathcal{N}(0,\sigma_{ax})\\ q_3 &= y && \dot{q_3} = \dot{y} = q_4\\ q_4 &= \dot{y} && \dot{q_4} = \ddot{y} = -\frac{c}{m}\dot{y} + g + \mathcal{N}(0,\sigma_{ay}) \end{align}

Simulation

Create some constants and initial conditions

\begin{align} g &= \text{gravitational acceleration} = 9.81m/s \\ m &= \text{projectile mass} = 1 kg\\ c &= \text{damping constant} = 2 kg/s\\ T &= \text{length of simulation} = 15sec\\ n &= \text{number of simulation steps} = 100\\ t &= \text{array of times}\\ \Delta t &= \text{size of timestep} = \frac{T}{n}\\ \sigma_x &= \text{standard deviation of gaussian buffeting in $x$} = 1 m/s^2\\ \sigma_y &= \text{standard deviation of gaussian buffeting in $y$} = 1 m/s^2\\ x_0 &= \begin{bmatrix} 50 & 80 & 10 & 0\end{bmatrix} \end{align}
In [137]:
g=9.81
m=1
c=2.
x0 = [50, 80, 10, 0]
stop = 15
numsteps = 100.
t = numpy.linspace(0,stop,numsteps)
delta_t = stop/numsteps
sigx = 1
sigy = 1
x_rand = sigx*numpy.random.randn(len(t))
y_rand = sigy*numpy.random.randn(len(t))

Functions for integrating the system. These functions will integrate the system of differential equations shown below, derived above, using some standard Python tools.

\begin{align} q_1 &= x && \dot{q_1} = \dot{x} = q_2\\ q_2 &= \dot{x} && \dot{q_2} = \ddot{x} = -\frac{c}{m}\dot{x} + \mathcal{N}(0,\sigma_{ax})\\ q_3 &= y && \dot{q_3} = \dot{y} = q_4\\ q_4 &= \dot{y} && \dot{q_4} = \ddot{y} = -\frac{c}{m}\dot{y} + g + \mathcal{N}(0,\sigma_{ay}) \end{align}
In [139]:
def derivatives(X, t):
    x, y, xdot, ydot = X
    noise_x = x_rand[int(t/delta_t)-2]
    noise_y = y_rand[int(t/delta_t)-2]
    return [xdot+delta_t*noise_x, ydot+delta_t*noise_y,
            -(c/m)*xdot+noise_x, -(c/m)*ydot - g + noise_y]

def noiselessDerivatives(X, t):
    x, y, xdot, ydot = X
    return [xdot, ydot,
            -(c/m)*xdot, -(c/m)*ydot - g]

def integrate():
    sol = odeint(derivatives, x0, t)
    return sol

We will also find it useful to have a function like those that integrate the full simulation, but that stops after one timestep. We can pass this function the current state and it will integrate one timestep and return the state at the next timestep (assuming zero noise).

In [140]:
def f(state):
    return odeint(noiselessDerivatives, state, numpy.arange(0, 2*delta_t, delta_t))[-1]

With these constants, functions, and initial conditions defined, let's integrate the equations of motion to create a trajectory. I will store that time history in an array so that it can be used for filter testing.

In [141]:
trajectory = integrate()
x_t, y_t, xd_t, yd_t = trajectory[:,0], trajectory[:,1] ,trajectory[:,2], trajectory[:,3]
true_trajectory = numpy.vstack((x_t,y_t,xd_t,yd_t))
In [142]:
def showTrajectory(results):
    plt.plot(trajectory[:,0], trajectory[:,1], label='trajectory')
    plt.title('Projectile Trajectory')
    plt.xlabel('meters'); plt.ylabel('meters')
    plt.plot(1.01,2,'r.', ms=20, label='RADAR')
    plt.legend(loc='best')
    plt.ylim([0,100])
    plt.show()
    plt.plot(t, trajectory[:,0])
    plt.title('Projectile x-Position')
    plt.xlabel('sec'); plt.ylabel('meters')
    plt.show()
    plt.plot(t, trajectory[:,1])
    plt.title('Projectile y-Position')
    plt.xlabel('sec'); plt.ylabel('meters')
    plt.show()
    plt.plot(t, trajectory[:,2])
    plt.title('Projectile x-Velocity')
    plt.xlabel('sec'); plt.ylabel('meters/sec')
    plt.show()
    plt.plot(t, trajectory[:,3])
    plt.title('Projectile y-Velocity')
    plt.xlabel('sec'); plt.ylabel('meters/sec')
    plt.show()
showTrajectory(trajectory)