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