One-Dimensional Kalman Filters

This section is adapted from Kalman and Bayesian Filters in Python by Roger Labbe. You already know the discrete Bayes filter and how a Gaussian describes uncertainty. In this section you combine the two and build a Kalman filter. You develop it step by step, by reasoning about a concrete tracking problem, before you write down the general equations. “One-dimensional” means that the filter tracks a single state variable, such as a position along a hallway. The multivariate form of the filter tracks many variables at once, such as position, velocity and acceleration. Its math is harder, and it hides how simple the core idea is. The one-dimensional filter shows that idea plainly: a Kalman filter is a Bayesian filter whose beliefs are Gaussians. You will represent uncertainty with Gaussian distributions and run a recursive cycle of prediction and update. You will see how the Kalman gain weights the prediction against the measurement. The examples estimate a position from noisy sensor readings and a voltage from a noisy thermometer.

%config InlineBackend.figure_formats = ["svg"]

import math
from collections import namedtuple

import filterpy.kalman as kf
import filterpy.stats as stats
import matplotlib.pyplot as plt
import numpy as np
from cycler import cycler

np.set_printoptions(linewidth=60)

plt.rcParams.update(
    {
        "figure.figsize": (8, 4),
        "axes.prop_cycle": cycler(
            color=[
                "#0072B2",
                "#D55E00",
                "#009E73",
                "#cb77a6",
                "#c28600",
                "#1d99e0",
                "#7f7f7f",
            ]
        ),
        "axes.grid": True,
        "grid.alpha": 0.3,
        "svg.fonttype": "path",
    }
)

# named colors from the palette above
BLUE, VERMILLION, GREEN, PINK, GOLD, SKY, GRAY = (
    "#0072B2",
    "#D55E00",
    "#009E73",
    "#cb77a6",
    "#c28600",
    "#1d99e0",
    "#7f7f7f",
)

np.random.seed(13)

Tracking a dog in a hallway

As in the discrete Bayes filter section, you track a dog that moves along a long hallway. A sensor reports the dog’s distance from the left end of the hallway in meters. A reading of 23.4 means the dog is 23.4 m from the left end. The sensor is not perfect. A reading of 23.4 could come from a dog at 23.7 or at 23.0. It is very unlikely to come from a dog at 47.6. Tests show that the errors are small and spread evenly on both sides of the true position: a dog at 23 m is as likely to read 22.9 as 23.1. A Gaussian is a natural model for this error. You also predict that the dog moves. That prediction is not perfect either. Sometimes it overshoots and sometimes it undershoots, and small misses are more likely than large ones. A Gaussian models this error too.

Beliefs as Gaussians

You can express your belief about the dog’s position with a Gaussian. Say you believe the dog is at 10 m, and the variance of that belief is 1 m

^2

. You write this belief as

\mathcal{N}(10,\, 1)

. Its probability density function (pdf) is

f(x;\, \mu, \sigma^2) = \frac{1}{\sqrt{2\pi\sigma^2}}\exp\left(-\frac{(x-\mu)^2}{2\sigma^2}\right)

xs = np.linspace(4, 16, 500)
belief_pdf = stats.gaussian(
    xs, mean=10.0, var=1.0, normed=False
)

The plot shows your uncertainty about the dog’s position. The belief is fairly loose. The most likely position is 10 m, but anything from about 9 m to 11 m is also quite likely. Now assume the dog stands still and you query the sensor again. This time it returns 10.2 m. Can this extra reading improve your estimate? Intuition says yes. If you read the sensor 500 times and every value fell between 8 and 12, centered on 10, you would be very confident that the dog is near 10. Here are 500 draws from

\mathcal N(10, 1)

readings = np.random.randn(500) * 1.0 + 10.0
print(f"Mean of readings is {np.mean(readings):.3f}")

Mean of readings is 9.966

No dog moves like this, but noisy sensor data looks exactly like this. The mean of the readings is almost exactly 10. If the dog is standing still, you conclude that it is at 10 m. Each reading has a variance of 1 m

^2

, but the average of 500 readings is far more certain: its variance is

1/500 = 0.002

^2

Tracking with Gaussian probabilities

The discrete Bayes filter used a histogram of probabilities to track the dog. Each bin of the histogram is a position, and its value is the probability that the dog is there. Tracking was a cycle of predictions and updates:

\begin{aligned} \bar {\mathbf x} &= \mathbf x \ast f_{\mathbf x}(\bullet)\, \, &\text{Predict} \\ \mathbf x &= \mathcal L \cdot \bar{\mathbf x}\, \, &\text{Update} \end{aligned}

Here

\bar{\mathbf x}

is the prior,

\mathcal L

is the likelihood

p(\mathbf z \mid \mathbf x)

of the measurement given the state,

f_{\mathbf x}(\bullet)

is the process model, and

\ast

denotes convolution.

\mathbf x

is bold because it is a histogram of numbers, that is, a vector. This method works, but its histograms can imply that the dog is in several places at once. It is also slow for large problems. Can you replace the histogram

\mathbf x

with a Gaussian

\mathcal N(\mu, \sigma^2)

? Yes. A Gaussian is a single pair of numbers, and it can stand in for an entire histogram of probabilities:

bin_centers = np.arange(4, 16.01, 0.5)
bin_probs = (
    stats.gaussian(
        bin_centers, mean=10.0, var=1.5**2, normed=False
    )
    * 0.5
)  # mass per 0.5 m bin
curve_x = np.linspace(4, 16, 400)
curve_y = stats.gaussian(
    curve_x, mean=10.0, var=1.5**2, normed=False
)

The Gaussian replaces hundreds or thousands of numbers with a single pair:

x = \mathcal N(\mu, \sigma^2)

. The tails of the Gaussian extend to infinity on both sides, so it covers arbitrarily many bins of the histogram. If it represents your belief about the dog, this one Gaussian covers the whole hallway. The dog is most likely near 10, but it could be at 8 or 14, or, with vanishingly small probability, at

10^{80}

. In this section you replace histograms with Gaussians:

\begin{array}{l|l|c} \text{discrete Bayes} & \text{Gaussian} & \text{Step}\\ \hline \bar {\mathbf x} = \mathbf x \ast f(\mathbf x) & \bar {x}_\mathcal{N} = x_\mathcal{N} \, \oplus \, f_{x_\mathcal{N}}(\bullet) & \text{Predict} \\ \mathbf x = \|\mathcal L \bar{\mathbf x}\| & x_\mathcal{N} = L \, \otimes \, \bar{x}_\mathcal{N} & \text{Update} \end{array}

Here

\oplus

and

\otimes

stand for operations on Gaussians that you have not defined yet. The subscript

\mathcal N

marks a Gaussian. Later equations drop it. The discrete Bayes filter used convolution for the prediction. Convolution applies the total probability theorem as a sum, so perhaps you can add the Gaussians. The update multiplied the likelihood into the prior, so perhaps you can multiply the Gaussians:

