Understanding Kalman Filter in case of 1D! (Super Easy)

Previous Story...

Kalman Filter

Kalman Filter is a type of Bayes Filter. It can be used only that has the estimator for the linear Gaussian case and the optimal solution for linear models and Gaussian distribution. Kalman Filter calculates the current position of the robot by recursive state prediction and correction. In state prediction, it is calculating the robot state with the state right before and its motion input. In the correction step, it checks the state prediction is reasonable or not with the sensor observation.

Kalman Filter in 1 Dimension

It is easy to understand in the case of 1 dimension. The mean and the covariance of the distribution can be gotten by just addition. 

In the prediction step, predict the next state by calculating with the robot's motion. Because it is only one dimension, it is already linear so the prediction can be gotten by just addition. Next, in the correction step, calculate new state with the observation of the sensor. This gaussian addition is quite difficult, you can read more here(The product of two Gaussian pdfs is not a pdfs, but it is Gaussian(a.k.a. loving algebra)).

Python Code

I made a simple code of these two steps in python.

# Kalman Filter 1Ddef correction(mean, var, sensor_mean, sensor_var):
    new_mean = (sensor_mean*var + mean*sensor_var)/(var + sensor_var)
    new_var = var*sensor_var/(var + sensor_var)
    return new_mean, new_var

def prediction(mean, var, motion_mean, motion_var):
    new_mean = mean + motion_mean
    new_var = var + motion_var
    return new_mean, new_var

There are two functions named 'correction' and 'prediction'. In 'prediction', input value 'mean' and 'var' are the previous state of the robot. Input value 'motion_mean' and 'motion_var' are the robot's motion.  In 'correction', input value 'mean' and 'var' will be the predicted value return by the function 'prediction'. Input value 'sensor_mean' and 'sensor_var' are the observation value by the sensor. The contents are same with above.

sensor = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
sensor_var = 2
motion = [1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
motion_var = 1
mean = 0var = 100

I set values like above.

for i in range(len(sensor)):
    # Step 1: State Prediction    mean, var = prediction(mean, var, motion[i], motion_var)
    print i
    print "- prediction -"    print "mean:%d var:%d" % (mean, var)
    # Step 2 : Correction    mean, var = correction(mean, var, sensor[i], sensor_var)
    print "- correction -"    print "mean:%d var:%d" % (mean, var)
    print '-------------------------------------------'

0

- prediction -

mean:1 var:101

- correction -

mean:1 var:1

-------------------------------------------

1

- prediction -

mean:2 var:2

- correction -

mean:2 var:1

-------------------------------------------

2

- prediction -

mean:3 var:2

- correction -

mean:3 var:1

-------------------------------------------

3

- prediction -

mean:4 var:2

- correction -

mean:4 var:1

-------------------------------------------

4

- prediction -

mean:5 var:2

- correction -

mean:5 var:1

-------------------------------------------

5

- prediction -

mean:6 var:2

- correction -

mean:6 var:1

-------------------------------------------

6

- prediction -

mean:7 var:2

- correction -

mean:7 var:1

-------------------------------------------

7

- prediction -

mean:8 var:2

- correction -

mean:8 var:1

-------------------------------------------

8

- prediction -

mean:9 var:2

- correction -

mean:9 var:1

-------------------------------------------

9

- prediction -

mean:10 var:2

- correction -

mean:10 var:1

-------------------------------------------

Process finished with exit code 0