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