\begin{aligned} \bar x &\stackrel{?}{=} x + f_x(\bullet) \\ x &\stackrel{?}{=} \mathcal L \cdot \bar x \end{aligned}

This works only if the sum and the product of two Gaussians are again Gaussians. Otherwise

x

would stop being Gaussian after the first step, and the scheme would fall apart.

Predictions with Gaussians

Newton’s equation of motion gives the current position from the previous position and the velocity:

\begin{aligned}\bar{x}_k &= x_{k-1} + v_k \Delta t \\ &= x_{k-1} + f_x\end{aligned}

The notation

f_x

replaces

f_x(\bullet)

to keep the equations short. If the dog is at 10 m, its velocity is 15 m/s, and the time step is 2 seconds long, then

\begin{aligned} f_x &= v\Delta t = 15\cdot 2\\ \bar{x}_k &= 10 + (15\cdot 2) = 40 \end{aligned}

You are uncertain about both the current position and the velocity, so a single number will not do. You need to express that uncertainty with Gaussians. Position is easy. You define

x

as a Gaussian. If you think the dog is at 10 m with a standard deviation of 0.2 m, then

x=\mathcal N(10, 0.2^2)

. The movement is a Gaussian too. If the dog’s velocity is 15 m/s, the time step is 1 second, and the standard deviation of your uncertainty is 0.7 m/s, then

f_x = \mathcal N (15, 0.7^2)

. The prior is

\bar x = x + f_x

The sum of two independent Gaussians is another Gaussian, with

\begin{gathered} \mu = \mu_1 + \mu_2 \\ \sigma^2 = \sigma^2_1 + \sigma^2_2 \end{gathered}

For the numbers above,

\begin{gathered} x=\mathcal N(10, 0.2^2)\\ f_x = \mathcal N (15, 0.7^2) \end{gathered}

and their sum is

\begin{aligned}\bar \mu &= \mu_x + \mu_{f_x} = 10 + 15 &&= 25 \\ \bar\sigma^2 &= \sigma_x^2 + \sigma_{f_x}^2 = 0.2^2 + 0.7^2 &&= 0.53\end{aligned}

The predicted position is the previous position plus the movement, which makes sense. The variance is harder to build intuition for. Recall that the predict step of the discrete Bayes filter always lost information. You do not know exactly how the dog moves, so your confidence should drop and the variance should grow.

\sigma_{f_x}^2

is the uncertainty that the imperfect motion prediction adds, so you add it to the uncertainty you already have. In code, a Gaussian is a pair of numbers: a mean and a variance. Giving the two fields names lets you write g.mean and g.var instead of g[0] and g[1].

gaussian = namedtuple("Gaussian", ["mean", "var"])
gaussian.__repr__ = (
    lambda s: "𝒩(μ={:.3f}, 𝜎²={:.3f})".format(s[0], s[1])
)

Create and print two Gaussians:

g1 = gaussian(3.4, 10.1)
g2 = gaussian(mean=4.5, var=0.2**2)
print(g1)
print(g2)

𝒩(μ=3.400, 𝜎²=10.100)
𝒩(μ=4.500, 𝜎²=0.040)

You can access the mean and the variance by index or by name:

g1.mean, g1[0], g1[1], g1.var

(3.4, 3.4, 10.1, 10.1)

Here is the predict function. Both pos and movement are Gaussians:

def predict(pos, movement):
    return gaussian(
        pos.mean + movement.mean, pos.var + movement.var
    )

Test it. What is the prior if the initial position is

\mathcal N(10, 0.2^2)

and the movement is

\mathcal N (15, 0.7^2)

pos = gaussian(10.0, 0.2**2)
move = gaussian(15.0, 0.7**2)
predict(pos, move)

𝒩(μ=25.000, 𝜎²=0.530)

The prior says the dog is at 25 m with a variance of 0.53 m

^2

, which matches the hand computation.

Updates with Gaussians

The discrete Bayes filter stores its belief about the dog’s position in a histogram of probabilities. That distribution is discrete and can be multimodal: it can express a strong belief that the dog is in two places at once. You now want to replace the histogram with a Gaussian. The discrete Bayes filter computed the posterior with this code:

def update(likelihood, prior):
    posterior = likelihood * prior
    return normalize(posterior)

which implements the equation

x = \| \mathcal L\bar x \|

You have just seen that the prior can be a Gaussian. What about the likelihood? The likelihood is the probability of the measurement given the current state. A measurement can also be a Gaussian. For example, if the sensor says the dog is at 23 m with a standard deviation of 0.4 m, the likelihood is

z = \mathcal N (23, 0.16)

. Both the likelihood and the prior are Gaussians. Can you multiply them, and is the product another Gaussian? Yes to the first, and almost to the second. The product of two Gaussian densities is proportional to another Gaussian, with

\begin{aligned} \mu &= \frac{\sigma_1^2 \mu_2 + \sigma_2^2 \mu_1} {\sigma_1^2 + \sigma_2^2}, \\ \sigma^2 &= \frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2} \end{aligned}

Several things follow at once. After normalization, the product is a Gaussian. If one input is the likelihood and the other is the prior, the posterior mean is a weighted sum of the prior mean and the measurement. The posterior variance combines the two variances. And the variances do not depend on the means at all. In Bayesian terms:

\begin{aligned} \mathcal N(\mu, \sigma^2) &= \| prior \cdot likelihood \|\\ &= \| \mathcal{N}(\bar\mu, \bar\sigma^2)\cdot \mathcal{N}(\mu_z, \sigma_z^2) \|\\ &= \mathcal N\left(\frac{\bar\sigma^2 \mu_z + \sigma_z^2 \bar\mu}{\bar\sigma^2 + \sigma_z^2},\frac{\bar\sigma^2\sigma_z^2}{\bar\sigma^2 + \sigma_z^2}\right) \end{aligned}

With a function gaussian_multiply() that implements this product, the update step of the filter is one line:

def gaussian_multiply(g1, g2):
    mean = (g1.var * g2.mean + g2.var * g1.mean) / (
        g1.var + g2.var
    )
    variance = (g1.var * g2.var) / (g1.var + g2.var)
    return gaussian(mean, variance)


def update(prior, likelihood):
    posterior = gaussian_multiply(likelihood, prior)
    return posterior


# test the update function
predicted_pos = gaussian(10.0, 0.2**2)
measured_pos = gaussian(11.0, 0.1**2)
estimated_pos = update(predicted_pos, measured_pos)
estimated_pos

𝒩(μ=10.800, 𝜎²=0.008)

The function names stay general on purpose. The same filter works for any one-dimensional problem, such as a dog in a hallway or a thermometer reading, and the name update(prior, likelihood) keeps in view that the update multiplies the likelihood by the prior. You now have most of a filter. The claim that multiplying Gaussians performs the update deserves a closer look, so the next part works through a few products.

Understanding Gaussian multiplication

Consider

\mathcal{N}(10,\, 1) \times \mathcal{N}(10,\, 1)

