Kalman Filter dog example

.KALMAN_DOG.md

https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/04-One-Dimensional-Kalman-Filters.ipynb

justin@justin-XPS-13-9360:~/work$ python3 kalman_dog.py 
(1.00000, 401.00000) 1.3535959735108178 (1.35184, 1.99007)
(2.35184, 2.99007) 1.8820653967131618 (2.07035, 1.19841)
(3.07035, 2.19841) 4.341047429453569 (3.73572, 1.04726)
(4.73572, 2.04726) 7.156332673205118 (5.96016, 1.01168)
(6.96016, 2.01168) 6.938695089418526 (6.94940, 1.00291)
(7.94940, 2.00291) 6.843912342028484 (7.39625, 1.00073)
(8.39625, 2.00073) 9.846824080052299 (9.12167, 1.00018)
(10.12167, 2.00018) 12.553482049375292 (11.33763, 1.00005)
(12.33763, 2.00005) 16.2730841073834 (14.30538, 1.00001)
(15.30538, 2.00001) 14.800411177015299 (15.05289, 1.00000)

kalman_dog.py

"""
- https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/04-One-Dimensional-Kalman-Filters.ipynb
"""

class Gaussian:

    def __init__(self, mean, var):
        self.mean=mean
        self.var=var

    @classmethod
    def add(self, g0, g1):
        return Gaussian(g0.mean+g1.mean,
                        g0.var+g1.var)

    @classmethod
    def mult(self, g0, g1):
        mean=(g0.var*g1.mean+g1.var*g0.mean)/(g0.var+g1.var)
        variance=(g0.var*g1.var)/(g0.var+g1.var)
        return Gaussian(mean, variance)
            
    def __repr__(self):
        return "(%.5f, %.5f)" % (self.mean, self.var)

ProcessVar=1.0 # variance in the dog's movement
SensorVar=2.0 # variance in the sensor
Velocity=1.0
Timestep=1.0 # time step in seconds
    
X=Gaussian(0., 20.**2)  # dog's position, N(0, 20**2)
    
ProcessModel=Gaussian(Velocity*Timestep, ProcessVar) # displacement to add to x

Z=[1.3535959735108178, 1.8820653967131618, 4.341047429453569, 7.156332673205118, 6.938695089418526, 6.843912342028484, 9.846824080052299, 12.553482049375292, 16.2730841073834, 14.800411177015299]

x=X
for z in Z:
    prior=Gaussian.add(x, ProcessModel)
    likelihood=Gaussian(z, SensorVar)
    x=Gaussian.mult(prior, likelihood)
    print (prior, z, x)

screenshot.png