Measure and update iteration in localization of self driving cars.

measure_and_update.py

# Write a program that will iteratively update and
# predict based on the location measurements 
# and inferred motions shown below. 

def update(mean1, var1, mean2, var2):
    new_mean = float(var2 * mean1 + var1 * mean2) / (var1 + var2)
    new_var = 1./(1./var1 + 1./var2)
    return [new_mean, new_var]

def predict(mean1, var1, mean2, var2):
    new_mean = mean1 + mean2
    new_var = var1 + var2
    return [new_mean, new_var]

# The means of all the measurement distributions.
measurement_means = [5., 6., 7., 9., 10.] #means
# The variance a.k.a uncertainties associated with the means above.
# The uncertainty/variance i the same for all measurements taken.
measurement_uncertainty = 4. #variance

motion_distance = [1., 1., 2., 1., 1.]
motion_uncertainty = 2.


prior_mean = 0.
prior_uncertainity = 10000.

for i in range(len(measurement_means)):
    prior_mean, prior_uncertainity = update(prior_mean, prior_uncertainity, measurement_means[i], measurement_uncertainty)
    print("measure")
    print(prior_mean, prior_uncertainity)
    prior_mean, prior_uncertainity = predict(prior_mean, prior_uncertainity, motion_distance[i], motion_uncertainty)
    
    print("predict")
    print(prior_mean, prior_uncertainity)
    print("\n")
    
print [prior_mean, prior_uncertainity]