. Before you look at the result, try to predict it. What is the new mean? Is the curve wider, narrower, or the same as

\mathcal{N}(10,\, 1)

z = gaussian(10.0, 1.0)  # Gaussian N(10, 1)
product = gaussian_multiply(z, z)
print(product)

xs = np.arange(5, 15, 0.05)

𝒩(μ=10.000, 𝜎²=0.500)

The product is taller and narrower than the original Gaussian, and its mean is unchanged. Think of the two Gaussians as two measurements. If you measure twice and get 10 meters both times, you conclude that the length is close to 10 meters. It would make no sense to conclude 11 or 9.5. You are also more confident with two measurements than with one, so the variance of the result is smaller. Gaussian multiplication is a mathematical model of “measure twice, cut once”. You are unlikely to get the same measurement twice in a row. Now consider

\mathcal{N}(10.2,\, 1) \times \mathcal{N}(9.7,\, 1)

. Predict the result, then look at the plot.

z1 = gaussian(10.2, 1)
z2 = gaussian(9.7, 1)
print(gaussian_multiply(z1, z2))

𝒩(μ=9.950, 𝜎²=0.500)

Suppose two people measure the distance from a table to a wall. One gets 10.2 meters and the other gets 9.7 meters. If you trust them equally, your best guess is the average, 9.95 meters. You should also be a bit more confident about 9.95 than about either single reading, because two readings near the same value make it less likely that either is wildly wrong. You might object that one person probably made a mistake, so the true distance is 10.2 or 9.7, but surely not 9.95. That is possible. But both measurements are noisy, and you have no reason to think one of them is noise-free or grossly wrong. Given all the information you have, the best estimate is 9.95. The update step of the Kalman filter does not combine two measurements. It combines one measurement with the prior, your estimate before the measurement arrived. The math is the same either way. Now try an inaccurate prior,

\mathcal N(8.5, 1.5)

, with a more accurate measurement,

\mathcal N(10.2, 0.5)

. “Accurate” here means that the sensor variance is smaller than the prior variance, not that you somehow know the dog is closer to 10.2 than to 8.5. The right panel reverses the roles: an accurate prior

\mathcal N(8.5, 0.5)

and an inaccurate measurement

\mathcal N(10.2, 1.5)

In both cases the product is taller than either input. You have added information, so the variance shrinks. The product also lies much closer to the input with the smaller variance. You have more confidence in that value, so it gets more weight. The next figure varies the means and variances of the two Gaussians over a few representative values. Watch two things: the product always sits between the two means, closer to the narrower input, and the product is always narrower than both inputs.

product_cases = [
    (gaussian(7.5, 1.0), gaussian(12.0, 1.0)),
    (gaussian(7.5, 0.3), gaussian(12.0, 1.8)),
    (gaussian(7.5, 1.8), gaussian(12.0, 0.3)),
    (gaussian(9.5, 0.5), gaussian(10.5, 0.5)),
]

Your first Kalman filter

You have predict() and update(). To try them you need a simulated dog and its measurements. The DogSimulation class below moves the dog by its velocity plus Gaussian process noise at each time step. It then returns the true position plus Gaussian sensor noise.

class DogSimulation:
    def __init__(
        self,
        x0=0.0,
        velocity=1.0,
        measurement_var=0.0,
        process_var=0.0,
    ):
        self.x = x0
        self.velocity = velocity
        self.meas_std = math.sqrt(measurement_var)
        self.process_std = math.sqrt(process_var)

    def move(self, dt=1.0):
        # move by the velocity, perturbed by process noise
        dx = (
            self.velocity
            + np.random.randn() * self.process_std
        )
        self.x += dx * dt

    def sense_position(self):
        # true position corrupted by sensor noise
        return self.x + np.random.randn() * self.meas_std

    def move_and_sense(self):
        self.move()
        return self.sense_position()

The next cell sets up the problem. It defines the means and variances and generates ten simulated measurements.

np.random.seed(13)

process_var = 1.0  # variance in the dog's movement
sensor_var = 2.0  # variance in the sensor

x = gaussian(0.0, 20.0**2)  # dog's position, N(0, 20**2)
velocity = 1
dt = 1.0  # time step in seconds
process_model = gaussian(
    velocity * dt, process_var
)  # displacement to add to x

# simulate dog and get measurements
dog = DogSimulation(
    x0=x.mean,
    velocity=process_model.mean,
    measurement_var=sensor_var,
    process_var=process_model.var,
)

# create list of measurements
zs = [dog.move_and_sense() for _ in range(10)]

And here is the Kalman filter. It prints the prior, the measurement and the posterior at each step.

priors, posteriors = [], []

print("PREDICT\t\t\tUPDATE")
print("     x      var\t\t  z\t    x      var")

# perform Kalman filter on measurement z
for z in zs:
    prior = predict(x, process_model)
    likelihood = gaussian(z, sensor_var)
    x = update(prior, likelihood)

    priors.append(prior)
    posteriors.append(x)
    pm, pv, xm, xv = prior.mean, prior.var, x.mean, x.var
    print(
        f"{pm:7.3f} {pv:8.3f}\t{z:7.3f}\t{xm:7.3f} {xv:8.3f}"
    )

print()
print("final estimate:        {:10.3f}".format(x.mean))
print("actual final position: {:10.3f}".format(dog.x))

PREDICT			UPDATE
     x      var		  z	    x      var
000  401.000	  1.354	  1.352    1.990
352    2.990	  1.882	  2.070    1.198
070    2.198	  4.341	  3.736    1.047
736    2.047	  7.156	  5.960    1.012
960    2.012	  6.939	  6.949    1.003
949    2.003	  6.844	  7.396    1.001
396    2.001	  9.847	  9.122    1.000
122    2.000	 12.553	 11.338    1.000
338    2.000	 16.273	 14.305    1.000
305    2.000	 14.800	 15.053    1.000

final estimate:            15.053
actual final position:     14.838

The figure shows every step of that run. Each prediction (the prior) is a triangle. After the prediction the filter receives the next measurement, drawn as a circle. A thin vertical line joins the two. The filter then forms its estimate somewhere on that line, and the estimates are joined by the solid line.

At each iteration of the loop the filter forms a prior, takes a measurement, turns the measurement into a likelihood, and folds the likelihood into the prior. The filter estimate always lies between the measurement and the prediction. It makes no sense to choose a value outside the two. If you predict that you are at 10 but measure 9, it would be foolish to decide that you are at 8, or at 11.

Code walkthrough

process_var = 1.
sensor_var = 2.

These are the variances of the process model and of the sensor. The sensor variance is how much each measurement scatters. The process variance is how much error the process model has. You predict that the dog moves forward one meter at each time step, but dogs rarely do what you expect. A hill or the smell of a squirrel changes its progress. A robot that follows digital commands would do much better, and its variance might be

\sigma^2=0.05

