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