def __init__(self, model, initialTime=0, initialPosition=np.zeros((3,1)), initialVelocity=np.zeros((3,1)), accelerationVar=0.2, velocityVar=0.5, rejectionProbabilityThreshold=0.4, **kwargs): """ @param initialVelocity: Initial velocity estimate (3x1 L{np.ndarray}) @param accelerationVar: Acceleration variance (float). @param velocityVar: Velocity variance (float). @param rejectionProbabilityThreshold: Probability below which a velocity update will be rejected (float, 0 <= p <= 1) """ PositionEstimator.__init__(self, model, initialTime, initialPosition) self._accelerationSigma = math.sqrt(accelerationVar) self._measurementVariance = np.diag([velocityVar]*3) self._rejectionThreshold = rejectionProbabilityThreshold self._points = list(model.points) for point in self._points: point.contactProbabilities = TimeSeries() self.kalman = KalmanFilter( stateTransitionMatrix = self.transitionMatrix(1), controlMatrix = self.controlMatrix(1), measurementMatrix = np.hstack((np.zeros((3,3)),np.eye(3))), state = np.vstack((initialPosition,initialVelocity)), stateCovariance = self.processCovariance(1), processCovariance = self.processCovariance(1), measurementCovariance = self._measurementVariance)
def kf(q, states): stateUpdate = np.diag([c] * states) control = None measurement = np.diag([1] * states) initialState = np.array([[0]] * states) initialCovariance = np.diag([0.5] * states) measurementCovariance = np.diag([r] * states) return KalmanFilter(stateUpdate, control, measurement, initialState, initialCovariance, np.diag(list(q) * states), measurementCovariance)
class ContactTrackingKalmanFilter(PositionEstimator): """ Estimates position based on root acceleration and estimated ground contact. """ @prepend_method_doc(PositionEstimator) def __init__(self, model, initialTime=0, initialPosition=np.zeros((3,1)), initialVelocity=np.zeros((3,1)), accelerationVar=0.2, velocityVar=0.5, rejectionProbabilityThreshold=0.4, **kwargs): """ @param initialVelocity: Initial velocity estimate (3x1 L{np.ndarray}) @param accelerationVar: Acceleration variance (float). @param velocityVar: Velocity variance (float). @param rejectionProbabilityThreshold: Probability below which a velocity update will be rejected (float, 0 <= p <= 1) """ PositionEstimator.__init__(self, model, initialTime, initialPosition) self._accelerationSigma = math.sqrt(accelerationVar) self._measurementVariance = np.diag([velocityVar]*3) self._rejectionThreshold = rejectionProbabilityThreshold self._points = list(model.points) for point in self._points: point.contactProbabilities = TimeSeries() self.kalman = KalmanFilter( stateTransitionMatrix = self.transitionMatrix(1), controlMatrix = self.controlMatrix(1), measurementMatrix = np.hstack((np.zeros((3,3)),np.eye(3))), state = np.vstack((initialPosition,initialVelocity)), stateCovariance = self.processCovariance(1), processCovariance = self.processCovariance(1), measurementCovariance = self._measurementVariance) def _update(self, data, dt, t): acceleration = self.getParameter(data, self._model.name, 'linearAcceleration', default=np.zeros((3,1))) self.kalman.stateTransitionMatrix = self.transitionMatrix(dt) self.kalman.controlMatrix = self.controlMatrix(dt) self.kalman.processCovariance = self.processCovariance(dt) self.kalman.predict(acceleration) velocity, probability = self.estimateVelocityProbabilities(dt, t) if probability > self._rejectionThreshold: self.kalman.correct(velocity) return self.kalman.state[:3], self.kalman.stateCovariance[:3, :3] def estimateVelocityProbabilities(self, dt, t): """ Estimate the possible velocities (and probabilities) of the model. Estimates are based on the assumption that a point is stationary in world co-ordinates in the interval `[t-dt,t]` @return: The velocity with the highest probabilty of not being wrong """ vPredicted = self.kalman.state[3:] inv_cov = self.kalman.stateCovariance[3:6,3:6].I highestProbability = 0 for point in self._points: vHat = -(point.position(t) - point.position(t-dt)) / dt vError = vHat - vPredicted mahalanobis_distance = np.sqrt(vError.T * inv_cov * vError) probability = 2 * scipy.stats.norm.cdf(-mahalanobis_distance) if probability > highestProbability: highestProbability = probability estimatedVelocity = vHat point.contactProbabilities.add(t, probability[0,0]) return estimatedVelocity, highestProbability def transitionMatrix(self, dt): A = np.matrix([ [1, 0, 0, dt, 0, 0], [0, 1, 0, 0, dt, 0], [0, 0, 1, 0, 0, dt], [0, 0, 0, 1, 0, 0 ], [0, 0, 0, 0, 1, 0 ], [0, 0, 0, 0, 0, 1 ]]) return A def controlMatrix(self, dt): a = 0.5 * dt**2 b = dt B = np.matrix([ [a, 0, 0], [0, a, 0], [0, 0, a], [b, 0, 0], [0, b, 0], [0, 0, b]]) return B def processCovariance(self, dt): a = (0.5 * dt**2)**2 * self._accelerationSigma**2 b = ((0.5 * dt**2) * self._accelerationSigma) * (dt * self._accelerationSigma) c = dt**2 * self._accelerationSigma**2 Q = np.matrix([ [a, 0, 0, b, 0, 0], [0, a, 0, 0, b, 0], [0, 0, a, 0, 0, b], [b, 0, 0, c, 0, 0], [0, b, 0, 0, c, 0], [0, 0, b, 0, 0, c]]) return Q