. These are not magic numbers: the square root of each variance is a distance error in meters. A Kalman filter runs with any numbers you plug in, but if they do not reflect reality, it performs poorly.

x = gaussian(0., 20.**2)

This is the dog’s initial position as a Gaussian. The mean is 0 m and the variance is 400 m

^2

, a standard deviation of 20 m. About 99.7% of a Gaussian’s mass lies within

\pm3\sigma

of the mean, so this says “I am about 99.7% sure the dog is within 60 m of 0”.

process_model = gaussian(velocity, process_var)

This is the process model, your description of how the dog moves. Where does the velocity come from? Treat it as a prediction, or as the reading of a second velocity sensor. On a robot it would be the control input. The multivariate Kalman filter handles the case where you have no velocity sensor or input, so accept this simplification for now. Next the code builds the simulation and creates 10 measurements:

dog = DogSimulation(
    x0=x.mean,
    velocity=process_model.mean,
    measurement_var=sensor_var,
    process_var=process_model.var)

zs = [dog.move_and_sense() for _ in range(10)]

Then it enters the predict and update loop:

for z in zs:
    prior = predict(x, process_model)
    likelihood = gaussian(z, sensor_var)
    x = update(prior, likelihood)

The first time through the loop, prior is

\mathcal N(1.0, 401.0)

, as the printed table shows. After the prediction you believe the dog is at 1.0, and the variance has grown from 400 to 401. The prediction always increases the variance, because it loses information. Then the update combines this prior with the first measurement. The prior variance of 401 m

^2

means you have almost no confidence in the prior. So the posterior mean lands very close to the measurement. The posterior variance is 1.990 m

^2

, down from 401 m

^2

. The sensor variance is only 2.0 m

^2

, so you trust it far more than the prior. The prior still carries a little information, so the posterior variance is slightly smaller than 2.0. The loop then alternates predict() and update(). By the end, the estimate is close to the true final position, and the posterior variance has converged to 1.0 m

^2

. The filtering itself takes three lines:

prior = predict(x, process_model)
likelihood = gaussian(z, sensor_var)
x = update(prior, likelihood)

Without the predict and update functions the loop would read:

for z in zs:
    # predict
    dx = velocity*dt
    pos = pos + dx
    var = var + process_var

    # update
    pos  = (var*z + sensor_var*pos) / (var + sensor_var)
    var = (var * sensor_var) / (var + sensor_var)

Five lines of simple arithmetic implement the whole filter. The run above had only 10 steps so that the printed table stays short. The next run has 25 steps and larger process and sensor variances, which makes the effects easier to see. The shaded band shows one standard deviation of the prior around the estimate. The cell also prints the posterior variance at each of the first ten steps.

process_var = 2.0
sensor_var = 4.5
x = gaussian(0.0, 400.0)
process_model = gaussian(1.0, process_var)
N = 25

dog = DogSimulation(
    x.mean, process_model.mean, sensor_var, process_var
)
zs = [dog.move_and_sense() for _ in range(N)]

xs, priors = np.zeros((N, 2)), np.zeros((N, 2))
for i, z in enumerate(zs):
    prior = predict(x, process_model)
    x = update(prior, gaussian(z, sensor_var))
    priors[i] = prior
    xs[i] = x

print("posterior variance, first 10 steps:")
print(np.array2string(xs[:10, 1], precision=4))

posterior variance, first 10 steps:
[4.4502 2.6507 2.2871 2.1955 2.1712 2.1647 2.1629 2.1625
 2.1623 2.1623]

The variance converges to 2.1623 m

^2

within about nine steps. You can check this by hand. At steady state the posterior variance

s

satisfies

s = \frac{(s+Q)R}{s+Q+R}

with

Q=2

and

R=4.5

, which gives

s^2 + 2s - 9 = 0

and

s = \sqrt{10} - 1 \approx 2.1623

. That is a standard deviation of

\sigma=1.47

m, compared with the sensor’s

\sigma=2.12

m. The first few estimates are uncertain because you knew little about the initial position. The filter quickly converges to an estimate with lower variance than the sensor. This is a complete Kalman filter. If you have read the literature, you may be surprised, because it looks nothing like the pages of matrix equations found there. As long as you focus on using the equations rather than deriving them, the topic is approachable. Beliefs are Gaussians, and they improve over time because each new measurement adds data.

Exercise: modify variance values

Change process_var and sensor_var and note the effect on the filter and on its variance. Which has the larger effect on the converged variance? For example, which of these settings gives the smaller variance?

process_var = 40
sensor_var = 2

process_var = 2
sensor_var = 40

Solution

The cell below runs the filter with both settings on the same random seed and reports the converged posterior variance.

def run_dog_filter(
    process_var,
    sensor_var,
    N=25,
    x0=gaussian(0.0, 400.0),
    velocity=1.0,
    seed=3,
):
    np.random.seed(seed)
    dog = DogSimulation(
        0.0, velocity, sensor_var, process_var
    )
    zs = np.array([dog.move_and_sense() for _ in range(N)])
    x, process_model = x0, gaussian(velocity, process_var)
    est = np.zeros((N, 2))
    for i, z in enumerate(zs):
        x = update(
            predict(x, process_model),
            gaussian(z, sensor_var),
        )
        est[i] = x
    return zs, est


variance_cases = [(40.0, 2.0), (2.0, 40.0)]
variance_runs = [
    run_dog_filter(q, r) for q, r in variance_cases
]
for (q, r), (_, est) in zip(variance_cases, variance_runs):
    var = est[-1, 1]
    print(
        f"Q={q:4.0f}, R={r:4.0f}: final variance {var:.3f}"
    )

Q=  40, R=   2: final variance 1.909
Q=   2, R=  40: final variance 8.000

The first setting gives the smaller variance (about 1.9 m

^2

against 8.0 m

^2

). The posterior variance is always below the sensor variance, so a precise sensor bounds it tightly even when the process is very noisy. With a large process variance the filter trusts each new measurement almost completely, and the estimate follows the measurements closely. With a large sensor variance the filter leans on its prediction and produces a smoother track.

The belief over time

The next figure draws the posterior Gaussian of the 25-step run at several time steps. At the first step the belief is low and wide, so the filter moves its estimate a long way toward the measurement. As the steps go by, the Gaussian becomes taller and narrower, and the filter’s output settles into a nearly straight line. After the variance converges, a single noisy measurement moves the estimate much less than the first measurement did.

Kalman gain

The filter works. Now return to the math to see why. The posterior

x

is the likelihood times the prior (

\mathcal L \bar x

), and both are Gaussians. So the mean of the posterior is

\mu=\frac{\bar\sigma^2\, \mu_z + \sigma_z^2 \, \bar\mu} {\bar\sigma^2 + \sigma_z^2}

where the subscript

z

denotes the measurement. You can rewrite this as

\mu = \left( \frac{\bar\sigma^2}{\bar\sigma^2 + \sigma_z^2}\right) \mu_z + \left(\frac{\sigma_z^2}{\bar\sigma^2 + \sigma_z^2}\right)\bar\mu

