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)
