def estimate_next_pos(measurement, OTHER = None): """Estimate the next (x, y) position of the wandering Traxbot based on noisy (x, y) measurements.""" #identity matrix I = matrix([[1., 0., 0., 0., 0., 0., 0.], [0., 1., 0., 0., 0., 0., 0.], [0., 0., 1., 0., 0. ,0., 0.], [0., 0., 0., 1., 0., 0., 0.], [0., 0., 0., 0., 1., 0., 0.], [0., 0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 0., 1.]]) #state transition matrix F = matrix([[1., 0., 1., 0., 0., 0.], [0., 1., 0., 1., 0., 0.], [0., 0., 1., 0., 1., 0.], [0., 0., 0., 1., 0., 1.], [0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 1.]]) #motion update matrix H = matrix([[1., 0., 0., 0., 0., 0., 0.], [0., 1., 0., 0., 0., 0., 0.]]) #measurement noise R = matrix([[measurement_noise, 0.], [0., measurement_noise]]) z = matrix([[measurement[0]], [measurement[1]]]) u = matrix([[0.], [0.], [0.], [0.], [0.], [0.], [0.]]) if OTHER is not None: X = OTHER['X'] P = OTHER['P'] else: X = matrix([[1.], [1.], [1.], [1.], [1.], [1.], [1.]]) #convariance matrix P = matrix([[1000., 0., 0., 0., 0., 0., 0.], [0., 1000., 0., 0., 0., 0., 0.], [0., 0., 1000., 0., 0., 0., 0.], [0., 0., 0., 1000., 0., 0., 0.], [0., 0., 0., 0., 1000., 0., 0.], [0., 0., 0., 0., 0., 1000., 0.], [0., 0., 0., 0., 0., 0., 1000.]]) #measurement update Y = z - (H * X) S = H * P* H.transpose() + R K = P * H.transpose() * S.inverse() X = X + (K * Y) P = (I- (K * H)) * P #Prediction new_X = matrix([[]]) new_X.zero(7, 1) omega = X.value[4][0] radius = X.value[3][0] angle = X.value[2][0] x_v = X.value[5][0] y_v = X.value[6][0] new_X.value[0][0] = radius * cos(omega + angle) new_X.value[1][0] = radius * sin(omega + angle) new_X.value[2][0] = omega + angle new_X.value[3][0] = radius new_X.value[4][0] = (x_v**2 + y_v**2)**.5 / radius new_X.value[5][0] = x_v new_X.value[6][0] = y_v A = matrix([[]]) A.zero(7,7) A.value[0][2] = -radius * sin(omega+angle) A.value[0][4] = -radius * sin(omega+angle) A.value[1][2] = radius * cos(omega+angle) A.value[1][4] = radius * cos(omega+angle) A.value[2][2] = 1. A.value[2][4] = 1. A.value[3][3] = 1. A.value[4][3] = -(x_v**2 + y_v**2)**0.5 / radius**2 A.value[4][5] = (x_v/radius) * (x_v**2 + y_v**2)**(-3./2) A.value[4][6] = (y_v/radius) * (x_v**2 + y_v**2)**(-3./2) A.value[5][5] = 1. A.value[6][6] = 1. P = A * P * A.transpose() X = new_X xy_estimate = [X.value[0][0], X.value[1][0]] OTHER = {'X': X, 'P': P} # You must return xy_estimate (x, y), and OTHER (even if it is None) # in this order for grading purposes. P.show() print '----------------------------------------------' return xy_estimate, OTHER
def estimate_next_pos(measurement, OTHER = None): """Estimate the next (x, y) position of the wandering Traxbot based on noisy (x, y) measurements.""" #identity matrix I = matrix([[1., 0., 0., 0., 0., 0., 0.], [0., 1., 0., 0., 0., 0., 0.], [0., 0., 1., 0., 0., 0., 0.], [0., 0., 0., 1., 0., 0., 0.], [0., 0., 0., 0., 1., 0., 0.], [0., 0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 0., 1.], ]) #motion update matrix H = matrix([[1., 0., 0., 0., 0., 0., 0.], [0., 1., 0., 0., 0., 0., 0.]]) #measurement noise R = matrix([[measurement_noise, 0.], [0., measurement_noise]]) z = matrix([[measurement[0]], [measurement[1]]]) u = matrix([[0.], [0.], [0.], [0.], [0.], [0.], [0.]]) if OTHER is not None: X = OTHER['X'] P = OTHER['P'] else: X = matrix([[1.], [1.], [1.], [1.], [1.], [1.], [1.]]) #convariance matrix P = matrix([[1000., 0., 0., 0., 0., 0., 0.], [0., 1000., 0., 0., 0., 0., 0.], [0., 0., 1000., 0., 0., 0., 0.], [0., 0., 0., 1000., 0., 0., 0.], [0., 0., 0., 0., 1000., 0., 0.], [0., 0., 0., 0., 0., 1000., 0.], [0., 0., 0., 0., 0., 0., 1000.], ]) #measurement update Y = z - (H * X) S = H * P* H.transpose() + R K = P * H.transpose() * S.inverse() X = X + (K * Y) P = (I- (K * H)) * P #Prediction new_X = matrix([[]]) new_X.zero(7, 1) x = X.value[0][0] y = X.value[1][0] centre_x = X.value[2][0] centre_y = X.value[3][0] angle = X.value[4][0] ta = X.value[5][0] radius = X.value[6][0] new_X.value[0][0] = radius * sin(ta + angle) + centre_x new_X.value[1][0] = radius * cos(ta + angle) + centre_y new_X.value[2][0] = centre_x new_X.value[3][0] = centre_y new_X.value[4][0] = angle new_X.value[5][0] = ta new_X.value[6][0] = radius A_values = [ [0., 0., 1., 0., radius*cos(angle+ta), radius*cos(angle+ta), sin(angle+ta)], [0., 0., 0., 1., -radius*sin(angle+ta), -radius*sin(angle+ta), cos(angle+ta)], [0., 0., 1., 0., 0., 0., 0.], [0., 0., 0., 1., 0., 0., 0.], [0., 0., 0., 0., 1., 1., 0.], [0., 0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 0., 1.], ] A = matrix(A_values) P = A * P * A.transpose() X = new_X xy_estimate = [X.value[0][0], X.value[1][0]] OTHER = {'X': X, 'P': P} # You must return xy_estimate (x, y), and OTHER (even if it is None) # in this order for grading purposes. P.show() print '----------------------------------------------' return xy_estimate, OTHER
def estimate_next_pos(measurement, OTHER=None): """Estimate the next (x, y) position of the wandering Traxbot based on noisy (x, y) measurements.""" #identity matrix I = matrix([ [1., 0., 0., 0., 0., 0., 0.], [0., 1., 0., 0., 0., 0., 0.], [0., 0., 1., 0., 0., 0., 0.], [0., 0., 0., 1., 0., 0., 0.], [0., 0., 0., 0., 1., 0., 0.], [0., 0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 0., 1.], ]) #motion update matrix H = matrix([[1., 0., 0., 0., 0., 0., 0.], [0., 1., 0., 0., 0., 0., 0.]]) #measurement noise R = matrix([[measurement_noise, 0.], [0., measurement_noise]]) z = matrix([[measurement[0]], [measurement[1]]]) u = matrix([[0.], [0.], [0.], [0.], [0.], [0.], [0.]]) if OTHER is not None: X = OTHER['X'] P = OTHER['P'] else: X = matrix([[1.], [1.], [1.], [1.], [1.], [1.], [1.]]) #convariance matrix P = matrix([ [1000., 0., 0., 0., 0., 0., 0.], [0., 1000., 0., 0., 0., 0., 0.], [0., 0., 1000., 0., 0., 0., 0.], [0., 0., 0., 1000., 0., 0., 0.], [0., 0., 0., 0., 1000., 0., 0.], [0., 0., 0., 0., 0., 1000., 0.], [0., 0., 0., 0., 0., 0., 1000.], ]) #measurement update Y = z - (H * X) S = H * P * H.transpose() + R K = P * H.transpose() * S.inverse() X = X + (K * Y) P = (I - (K * H)) * P #Prediction new_X = matrix([[]]) new_X.zero(7, 1) x = X.value[0][0] y = X.value[1][0] centre_x = X.value[2][0] centre_y = X.value[3][0] angle = X.value[4][0] ta = X.value[5][0] radius = X.value[6][0] new_X.value[0][0] = radius * sin(ta + angle) + centre_x new_X.value[1][0] = radius * cos(ta + angle) + centre_y new_X.value[2][0] = centre_x new_X.value[3][0] = centre_y new_X.value[4][0] = angle new_X.value[5][0] = ta new_X.value[6][0] = radius A_values = [ [ 0., 0., 1., 0., radius * cos(angle + ta), radius * cos(angle + ta), sin(angle + ta) ], [ 0., 0., 0., 1., -radius * sin(angle + ta), -radius * sin(angle + ta), cos(angle + ta) ], [0., 0., 1., 0., 0., 0., 0.], [0., 0., 0., 1., 0., 0., 0.], [0., 0., 0., 0., 1., 1., 0.], [0., 0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 0., 1.], ] A = matrix(A_values) P = A * P * A.transpose() X = new_X xy_estimate = [X.value[0][0], X.value[1][0]] OTHER = {'X': X, 'P': P} # You must return xy_estimate (x, y), and OTHER (even if it is None) # in this order for grading purposes. P.show() print '----------------------------------------------' return xy_estimate, OTHER