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]]
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 LQGPredController:
measurement = [0.0, 0.0, 0.0]
controlForce = 0.0
def __init__(self, inertia, damping, stiffness, timeStep):
self.dt = timeStep
self.localController = LQGController(inertia, damping, stiffness,
timeStep)
self.remoteStateObserver = KalmanFilter(3, 3)
self.remoteStateObserver.SetMeasurement(0, 0, 4.0)
self.remoteStateObserver.SetMeasurement(1, 1, 4.0)
self.remoteStateObserver.SetMeasurement(2, 2, 4.0)
self.remoteInputObserver = KalmanFilter(3, 1)
self.remoteInputObserver.SetMeasurement(0, 0, 1.0)
def SetSystem(self, inertia, damping, stiffness):
self.localController.SetSystem(inertia, damping, stiffness)
def Predict(self, remoteMeasurement, remoteForce, timeDelay):
timeDelay = int(timeDelay / self.dt) * self.dt
predictedState = list(remoteMeasurement)
predictedState[0] += predictedState[1] * timeDelay + predictedState[
2] * 0.5 * timeDelay**2
predictedState[1] += predictedState[2] * timeDelay
remoteState, predictedState = self.remoteStateObserver.Process(
predictedState)
remoteState, predictedForce = self.remoteInputObserver.Predict()
remoteState, predictedForce = self.remoteInputObserver.Update(
[remoteForce], [remoteState[0]])
predictedForce[0] += remoteState[1] * timeDelay + remoteState[
2] * 0.5 * timeDelay**2
return (tuple(predictedState), predictedForce[0])
def Process(self, setpoint, measurement, externalForce):
return self.localController.Process(setpoint, measurement,
externalForce)