In this form the measurement and the prior are each scaled by a weight:

\mu = W_1 \mu_z + W_2 \bar\mu

The weights sum to one because the denominator normalizes them. Call the first weight

K=W_1

\begin{aligned} \mu &= K \mu_z + (1-K) \bar\mu\\ &= \bar\mu + K(\mu_z - \bar\mu) \end{aligned}

where

K = \frac {\bar\sigma^2}{\bar\sigma^2 + \sigma_z^2}

K

is the Kalman gain, the heart of the Kalman filter. It is a scaling term that picks a value partway between

\mu_z

and

\bar\mu

. Work a few examples. If the measurement is nine times more accurate than the prior, then

\bar\sigma^2 = 9\sigma_z^2

, and

\begin{aligned} \mu&=\frac{9 \sigma_z^2 \mu_z + \sigma_z^2\, \bar\mu} {9 \sigma_z^2 + \sigma_z^2} \\ &= \left(\frac{9}{10}\right) \mu_z + \left(\frac{1}{10}\right) \bar\mu \end{aligned}

K = \frac 9 {10}

: the posterior takes nine tenths of the measurement and one tenth of the prior. If the measurement and the prior are equally accurate, then

\bar\sigma^2 = \sigma_z^2

and

\begin{aligned} \mu&=\frac{\sigma_z^2\, (\bar\mu + \mu_z)}{2\sigma_z^2} \\ &= \left(\frac{1}{2}\right)\bar\mu + \left(\frac{1}{2}\right)\mu_z \end{aligned}

which is the average of the two means. Taking the average of two equally accurate values makes sense. You can also write the variance in terms of the Kalman gain:

\begin{aligned} \sigma^2 &= \frac{\bar\sigma^2 \sigma_z^2 } {\bar\sigma^2 + \sigma_z^2} \\ &= K\sigma_z^2 \\ &= (1-K)\bar\sigma^2 \end{aligned}

The chart below shows one step of the filter. The prediction moves the previous estimate forward. The residual is the difference between the measurement and the prediction. The new estimate lies on the residual, a fraction

K

of the way from the prediction to the measurement.

The Kalman gain

K

is a scale factor that chooses a value along the residual. This leads to an alternative but equivalent implementation of update() and predict():

def update(prior, measurement):
    x, P = prior  # mean and variance of prior
    z, R = measurement  # mean and variance of measurement

    y = z - x  # residual
    K = P / (P + R)  # Kalman gain

    x = x + K * y  # posterior
    P = (1 - K) * P  # posterior variance
    return gaussian(x, P)


def predict(posterior, movement):
    x, P = posterior  # mean and variance of posterior
    dx, Q = movement  # mean and variance of movement
    x = x + dx
    P = P + Q
    return gaussian(x, P)

Most books and papers present the Kalman filter in this form. Other derivations lead to it naturally, and the multivariate Kalman filter equations look almost exactly like these. So you need to know them. The names are standard. In the literature

R

is almost always the measurement noise,

Q

the process noise, and

P

the variance of the state. Using

z

for the measurement is common, though not universal. Almost every book and paper you read uses these names. This form is also a useful way to think about filtering. It emphasizes the residual

y = \mu_z - \bar\mu

, the Kalman gain as a ratio of uncertainties

K = P/(P+R)

, and the posterior as the prior plus

Ky

. The Bayesian view is hidden in this form, and so is the fact that the update multiplies the likelihood by the prior. Both views are equivalent because the math is identical. The Bayesian view gives an intuitive grasp of the probabilistic reasoning. The gain form corresponds to the orthogonal projection approach, which is the derivation Kalman himself used.

Full description of the algorithm

The Kalman filter makes a prediction, takes a measurement, and forms a new estimate somewhere between the two, exactly as in the residual chart above. Every filter in this family implements the same algorithm with different mathematical details. The math can become hard, but the idea is easy to state. Look past the details of a specific filter’s equations and ask what they compute and why. There are many filters. Their choice of math affects the quality of the results and which problems they can represent, but not the underlying idea. Here is the generic algorithm. First, initialize the state of the filter and your belief in that state. Then repeat two steps. The predict step:

Use the system behavior to predict the state at the next time step.
Adjust the belief to account for the uncertainty in the prediction.

The update step:

Get a measurement and a belief about its accuracy.
Compute the residual between the estimated state and the measurement.
Compute a scaling factor based on whether the measurement or the prediction is more accurate.
Set the state between the prediction and the measurement, according to the scaling factor.
Update the belief in the state based on how certain you are of the measurement.

You will be hard pressed to find a Bayesian filter that does not fit this form. Some filters leave out parts, such as error in the prediction, and others compute each part in complicated ways, but they all do this. The equations of the univariate Kalman filter are as follows. For the predict step:

\begin{array}{|l|l|l|} \hline \text{Equation} & \text{Implementation} & \text{Kalman form}\\ \hline \bar x = x + f_x & \bar\mu = \mu + \mu_{f_x} & \bar x = x + dx\\ & \bar\sigma^2 = \sigma^2 + \sigma_{f_x}^2 & \bar P = P + Q\\ \hline \end{array}

For the update step:

\begin{array}{|l|l|l|} \hline \text{Equation} & \text{Implementation}& \text{Kalman form}\\ \hline x = \| \mathcal L\bar x\| & y = z - \bar\mu & y = z - \bar x\\ & K = \frac {\bar\sigma^2} {\bar\sigma^2 + \sigma_z^2} & K = \frac {\bar P}{\bar P+R}\\ & \mu = \bar \mu + Ky & x = \bar x + Ky\\ & \sigma^2 = \frac {\bar\sigma^2 \sigma_z^2} {\bar\sigma^2 + \sigma_z^2} & P = (1-K)\bar P\\ \hline \end{array}

Comparison with g-h and discrete Bayes filters

The three filters differ in how they model errors. A g-h filter blends a prediction and a measurement with fixed weights. It treats each measurement as a value with an error bar, as in this graph:

Sensor A returned 160 and sensor B returned 170. The bars are error bars: they show the possible range of error for each measurement. So the value A measures lies between 157 and 163, and the value B measures lies between 161 and 179. This is a uniform distribution. It assigns equal probability to every value in the range. Under this model sensor A is equally likely to read 157, 160 or 163, and any value outside the range has probability 0. You can model the same situation with Gaussians:

\mathcal{N}(160, 3^2)

for sensor A and

\mathcal{N}(170, 9^2)

for sensor B. The plot shows them with the uniform error bars for comparison.

grid = np.arange(145, 190, 0.1)
pdf_a = stats.gaussian(grid, 160, 3**2, normed=False)
pdf_b = stats.gaussian(grid, 170, 9**2, normed=False)

