class KalmanPredictor:
state = [0.0, 0.0, 0.0]
def __init__(self, timeStep):
self.dt = timeStep
self.localObserver = KalmanFilter(3, 3)
self.localObserver.SetMeasurement(0, 0, 1.0)
self.localObserver.SetMeasurement(1, 1, 1.0)
self.localObserver.SetMeasurement(2, 2, 1.0)
self.localObserver.SetStatePredictionFactor(0, 1, self.dt)
self.localObserver.SetStatePredictionFactor(0, 2, 0.5 * self.dt**2)
self.localObserver.SetStatePredictionFactor(1, 2, self.dt)
self.remoteObserver = KalmanFilter(3, 3)
self.remoteObserver.SetMeasurement(0, 0, 1.0)
self.remoteObserver.SetMeasurement(1, 1, 1.0)
self.remoteObserver.SetMeasurement(2, 2, 1.0)
self.remoteObserver.SetPredictionNoise(0, 2.0)
self.remoteObserver.SetPredictionNoise(1, 2.0)
self.remoteObserver.SetPredictionNoise(2, 2.0)
def Process(self, inputPacket, remoteTime, timeDelay):
setpoint, setpointVelocity, setpointAcceleration = inputPacket
measurement = [setpoint, setpointVelocity, setpointAcceleration]
timeDelay = int(timeDelay / self.dt) * self.dt
self.remoteObserver.SetStatePredictionFactor(0, 1, timeDelay)
self.remoteObserver.SetStatePredictionFactor(0, 2, 0.5 * timeDelay**2)
self.remoteObserver.SetStatePredictionFactor(1, 2, timeDelay)
newState, prediction = self.remoteObserver.Predict(self.state)
self.state, estimatedMeasurement = self.localObserver.Process(
measurement)
self.remoteObserver.Update(measurement, estimatedMeasurement)
self.remoteObserver.Correct()
return [prediction[0], prediction[1], prediction[2]]
class KalmanPredictor:
predictions = []
def __init__(self, timeStep):
self.dt = timeStep
self.remoteObserver = KalmanFilter(3, 3)
self.remoteObserver.SetMeasurement(0, 0, 1.0)
self.remoteObserver.SetMeasurement(1, 1, 1.0)
self.remoteObserver.SetMeasurement(2, 2, 1.0)
self.remoteObserver.SetPredictionNoise(0, 2.0)
self.remoteObserver.SetPredictionNoise(1, 2.0)
self.remoteObserver.SetPredictionNoise(2, 2.0)
def Process(self, inputPacket, remoteTime, timeDelay):
setpoint, setpointVelocity, setpointAcceleration = inputPacket
measurement = [setpoint, setpointVelocity, setpointAcceleration]
timeDelay = int(timeDelay / self.dt) * self.dt
self.remoteObserver.SetStatePredictionFactor(0, 1, timeDelay)
self.remoteObserver.SetStatePredictionFactor(0, 2, 0.5 * timeDelay**2)
self.remoteObserver.SetStatePredictionFactor(1, 2, timeDelay)
state, prediction = self.remoteObserver.Predict(measurement)
if len(self.predictions) > 0:
if remoteTime >= self.predictions[0][1]:
estimatedMeasurement = self.predictions.pop(0)[0]
self.remoteObserver.Update(measurement, estimatedMeasurement)
self.remoteObserver.Correct()
self.predictions.append((prediction, remoteTime + timeDelay))
return [prediction[0], prediction[1], prediction[2]]
