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