Choosing a uniform or a Gaussian distribution is a modeling decision. Neither describes reality exactly. In most cases the Gaussian is more realistic: most sensors are more likely to return readings near the true value and unlikely to return readings far from it. The uniform distribution instead says that every reading within a range is equally likely. Now consider the discrete distribution used by the discrete Bayes filter. It divides the range of possible values into bins and assigns a probability to each bin. The assignment can be entirely arbitrary, as long as the probabilities sum to one. The next plot shows one sensor three ways: as a uniform distribution, a Gaussian, and a discrete distribution.

rng = np.random.default_rng(7)
belief = rng.random(40)
belief = belief / belief.sum()
bins = np.linspace(155, 165, len(belief))

The discrete distribution here is random, to show that it can model any probability distribution. That gives it great power: with enough bins you can model the error of any sensor, however complicated. The price is mathematical intractability. Adding or multiplying Gaussians takes two lines of math, and the result is another Gaussian. That regularity lets you analyze the performance and behavior of a filter. Adding or multiplying discrete distributions requires looping over the data, and there is no simple way to characterize the result. Analyzing a filter built on discrete distributions is extremely hard. There is no single correct choice. The particle filter uses a discrete, sample-based distribution. It is very powerful because it handles arbitrarily complex situations, at the cost of speed and of analytical tractability. Gaussians give you the opposite tradeoff, and the rest of this section stays with them.

Designing a filter for a thermometer

So far you have filtered a position sensor. To get a feel for designing a filter for a different problem, build one for a thermometer. The sensor outputs a voltage that corresponds to the measured temperature. The manufacturer’s specification says that the sensor has white noise with a standard deviation of 0.13 volts. This function simulates a sensor reading:

def volt(voltage, std):
    return voltage + (np.random.randn() * std)

Now write the processing loop. As before it alternates predict and update. The measurement is clear: call volt() and pass the result to update(). But what about the prediction? There is no sensor for “movement” of the voltage, and over a short time you expect the voltage to stay constant. Trust the math. There is no known movement, so set it to zero. But a zero-movement prediction with zero uncertainty says the temperature never changes. The filter would then grow ever more confident and eventually ignore measurements caused by a real temperature change. This is called a smug filter, and you want to avoid it. So add a little error to the prediction step, which tells the filter not to discount changes in voltage over time. The code sets process_var = 0.05**2, the expected variance of the change in voltage over each time step. For a real sensor you would set it to the change you actually expect: a very small number for a thermometer measuring room air, and a much larger one for a thermocouple in a chemical reaction chamber. The loop also records the Kalman gain at each step.

np.random.seed(5)

voltage_std = 0.13
process_var = 0.05**2
actual_voltage = 16.3

x = gaussian(25.0, 1000.0)  # initial state
process_model = gaussian(0.0, process_var)

N = 50
zs = [volt(actual_voltage, voltage_std) for i in range(N)]
ps, estimates, gains = [], [], []

for z in zs:
    prior = predict(x, process_model)
    gains.append(prior.var / (prior.var + voltage_std**2))
    x = update(prior, gaussian(z, voltage_std**2))

    # save for later plotting
    estimates.append(x.mean)
    ps.append(x.var)

print(
    "Final estimate {:.3f} V (true value {:.1f} V)".format(
        estimates[-1], actual_voltage
    )
)
print("Variance converges to {:.4f}".format(ps[-1]))

Final estimate 16.326 V (true value 16.3 V)
Variance converges to 0.0054

Despite heavy sensor noise, the filter quickly finds the approximate voltage. With a different random draw, the final estimate lands slightly higher or lower. The second figure explains why. At the first step the prior is very uncertain, so the gain is close to 1 and the filter jumps almost all the way to the measurement. After a few updates the variance is small, the gain drops, and the filter adjusts its output only a little, even when a measurement lies far away. A g-h filter does the same thing with a fixed scaling factor

g

. The Kalman filter’s scaling factor varies with time: as the filter becomes more confident, it favors its prediction over the measurement. Specification sheets are what they sound like: specifications. Each individual sensor performs differently because of normal manufacturing variation. Values are often maximums, so the specification guarantees that performance is at least that good. Expensive equipment often comes with test results for your specific unit, which are usually trustworthy. A cheap sensor probably received little or no testing before sale. For a critical application, read the specification carefully to learn exactly what the numbers mean. Is the number a maximum, or a

3\sigma

error rate? Is every unit tested? Is the error Gaussian, or some other distribution? And your unit might be defective. For example, a data sheet for an airflow sensor might list a repeatability of

\pm 0.50\%

. Is that Gaussian? Is there a bias? Perhaps the repeatability is nearly

0.0\%

at low temperatures and always nearly

+0.50\%

at high temperatures. Data sheets for electrical parts often include “typical performance characteristics” charts. A chart of output voltage against current for a timer chip might show three curves for three temperatures. The ideal response is linear, but all three curves bend. Such a chart tells you that the voltage errors are probably not Gaussian. Modeling sensor performance is one of the hardest parts of building a Kalman filter that works well.

Example: extreme amounts of noise

The dog filter above used modest noise and guessed that the dog started at 0. How does the filter cope with harsher conditions? Start by adding far more noise to the sensor while keeping the process variance at 2 m

^2

. The sensor now has a standard deviation of 300 meters. An actual position of 1.0 m might be reported as 287.9 m, or as

-589.6

m. What do you expect the filter to do?

np.random.seed(21)

sensor_var = 300.0**2
process_var = 2.0
process_model = gaussian(1.0, process_var)
pos = gaussian(0.0, 500.0)
N = 1000
dog = DogSimulation(pos.mean, 1.0, sensor_var, process_var)
zs = [dog.move_and_sense() for _ in range(N)]
ps = []

for i in range(N):
    prior = predict(pos, process_model)
    pos = update(prior, gaussian(zs[i], sensor_var))
    ps.append(pos.mean)

The noise is extreme, yet the filter outputs a nearly straight line. The reason is the small process error. A small process error tells the filter that its prediction is very trustworthy, and the prediction is a straight line, so the filter outputs a nearly straight line.

Example: incorrect process variance

The last result looks excellent. So why not always set the process variance very low, to guarantee a smooth, straight output? The process variance tells the filter how much the system changes over time. If you set it artificially low, the filter cannot react to changes that really happen. In the next run the dog speeds up by a small amount at every time step, while the filter uses a process variance of 0.001 m

^2

np.random.seed(22)

sensor_var = 20.0
process_var = 0.001
process_model = gaussian(1.0, process_var)
pos = gaussian(0.0, 500.0)
N = 100
dog = DogSimulation(
    pos.mean, 1, sensor_var, process_var * 10000
)
zs, ps = [], []
for _ in range(N):
    dog.velocity += 0.04
    zs.append(dog.move_and_sense())

for z in zs:
    prior = predict(pos, process_model)
    pos = update(prior, gaussian(z, sensor_var))
    ps.append(pos.mean)

