Kalman Filter - 19. Kalman Filter Code

We write a main program, including two functions update function and predict function, and then import a series of measurement and motion data.

If the initial estimate is 5, great, but we set it to mu=0 and the uncertainty is very high to sig=10000.

We assume a constant measurement uncertainty of 4 and a motion uncertainty of 2.

Your first position estimate while moving should be 4.9. The reason is that the initial uncertainty is high and the first measurement of 5 dominates the estimate.

Your uncertainty is reduced to 3,99

Slightly better than measurement uncertainty.

Then your prediction increases by 1, but the uncertainty increases to 5.99

That is, the motion uncertainty is equal to 2, then update your estimate 5.99 again based on the measurement data 6

You move 1 more measurement data is 7 you move 2 measurement data 9 you move 1 measurement data 10 and finally move 1.

The final result came out that the prediction for this position was 10.99

That is, your position 10 moves 1 with a residual uncertainty of 4.

# 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]

measurements = [5., 6., 7., 9., 10.]
motion = [1., 1., 2., 1., 1.]
measurement_sig = 4.
motion_sig = 2 .
mu = 0 .
sig = 10000 .

#Please print out ONLY the final values of the mean
#and the variance in a list [mu, sig]. 

# Insert code here
for n in range(len(measurements)):
    [mu,sig]=update(mu,sig,measurements[n],measurement_sig)
    print 'update:',[mu,sig]
    [mu,sig]=predict(mu,sig,motion[n],motion_sig)
    print 'predict:',[mu,sig]

print [mu, sig]

This code deploys the entire Kalman filter, it checks all measurement elements and by default, the number of measurements is the nth power of motion, it updates mu and sigma using the update recursive formula. If we import the nth measurement And the measurement uncertainty, he does the same for the motion, the prediction part here, it recursively updates mu and sigma with the nth motion and the motion uncertainty and prints them all.

The above is a complete one-dimensional Kalman filter.