As for Unit 3, there seems to be a mix-up in using standard deviation for variance. The snippet below shows the solution “kalman_filter_solution.py” taking inputs of variance and outputting the new variance.
Line 27 - 42:
Add correct_step function HERE
def correct_step(mean1, var1, mean2, var2):
‘’’ This function takes in two means and two squared variance terms,
and returns updated gaussian parameters’‘’
# Calculate the new gaussian parameters
new_mean = (var1 * mean2 + var2 * mean1) / (var1 + var2)
new_var = 1 / (1 / var1 + 1 / var2)
return new_mean, new_varAdd predict_step function HERE
def state_prediction(mean1, var1, mean2, var2):
global new_mean, new_var
new_mean = mean1 + mean2
new_var = var1 + var2
return new_mean, new_var
But as shown in the function call, the standard deviation (sigmoid) is referenced and used as input. This is also labelled in initializing the variance for motion and measurement.
Line 21 - 23:
initialize motion sigma (the standard deviation of the motions normal distribution)
motion_sig = 4.0
measurement_sig = 0.05
Line 59 - 63
ADD KALMAN FILTER CYCLE HERE
mu, sig = state_prediction(mu, sig, motion, motion_sig)
rospy.loginfo(“predict step: [%s, %s]”, mu, sig)
mu, sig = correct_step(mu, sig, last_measurement, measurement_sig)
rospy.loginfo(“correct_step: [%s, %s]”, mu, sig)
The notebook also includes equations for calculating the mean and variance based on the previous variance.
Just before the start of section 3.5.1
Writing those math formulas as Python code is straightforward:
new_mean = (var1 * mean2 + var2 * mean1) / (var1 + var2)
new_var = 1 / (1 / var1 + 1 / var2)
Just before the start of section 3.6.1
Written in Python:
new_mean = mean1 + mean2;
new_var = var1 + var2;
The outputs from the rostopic “/filtered_pos/data” yield visually similar results, perhaps due to the robustness of the filter. A plausible solution is as shown:
Add correct_step function HERE
def correct_step(mean1, sig1, mean2, sig2):
var1 = sig1 ** 2
var2 = sig2 ** 2
new_mean = (var1 * mean2 + var2 * mean1) / (var1 + var2)
new_var = (1 / (1 / var1 + 1 / var2))
new_sig = new_var ** 0.5
return new_mean, new_sigAdd predict_step function HERE
def predict_step(mean1, sig1, mean2, sig2):
var1 = sig1 ** 2
var2 = sig2 ** 2
new_mean = mean1 + mean2
new_var = var1 + var2
new_sig = new_var ** 0.5
return new_mean, new_sig
Just want to raise this potential issue (if any) to prevent any misunderstandings for future learners.