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
