def correct(self, measurement):
"""Correct the state estimate and covariance matrix.
State estimates are fused, while covariances are accumulated.
Parameters
----------
measurement: localization.util.Measurement
Measured value used to correct the state prediction.
"""
update_size = len(measurement.update_vector)
state_subset = numpy.zeros([update_size, 1]) # x
measurement_subset = numpy.zeros([update_size, 1]) # z
measurement_covariance_subset = numpy.zeros([update_size,
update_size]) # R
state_to_measurement_subset = numpy.zeros(
[update_size, self.state.shape[0]]) # H
kalman_gain_subset = numpy.zeros([self.state.shape[0],
update_size]) # K
for idx in xrange(update_size):
measurement_subset[idx] = measurement.measurement[idx]
state_subset[idx] = self.state[measurement.update_vector[idx]]
for jdx in xrange(update_size):
measurement_covariance_subset[idx, jdx] = (
measurement.covariance[idx, jdx])
if measurement_covariance_subset[idx, idx] < 0.0:
measurement_covariance_subset[idx, idx] = math.fabs(
measurement_covariance_subset[idx, idx])
if measurement_covariance_subset[idx, idx] < 1e-9:
measurement_covariance_subset[idx, idx] = 1e-9
for idx, iui in enumerate(measurement.update_vector):
state_to_measurement_subset[idx, iui] = 1.0
pht = self.estimate_error_covariance.dot(state_to_measurement_subset.T)
hphr_inv = numpy.linalg.inv(
state_to_measurement_subset.dot(pht) +
measurement_covariance_subset)
kalman_gain_subset = pht.dot(hphr_inv)
innovation_subset = measurement_subset - state_subset # z - Hx
if self.checkMahalanobisThreshold(innovation_subset, hphr_inv,
measurement.mahalanobis_threshold):
for idx, iui in enumerate(measurement.update_vector):
if iui >= StateMember.roll and iui <= StateMember.yaw:
innovation_subset[idx] = clampRotation(
innovation_subset[idx])
self.state += kalman_gain_subset.dot(innovation_subset)
gain_residual = numpy.identity(self.state.shape[0])
gain_residual -= kalman_gain_subset.dot(
state_to_measurement_subset)
self.estimate_error_covariance = gain_residual.dot(
self.estimate_error_covariance).dot(gain_residual.T)
self.estimate_error_covariance += kalman_gain_subset.dot(
measurement_covariance_subset).dot(kalman_gain_subset.T)
self._wrapStateAngles(StateMember.roll, StateMember.pitch,
StateMember.yaw)
def correct(self, measurement):
"""Correct the state estimate and covariance matrix.
State estimates are fused, while covariances are accumulated.
Parameters
----------
measurement: localization.util.Measurement
Measured value used to correct the state prediction.
"""
update_size = len(measurement.update_vector)
state_subset = numpy.zeros([update_size, 1]) # x
measurement_subset = numpy.zeros([update_size, 1]) # z
measurement_covariance_subset = numpy.zeros(
[update_size, update_size]) # R
state_to_measurement_subset = numpy.zeros(
[update_size, self.state.shape[0]]) # H
kalman_gain_subset = numpy.zeros(
[self.state.shape[0], update_size]) # K
for idx in xrange(update_size):
measurement_subset[idx] = measurement.measurement[idx]
state_subset[idx] = self.state[measurement.update_vector[idx]]
for jdx in xrange(update_size):
measurement_covariance_subset[idx, jdx] = (
measurement.covariance[idx, jdx])
if measurement_covariance_subset[idx, idx] < 0.0:
measurement_covariance_subset[idx, idx] = math.fabs(
measurement_covariance_subset[idx, idx])
if measurement_covariance_subset[idx, idx] < 1e-9:
measurement_covariance_subset[idx, idx] = 1e-9
for idx, iui in enumerate(measurement.update_vector):
state_to_measurement_subset[idx, iui] = 1.0
pht = self.estimate_error_covariance.dot(state_to_measurement_subset.T)
hphr_inv = numpy.linalg.inv(state_to_measurement_subset.dot(pht) +
measurement_covariance_subset)
kalman_gain_subset = pht.dot(hphr_inv)
innovation_subset = measurement_subset - state_subset # z - Hx
if self.checkMahalanobisThreshold(
innovation_subset, hphr_inv, measurement.mahalanobis_threshold):
for idx, iui in enumerate(measurement.update_vector):
if iui >= StateMember.roll and iui <= StateMember.yaw:
innovation_subset[idx] = clampRotation(
innovation_subset[idx])
self.state += kalman_gain_subset.dot(innovation_subset)
gain_residual = numpy.identity(self.state.shape[0])
gain_residual -= kalman_gain_subset.dot(state_to_measurement_subset)
self.estimate_error_covariance = gain_residual.dot(
self.estimate_error_covariance).dot(
gain_residual.T)
self.estimate_error_covariance += kalman_gain_subset.dot(
measurement_covariance_subset).dot(
kalman_gain_subset.T)
self._wrapStateAngles(
StateMember.roll, StateMember.pitch, StateMember.yaw)
def _wrapStateAngles(self, *args):
"""Clamp all passed variables as if they were angles
Parameters
----------
*args: localization.util.StateMember
States that should be clamped to [-pi, pi].
"""
for s in args:
self.state[s] = clampRotation(self.state[s])
def _wrapStateAngles(self, *args):
"""Clamp all passed variables as if they were angles
Parameters
----------
*args: localization.util.StateMember
States that should be clamped to [-pi, pi].
"""
for s in args:
self.state[s] = clampRotation(self.state[s])
def correct(self, measurement):
"""Correct the state estimate and covariance matrix.
The particle positions are averaged to estimate a new state.
The covariance is estimated via the positions between particles.
State estimates are fused, while covariances are accumulated.
Parameters
----------
measurement: localization.util.Measurement
Measured value used to correct the state prediction.
"""
if not self._uncorrected:
self.__setSigmaPoints()
update_size = len(measurement.update_vector)
state_subset = numpy.zeros([update_size, 1]) # x
measurement_subset = numpy.zeros([update_size, 1]) # z
measurement_covariance_subset = numpy.zeros([update_size,
update_size]) # R
state_to_measurement_subset = numpy.zeros(
[update_size, self.state.shape[0]]) # H
kalman_gain_subset = numpy.zeros([self.state.shape[0],
update_size]) # K
innovation_subset = numpy.zeros([update_size, 1]) # z - Hx
predicted_measurement = numpy.zeros([update_size, 1])
predicted_meas_covar = numpy.zeros([update_size, update_size])
cross_covar = numpy.zeros([self.state.shape[0], update_size])
sigma_point_measurements = [
numpy.zeros([update_size, 1])
for _ in xrange(len(self._sigma_points))
]
for idx in xrange(update_size):
measurement_subset[idx] = measurement.measurement[idx]
state_subset[idx] = self.state[measurement.update_vector[idx]]
for jdx in xrange(update_size):
measurement_covariance_subset[idx, jdx] = (
measurement.covariance[idx, jdx])
if measurement_covariance_subset[idx, idx] < 0.0:
measurement_covariance_subset[idx, idx] = math.fabs(
measurement_covariance_subset[idx, idx])
if measurement_covariance_subset[idx, idx] < 1e-9:
measurement_covariance_subset[idx, idx] = 1e-9
for idx, iui in enumerate(measurement.update_vector):
state_to_measurement_subset[idx, iui] = 1.0
for idx in xrange(len(self._sigma_points)):
sigma_point_measurements[idx] = state_to_measurement_subset.dot(
self._sigma_points[idx])
predicted_measurement += (self._state_weights[idx] *
sigma_point_measurements[idx])
for idx in xrange(len(self._sigma_points)):
sigma_diff = sigma_point_measurements[idx] - predicted_measurement
predicted_meas_covar += self._covar_weights[idx] * (sigma_diff.dot(
sigma_diff.T))
cross_covar += self._covar_weights[idx] * (
(self._sigma_points[idx] - self.state).dot(sigma_diff.T))
inv_innov_cov = numpy.linalg.inv(predicted_meas_covar +
measurement_covariance_subset)
kalman_gain_subset = cross_covar.dot(inv_innov_cov)
innovation_subset = measurement_subset - predicted_measurement
if self.checkMahalanobisThreshold(innovation_subset, inv_innov_cov,
measurement.mahalanobis_threshold):
for idx, iui in enumerate(measurement.update_vector):
if iui >= StateMember.roll and iui <= StateMember.yaw:
innovation_subset[idx] = clampRotation(
innovation_subset[idx])
self.state += kalman_gain_subset.dot(innovation_subset)
self.estimate_error_covariance -= kalman_gain_subset.dot(
predicted_meas_covar).dot(kalman_gain_subset.T)
self._wrapStateAngles(StateMember.roll, StateMember.pitch,
StateMember.yaw)
self._uncorrected = False
