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
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])
]