The filter clearly does not follow the measurements. They show that the dog is changing speed, but the filter has been told its predictions are nearly perfect, so it almost ignores them. The variances must describe your system correctly. The filter does not notice that it is drifting away from the measurements and correct itself. It computes the Kalman gain from the variances of the prior and the measurement and weights each accordingly.

Example: bad initial estimate

Now look at what happens with a bad initial estimate of position. To keep the picture clear, the sensor variance drops to

5^2

, but the initial position is set to 1000 meters. Can the filter recover from a 1000 meter error?

np.random.seed(23)

sensor_var = 5.0**2
process_var = 2.0
pos = gaussian(1000.0, 500.0)
process_model = gaussian(1.0, process_var)
N = 100
dog = DogSimulation(0, 1, sensor_var, process_var)
zs = [dog.move_and_sense() for _ in range(N)]
ps = []

for z in zs:
    prior = predict(pos, process_model)
    pos = update(prior, gaussian(z, sensor_var))
    ps.append(pos.mean)

Yes. The initial belief has a variance of 500, while the sensor variance is only

5^2

, so after the first step the estimate jumps from 1000 m to roughly 50 m. After another five to ten measurements it has converged. (The plot clips the first point to keep the rest readable.) This is how the filter escapes the chicken-and-egg problem of initial guesses. In practice you would usually take the first measurement as the initial value, but even a wild initial guess does little harm, as long as the filter’s variances match the actual process and measurement variances.

Example: large noise and bad initial estimate

What about the worst of both worlds, large noise and a bad initial estimate?

np.random.seed(24)

sensor_var = 30000.0
process_var = 2.0
pos = gaussian(1000.0, 500.0)
process_model = gaussian(1.0, process_var)

N = 1000
dog = DogSimulation(0, 1, sensor_var, process_var)
zs = [dog.move_and_sense() for _ in range(N)]
ps = []

for z in zs:
    prior = predict(pos, process_model)
    pos = update(prior, gaussian(z, sensor_var))
    ps.append(pos.mean)

This time the filter struggles. The previous example ran 100 updates, and this one runs 1000. It takes a few hundred iterations for the estimate to become reasonably accurate. Kalman filters are good, but they cannot do miracles. With extremely noisy data and extremely bad initial conditions, this is as good as it gets. Now use the first measurement as the initial position.

np.random.seed(24)

sensor_var = 30000.0
process_var = 2.0
process_model = gaussian(1.0, process_var)
N = 1000
dog = DogSimulation(0, 1, sensor_var, process_var)
zs = [dog.move_and_sense() for _ in range(N)]

# the first measurement seeds the estimate, so the loop starts at the second
pos = gaussian(zs[0], 500.0)
ps = [pos.mean]
for z in zs[1:]:
    prior = predict(pos, process_model)
    pos = update(prior, gaussian(z, sensor_var))
    ps.append(pos.mean)

This simple change improves the result a great deal. How fast the filter settles depends on the noise in the first measurement: a large error there puts the initial estimate far from the dog. A few hundred iterations may seem like a lot, but the noise here is huge. Real sensors such as thermometers, laser range finders, GPS receivers and cameras have nothing like these errors. A reasonable variance for a cheap thermometer might be 0.2

^{\circ}\text{C}^2

, and this example uses 30,000.

Exercise: a parameter study

Write a function that simulates the dog from a start position, sensor noise, velocity and process noise, runs the Kalman filter on the measurements, and returns the measurements and the estimates. Then compare a few settings side by side to see how each parameter changes the filter’s behavior.

Solution

The function below does that. Process noise models how much the dog wanders off course at each time step, so the simulation adds it to the dog’s position at every step. The filter always starts from

\mathcal N(0, 1000)

, whatever the true start position. Each run uses the same random seed, so the panels differ only in the parameters.

def kalman_dog_run(
    start_pos,
    sensor_noise,
    velocity,
    process_noise,
    N=20,
    seed=303,
):
    np.random.seed(seed)
    dog = DogSimulation(
        start_pos, velocity, sensor_noise, process_noise
    )
    zs = [dog.move_and_sense() for _ in range(N)]
    pos = gaussian(0.0, 1000.0)  # mean and variance
    process_model = gaussian(velocity, process_noise)

    ps = []
    for z in zs:
        pos = predict(pos, process_model)
        pos = update(pos, gaussian(z, sensor_noise))
        ps.append(pos.mean)
    return zs, ps


study = [
    dict(
        start_pos=0,
        sensor_noise=5,
        velocity=1,
        process_noise=0.1,
    ),
    dict(
        start_pos=0,
        sensor_noise=50,
        velocity=1,
        process_noise=0.1,
    ),
    dict(
        start_pos=8,
        sensor_noise=5,
        velocity=-1,
        process_noise=0.1,
    ),
    dict(
        start_pos=0,
        sensor_noise=5,
        velocity=1,
        process_noise=20,
    ),
]
study_runs = [kalman_dog_run(**params) for params in study]

In the top left the filter tracks a steady dog closely. With ten times the sensor noise (top right) the measurements scatter widely, but the small process noise keeps the estimate close to a straight line. A negative velocity and a start position away from the initial guess (bottom left) only cost the filter its first step. A large process noise (bottom right) makes the filter trust each measurement, so the estimate follows the measurements much more closely.

Exercise: nonlinear systems

The Kalman filter equations are linear:

\begin{aligned} \mathcal{N}(\bar\mu,\, \bar\sigma^2) &= \mathcal{N}(\mu,\, \sigma^2) + \mathcal{N}(\mu_\mathtt{move},\, \sigma^2_\mathtt{move})\\ \mathcal{N}(\mu,\, \sigma^2) &= \mathcal{N}(\bar\mu,\, \bar\sigma^2) \times \mathcal{N}(\mu_\mathtt{z},\, \sigma^2_\mathtt{z}) \end{aligned}

Do you expect this filter to work well or poorly on a nonlinear system? Implement a Kalman filter whose measurements come from

for i in range(100):
    z = math.sin(i/3.) * 2

Adjust the variances and the initial position to see the effect. What happens, for example, with a very bad initial guess?

Solution

np.random.seed(31)

sensor_var = 30.0
process_var = 2.0
pos = gaussian(100.0, 500.0)
process_model = gaussian(1.0, process_var)

zs, ps = [], []

for i in range(100):
    pos = predict(pos, process_model)

    z = math.sin(i / 3.0) * 2 + np.random.randn() * 1.2
    zs.append(z)

    pos = update(pos, gaussian(z, sensor_var))
    ps.append(pos.mean)

Discussion

The result is poor. The output looks nothing like a sine wave, except in the crudest way. With linear systems you could add extreme noise and still extract an accurate result, but here even modest noise gives a bad result. The filter always chooses a value partway between its prediction and the measurement. A signal like this one is always accelerating, while the process model assumes a constant velocity, so the filter is guaranteed to lag the signal. Practitioners noticed the poor performance of Kalman filters on nonlinear systems soon after they began using them, and devised extensions such as the extended and unscented Kalman filters to deal with it.

Fixed-gain filters

