def calcOutput(self, t, controller_output, sensor_output):
if self.firstRun:
self.lin = Linearization(self.settings['lin state'],
self.settings['lin tau'])
self.A = self.lin.A
self.B = self.lin.B
self.C = self.lin.C
L_T = self.lin.ackerSISO(self.lin.A.transpose(),
self.lin.C.transpose(),
self.settings['poles'])
self.L = L_T.transpose()
self.x0 = self.settings['initial state'] #TODO set to zero
self.nextObserver_output = np.array([self.x0]).reshape(4, 1)
self.firstRun = False
self.observer_output = self.nextObserver_output
self.sensor_output = sensor_output
self.controller_output = controller_output
# sim_core Timer ist bereits einen Zeitschritt weiter
dt = self.settings['tick divider'] * self.step_width
dy = self.stateFuncApprox(t, self.observer_output)
self.nextObserver_output = self.observer_output + dt * dy
return [
float(self.observer_output[i, 0])
for i in range(self.observer_output.shape[0])
]
def calcOutput(self, t, controller_output, sensor_output):
if self.firstRun:
self.lin = Linearization(self.settings['lin state'],
self.settings['lin tau'])
self.A = self.lin.A
self.B = self.lin.B
self.C = self.lin.C
L_T = self.lin.ackerSISO(self.lin.A.transpose(),
self.lin.C.transpose(),
self.settings['poles'])
self.L = L_T.transpose()
self.solver = ode(self.stateFuncApprox)
self.solver.set_integrator('dopri5', method=self.settings['Method'], \
rtol=self.settings['rTol'],\
atol=self.settings['aTol'])
self.solver.set_initial_value(self.settings['initial state'])
self.x0 = self.settings['initial state'] #TODO set to zero
self.nextObserver_output = self.x0
self.firstRun = False
self.observer_output = self.nextObserver_output
self.sensor_output = sensor_output
self.controller_output = controller_output
# sim_core Timer ist bereits einen Zeitschritt weiter
self.nextObserver_output = self.solver.integrate(t)
return self.observer_output
def calcOutput(self, t, controller_output, sensor_output):
if self.firstRun:
self.lin = Linearization(self.settings['lin state'], self.settings['lin tau'])
self.A = self.lin.A
self.B = self.lin.B
self.C = self.lin.C
L_T = self.lin.ackerSISO(self.lin.A.transpose(),self.lin.C.transpose(),self.settings['poles'])
self.L = L_T.transpose()
self.x0 = self.settings['initial state'] #TODO set to zero
self.nextObserver_output = np.array([self.x0]).reshape(4,1)
self.firstRun = False
self.observer_output = self.nextObserver_output
self.sensor_output = sensor_output
self.controller_output = controller_output
# sim_core Timer ist bereits einen Zeitschritt weiter
dt = self.settings['tick divider']*self.step_width
dy = self.stateFuncApprox(t, self.observer_output)
self.nextObserver_output = self.observer_output + dt*dy
return [float(self.observer_output[i, 0]) for i in range(self.observer_output.shape[0])]
def calcOutput(self, t, controller_output, sensor_output):
if self.firstRun:
self.lin = Linearization(self.settings['lin state'], self.settings['lin tau'])
self.A = self.lin.A
self.B = self.lin.B
self.C = self.lin.C
L_T = self.lin.ackerSISO(self.lin.A.transpose(),self.lin.C.transpose(),self.settings['poles'])
self.L = L_T.transpose()
self.solver = ode(self.stateFuncApprox)
self.solver.set_integrator('dopri5', method=self.settings['Method'], \
rtol=self.settings['rTol'],\
atol=self.settings['aTol'])
self.solver.set_initial_value(self.settings['initial state'])
self.x0 = self.settings['initial state'] #TODO set to zero
self.nextObserver_output = self.x0
self.firstRun = False
self.observer_output = self.nextObserver_output
self.sensor_output = sensor_output
self.controller_output = controller_output
# sim_core Timer ist bereits einen Zeitschritt weiter
self.nextObserver_output = self.solver.integrate(t)
return self.observer_output
class LuenbergerObserver(Observer):
'''
Luenberger Observer uses EULER integration
'''
settings = {\
'initial state': [0, 0, 0, 0],\
'poles': [-20, -20, -20, -20],\
'lin state': [0, 0, 0, 0],\
'lin tau': 0,\
'tick divider': 1,\
}
def __init__(self):
self.output_dim = 4 #observer complete state
Observer.__init__(self)
self.firstRun = True
def setStepWidth(self, width):
self.step_width = width
def stateFuncApprox(self, t, q):
x_o = q
y = np.array(self.sensor_output)
u = self.controller_output
dx_o = np.dot(self.A - np.dot(self.L, self.C), x_o) + self.B*u + self.L * y[0]
return dx_o
#return [dx1_o, dx2_o, dx3_o, dx4_o]
def calcOutput(self, t, controller_output, sensor_output):
if self.firstRun:
self.lin = Linearization(self.settings['lin state'], self.settings['lin tau'])
self.A = self.lin.A
self.B = self.lin.B
self.C = self.lin.C
L_T = self.lin.ackerSISO(self.lin.A.transpose(),self.lin.C.transpose(),self.settings['poles'])
self.L = L_T.transpose()
self.x0 = self.settings['initial state'] #TODO set to zero
self.nextObserver_output = np.array([self.x0]).reshape(4,1)
self.firstRun = False
self.observer_output = self.nextObserver_output
self.sensor_output = sensor_output
self.controller_output = controller_output
# sim_core Timer ist bereits einen Zeitschritt weiter
dt = self.settings['tick divider']*self.step_width
dy = self.stateFuncApprox(t, self.observer_output)
self.nextObserver_output = self.observer_output + dt*dy
return [float(self.observer_output[i, 0]) for i in range(self.observer_output.shape[0])]
class LuenbergerObserverReduced(Observer):
'''
Luenberger Observer Reduced
'''
settings = {\
'initial state': [0, 0, 0, 0],\
'poles': [-3, -3, -3],\
'lin state': [0, 0, 0, 0],\
'lin tau': 0,\
'tick divider': 1,\
}
def __init__(self):
self.output_dim = 4 #observer complete state
Observer.__init__(self)
self.firstRun = True
def setStepWidth(self, width):
self.step_width = width
def swap_cols(self, arr, frm, to):
arr[:,[frm, to]] = arr[:,[to, frm]]
return arr
def swap_rows(self, arr, frm, to):
arr[[frm, to],:] = arr[[to, frm],:]
return arr
def calcOutput(self, t, controller_output, sensor_output):
if self.firstRun:
self.lin = Linearization(self.settings['lin state'], self.settings['lin tau'])
self.A = self.lin.A
self.B = self.lin.B
self.C = self.lin.C
n = self.A.shape[0]
r = self.C.shape[0]
if self.C.shape != (1, 4):
raise Exception('LuenbergerObserverReduced only useable for SISO')
# which cols and rows must be sorted
self.switch = 0
for i in range(self.C.shape[1]):
if self.C[0, i] == 1:
self.switch = i
break
# sort A,B,C for measured and unmeasured states
self.A = self.swap_cols(self.A, 0, self.switch)
# print 'A', self.A
self.A = self.swap_rows(self.A, 0, self.switch)
# print 'A', self.A
self.C = self.swap_cols(self.C, 0, self.switch)
self.B = self.swap_rows(self.B, 0, self.switch)
# get reduced system
self.A_11 = self.A[0:r, 0:r]
self.A_12 = self.A[0:r, r:n]
self.A_21 = self.A[r:n, 0:r]
self.A_22 = self.A[r:n, r:n]
self.B_1 = self.B[0:r, :]
self.B_2 = self.B[r:n, :]
L_T = self.lin.ackerSISO(self.A_22.transpose(), self.A_12.transpose(), self.settings['poles'])
self.L = L_T.transpose()
# init observer
self.x0 = self.settings['initial state']
self.nextObserver_output = np.array([self.x0]).reshape(4,1)
self.firstRun = False
self.observer_output = self.nextObserver_output
# ObserverOdeSolver reduced to EULER --> better performance
# the output is calculated in a reduced transformated system
dt = self.settings['tick divider']*self.step_width
n = self.A.shape[0]
r = self.C.shape[0]
# transform ((un-)measured) and collect necessary from observer_output
x_o = self.observer_output
x_o = self.swap_rows(x_o, 0, self.switch)
x_o = x_o[r:n, :]
u = controller_output
# FIXME: sensor_output is here a (1,4) list but should only be the
# measured value and check sensor_output order after transformation
y = np.array(sensor_output)
# here: y is r, the distance
y = y[0]
# transform system, so you dont need ydot
x_oTransf = x_o - self.L * y
dy = np.dot(self.A_22 - np.dot(self.L, self.A_12), x_oTransf)\
+ (self.A_21 - np.dot(self.L, self.A_11) + np.dot(self.A_22 - np.dot(self.L, self.A_12), self.L)) * y\
+ (self.B_2 - np.dot(self.L, self.B_1)) * u
# EULER integration
x_oTransf_next = x_oTransf + dt * dy
# transform system back to original observer coordinates
x_o_next = x_oTransf_next + self.L * y
observerOut = np.concatenate((np.array([[y]]), x_o_next), axis=0)
# change state order back to the original order
self.nextObserver_output = self.swap_rows(observerOut, 0, self.switch)
# we have the convention to return a list with shape (1, 4)
return [float(self.observer_output[i, 0]) for i in range(self.observer_output.shape[0])]
class LuenbergerObserverInt(Observer):
'''
Luenberger Observer that uses solver to integrate
'''
settings = {'Method': 'adams',\
'step size': 0.01,\
'rTol': 1e-6,\
'aTol': 1e-9,\
'end time': 10,\
'initial state': [0, 0, 0, 0],\
'poles': [-3, -3, -3, -3],\
'lin state': [0, 0, 0, 0],\
'lin tau': 0,\
'tick divider': 1,\
}
def __init__(self):
self.output_dim = 4 #observer complete state
Observer.__init__(self)
self.firstRun = True
def setStepWidth(self, width):
self.step_width = width
def stateFuncApprox(self, t, q):
x1_o, x2_o, x3_o, x4_o = q
y = np.array(self.sensor_output)
u = self.controller_output
x_o = np.array([[x1_o],\
[x2_o],\
[x3_o],\
[x4_o]])
#FIXME: sensorausgang mit C vermanschen
# ACHTUNG!!!! y[0] überdenken für mehrgrößenfall
#y = np.dot(self.C, y.transpose())
dx_o = np.dot(self.A - np.dot(self.L, self.C), x_o) + np.dot(self.B, u) + np.dot(self.L, y[0])
dx1_o = dx_o[0, 0]
dx2_o = dx_o[1, 0]
dx3_o = dx_o[2, 0]
dx4_o = dx_o[3, 0]
return [dx1_o, dx2_o, dx3_o, dx4_o]
def calcOutput(self, t, controller_output, sensor_output):
if self.firstRun:
self.lin = Linearization(self.settings['lin state'], self.settings['lin tau'])
self.A = self.lin.A
self.B = self.lin.B
self.C = self.lin.C
L_T = self.lin.ackerSISO(self.lin.A.transpose(),self.lin.C.transpose(),self.settings['poles'])
self.L = L_T.transpose()
self.solver = ode(self.stateFuncApprox)
self.solver.set_integrator('dopri5', method=self.settings['Method'], \
rtol=self.settings['rTol'],\
atol=self.settings['aTol'])
self.solver.set_initial_value(self.settings['initial state'])
self.x0 = self.settings['initial state'] #TODO set to zero
self.nextObserver_output = self.x0
self.firstRun = False
self.observer_output = self.nextObserver_output
self.sensor_output = sensor_output
self.controller_output = controller_output
# sim_core Timer ist bereits einen Zeitschritt weiter
self.nextObserver_output = self.solver.integrate(t)
return self.observer_output
class LuenbergerObserverReduced(Observer):
'''
Luenberger Observer Reduced
'''
settings = {\
'initial state': [0, 0, 0, 0],\
'poles': [-3, -3, -3],\
'lin state': [0, 0, 0, 0],\
'lin tau': 0,\
'tick divider': 1,\
}
def __init__(self):
self.output_dim = 4 #observer complete state
Observer.__init__(self)
self.firstRun = True
def setStepWidth(self, width):
self.step_width = width
def swap_cols(self, arr, frm, to):
arr[:, [frm, to]] = arr[:, [to, frm]]
return arr
def swap_rows(self, arr, frm, to):
arr[[frm, to], :] = arr[[to, frm], :]
return arr
def calcOutput(self, t, controller_output, sensor_output):
if self.firstRun:
self.lin = Linearization(self.settings['lin state'],
self.settings['lin tau'])
self.A = self.lin.A
self.B = self.lin.B
self.C = self.lin.C
n = self.A.shape[0]
r = self.C.shape[0]
if self.C.shape != (1, 4):
raise Exception(
'LuenbergerObserverReduced only useable for SISO')
# which cols and rows must be sorted
self.switch = 0
for i in range(self.C.shape[1]):
if self.C[0, i] == 1:
self.switch = i
break
# sort A,B,C for measured and unmeasured states
self.A = self.swap_cols(self.A, 0, self.switch)
# print 'A', self.A
self.A = self.swap_rows(self.A, 0, self.switch)
# print 'A', self.A
self.C = self.swap_cols(self.C, 0, self.switch)
self.B = self.swap_rows(self.B, 0, self.switch)
# get reduced system
self.A_11 = self.A[0:r, 0:r]
self.A_12 = self.A[0:r, r:n]
self.A_21 = self.A[r:n, 0:r]
self.A_22 = self.A[r:n, r:n]
self.B_1 = self.B[0:r, :]
self.B_2 = self.B[r:n, :]
L_T = self.lin.ackerSISO(self.A_22.transpose(),
self.A_12.transpose(),
self.settings['poles'])
self.L = L_T.transpose()
# init observer
self.x0 = self.settings['initial state']
self.nextObserver_output = np.array([self.x0]).reshape(4, 1)
self.firstRun = False
self.observer_output = self.nextObserver_output
# ObserverOdeSolver reduced to EULER --> better performance
# the output is calculated in a reduced transformated system
dt = self.settings['tick divider'] * self.step_width
n = self.A.shape[0]
r = self.C.shape[0]
# transform ((un-)measured) and collect necessary from observer_output
x_o = self.observer_output
x_o = self.swap_rows(x_o, 0, self.switch)
x_o = x_o[r:n, :]
u = controller_output
# FIXME: sensor_output is here a (1,4) list but should only be the
# measured value and check sensor_output order after transformation
y = np.array(sensor_output)
# here: y is r, the distance
y = y[0]
# transform system, so you dont need ydot
x_oTransf = x_o - self.L * y
dy = np.dot(self.A_22 - np.dot(self.L, self.A_12), x_oTransf)\
+ (self.A_21 - np.dot(self.L, self.A_11) + np.dot(self.A_22 - np.dot(self.L, self.A_12), self.L)) * y\
+ (self.B_2 - np.dot(self.L, self.B_1)) * u
# EULER integration
x_oTransf_next = x_oTransf + dt * dy
# transform system back to original observer coordinates
x_o_next = x_oTransf_next + self.L * y
observerOut = np.concatenate((np.array([[y]]), x_o_next), axis=0)
# change state order back to the original order
self.nextObserver_output = self.swap_rows(observerOut, 0, self.switch)
# we have the convention to return a list with shape (1, 4)
return [
float(self.observer_output[i, 0])
for i in range(self.observer_output.shape[0])
]
class LuenbergerObserverInt(Observer):
'''
Luenberger Observer that uses solver to integrate
'''
settings = {'Method': 'adams',\
'step size': 0.01,\
'rTol': 1e-6,\
'aTol': 1e-9,\
'end time': 10,\
'initial state': [0, 0, 0, 0],\
'poles': [-3, -3, -3, -3],\
'lin state': [0, 0, 0, 0],\
'lin tau': 0,\
'tick divider': 1,\
}
def __init__(self):
self.output_dim = 4 #observer complete state
Observer.__init__(self)
self.firstRun = True
def setStepWidth(self, width):
self.step_width = width
def stateFuncApprox(self, t, q):
x1_o, x2_o, x3_o, x4_o = q
y = np.array(self.sensor_output)
u = self.controller_output
x_o = np.array([[x1_o],\
[x2_o],\
[x3_o],\
[x4_o]])
#FIXME: sensorausgang mit C vermanschen
# ACHTUNG!!!! y[0] überdenken für mehrgrößenfall
#y = np.dot(self.C, y.transpose())
dx_o = np.dot(self.A - np.dot(self.L, self.C), x_o) + np.dot(
self.B, u) + np.dot(self.L, y[0])
dx1_o = dx_o[0, 0]
dx2_o = dx_o[1, 0]
dx3_o = dx_o[2, 0]
dx4_o = dx_o[3, 0]
return [dx1_o, dx2_o, dx3_o, dx4_o]
def calcOutput(self, t, controller_output, sensor_output):
if self.firstRun:
self.lin = Linearization(self.settings['lin state'],
self.settings['lin tau'])
self.A = self.lin.A
self.B = self.lin.B
self.C = self.lin.C
L_T = self.lin.ackerSISO(self.lin.A.transpose(),
self.lin.C.transpose(),
self.settings['poles'])
self.L = L_T.transpose()
self.solver = ode(self.stateFuncApprox)
self.solver.set_integrator('dopri5', method=self.settings['Method'], \
rtol=self.settings['rTol'],\
atol=self.settings['aTol'])
self.solver.set_initial_value(self.settings['initial state'])
self.x0 = self.settings['initial state'] #TODO set to zero
self.nextObserver_output = self.x0
self.firstRun = False
self.observer_output = self.nextObserver_output
self.sensor_output = sensor_output
self.controller_output = controller_output
# sim_core Timer ist bereits einen Zeitschritt weiter
self.nextObserver_output = self.solver.integrate(t)
return self.observer_output
class LuenbergerObserver(Observer):
'''
Luenberger Observer uses EULER integration
'''
settings = {\
'initial state': [0, 0, 0, 0],\
'poles': [-20, -20, -20, -20],\
'lin state': [0, 0, 0, 0],\
'lin tau': 0,\
'tick divider': 1,\
}
def __init__(self):
self.output_dim = 4 #observer complete state
Observer.__init__(self)
self.firstRun = True
def setStepWidth(self, width):
self.step_width = width
def stateFuncApprox(self, t, q):
x_o = q
y = np.array(self.sensor_output)
u = self.controller_output
dx_o = np.dot(self.A - np.dot(self.L, self.C),
x_o) + self.B * u + self.L * y[0]
return dx_o
#return [dx1_o, dx2_o, dx3_o, dx4_o]
def calcOutput(self, t, controller_output, sensor_output):
if self.firstRun:
self.lin = Linearization(self.settings['lin state'],
self.settings['lin tau'])
self.A = self.lin.A
self.B = self.lin.B
self.C = self.lin.C
L_T = self.lin.ackerSISO(self.lin.A.transpose(),
self.lin.C.transpose(),
self.settings['poles'])
self.L = L_T.transpose()
self.x0 = self.settings['initial state'] #TODO set to zero
self.nextObserver_output = np.array([self.x0]).reshape(4, 1)
self.firstRun = False
self.observer_output = self.nextObserver_output
self.sensor_output = sensor_output
self.controller_output = controller_output
# sim_core Timer ist bereits einen Zeitschritt weiter
dt = self.settings['tick divider'] * self.step_width
dy = self.stateFuncApprox(t, self.observer_output)
self.nextObserver_output = self.observer_output + dt * dy
return [
float(self.observer_output[i, 0])
for i in range(self.observer_output.shape[0])
]
