kalman.py

#

Single variate functions

#

Calculate new mean and variance given priors

def update(m1, v1, m2, v2):

#

This will reduce the variance, increasing convidence of the estimate.

    mean = (v2 * m1 + v1 * m2) / (v1 + v2)
    var = 1 / (1 / v1 + 1 / v2)
    return mean, var

#

Convolution of two gaussian distributions. Overall, this will increase the variance.

def predict(m1, v1, m2, v2):

#

    mean = m1 + m2
    var = v1 + v2
    return mean, var

#

Multivariate

This is the same operation as above, but in multiple dimensions using numpy matrices.

from numpy import matrix, identity

#

Kalman filter implementation, see https://en.wikipedia.org/wiki/Kalman_filter

class Kalman(object):

#

    dt = delta time
    F = next state function
    H = measurement function
    R = measurement uncertainty
    u = external motion

    def __init__(self, dt, F, H, R, u):

#

        self.dt = dt
        self.F = F
        self.H = H
        self.R = R
        self.u = u

#

    x = initial state (location and velocity)
    P = initial uncertainty

    def filter(self, x, P, measurements):

#

(Credit to Sebastian Thrun for the matrix operations)

        I = identity(P.shape)
        for m in measurements:

#

Prediction step. Typically movement.

            x = (self.F * x) + self.u
            P = self.F * P * self.F.transpose()

#

Measurement update.

            Z = matrix(m)
            y = Z.transpose() - (self.H * x)
            S = self.H * P * self.H.transpose() + self.R
            K = P * self.H.transpose() * S.inverse()
            x = x + (K * y)
            P = (I - (K * self.H)) * P