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)