Embedded computers often have very limited processors, and many lack floating point hardware. Even these simple equations can be a burden. That matters less as hardware improves, but do not underestimate the value of saving one dollar per processor when you buy millions of them. In the examples above, the variance of the filter converged to a fixed value. This always happens when the measurement and process variances are constant. If the variance converges, so does the Kalman gain. You can find the converged gain once, offline, and hard-code it. As long as you start the filter from a good initial guess, such as the first measurement, it performs very well. The next cell finds the steady-state gain of the 25-step dog filter by iterating the variance recursion. It then defines a fixed-gain filter that never touches a variance.

# process and sensor variance of the 25-step dog filter
Q, R = 2.0, 4.5
P = 400.0
for _ in range(100):
    P_prior = P + Q
    K = P_prior / (P_prior + R)
    P = (1 - K) * P_prior
K_fixed = K
print(f"steady-state Kalman gain K = {K_fixed:.5f}")

vel, dt = 1.0, 1.0


def update_fixed(x, z):
    y = z - x  # residual
    return x + K_fixed * y  # posterior


def predict_fixed(x):
    return x + vel * dt


np.random.seed(41)
dog = DogSimulation(0.0, vel, R, Q)
zs = [dog.move_and_sense() for _ in range(50)]

# both filters start from the first measurement and filter the rest
x_fixed, est_fixed = zs[0], [zs[0]]
x_full, est_full = gaussian(zs[0], R), [zs[0]]
for z in zs[1:]:
    x_fixed = update_fixed(predict_fixed(x_fixed), z)
    est_fixed.append(x_fixed)
    x_full = update(
        predict(x_full, gaussian(vel, Q)), gaussian(z, R)
    )
    est_full.append(x_full.mean)

gap = np.max(
    np.abs(np.array(est_fixed) - np.array(est_full))
)
print(f"largest gap between the two filters: {gap:.4f} m")

steady-state Kalman gain K = 0.48051
largest gap between the two filters: 0.2021 m

The two filters agree closely, and after the first few steps they are nearly indistinguishable. The fixed-gain filter uses the Kalman gain form of the update to make clear that it needs no variances at all.

FilterPy’s implementation

FilterPy implements predict() and update(). They work for the univariate case in this section and for the multivariate case, so their interface is slightly different from the functions above. They do not take Gaussians as pairs. They take the mean and the variance as two separately named arguments. predict() takes several arguments, but you need only these four:

predict(x, P, u, Q)

x is the state of the system, P is its variance, u is the movement due to the process, and Q is the process noise. Use named arguments when you call predict(), because most of its arguments are optional. The third positional argument of predict() is not u. These names may strike you as terrible. They come from the long history of control theory, and every paper and book uses them, so it pays to learn them. Try it on the state

\mathcal N(10, 3)

and the movement

\mathcal N(1, 4)

. You expect a position of 11 (

10+1

) with a variance of 7 (

3+4

kf.predict(x=10.0, P=3.0, u=1.0, Q=4.0)

(np.float64(11.0), np.float64(7.0))

update() also takes several arguments. For now you need these four:

update(x, P, z, R)

As before, x and P are the state and its variance. z is the measurement and R is the measurement variance. Run the predict step again to get the prior, and then update it with a measurement:

x, P = kf.predict(x=10.0, P=3.0, u=1.0, Q=2.0**2)
print("%.3f" % x)

x, P = kf.update(x=x, P=P, z=12.0, R=3.5**2)
print("%.3f" % x, "%.3f" % P)

11.000
11.364 4.455

The measurement is noisy, with a large variance, so the estimate stays close to the prior of 11. Notice that the output of the predict step is called x, not prior. The Kalman filter equations use

x

for both. Both the prior and the posterior are estimates of the state of the system. The prior is the estimate before the measurement is incorporated, and the posterior is the estimate after.

Summary

The one-dimensional Kalman filter is a special, restricted case of the multivariate filter. Many texts skip it, but it is a valuable stepping stone. It shows that the g-h filter, the discrete Bayes filter and the Kalman filter all use the same algorithm and the same reasoning: predict, measure, and choose a value between the prediction and the measurement. The multivariate filter uses more sophisticated math, which can hide that simplicity, but in exchange it performs markedly better than the filters in this section. Gaussians are a good representation of a unimodal belief, whether that is the position of a dog in a hallway, the position of an aircraft in the sky, or the temperature of a chemical reaction chamber. Multiplying Gaussians computes a new belief from the prior belief and a new measurement. Adding Gaussians updates the belief when the state moves. Spend time with the full description of the algorithm until you see how it relates to the g-h filter and the discrete Bayes filter. Each algorithm performs the same step, choosing a value between a prediction and a measurement, with different math.

Found an error on this page? Report an issue.

State Estimation & Mapping

State Estimation

SLAM

One-Dimensional Kalman Filters

Tracking a dog in a hallway

Beliefs as Gaussians

Tracking with Gaussian probabilities

Predictions with Gaussians

Updates with Gaussians

Understanding Gaussian multiplication

Your first Kalman filter

Code walkthrough

Exercise: modify variance values

Solution

The belief over time

Kalman gain

Full description of the algorithm

Comparison with g-h and discrete Bayes filters

Designing a filter for a thermometer

Example: extreme amounts of noise

Example: incorrect process variance

Example: bad initial estimate

Example: large noise and bad initial estimate

Exercise: a parameter study

Solution

Exercise: nonlinear systems

Solution

Discussion

Fixed-gain filters

FilterPy’s implementation

Summary

​Tracking a dog in a hallway

​Beliefs as Gaussians

​Tracking with Gaussian probabilities

​Predictions with Gaussians

​Updates with Gaussians

​Understanding Gaussian multiplication

​Your first Kalman filter

​Code walkthrough

​Exercise: modify variance values

​Solution

​The belief over time

​Kalman gain

​Full description of the algorithm

​Comparison with g-h and discrete Bayes filters

​Designing a filter for a thermometer

​Example: extreme amounts of noise

​Example: incorrect process variance

​Example: bad initial estimate

​Example: large noise and bad initial estimate

​Exercise: a parameter study

​Solution

​Exercise: nonlinear systems

​Solution

​Discussion

​Fixed-gain filters

​FilterPy’s implementation

​Summary

Tracking a dog in a hallway

Beliefs as Gaussians

Tracking with Gaussian probabilities

Predictions with Gaussians

Updates with Gaussians

Understanding Gaussian multiplication

Your first Kalman filter

Code walkthrough

Exercise: modify variance values

Solution

The belief over time

Kalman gain

Full description of the algorithm

Comparison with g-h and discrete Bayes filters

Designing a filter for a thermometer

Example: extreme amounts of noise

Example: incorrect process variance

Example: bad initial estimate

Example: large noise and bad initial estimate

Exercise: a parameter study

Solution

Exercise: nonlinear systems

Solution

Discussion

Fixed-gain filters

FilterPy’s implementation

Summary