def generateMeasurement(self, real_state): """Generate a vision measurement based on the given state. Parameters ---------- real_state: numpy.ndarray A 15x1 array representing the actual state. Returns ------- localization.util.Measurement Generate a measurement based on images and changes. None In case that a measurement fails to get generated. """ view_points = self._generateVisibleSpatialPoints(real_state, 0.9) if len(self.view_points) == 0 or len(self.old_points) == 0: self.old_points = view_points return None # Transform all old points by our best transformation guess translation = self.v_lin * self.delta_time rotation = rpyToRotationMatrix(self.v_ang * self.delta_time) matches = matchFlann(view_points, self.old_points, translation, rotation) self.old_points = view_points raise NotImplementedError("Please Implement this method")
def generateMeasurement(self, real_state): """Generate a vision measurement based on the given state. Parameters ---------- real_state: numpy.ndarray A 15x1 array representing the actual state. Returns ------- localization.util.Measurement Generate a measurement based on images and changes. None In case that a measurement fails to get generated. """ view_points = self._generateVisibleSpatialPoints(real_state, 0.9) if len(self.view_points) == 0 or len(self.old_points) == 0: self.old_points = view_points return None # Transform all old points by our best transformation guess translation = self.v_lin * self.delta_time rotation = rpyToRotationMatrix(self.v_ang * self.delta_time) matches = matchFlann( view_points, self.old_points, translation, rotation) self.old_points = view_points raise NotImplementedError("Please Implement this method")
def generateMeasurement(self, real_state): """Generate an IMU measurement based on the given state. Gravity and gyro drift are added to this measurement. Parameters ---------- real_state: numpy.ndarray A 15x1 array representing the actual state. Returns ------- localization.util.Measurement Generate a measurement with added offsets, errors and noises. """ # Apply noise. meas = numpy.asarray( real_state[StateMember.v_roll:StateMember.a_z+1]).reshape(6) meas = numpy.random.multivariate_normal(meas, self.covariance) meas = numpy.asarray(meas).reshape([6, 1]) # Apply gyro drift. meas[0:3] += self.gyro_drift # Apply gravity. rpy = real_state[StateMember.roll:StateMember.yaw+1, 0].tolist() meas[3:6] += rpyToRotationMatrix(*rpy).dot(self.gravity_vector) return Measurement(0., meas, self.covariance, self.update_vector)
def generateMeasurement(self, real_state): """Generate an IMU measurement based on the given state. Gravity and gyro drift are added to this measurement. Parameters ---------- real_state: numpy.ndarray A 15x1 array representing the actual state. Returns ------- localization.util.Measurement Generate a measurement with added offsets, errors and noises. """ # Apply noise. meas = numpy.asarray(real_state[StateMember.v_roll:StateMember.a_z + 1]).reshape(6) meas = numpy.random.multivariate_normal(meas, self.covariance) meas = numpy.asarray(meas).reshape([6, 1]) # Apply gyro drift. meas[0:3] += self.gyro_drift # Apply gravity. rpy = real_state[StateMember.roll:StateMember.yaw + 1, 0].tolist() meas[3:6] += rpyToRotationMatrix(*rpy).dot(self.gravity_vector) return Measurement(0., meas, self.covariance, self.update_vector)
def predict(self, delta): """Predict the particle state estimates after a certain delay. The particle positions are estimated using the transfer function. Parameters ---------- delta: float Time in seconds since the last measurement. """ orientation = self.state[StateMember.roll:StateMember.yaw + 1] roll, pitch, yaw = orientation.reshape(3) i_delta = delta * delta * 0.5 rot = rpyToRotationMatrix(roll, pitch, yaw) rot_i = rot * delta rot_ii = rot * i_delta self._transfer_function[StateMember.x:StateMember.z + 1, StateMember.v_x:StateMember.v_z + 1] = rot_i self._transfer_function[StateMember.x:StateMember.z + 1, StateMember.a_x:StateMember.a_z + 1] = rot_ii self._transfer_function[StateMember.roll:StateMember.yaw + 1, StateMember.v_roll:StateMember.v_yaw + 1] = rot_i self._transfer_function[StateMember.v_x, StateMember.a_x] = delta self._transfer_function[StateMember.v_y, StateMember.a_y] = delta self._transfer_function[StateMember.v_z, StateMember.a_z] = delta self.__setSigmaPointsWithTF() self.state.fill(0.) for weight, point in zip(self._state_weights, self._sigma_points): self.state += weight * point self._wrapStateAngles(StateMember.roll, StateMember.pitch, StateMember.yaw) self._uncorrected = True self.estimate_error_covariance.fill(0.) for weight, point in zip(self._covar_weights, self._sigma_points): sigma_diff = point - self.state self.estimate_error_covariance += sigma_diff.dot( sigma_diff.T) * weight self.estimate_error_covariance += self.process_noise_covariance * delta