def measurement_callback(self, marker_position_world, marker_yaw_world,
marker_position_relative, marker_yaw_relative):
'''
:param measurement: vector of measured coordinates
'''
while (marker_yaw_relative > math.pi):
marker_yaw_relative -= 2 * math.pi
while (marker_yaw_relative < -math.pi):
marker_yaw_relative += 2 * math.pi
from math import sin, cos
#meas = Pose2D(marker_yaw_world, marker_position_world) * Pose2D(marker_yaw_relative, marker_position_relative).inv();
measurement = np.array([[
marker_position_relative[0], marker_position_relative[1],
marker_yaw_relative
]]).T
s_psi = sin(self.x[2])
c_psi = cos(self.x[2])
#TODO: Enter the Jacobian derived in the previous assignment
dx = marker_position_world[0] - self.x[0]
dy = marker_position_world[1] - self.x[1]
self.H = np.array([[-c_psi, -s_psi, s_psi * dx - c_psi * dy],
[s_psi, -c_psi, c_psi * dx + s_psi * dy],
[0, 0, -1]])
I = np.zeros((3, 3))
self.H = np.hstack((self.H, I))
predicted_meas = Pose2D(self.x[2], self.x[0:2]).inv() * Pose2D(
marker_yaw_world, marker_position_world)
predicted_measurement = np.array([[
predicted_meas.translation[0], predicted_meas.translation[1],
predicted_meas.yaw()
]]).T
while (predicted_measurement[2] > math.pi):
predicted_measurement[2] -= 2 * math.pi
while (predicted_measurement[2] < -math.pi):
predicted_measurement[2] += 2 * math.pi
print "z_predicted", predicted_measurement.T
print "z", measurement.T
# visualize measurement
plot_point("gps", measurement[0:2])
k = self.calculateKalmanGain(self.sigma, self.H, self.R)
#print "kalman", k
#print "before", self.x.T
self.x = self.correctState(measurement, self.x, k,
predicted_measurement)
self.sigma = self.correctCovariance(self.sigma, k, self.H)
#print "after", self.x.T
# visualize position state
p = self.x[0:2]
plot_trajectory("kalman", p)
plot_covariance_2d("kalman", self.sigma[0:2, 0:2])
def measurement_callback(self, measurement):
'''
param measurement: vector of measured coordinates
'''
# Visualize measurement
plot_point("gps", measurement)
k = self.calculateKalmanGain(self.sigma, self.H, self.R)
self.x = self.correctState(measurement, self.x, k, self.H)
self.sigma = self.correctCovariance(self.sigma, k, self.H)
# Visualize position state
plot_trajectory("kalman", self.x[0:2])
plot_covariance_2d("kalman", self.sigma[0:2, 0:2])
def measurement_callback(self, measurement):
'''
:param measurement: vector of measured coordinates
'''
# visualize measurement
plot_point("gps", measurement)
k = self.calculateKalmanGain(self.sigma, self.H, self.R)
self.x = self.correctState(measurement, self.x, k, self.H)
self.sigma = self.correctCovariance(self.sigma, k, self.H)
# visualize position state
plot_trajectory("kalman", self.x[0:2])
plot_covariance_2d("kalman", self.sigma[0:2,0:2])