Kalman Filter

kalman.py

import math
import random
import matplotlib as mp

rndm = random.Random()

class KalmanFilter():
    def __init__(self):
        self.kalmanGain = 0
        self.covariance = 1.0
        self.measurmentNoiseModel = 1.5
        self.prediction = 0
        
    def updatePrediction(self, sensorValue):
        if sensorValue != None:
            self.prediction = self.prediction+self.kalmanGain*(sensorValue-self.prediction);
        else:
            self.prediction = self.prediction+self.kalmanGain*(0-self.prediction);
    
    def updateCovariance(self):
        self.covariance = (self.kalmanGain)*self.covariance;
    
    def updateKalmanGain(self):
        self.kalmanGain = (1+self.covariance)/(self.covariance+self.measurmentNoiseModel)
        
    def step(self, sensorValue):
        self.covariance = self.covariance + .1
        self.updateKalmanGain()
        self.updatePrediction(sensorValue)
        
        if sensorValue != None:
            self.updateCovariance()
        else:
            self.covariance = (1-self.kalmanGain)*self.covariance
        
        
filterX = KalmanFilter()
filterY = KalmanFilter()

loc = (0,0)

def moveNERand(loc):
    mult = 1
    
    x = loc[0]+mult*rndm.random()
    y = loc[1]+mult*rndm.random()
    
    return (x, y)


NUM_STEMPS = 1000
SENSOR_SKIP = .7 #.3 means we only apply a sensor value 1/3 of the time
stepsSinceSensor = 1

for i in range(0, NUM_STEMPS):
    locLast = loc
    loc = moveNERand(loc)
    
    if rndm.random() < SENSOR_SKIP:
        
        filterX.step((loc[0]-locLast[0])/stepsSinceSensor)
        filterY.step((loc[1]-locLast[1])/stepsSinceSensor)
        stepsSinceSensor = 1
    else:
        filterX.step(None)
        filterY.step(None)
        stepsSinceSensor += 1        
        
    print("X Pred: %s" % (filterX.prediction + locLast[0]))
    print("X Actual: %s" % loc[0])
    print("Y Pred: %s" % (filterY.prediction + locLast[1]))    
    print("Y Actual: %s" % loc[1])