Kalman filters are notoriously difficult to comprehend. I’ve come to the conclusion that, for people like me, it’s mostly due to the notation used in their description. For those of you like me, I offer the following, fancy-equation-free explanation of the one-dimensional Kalman filter.
I prefer code to equations. Those of you like me know that mathematicians and physicists are fond of their single Greek letter variables, subscripts, superscripts, and the other secret scrawls they use to exchange knowledge of simple concepts. This explanation is in Python and uses only simple math. If simple addition, subtraction, multiplication, and division scare you away, then I can’t help you.
The fanciest thing we’ll be talking about are Gaussian (aka normal) distributions. If those are unfamiliar to you, then you’ll need to start by learning about those someplace else.
Instead of jumping directly into implementing a Kalman filter, I’m going to let it sneak up on you.
We’ll start with a trivial filter. This filter assumes that we can perfectly measure our state. For example, imagine you’re modeling the position of a robot moving along a number line. If you have a sensor that tells you the exact position of the robot at all times, your filter may look like this:
#!/usr/bin/python3.2
class TrivialFilter(object):
def step(self, measurement):
print('measurement: %s' % measurement)
return measurement
if __name__ == '__main__':
trivial_filter = TrivialFilter()
state = 0
step = 0
while True:
print(('Step: %d' % step).center(80, '-'))
state = trivial_filter.step(state)
step += 1Awesome, right? A more realistic example would involve noisy measurements.
We’ll start with an intuitive filter that I’ll call a prediction filter. The prediction filter isn’t much of a filter at all. It takes the previous state, modifies it in a controlled way, and returns the new state. For example, imagine you’re modeling the position of a robot moving along a number line at a constant speed. The position of the robot will change in a predictable way at each time step like so: position += speed * time_between_steps
Let’s implement the PredictionFilter in Python. In this example, the state is the robot’s position, the control is the robot’s speed, and the time between steps is 1
.
xxxxx instead, start with a stupid filter that actually just measures the real value, then add noise to that value and compensate for it with a known control value
#!/usr/bin/python3.2
class PredictionFilter(object):
def step(self, previous_state, control):
state = previous_state + control
print('previous_state: %f' % previous_state)
print('state: %s' % state)
print('control: %s' % control)
return state
if __name__ == '__main__':
prediction_filter = PredictionFilter()
position = 0
speed = 1
step = 0
while True:
print(('Step: %d' % step).center(80, '-'))
position = prediction_filter.step(position, speed)
step += 1And the output:
$ ./predict.py
------------------------------------Step: 0-------------------------------------
previous_state: 0.000000
state: 1
control: 1
------------------------------------Step: 1-------------------------------------
previous_state: 1.000000
state: 2
control: 1
------------------------------------Step: 2-------------------------------------
previous_state: 2.000000
state: 3
control: 1
------------------------------------Step: 3-------------------------------------
previous_state: 3.000000
state: 4
control: 1
------------------------------------Step: 4-------------------------------------
previous_state: 4.000000
state: 5
control: 1
------------------------------------Step: 5-------------------------------------
previous_state: 5.000000
state: 6
control: 1Awesome, it works! How do we know it works? Well, because it makes sense. If the robot is at position 5 and increases its position by 1 then it is at position 6. If the control (the robot’s speed) changes unpredictably, our filter stops working very well. Let’s change it a bit and see what happens.
import random
...
while True:
print(('Step: %d' % step).center(80, '-'))
position = prediction_filter.step(position, speed + random.gauss(0, 1))
step += 1You can see we’ve added a bit of Gaussian noise to the speed. Let’s see what it does to the output.
$ ./predict.py
------------------------------------Step: 0-------------------------------------
previous_state: 0.000000
state: 3.0003131009691333
control: 3.0003131009691333
------------------------------------Step: 1-------------------------------------
previous_state: 3.000313
state: 3.3164885488423366
control: 0.3161754478732032
------------------------------------Step: 2-------------------------------------
previous_state: 3.316489
state: 2.2411313976400775
control: -1.075357151202259
------------------------------------Step: 3-------------------------------------
previous_state: 2.241131
state: 2.4912475435214385
control: 0.250116145881361
------------------------------------Step: 4-------------------------------------
previous_state: 2.491248
state: 4.127774023303738
control: 1.6365264797823
------------------------------------Step: 5-------------------------------------
previous_state: 4.127774
state: 4.232442829737389
control: 0.1046688064336504Oh noes! At step 5, we should be at position 6 and we’re somewhere between 4 and 5. Bummer. We need a better filter.
import random
import sys
import time
class AlphaBetaFilter(object):
def __init__(self, alpha, beta):
self.alpha = alpha
self.beta = beta
def step(self, previous_state, previous_control, measurement):
predicted_state = previous_state + previous_control
innovation = measurement - predicted_state
state = predicted_state + self.alpha * innovation
control = previous_control + self.beta * innovation
print('measurement: %f' % measurement)
print('predicted_state: %f' % predicted_state)
print('innovation: %f' % innovation)
print('state: %s' % state)
print('control: %s' % control)
return state, control
class Gaussian(object):
def __init__(self, mean, variance):
self.mean = mean
self.variance = variance
def __str__(self):
return '(%f, %f)' % (self.mean, self.variance)
class KalmanFilter(object):
def step(self, previous_belief, control, measurement):
predicted_mean = previous_belief.mean + control.mean
predicted_variance = previous_belief.variance + control.variance
kalman_gain = predicted_variance / (predicted_variance +
measurement.variance)
innovation = measurement.mean - predicted_mean
mean = predicted_mean + kalman_gain * innovation
variance = predicted_variance * (1 - kalman_gain)
belief = Gaussian(mean, variance)
print('control: %s' % control)
print('measurement: %s' % measurement)
print('predicted_mean: %f' % predicted_mean)
print('predicted_variance: %f' % predicted_variance)
print('kalman_gain: %f' % kalman_gain)
print('innovation: %f' % innovation)
print('belief: %s' % belief)
return belief
if __name__ == '__main__':
if sys.argv[1] == 'alphabeta':
abf = AlphaBetaFilter(0.01, 0.0001)
state = 0
control = 0
step = 0
while True:
print(('Step: %d' % step).center(80, '-'))
state, control = abf.step(state, control, random.gauss(2, 1))
#time.sleep(0.5)
step += 1
if sys.argv[1] == 'kalman':
kf = KalmanFilter()
belief = Gaussian(0, 10)
step = 0
while True:
print(('Step: %d' % step).center(80, '-'))
belief = kf.step(belief, Gaussian(0, 0), Gaussian(random.gauss(2, 1), 1))
#time.sleep(0.5)
step += 1