def full_observer(system, poles_o, poles_s, debug=False) -> ObserverResult:
"""implementation of the complete observer for an autonomous system
:param system : tuple (a, b, c) of system matrices
:param poles_o: tuple of complex poles/eigenvalues of the observer
dynamics
:param poles_s: tuple of complex poles/eigenvalues of the closed
system
:param debug: output control for debugging in unittest(False:normal
output,True: output local variables and normal output)
:return: dataclass ObserverResult (controller function, feedback matrix, observer_gain, and pre-filter)
"""
# systemically relevant matrices
a = system[0]
b = system[1]
c = system[2]
d = system[3]
# ignore the PendingDeprecationWarning for built-in packet control
warnings.filterwarnings('ignore', category=PendingDeprecationWarning)
ctr_matrix = ctr.ctrb(a, b) # controllabilty matrix
assert np.linalg.det(ctr_matrix) != 0, 'this system is not controllable'
# State feedback
f_t = ctr.acker(a, b, poles_s)
# pre-filter
a1 = a - b * f_t
v = (-1 * (c * a1**(-1) * b)**(-1)) # pre-filter
obs_matrix = ctr.obsv(a, c) # observability matrix
assert np.linalg.det(obs_matrix) != 0, 'this system is unobservable'
# calculate observer gain
l_v = ctr.acker(a.T, c.T, poles_o).T
# controller function
# in this case controller function is: -1 * feedback * states + pre-filter * reference output
# disturbance value ist not considered
# t = sp.Symbol('t')
# sys_input = -1 * (f_t * sys_state)[0]
# input_func = sp.lambdify((sys_state, t), sys_input, modules='numpy')
# this method returns controller function, feedback matrix, observer gain and pre-filter
result = ObserverResult(f_t, l_v, v, None)
# return internal variables for unittest
if debug:
c_locals = Container(fetch_locals=True)
result.debug = c_locals
return result
def resolve_keys(entity):
"""
For quick progress almost all model fields are strings. This function converts those fields, which contains keys
to contain the real reference (or list of references).
:param entity:
:return:
"""
entity_type = type(entity)
fields = entity_type.get_fields()
# endow every entity with an object container:
entity.oc = Container()
for field in fields:
if isinstance(field, models.EntityKeyField):
# example: get the content of entity.predecessor_key
refkey = getattr(entity, field.name)
if refkey:
try:
ref_entity = get_entity(refkey)
except ValueError as ve:
msg = f"Bad refkey detected when processing field {field.name} of {entity}. " \
f"Original error: {ve.args[0]}"
raise InconsistentMetaDataError(msg)
else:
ref_entity = None
# save the real object to the object container (allow later access)
setattr(entity.oc, field.name, ref_entity)
elif isinstance(field, models.EntityKeyListField):
refkeylist_str = getattr(entity, field.name)
if refkeylist_str is None:
msg = f"There is a problem with the field {field.name} in entity {entity.key}."
raise InconsistentMetaDataError(msg)
refkeylist = yaml.load(refkeylist_str, Loader=yaml.FullLoader)
if refkeylist in (None, [], [""]):
refkeylist = []
try:
entity_list = [get_entity(refkey) for refkey in refkeylist]
except ValueError as ve:
msg = f"Bad refkey detected when processing field {field.name} of {entity}. "\
f"Original error: {ve.args[0]}"
raise InconsistentMetaDataError(msg)
setattr(entity.oc, field.name, entity_list)
def transformation(system, debug=False):
"""This function is used to divide system, input and
output matrix into measurable and unmeasurable states
or renumber them.
:param system : tuple (a, b, c) of system matrices
:param debug: output control for debugging in unittest(False:normal
output,True: output local variables and normal output)
:return: renumbered matrices a,b,c
"""
a = np.array(system[0]).copy()
b = np.array(system[1]).copy()
c = np.array(system[2]).copy()
d = np.array(system[3]).copy()
index_list = []
# find out which elements in C are equal to one.
for i in range(c.shape[0]):
for j in range(c.shape[1]):
if c[i, j] == 1:
index_list.append([i, j])
# exchange row and column
for [i1, j1] in index_list:
if i1 != j1:
c[:, [j1, i1]] = c[:, [i1, j1]]
a[[[j1, i1]], :] = a[[[i1, j1]], :]
a[:, [j1, i1]] = a[:, [i1, j1]]
b[[j1, i1], :] = b[[i1, j1], :]
if debug:
c_locals = Container(fetch_locals=True)
return c_locals
return a, b, c, d
def reduced_observer(system, poles_o, poles_s, x0, tt, debug=False):
"""
:param system: tuple (A, B, C) of system matrices
:param poles_o: tuple of complex poles/eigenvalues of the
observer dynamics
:param poles_s: tuple of complex poles/eigenvalues of the
closed system
:param x0: initial condition
:param tt: vector for the time axis
:param debug: output control for debugging in unittest
(False:normal output,True: output local variables
and normal output)
:return yy: output of the system
:return xx: states of the system
:return tt: time of the simulation
"""
# renumber the matrices to construct the matrices
# in the form of the reduced observer
a, b, c, d = ob_func.transformation(system, debug=False)
# row rank of the output matrix
rank = np.linalg.matrix_rank(c[0])
# submatrices of the system matrix A
# and the input matrix
a_11 = a[0:rank, 0:rank]
a_12 = a[0:rank, rank:a.shape[1]]
a_21 = a[rank:a.shape[1], 0:rank]
a_22 = a[rank:a.shape[1], rank:a.shape[1]]
b_1 = b[0:rank, :]
b_2 = b[rank:b.shape[0], :]
# construct the observability matrix
# q_1 = a_12
# q_2 = a_12 * a_22
# q = np.vstack([q_1, q_2])
obs_matrix = ctr.obsv(a_22, a_12) # observability matrix
rank_q = np.linalg.matrix_rank(obs_matrix)
assert np.linalg.det(obs_matrix) != 0, 'this system is unobservable'
# ignore the PendingDeprecationWarning for built-in packet control
warnings.filterwarnings('ignore', category=PendingDeprecationWarning)
# state feedback
f_t = ctr.acker(a, b, poles_s)
c_ = c - d * f_t
f_1 = f_t[:, 0:1]
f_2 = f_t[:, 1:]
l_ = ctr.acker(a_22.T, a_12.T, poles_o).T
# State representation of the entire system
# Original system and observer error system,
# Dimension: 4 x 4)
sys = ob_func.state_space_func(a, a_11, a_12, a_21, a_22, b, b_1, b_2, c_,
l_, f_1, f_2)
# simulation for the proper movement
yy, tt, xx = ctr.matlab.initial(sys, tt, x0, return_x=True)
result = yy, xx, tt, sys
# return internal variables for unittest
if debug:
c_locals = Container(fetch_locals=True)
return result, c_locals
return result
def check_solution(key):
"""
:param key: entity key of the ProblemSolution
:return:
"""
sol_entity = get_entity(key)
resolve_keys(sol_entity)
assert isinstance(sol_entity, models.ProblemSolution)
# get path for solution
solution_file = sol_entity.solution_file
if solution_file != "solution.py":
msg = "Arbitrary filename will be supported in the future"
raise NotImplementedError(msg)
c = Container(
) # this will be our easily accessible context dict for the template
# TODO: handle the filename (see also template)
c.solution_path = os.path.join(root_path, sol_entity.base_path)
c.ackrep_core_path = core_pkg_path
# noinspection PyUnresolvedReferences
assert isinstance(sol_entity.oc, ObjectContainer)
assert len(sol_entity.oc.solved_problem_list) >= 1
if sol_entity.oc.solved_problem_list == 0:
msg = f"{sol_entity}: Expected at least one solved problem."
raise InconsistentMetaDataError(msg)
elif sol_entity.oc.solved_problem_list == 1:
problem_spec = sol_entity.oc.solved_problem_list[0]
else:
print(
"Applying a solution to multiple problems is not yet supported. Taking the last one."
)
problem_spec = sol_entity.oc.solved_problem_list[-1]
if problem_spec.problem_file != "problem.py":
msg = "Arbitrary filename will be supported in the future"
raise NotImplementedError(msg)
# TODO: handle the filename (see also template)
c.problem_spec_path = os.path.join(root_path, problem_spec.base_path)
# list of the build_paths
c.method_package_list = []
for mp in sol_entity.oc.method_package_list:
full_build_path = make_method_build(mp, accept_existing=True)
assert os.path.isdir(full_build_path)
c.method_package_list.append(full_build_path)
context = dict(c.item_list())
print(" ... Creating exec-script ... ")
scriptname = "execscript.py"
assert not sol_entity.base_path.startswith(os.path.sep)
# determine whether the entity comes from ackrep_data or ackrep_data_for_unittests ore ackrep_data_import
data_repo_path = pathlib.Path(sol_entity.base_path).parts[0]
scriptpath = os.path.join(root_path, data_repo_path, scriptname)
render_template("templates/execscript.py.template",
context,
target_path=scriptpath)
print(f" ... running exec-script {scriptpath} ... ")
# TODO: plug in containerization here:
# Note: this hangs on any interactive element inside the script (such as IPS)
res = subprocess.run(["python", scriptpath], capture_output=True)
res.exited = res.returncode
res.stdout = res.stdout.decode("utf8")
res.stderr = res.stderr.decode("utf8")
return res
def state_feedback(system: tuple,
poles_o: list,
sys_state: sp.Matrix,
eqrt: list,
yr,
debug=False) -> StateFeedbackResult:
"""
:param system : tuple (a, b, c, d) of system matrices
:param poles_o: tuple of closed-loop poles of the system
:param sys_state: states of nonlinear system
:param eqrt: equilibrium points of the system
:param yr: reference output
:param debug: output control for debugging in unittest(False:normal
output,True: output local variables and normal output)
:return: dataclass StateFeedbackResult (controller function, feedback matrix and pre-filter)
"""
# ignore the PendingDeprecationWarning for built-in packet control
warnings.filterwarnings('ignore', category=PendingDeprecationWarning)
# system matrices
a = system[0]
b = system[1]
c = system[2]
d = system[3]
ctr_matrix = ctr.ctrb(a, b) # controllabilty matrix
assert np.linalg.det(ctr_matrix) != 0, 'this system is not controllable'
# full state feedback
f_t = ctr.acker(a, b, poles_o)
# pre-filter
a1 = a - b * f_t
v = -1 * (c * a1**(-1) * b)**(-1) # pre-filter
'''Since the controller, which is designed on the basis of a linearized system,
is a small signal model, the states have to be converted from the large signal model
to the small signal model. i.e. the equilibrium points of the original non-linear system must
be subtracted from the returned states.
x' = x - x0
x: states of nonlinear system (large signal model)
x0: equilibrium points
x': states of controller (small signal model)
And for the same reason, the equilibrium position of the
input must be added to controller function.
'''
t = sp.Symbol('t')
# convert states to small signal model
small_state = sys_state - sp.Matrix(
[eqrt[i][1] for i in range(len(sys_state))])
# creat controller function
# in this case controller function is: -1 * feedback * states + pre-filter * reference output
# disturbance value ist not considered
sys_input = -1 * (f_t *
small_state)[0] + v[0] * yr + eqrt[len(sys_state)][1]
input_func = sp.lambdify((sys_state, t), sys_input, modules='numpy')
# this method returns controller function, feedback matrix and pre-filter
result = StateFeedbackResult(input_func, f_t, v, None)
# return internal variables for unittest
if debug:
c_locals = Container(fetch_locals=True)
result.debug = c_locals
return result
def lqr_method(system, q_matrix, r_matrix, sys_state, eqrt, yr, debug=False):
"""
:param system : tuple (a, b, c, d) of system matrices
:param q_matrix: State weights matrix
:param r_matrix: input weights matrix
:param sys_state: states of nonlinear system
:param eqrt: equilibrium points of the system
:param yr: reference output
:param debug: output control for debugging in unittest(False:normal
output,True: output local variables and normal output)
:return result: data class LQR_Result (controller function, feedback matrix pre-filter
and poles of closed loop)
"""
# ignore the PendingDeprecationWarning for built-in packet control
warnings.filterwarnings('ignore', category=PendingDeprecationWarning)
# system matrices
a = system[0]
b = system[1]
c = system[2]
d = system[3]
# computes the optimal state feedback controller that minimizes the quadratic cost function
res = ctr.lqr(a, b, q_matrix, r_matrix)
# full state feedback
f_t = res[0]
a1 = a - b * f_t
v = -1 * (c * a1**(-1) * b)**(-1) # pre-filter
'''Since the controller, which is designed on the basis of a linearized system,
is a small signal model, the states have to be converted from the large signal model
to the small signal model. i.e. the equilibrium points of the original non-linear system must
be subtracted from the returned states.
x' = x - x0
x: states of nonlinear system (large signal model)
x0: equilibrium points
x': states of controller (small signal model)
And for the same reason, the equilibrium position of the
input must be added to controller function.
'''
t = sp.Symbol('t')
# convert states to small signal model
small_state = sys_state - sp.Matrix(
[eqrt[i][1] for i in range(len(sys_state))])
# creat controller function
# in this case controller function is: -1 * feedback * states + pre-filter * reference output +
# equilibrium point of input
# disturbance value ist not considered
sys_input = -1 * (f_t *
small_state)[0] + v[0] * yr + eqrt[len(sys_state)][1]
input_func = sp.lambdify((sys_state, t), sys_input, modules='numpy')
poles_lqr = res[2]
result = LQR_Result(input_func, f_t, v, poles_lqr, None)
# return internal variables for unittest
if debug:
c_locals = Container(fetch_locals=True)
result.debug = c_locals
return result