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 WavePredTeleoperator:
remotePosition = 0.0
outputWaveIntegral = 0.0
def __init__( self, impedance, timeStep ):
self.waveController = WaveController( impedance[ 0 ] + impedance[ 1 ] + impedance[ 2 ], timeStep )
self.dt = timeStep
self.waveObserver = KalmanFilter( 2, 2 )
self.waveObserver.SetMeasurement( 0, 0, 4.0 )
self.waveObserver.SetMeasurement( 1, 1, 4.0 )
self.waveObserver.SetStatePredictionFactor( 0, 1, self.dt )
def SetSystem( self, impedance ):
waveImpedance = ( max( impedance[ 0 ], 0.0 ), max( impedance[ 1 ], 1.0 ), max( impedance[ 2 ], 0.0 ) )
self.waveController.SetImpedance( waveImpedance[ 0 ] + waveImpedance[ 1 ] + waveImpedance[ 2 ] )
def Process( self, localState, localForce, remotePacket, timeDelay ):
localPosition, localVelocity, localAcceleration = localState
inputWave, inputWaveIntegral, inputEnergy, remoteImpedance = remotePacket
remoteState, predictedWave = self.waveObserver.Process( [ inputWaveIntegral + inputWave * timeDelay, inputWave ] )
remoteVelocity = self.waveController.PreProcess( predictedWave[ 1 ], inputEnergy, remoteImpedance )
feedbackForce, outputWave, outputEnergy, localImpedance = self.waveController.PostProcess( localVelocity, localForce )
self.remotePosition += remoteVelocity * self.dt
self.outputWaveIntegral += outputWave * self.dt
return ( feedbackForce, ( self.remotePosition, remoteVelocity, 0.0 ), ( outputWave, self.outputWaveIntegral, outputEnergy, localImpedance ) )
class LQGController:
controlForce = 0.0
def __init__(self, inertia, damping, stiffness, timeStep):
self.dt = timeStep
self.observer = KalmanFilter(3, 3, 1)
self.observer.SetMeasurement(0, 0, 1.0)
self.observer.SetMeasurement(1, 1, 1.0)
self.observer.SetMeasurement(2, 2, 1.0)
self.observer.SetStatePredictionFactor(0, 1, self.dt)
self.observer.SetStatePredictionFactor(0, 2, 0.5 * self.dt**2)
self.observer.SetStatePredictionFactor(1, 2, self.dt)
self.observer.SetStatePredictionFactor(2, 2, 0.0)
self.SetSystem(inertia, damping, stiffness)
def SetSystem(self, inertia, damping, stiffness):
A = [[1, self.dt, 0.5 * self.dt**2], [0, 1, self.dt],
[-stiffness / inertia, -damping / inertia, 0]]
B = [[0], [0], [1 / inertia]]
C = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
self.feedbackGain = GetLQRController(A, B, C, 0.0001)
self.observer.SetStatePredictionFactor(2, 0, -stiffness / inertia)
self.observer.SetStatePredictionFactor(2, 1, -damping / inertia)
self.observer.SetInputPredictionFactor(2, 0, 1 / inertia)
def Process(self, setpoint, measurement, externalForce):
reference = [
measurement[0] - setpoint[0], measurement[1] - setpoint[1],
measurement[2] - setpoint[2]
]
state, error = self.observer.Process(
reference, [self.controlForce + externalForce])
self.controlForce = -self.feedbackGain.dot(state)[0]
return self.controlForce
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]]
