def solve(problem_spec):
""" solution of full state feedback
:param problem_spec: ProblemSpecification object
:return: solution_data: states and output values of the stabilized system
"""
sys_f_body = msp.System_Property() # instance of the class System_Property
sys_f_body.sys_state = problem_spec.xx # state of the system
sys_f_body.tau = problem_spec.u # inputs of the system
# original nonlinear system functions
sys_f_body.n_state_func = problem_spec.rhs(problem_spec.xx, problem_spec.u)
# original output functions
sys_f_body.n_out_func = problem_spec.output_func(problem_spec.xx,
problem_spec.u)
sys_f_body.eqlbr = problem_spec.eqrt # equilibrium point
# linearize nonlinear system around the chosen equilibrium point
sys_f_body.sys_linerazition()
tuple_system = (sys_f_body.aa, sys_f_body.bb, sys_f_body.cc, sys_f_body.dd
) # system tuple
# call the state_feedback method to stabilize an unstable system
observer_res = ofr.full_observer(tuple_system,
problem_spec.poles_o,
problem_spec.poles_cl,
debug=False)
# simulation original nonlinear system with controller
f = sys_f_body.n_state_func.subs(st.zip0(
sys_f_body.tau)) # x_dot = f(x) + g(x) * u
g = sys_f_body.n_state_func.jacobian(sys_f_body.tau)
# observer simulation function
observer_sim = osf.Observer_SimModel(f, g, problem_spec.xx, tuple_system,
problem_spec.eqrt,
observer_res.observer_gain,
observer_res.state_feedback,
observer_res.pre_filter,
problem_spec.yr)
rhs = observer_sim.calc_observer_rhs_func()
res = odeint(rhs, problem_spec.xx0, problem_spec.tt)
# add equilibrium points to the estimated states from observer
res[:, len(problem_spec.xx):] += np.array(
[problem_spec.eqrt[i][1] for i in range(len(problem_spec.xx))])
# output function
output_function = sp.lambdify(problem_spec.xx,
sys_f_body.n_out_func,
modules='numpy')
yy = output_function(*res[:, 0:4].T)
solution_data = SolutionData()
solution_data.yy = yy # output of the stabilized system
solution_data.res = res # states of the stabilized system
solution_data.state_feedback = observer_res.state_feedback # controller gains
solution_data.observer_gain = observer_res.observer_gain # observer gains
solution_data.pre_filter = observer_res.pre_filter # pre-filter
return solution_data
def solve(problem_spec):
""" solution of reduced observer
the design of a linear full observer is based on a linear system.
therefore the non-linear system should first be linearized at the beginning
:param problem_spec: ProblemSpecification object
:return: solution_data: states and output values of the system
"""
sys_f_body = msp.System_Property() # instance of the class System_Property
sys_f_body.sys_state = problem_spec.xx # state of the system
sys_f_body.tau = problem_spec.u # inputs of the system
# original nonlinear system functions
sys_f_body.n_state_func = problem_spec.rhs(problem_spec.xx, problem_spec.u)
# original output functions
sys_f_body.n_out_func = problem_spec.output_func(problem_spec.xx,
problem_spec.u)
sys_f_body.eqlbr = problem_spec.eqrt # equilibrium point
# linearize nonlinear system around the chosen equilibrium point
sys_f_body.sys_linerazition()
tuple_system = (sys_f_body.aa, sys_f_body.bb, sys_f_body.cc, sys_f_body.dd
) # system tuple
yy_f, xx_f, tt_f, sys_f = ofr.reduced_observer(tuple_system,
problem_spec.poles_o,
problem_spec.poles_cl,
problem_spec.xx0,
problem_spec.tt,
debug=False)
solution_data = SolutionData()
solution_data.yy = yy_f
solution_data.xx = xx_f
return solution_data
def solve(problem_spec):
""" solution of full state feedback
:param problem_spec: ProblemSpecification object
:return: solution_data: states and output values of the stabilized system, and controller gain
"""
sys_f_body = msp.System_Property() # instance of the class System_Property
sys_f_body.sys_state = problem_spec.xx # state of the system
sys_f_body.tau = problem_spec.u # inputs of the system
# original nonlinear system functions
sys_f_body.n_state_func = problem_spec.rhs(problem_spec.xx, problem_spec.u)
# original output functions
sys_f_body.n_out_func = problem_spec.output_func(problem_spec.xx,
problem_spec.u)
sys_f_body.eqlbr = problem_spec.eqrt # equilibrium point
# linearize nonlinear system around the chosen equilibrium point
sys_f_body.sys_linerazition()
tuple_system = (sys_f_body.aa, sys_f_body.bb, sys_f_body.cc, sys_f_body.dd
) # system tuple
# calculate controller function
# sfb for dataclass StateFeedbackResult
sfb = ftf.state_feedback(tuple_system,
problem_spec.poles_cl,
problem_spec.xx,
problem_spec.eqrt,
problem_spec.yr,
debug=False)
# simulation original nonlinear system with controller
f = sys_f_body.n_state_func.subs(st.zip0(
sys_f_body.tau)) # x_dot = f(x) + g(x) * u
g = sys_f_body.n_state_func.jacobian(sys_f_body.tau)
rhs = rhs_for_simulation(f, g, problem_spec.xx, sfb.input_func)
res = odeint(rhs, problem_spec.xx0, problem_spec.tt)
output_function = sp.lambdify(problem_spec.xx,
sys_f_body.n_out_func,
modules='numpy')
yy = output_function(*res.T)
solution_data = SolutionData()
solution_data.res = res # states of system
solution_data.pre_filter = sfb.pre_filter # pre-filter
solution_data.ff = sfb.state_feedback # controller gain
solution_data.input_fun = sfb.input_func # controller function
solution_data.yy = yy[0][0] # system output
return solution_data
def solve(problem_spec):
""" the design of a linear full observer is based on a linear system.
therefore the non-linear system should first be linearized at the beginning
:param problem_spec: ProblemSpecification object
:return: solution_data: states and output values of the stabilized system
"""
sys_f_body = msp.System_Property() # instance of the class System_Property
sys_f_body.sys_state = problem_spec.xx # state of the system
sys_f_body.tau = problem_spec.u # inputs of the system
# original nonlinear system functions
sys_f_body.n_state_func = problem_spec.rhs(problem_spec.xx, problem_spec.u)
# original output functions
sys_f_body.n_out_func = problem_spec.output_func(problem_spec.xx,
problem_spec.u)
sys_f_body.eqlbr = problem_spec.eqrt # equilibrium point
# linearize nonlinear system around the chosen equilibrium point
sys_f_body.sys_linerazition(parameter_values=None)
tuple_system = (sys_f_body.aa, sys_f_body.bb, sys_f_body.cc, sys_f_body.dd
) # system tuple
# calculate controller function
LQR_res = mlqr.lqr_method(tuple_system,
problem_spec.q,
problem_spec.r,
problem_spec.xx,
problem_spec.eqrt,
problem_spec.yr,
debug=False)
# simulation original nonlinear system with controller
f = sys_f_body.n_state_func.subs(st.zip0(
sys_f_body.tau)) # x_dot = f(x) + g(x) * u
g = sys_f_body.n_state_func.jacobian(sys_f_body.tau)
rhs = rhs_for_simulation(f, g, problem_spec.xx, LQR_res.input_func)
res = odeint(rhs, problem_spec.xx0, problem_spec.tt)
output_function = sp.lambdify(problem_spec.xx,
sys_f_body.n_out_func,
modules='numpy')
yy = output_function(*res.T)
solution_data = SolutionData()
solution_data.res = res # states of system
solution_data.pre_filter = LQR_res.pre_filter # pre-filter
solution_data.state_feedback = LQR_res.state_feedback # controller gain
solution_data.poles = LQR_res.poles_lqr
solution_data.yy = yy[0][0]
return solution_data