The Kalman Filter is an algorithm that estimates the state of a stochastic dynamical system at a given time $x_t$ by using noisy measurements $z_t$ of the system. In particular, the Kalman Filter is used when the dynamics of the system can be represented by a linear finite-difference equation of the form
$$ x_t = F_t x_{t-1} + B_t u_t + w_t $$where $F_t$ is the state transition matrix, $u_t$ is a control input vector, $B_t$ is the matrix which controls how the input changes the state, and $w_t$ is process noise.
Measurements $z_t$ are assumed to be linearly related to the state
$$ z_t = H_t x_t + v_t $$where $H_t$ is the transformation matrix from state to measurement and $v_t$ is the measurement noise.
The Kalman Filter method assumes that $w_t$ and $v_t$ are Gaussian distributed random variables with zero mean and covariance matrices $Q_t$ and $R_t$ respectively.
The Kalman Filter algorithm repeatedly estimates the current state based on previous state estimates and current measurements. In particular, the algorithm models the current state's probability distribution as a Gaussian with mean $\hat{x}_{t|t}$ and covariance matrix $\hat{P}_{t|t}$.
This procedure can be though of in two steps:
First, we would like to estimate the current state $x_t$ based on past predictions, $\hat{x}_{t-1|t-1}, \hat{P}_{t-1|t-1}$. This estimate, which is based purely on the dynamics, we call $\hat{x}_{t|t-1}$. And the estimate for the covariance matrix associated with the state's fluctuations is likewise called $\hat{P}_{t|t-1}$.
Inserting the dynamical equation for $x_t$ into the definitions of mean and covariance, we obtain equations for these two estimates:
$$ \hat{x}_{t|t-1} = F_t \hat{x}_{t-1|t-1} + B_t u_t \\ \hat{P}_{t|t-1} = F_t \hat{P}_{t-1|t-1}F_t^T + Q_t. $$Next, we want to incorporate the measurements $z_t$ taken at time $t$ into our estimate for $x_t$ and its covariance matrix. We invert the measurement equation for $z_t$ to solve for $x_t$. This defines a Gaussian probability distribution for $x_t$ based on the measurement at time $t$. By multiplying this by the Gaussian distribution determined from the prediction step, we get a new combined Gaussian with mean and covariance specified by the current measurement $z_t$ (and their covariance $R_t$) and the past predictions $\hat{x}_{t|t-1}$ and $\hat{P}_{t|t-1}$.
This gives us, after some algebraic manipulations, the measurement equations:
$$ \hat{x}_{t|t} = \hat{x}_{t|t-1} + K_t(z_t - H_t \hat{x}_{t|t-1}) \\ \hat{P}_{t|t} = \hat{P}_{t|t-1} - K_t H_t \hat{P}_{t|t-1} $$where $K_t$ is the so-called Kalman gain, which has the form
$$ K_t = \hat{P}_{t|t-1} H_t^T (H_t P_{t|t-1} H_t^T + R_t)^{-1}. $$Here is a few lines of code in python that implements an iteration of the Kalman Filter algorithm.
import numpy as np
from numpy.linalg import inv
def kalman(x_prev, P_prev, z, F, Q, H, R):
# Prediction estimates
x_pred = np.dot(F, x_prev)
P_pred = np.dot(F, np.dot(P_prev, np.transpose(F))) + Q
# Measurement estimates
K = np.dot(P_prev, np.dot(np.transpose(H), inv(np.dot(H, np.dot(P_prev, np.transpose(H))) + R)))
x_meas = np.dot(K, z - np.dot(H, x_prev))
P_meas = -np.dot(K, np.dot(H, P_prev))
return (x_pred + x_meas, P_pred + P_meas)
Suppose we have a state that is oscillating as an SHO subject to random noise.
The SHO differential equation
$$ \ddot{x} = -\omega_0^2 x $$can be written as a set of coupled ODEs
$$ \dot{x_1} = x_2 \\ \dot{x_2} = -\omega_0^2 x_1 $$where $x_1 \equiv x$ and $x_2 \equiv \dot{x}.$ The discretized finite-difference version of the coupled SHO equations is
$$ \begin{pmatrix} x_1 \\ x_2 \end{pmatrix}_t = \begin{pmatrix} 1 & \Delta t \\ -\omega_0^2 \Delta t & 1 \end{pmatrix} \begin{pmatrix} x_1 \\ x_2 \end{pmatrix}_{t-1} $$where $\Delta t$ is the time between times steps $t-1$ and $t$.
Note that any linear ODE can be written as a set of coupled first-order ODEs and discretized. The Kalman Filter can be used to estimate the state of all such systems.
import matplotlib
import matplotlib.pyplot as plt
# number of time steps
Nt = 1000
# oscillator frequency
w0 = 1.0
# discretization time step
dt = 0.01
# total time elapsed
T = Nt * dt
time = np.linspace(0.0, T, Nt)
# The initial state
x0 = np.array([[0.0],[1.0]])
# The transition matrix
F = np.array([[1.0, dt],[-w0*w0*dt, 1.0]])
# The (constant) process noise
Qvar = 0.0005
Q = np.zeros((2,2))
Q[0,0] = Qvar
Q[1,1] = Qvar
# The measurement noise
Rvar = 4.0
R = np.zeros((2,2))
R[0,0] = Rvar
R[1,1] = Rvar
# The actual state
x = np.zeros((2,1,Nt))
# The measurements
z = np.zeros((2,1,Nt))
# The predictions
x_pred = np.zeros((2,1,Nt))
P_pred = np.zeros((2,2,Nt))
# Simulating the dynamics and running the filter
x[:,:,0] = x0
x_pred[:,:,0] = x0
P_pred[:,:,0] = R
for t in range(1,Nt):
x[:,:,t] = np.dot(F, x[:,:,t-1]) + np.sqrt(Qvar) * np.random.randn(2,1)
z[:,:,t] = x[:,:,t-1] + np.sqrt(Rvar) * np.random.randn(2,1)
(x_pred[:,:,t], P_pred[:,:,t]) = kalman(x_pred[:,:,t-1], P_pred[:,:,t-1], z[:,:,t], F, Q, 1, R)
matplotlib.rcParams.update({'font.size': 34})
# Plotting the actual dynamics versus the Kalman Filter predictions (shown with error bars)
plt.plot(time, x[0,0,:], 'k', label='actual dynamics', linewidth=5.0)
plt.plot(time, z[0,0,:], 'rD', label='measurements', markersize=6)
plt.errorbar(time, x_pred[0,0,:], yerr=np.sqrt(P_pred[0,0,:]), color='g',ecolor='g', label='predicted dynamics', linewidth=2.5)
plt.legend()
plt.title('Kalman Filter for Simple Harmonic Oscillator')
plt.xlabel('time')
plt.ylabel('position')
plt.show()
One of the most successful applications of Kalman Filters has been in the control of trajectories for rockets in space flight missions.
Note that the Kalman Filter itself does not inherently have to do with control theory. As we have described, it simply provides an estimate for a linear dynamic process based on measurements. However, these estimates can be combined with a feedback control system to improve the accuracy and stability of a desired trajectory.
For this example, we will use a very simple type of control system that implements feedback. It is called proportional–integral–derivative (PID) control and is frequently implemented by engineers through software and hardware to control various devices.
The idea is very simple. Suppose we have a dynamic process, whose state takes on a value $x(t)$ at a given time. We want to control the system so that the state takes on the value $y(t)$. We do this by trying to minimize the error $e(t) = x(t)-y(t)$. We want to push $x(t)$ towards $y(t)$ as a function of the error. We achieve this through the control (driving force) $u(t)$ which we apply to the system.
The three simplest ways to update $x(t)$ is through:
Altogether, in discrete time, the PID control equation looks like $$ u_t = -K_P (x_t-y_t) - K_I \sum_{t'=0}^t (x_{t'}-y_{t'}) \Delta t - K_D \frac{(x_t-y_t) - (x_{t-1} - y_{t-1})}{\Delta t}. $$
This $u_t$ is what we use in our equation of motion for the system state $$ x_t = F_t x_{t-1} + B_t u_t + w_t. $$
Suppose we are a telecommunications company that wants to launch a satellite into geosynchronous orbit. Our satellite is launched by a rocket, which detaches shortly after launch. The satellite has thrusters that propel it in the radial and transverse directions. We also have a measuring device that tells us the satellites position and velocity with some noise.
Our engineers want to use PID control to guide the satellite into the orbit. The satellite's equations of motion in polar coordinates look like:
$$ \ddot{r} + r\dot{\phi}^2 - \frac{u_r}{m} + \frac{GM}{r^2} = 0 \\ 2\dot{r}\dot{\phi} + r \ddot{\phi} - \frac{u_\phi}{m} = 0. $$This is a non-linear second-order ODE.
We can break this up into a system of non-linear first-order ODEs. Then, the equations can be linearized by computing the Jacobian at each time step. Finally, the derivatives can be approximated by finite-differences and we can obtain a finite-difference equation for the satellite's position $(r_t,\phi_t)$ over time.
The resulting linearized finite-difference equation (that I computed) was
$$ x_t = F_t x_{t-1} + B_t u_t + w_t $$where
$$ x_t = \begin{pmatrix} x_1 \\ x_2 \\ x_3 \\ x_4 \end{pmatrix}_t = \begin{pmatrix} r \\ \dot{r} \\ \phi \\ \dot{\phi} \end{pmatrix}_t $$is the state vector,
$$ u_t = \begin{pmatrix} u_1 \\ u_2 \end{pmatrix}_t = \begin{pmatrix} u_r \\ u_\phi \end{pmatrix}_t $$is the driving force vector (there are radial and tangential thrusters on the satellite),
$$ F_t = \begin{pmatrix} 1 & \Delta t & 0 & 0 \\ \left(-x_4^2 + \frac{2GM}{x_1^3}\right)\Delta t & 1 & 0 & -2x_1 x_4 \Delta t \\ 0 & 0 & 1 & \Delta t \\ \left(\frac{2x_2x_4}{x_1^2} - \frac{u_2}{mx_1^2}\right)\Delta t & -\frac{2x_4}{x_1}\Delta t & 0 & 1-\frac{2x_2}{x_1}\Delta t \end{pmatrix}_{t-1} $$is the linearized state transition matrix (i.e., the $x_t$-Jacobian), and
$$ B_t = \frac{\Delta t}{m}\begin{pmatrix} 0 & 0 \\ 1 & 0 \\ 0 & 0 \\ 0 & \frac{1}{x_1} \end{pmatrix}_t $$is the linearized control to state matrix (i.e., the $u_t$-Jacobian).
The filter that uses Jacobian's to linearize non-linear finite-difference equations for the Kalman Filter is called the Extended Kalman Filter. Another non-linear filter is the Unscented Kalman Filter, which does something else, that attempts to preserve the Gaussian properties of the noise.
Below is code I wrote to implement this example. I could not get it to work properly, but I include it for completeness. It could be that I derived the equations of motion incorrectly, or that I picked bad parameter values, or that there's a bug in the code somewhere.
import numpy as np
import matplotlib
import matplotlib.pyplot as plt
# number of time steps
Nt = 1000
# Newton's constant
G = 6.674e-11
# Mass of satellite
m = 2200000.0
# Mass of Earth
M = 6.0e24
# Radius of Earth
R0 = 6400000.0
# Period of Earth rotation
T0 = 24*60*60
# Angular velocity of Earth
omega = 2.0*np.pi/T0
# Initial vertical velocity of satellite
v0 = 1.0e-2*11000.0
# discretization time step (in seconds)
dt = 1.0
# total time elapsed
T = Nt * dt
# time vector
time = np.linspace(dt, dt*Nt, Nt)
# number of variables (r,\phi,\dot{r},\dot{\phi})
Nvars = 4
# The initial state vector
x0 = np.array([[R0], [v0], [0.0], [omega]])
# The (constant) process noise
Q = np.zeros((Nvars,Nvars))
Q[0,0] = 1.0
Q[1,1] = 0.1
Q[2,2] = 0.001
Q[3,3] = 0.001
Q = 1.0e-9*Q
# The measurement noise
R = 10.0*Q
# The state
x = np.zeros((Nvars,1,Nt))
# The measurements
z = np.zeros((Nvars,1,Nt))
# The controls
u = np.zeros((Nvars,1,Nt))
yt = np.array([0.0, 0.0, 0.0, omega])
kP = 0.1
kI = 0.0 #1.0e-9
kD = 0.0 #1.0e-8
integral = 0.0
timeDelay = 0.5*Nt
# The predictions
x_pred = np.zeros((Nvars,1,Nt))
P_pred = np.zeros((Nvars,Nvars,Nt))
# Simulating the dynamics and running the filter
x[:,:,0] = x0
z[:,:,0] = x0
x_pred[:,:,0] = x0
P_pred[:,:,0] = R
prev_error = 0.0
for t in range(0,Nt-1):
xt = x[:,0,t]
x_predt = x_pred[:,0,t]
# PID control
error = yt - x_predt
# P
uP = -kP * error
# I
if t > timeDelay:
integral += error * dt
uI = -kI * integral
else:
uI = np.zeros(Nvars)
# D
if t > 0:
uD = -kD * (error - prev_error)/dt
prev_error = error
else:
uD = np.zeros(Nvars)
u[:,0,t+1] = uP + uI + uD
# We only have two control variables, the others are not used
u_r = u[1,0,t+1]
u_phi = u[3,0,t+1]
# The state Jacobian (linearizes the eqns. of motion at time t+1)
F = np.array([[1.0, dt, 0.0, 0.0],
[-dt*xt[3]*xt[3] + dt*2.0*G*M/(xt[0]*xt[0]*xt[0]), 1.0, 0.0, -dt*2.0*xt[0]*xt[3]],
[0.0, 0.0, 1.0, dt],
[dt*2.0*xt[1]*xt[3]/(xt[0]*xt[0]) - dt*u_phi/(m*xt[0]*xt[0]), -dt*2.0*xt[3]/xt[0], 0.0, 1.0-dt*2.0*xt[1]/xt[0]]])
# The control Jacobian
B = np.zeros((Nvars,Nvars))
B[1,1] = dt/m * u_r
B[3,3] = dt/m * u_phi / xt[0]
# State and measurement updates
x[:,:,t+1] = np.dot(F, x[:,:,t]) + np.dot(B, u[:,:,t+1]) + np.dot(np.sqrt(Q), np.random.randn(Nvars,1))
z[:,:,t+1] = x[:,:,t+1] + np.dot(np.sqrt(R), np.random.randn(Nvars,1))
# Kalman filter state estimation
(x_pred[:,:,t+1], P_pred[:,:,t+1]) = kalman(x_pred[:,:,t], P_pred[:,:,t], z[:,:,t+1], F, Q, 1, R)
#print error
matplotlib.rcParams.update({'font.size': 34})
# Plotting the actual dynamics versus the Kalman Filter predictions (shown with error bars)
plt.figure(0)
plt.plot(time, (x[0,0,:])/R0, 'k', label='actual dynamics', linewidth=5.0)
plt.plot(time, (z[0,0,:])/R0, 'rD', label='measurements', markersize=6)
plt.errorbar(time, (x_pred[0,0,:])/R0, yerr=np.sqrt(P_pred[0,0,:])/R0, color='g',ecolor='g', label='predicted dynamics', linewidth=2.5)
plt.legend()
plt.title('Kalman Filter for Orbital Trajectory')
plt.xlabel('time')
plt.ylabel('$r/R_E$')
# Plotting the actual dynamics versus the Kalman Filter predictions (shown with error bars)
plt.figure(1)
plt.plot(time, x[1,0,:], 'k', label='actual dynamics', linewidth=5.0)
plt.plot(time, z[1,0,:], 'rD', label='measurements', markersize=6)
plt.errorbar(time, x_pred[1,0,:], yerr=np.sqrt(P_pred[1,0,:]), color='g',ecolor='g', label='predicted dynamics', linewidth=2.5)
plt.legend()
plt.title('Kalman Filter for Orbital Trajectory')
plt.xlabel('time')
plt.ylabel('$v_r$')
plt.show()