def regulator_with_observer(sys,
poles=None,
po=None,
ts=None,
extra_poles=[],
ck=[]):
final_poles = []
if poles is not None:
final_poles = poles
if po is not None and ts is not None:
log_po = np.log(100 / po)
psi = log_po / np.sqrt(np.pi**2 + log_po**2)
wn = 4 / psi / ts
s1 = -psi * wn + 1j * wn * np.sqrt(1 - psi**2)
s2 = -psi * wn - 1j * wn * np.sqrt(1 - psi**2)
final_poles.append(s1)
final_poles.append(s2)
for p in extra_poles:
final_poles.append(p)
rank = np.linalg.matrix_rank(ct.ctrb(sys.A, sys.B))
total_poles = len(final_poles)
if total_poles < rank:
raise BaseException(
f"you have {total_poles} but you need {rank} to implement a FSFB")
k = ct.acker(sys.A, sys.B, final_poles)
mo = ct.obsv(sys.A, sys.C)
obs_rank = np.linalg.matrix_rank(mo)
print("obs_rank=", obs_rank)
final_poles_obs = 5 * final_poles
L = ct.acker(sys.A.T, sys.C.T, final_poles_obs)
L = L.T
print("")
print("L=")
print(L)
pa = np.concatenate([sys.A, -sys.B * k], axis=1)
pb = np.concatenate([L * sys.C, sys.A - sys.B * k - L * sys.C], axis=1)
alc = np.concatenate([pa, pb], axis=0)
blc = np.concatenate(
[np.zeros((sys.A.shape[0], 1)),
np.zeros((sys.A.shape[0], 1))])
clc = np.concatenate([sys.C, -sys.D * k], axis=1)
dlc = np.array([[0]])
return ct.ss(alc, blc, clc, dlc)
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 __init__(self, save_data=True, barriers=True):
self.number_of_agents = rand.randint(3, 10)
self.initial_poses = np.array([])
self.poses = np.array([])
self.desired_poses = np.zeros((self.number_of_agents, 3))
self.desired_vels = np.zeros((self.number_of_agents, 3))
self.crazyflie_objects = {}
self.time = time.time()
self.x_state = dict()
self.vel_prev = dict()
self.des_vel_prev = dict()
self.xd = dict()
self.u = dict()
self.orientation_real = dict()
self.pose_real = dict()
self.dt = 0.03
self.count = 0
self.time_record = dict()
self.x_record = dict()
self.input_record = dict()
self.orientation_record = dict()
self.AA = np.array([[0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1],
[0, 0, 0, 0]])
self.bb = np.array([[0], [0], [0], [1]])
self.Kb = np.asarray(
acker(self.AA, self.bb, [-12.2, -12.4, -12.6, -12.8]))
self.robotarium_simulator_plot = None
self.barriers = barriers
self.save_flag = save_data
solvers.options['show_progress'] = False
def calculate_pp_gains(A, B, C, D, dp):
# pole placement via ackermann's formula
K = ctl.acker(A, B, dp)
# calculate Nu, Nx matrices for including reference input
Nu, Nx = getInputMatrices(A, B, C, D)
return K, Nu, Nx
def gen_chain_of_integrators():
"""Defines a chain of integrator for xyz: position, velocity, acceleration and jerk.
Dynamics of the form: xd = Ax + bu
Returns
-------
A : ndarray
b : ndarray
Kb : ndarray
"""
# Define chain of integrator dynamics and controller gains.
A = np.array([[0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1], [0, 0, 0, 0]])
b = np.array([[0], [0], [0], [1]])
Kb = np.array(acker(
A, b,
[-12.2, -12.4, -12.6, -12.8])) # Generate gains using pole placement.
return A, b, Kb
def regulator(sys, poles=None, po=None, ts=None, extra_poles=[], ck=[]):
final_poles = []
if poles is not None:
final_poles = poles
if po is not None and ts is not None:
log_po = np.log(100 / po)
psi = log_po / np.sqrt(np.pi**2 + log_po**2)
wn = 4 / psi / ts
s1 = -psi * wn + 1j * wn * np.sqrt(1 - psi**2)
s2 = -psi * wn - 1j * wn * np.sqrt(1 - psi**2)
final_poles.append(s1)
final_poles.append(s2)
for p in extra_poles:
final_poles.append(p)
rank = np.linalg.matrix_rank(ct.ctrb(sys.A, sys.B))
total_poles = len(final_poles)
if total_poles < rank:
raise BaseException(
f"you have {total_poles} but you need {rank} to implement a FSFB")
kk = ct.acker(sys.A, sys.B, final_poles)
ak = sys.A - sys.B * kk
bk = np.zeros((sys.A.shape[0], 1))
default = np.zeros((1, sys.A.shape[0]))
default[0] = 1
ck2 = np.array(ck) if ck else default
dk = np.array([[0]])
return ct.ss(ak, bk, ck2, dk)
zeta = 0.707
tr = 2.1
wn = 2.2 / tr
# -- PID -- #
kp = wn**2 * m - k
kd = 2.0 * zeta * wn * m - b
ki = 3.7
# -- FULL STATE -- #
A = np.matrix([[0.0, 1.0], [-k / m, -b / m]])
B = np.matrix([[0.0], [1.0 / m]])
C = np.matrix([[1.0, 0.0]])
cl_poles = list(np.roots([1, 2 * wn * zeta, wn**2]))
K = ctrl.acker(A, B, cl_poles)
kr = -1.0 / np.dot(C, np.dot(np.linalg.inv(A - np.dot(B, K)), B)).item(0)
# -- FULL STATE INTEGRATOR -- #
USE_FULL_STATE_INTEGRATOR = True
if USE_FULL_STATE_INTEGRATOR:
A1 = np.concatenate((A, -C), 0)
A1 = np.concatenate((A1, np.zeros((A1.shape[0], C.shape[0]))), 1)
B1 = np.concatenate((B, np.zeros((C.shape[0], B.shape[1]))), 0)
int_pole = -8.0
cl_poles.append(int_pole)
K = ctrl.acker(A1, B1, cl_poles)
ki = K.take([-1]).item(0)
K = K.take(range(0, K.shape[1] - 1))
A1 = np.matrix([[0.0, 1.0, 0.0], [0.0, -1.0 * P.b / P.m / (P.ell**2), 0.0],
[-1.0, 0.0, 0.0]])
B1 = np.matrix([[0.0], [3.0 / P.m / (P.ell**2)], [0.0]])
# gain calculation
wn = 2.2 / tr # natural frequency
des_char_poly = np.convolve([1, 2 * zeta * wn, wn**2],
np.poly(integrator_pole))
des_poles = np.roots(des_char_poly)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3:
print("The system is not controllable")
else:
K1 = cnt.acker(A1, B1, des_poles)
K = np.matrix([K1.item(0), K1.item(1)])
ki = K1.item(2)
# observer design
des_obsv_char_poly = [1, 2 * zeta_obs * wn_obs, wn_obs**2]
des_obsv_poles = np.roots(des_obsv_char_poly)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A.T, C.T)) != 2:
print("The system is not observerable")
else:
L = cnt.acker(A.T, C.T, des_obsv_poles).T
print('K: ', K)
print('ki: ', ki)
th_alpha0 = th_wn**2
integrator_pole_long = -10.0 #-10.0
# Desired Poles
des_char_poly_long = np.convolve([1, th_alpha1, th_alpha0],
np.poly(integrator_pole_long))
des_poles_long = np.roots(des_char_poly_long)
print 'des_poles_long'
print des_poles_long
# Controllability Matrix
if np.linalg.matrix_rank(cnt.ctrb(A1_long, B1_long)) != 3:
print("The longitudinal system is not controllable")
else:
K1_long = cnt.acker(A1_long, B1_long, des_poles_long)
K_long = K1_long[0, 0:2]
ki_long = K1_long[0, 2]
print('K1_long: ', K1_long)
print('K_long: ', K_long)
print('ki_long: ', ki_long)
###############################################
############ Lateral ##########################
###############################################
A_lat = np.matrix(
[[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 0.0],
[l1 * F_eq / (m1 * (l1**2) + m2 * (l2**2) + Jz), 0.0, 0.0, 0.0]])
B_lat = np.matrix([[0.0], [0.0], [1.0 / Jx], [0.0]])
# gain calculation
wn = 2.2 / tr # natural frequency
#des_char_poly = np.convolve(
# [1, 2*zeta*wn, wn**2],
# np.poly(integrator_pole))
#des_poles = np.roots(des_char_poly)
des_char_poly = np.array([1., 2. * zeta * wn_obs, wn_obs**2.])
des_poles_poly = np.roots(des_char_poly)
des_poles = np.concatenate((des_poles_poly, integrator_pole * np.ones(1)))
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3:
print("The system is not controllable")
else:
K1 = cnt.acker(A1, B1, des_poles)
K = np.array([K1.item(0), K1.item(1)])
ki = K1.item(2)
# observer design
# Augmented Matrices
A2 = np.concatenate((np.concatenate((A, B), axis=1), np.zeros((1, 3))), axis=0)
B2 = np.concatenate((B, np.zeros((1, 1))), axis=0)
C2 = np.concatenate((C, np.zeros((1, 1))), axis=1)
#des_obsv_char_poly = np.convolve(
# [1, 2*zeta*wn_obs, wn_obs**2],
# np.poly(dist_obsv_pole))
#des_obsv_poles = np.roots(des_obsv_char_poly)
des_char_est = np.array([1., 2. * zeta * wn_obs, wn_obs**2.])
des_poles_est = np.roots(des_char_est)
def get_observer(A,C,poles):
C_tr=np.matrix.transpose(C)
A_tr=np.matrix.transpose(A)
L_tr=control.acker(A_tr,C_tr,poles)
return np.matrix.transpose(L_tr)
# S**2 + alpha1*S + alpha0
h_alpha1 = 2.0*h_zeta*h_wn
h_alpha0 = h_wn**2
# Desired Poles
des_char_poly_lat = np.convolve([1,z_alpha1,z_alpha0],[1,th_alpha1,th_alpha0])
des_char_poly_lon = [1,h_alpha1,h_alpha0]
des_poles_lat = np.roots(des_char_poly_lat)
des_poles_lon = np.roots(des_char_poly_lon)
# Latitudinal Controllability Matrix
if np.linalg.matrix_rank(cnt.ctrb(A_lat,B_lat))!=4:
print("The Latitudinal system is not controllable")
else:
K_lat = cnt.acker(A_lat,B_lat,des_poles_lat)
kr_lat = -1.0/(C_lat*np.linalg.inv(A_lat-B_lat*K_lat)*B_lat)
# Longitudinal Controllability Matrix
if np.linalg.matrix_rank(cnt.ctrb(A_lon,B_lon))!=2:
print("The Longitudinal system is not controllable")
else:
K_lon = cnt.acker(A_lon,B_lon,des_poles_lon)
kr_lon = -1.0/(C_lon*np.linalg.inv(A_lon-B_lon*K_lon)*B_lon)
print('K_lat: ', K_lat)
print('kr_lat: ', kr_lat)
print('K_lon: ', K_lon)
print('kr_lon: ', kr_lon)
#################################################
# init ploting handles
Cfplt = dict()
cfs = dict()
CfCoord = dict()
for i in range(N):
Cfplt[i] = Shpere3D(p0[i][0, :], ax)
CfCoord[i] = Coord3D(ax, p0[i][0, :])
cfs[i] = CF(p0[i][0, :], i)
plt.draw()
# pole placement for CLF and CBF
AA = np.array([[0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1], [0, 0, 0, 0]])
bb = np.array([[0], [0], [0], [1]])
#Kb=acker(AA,bb,[-2.2,-2.4,-2.6,-2.8]);
#Kb=acker(AA,bb,[-5.2,-5.4,-5.6,-5.8]);
Kb = np.asarray(acker(AA, bb, [-12.2, -12.4, -12.6, -12.8]))
# Bezier Interpolation
Cout = dict()
T = 4.0 #time to complete, also scaling factor
for i in range(N - 1):
Cout[i] = BezierInterp(T, p0[i], p1[i])
Cout[4] = BezierInterp(T, pc[0], pc[1])
# start auto mode program
CBF_on = 1
tk = 0
dt = 0.02
t_total = 35
phat = dict() #nominal state from interpolation
uhat = dict() #nominal control
0.0, -3 * P.m1 * P.g / 4 / (.25 * P.m1 + P.m2),
-P.b / (.25 * P.m1 + P.m2), 0.0
],
[
0.0,
3 * (P.m1 + P.m2) * P.g / 2 / (.25 * P.m1 + P.m2) / P.ell,
3 * P.b / 2 / (.25 * P.m1 + P.m2) / P.ell, 0.0
]])
B = np.matrix([[0.0], [0.0], [1 / (.25 * P.m1 + P.m2)],
[-3.0 / 2 / (.25 * P.m1 + P.m2) / P.ell]])
C = np.matrix([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0]])
# gain calculation
wn_th = 2.2 / tr_theta # natural frequency for angle
wn_z = 2.2 / tr_z # natural frequency for position
des_char_poly = np.convolve([1, 2 * zeta_z * wn_z, wn_z**2],
[1, 2 * zeta_th * wn_th, wn_th**2])
des_poles = np.roots(des_char_poly)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A, B)) != 4:
print("The system is not controllable")
else:
K = cnt.acker(A, B, des_poles)
kr = -1.0 / (C[0] * np.linalg.inv(A - B * K) * B)
print('K: ', K)
print('kr: ', kr)
# Augmented Longitudinal Parameters
A1 = np.matrix([[0.0, 1.0, 0.0], [0.0, 0.0, 0.0], [-1.0, 0.0, 0.0]])
B1 = np.matrix([[0], [1 / (P.mc + 2 * P.mr)], [0.0]])
# gain calculation
des_char_poly = np.convolve([1, 2 * zeta_h * wn_h, wn_h**2],
np.poly([h_integrator_pole]))
des_poles = np.roots(des_char_poly)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3:
print("The longitudinal system is not controllable")
else:
K1 = cnt.acker(A1, B1, des_poles)
K = np.matrix([K1.item(0), K1.item(1)])
ki = K1.item(2)
# ----------------------------------------- OBSERVER GAINS -----------------------------------------
# compute longitudinal observer gains
des_obs_char_poly = [1, 2 * zeta_h * wn_h_obs, wn_h_obs**2]
des_obs_poles = np.roots(des_obs_char_poly)
# Compute the gains if the system is observable
if np.linalg.matrix_rank(cnt.ctrb(A.T, C.T)) != 2:
print("The longitudinal observer system is not observable")
else:
# place_poles returns an object with various properties. The gains are accessed through .gain_matrix
# .T transposes the matrix
L = signal.place_poles(A.T, C.T, des_obs_poles).gain_matrix.T
[0.0], [b_th] ]) C_lon = np.matrix([ [1.0, 0.0] ]) A1_lon = np.concatenate(( np.concatenate((A_lon, np.zeros((2,1))), 1), np.concatenate((-C_lon, np.zeros((1,1))), 1)), 0) B1_lon = np.concatenate((B_lon, np.zeros((1,1)))) i_pole_lon = -1*wn_th cl_poles_lon = list(np.roots([1,2*zeta_lon*wn_th,wn_th**2])) + [i_pole_lon] K1_lon = ctrl.acker(A1_lon, B1_lon, cl_poles_lon) K_lon = K1_lon[0,:2] ki_lon = K1_lon[0,2] #kr_lon = -1.0 / (C_lon * (np.linalg.inv(A_lon - B_lon*K_lon) * B_lon)).item(0) # Lateral c = l1*Feq / (m1*l1**2 + m2*l2**2 + Jz) A_lat = np.matrix([ [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 0.0], [c , 0.0, 0.0, 0.0] ]) B_lat = np.matrix([ [0.0], [0.0],
C_lat = np.array([[1.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0]])
## LONGITUDINAL GAINS CALCULATIONS
wn_h = 2.2/tr_h # natural frequency for angle
des_char_poly_long = [1, 2*zeta_h*wn_h, wn_h**2]
des_poles_long = np.roots(des_char_poly_long)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A_long, B_long)) != 2:
print('The system is not controllable')
else:
# A just turn K matrix into a numpy array
K_long = (cnt.acker(A_long, B_long, des_poles_long)).A
kr_long = -1.0/(C_long @ np.linalg.inv(A_long - B_long@K_long) @ B_long)
print('K_long: ', K_long)
print('kr_long: ', kr_long)
## LATERAL GAINS CALCULATIONS
wn_th = 2.2/tr_th # natural frequency for angle
wn_z = 2.2/tr_z # natural frequency for position
des_char_poly_lat = np.convolve([1, 2*zeta_z*wn_z, wn_z**2],
[1, 2*zeta_th*wn_th, wn_th**2])
des_poles_lat = np.roots(des_char_poly_lat)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A_lat, B_lat)) != 4:
pid.setKp(6)
A = np.matrix('[2 -3; 1 3]')
B = np.matrix('[1;-1]')
C = np.matrix('[1 0]')
D = 0
A2 = np.matrix('[0 1; -2 -3]')
B2 = np.matrix('[0;-1]')
C2 = np.matrix('[1 0]')
D2 = 0
sys = control.ss(A, B, C, D)
tf = control.ss2tf(sys)
poles = control.pole(sys)
acker_k = control.acker(A, B, ["-1", "-1"])
sys_stabil = control.ss(A2, B2, C2, D2)
Acl = A - B * acker_k
sys2 = control.ss(Acl, B, C, D)
pid_ss = pid.create_tf()
sys3 = control.series(sys2, pid_ss)
sys4 = control.feedback(sys3, 1, -1)
while True:
value = socket.recv_pyobj()
sys_step = control.tf(value[0], [0, 1])
sys_command = control.series(sys_step, sys4)
T, yout = control.step_response(sys_command)
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
while (s < 1) or (sum(fflag) < N):
cfs[i].send_cmd_diff(0, 0, 0, thrust_hover[i])
t = rospy.Time.now()
s = min((t - t0).to_sec() / t_wp, 1.0)
for i in range(N):
fflag[i] = cfs[i].gotoT(pc[0][0, :], s)
print 'Initialization Done! Experiment Starts!'
#-------------------------Actual control experiment--------------------#
# pole placement for CLF and CBF
AA = np.array([[0, 1, 0], [0, 0, 1], [0, 0, 0]])
bb = np.array([[0], [0], [1]])
#Kb=acker(AA,bb,[-2.2,-2.4,-2.6,-2.8])
#Kb=acker(AA,bb,[-5.2,-5.4,-5.6,-5.8])
Kb = np.asarray(acker(AA, bb, [-8.2, -8.4, -8.6]))
# start auto mode program
print 'Auto mode: differential flatness'
CBF_on = 0
Cout = dict() # Bezier Control points
tk = 0
dt = 0.02
t_total = sum(Tf) + 1
phat = dict() #nominal state from interpolation
uhat = dict() #nominal control
x = dict() #actual states
xd = dict() #derivative of x
for i in range(N):
x[i] = pc[0][0:-1, :]
xd[i] = np.zeros((3, 3))
cfs[i].send_cmd_diff(0, 0, 0, thrust_hover[i])
t = rospy.Time.now()
s = min((t - t0).to_sec() / t_wp, 1.0)
for i in range(N):
fflag[i] = cfs[i].gotoT(pc[0][0, :], s)
print 'Initialization Done! Experiment Starts!'
#-------------------------Actual control experiment--------------------#
# pole placement for CLF and CBF
AA = np.array([[0, 1, 0], [0, 0, 1], [0, 0, 0]])
bb = np.array([[0], [0], [1]])
#Kb=np.asarray(acker(AA,bb,[-2.2,-2.4,-2.6]))
#Kb=acker(AA,bb,[-5.2,-5.4,-5.6,-5.8])
#Kb=np.asarray(acker(AA,bb,[-6.6,-6.8,-7.0])) # works, err 0.2
Kb = np.asarray(acker(AA, bb, [-6.6, -6.8, -7.0])) # works, err 0.2
#Kb=np.asarray(acker(AA,bb,[-12.2,-6.4,-2.6])) # works when kbz set to 0
#Kb=np.asarray(acker(AA,bb,[-12.2,-6.4,-2.6]))
Kb[0, 2] = Kb[0, 2] * 1.0
# start auto mode program
print 'Auto mode: differential flatness'
CBF_on = 0
Cout = dict() # Bezier Control points
tk = 0
dt = 0.02
t_total = sum(Tf) + 1
phat = dict() #nominal state from interpolation
uhat = dict() #nominal control
x = dict() #actual states
xnext = dict() #actual states
ax0.plot(t, ref, color="k", label='desired')
ax0.set_title("Step response of close loop system", fontsize='small')
ax0.legend(loc='lower right', shadow=True, fontsize='small')
ax0.set_xlabel("time {s}")
ax0.set_ylabel("Height {m}")
ax0.set_yticks(h_ticks)
ax0t.set_ylabel("Velocity {m/s}")
ax0t.set_yticks(v_ticks)
#*****************************************#
# FULL STATE FEEDBACK CONTROLLER
#*****************************************#
#desired controller poles p1= -3, p2, -4
dcp = np.array([-3.0, -3.0])
K = ctl.acker(A, B, dcp)
print("Controller gains: {},{}".format(K.item(0, 0), K.item(0, 1)))
# The system with full state feedback is modified as
# x'(t) = (A-BK)x + B(Nu + KNx)*r
#
Nx = np.matrix([[0.0], [1.0]])
Nu = 0.0
#**************** ADDED LQR Controller
# Define performance index weights for Vertical Dynamics
Qx1 = np.diag([10, 100])
Qu1a = np.diag([1])
(K, X, E) = ctl.lqr(A, B, Qx1, Qu1a)
K = np.matrix(K)
th_alpha0 = th_wn**2
# Desired Poles
des_char_poly_lat = np.convolve(
np.convolve([1, s_alpha1, s_alpha0], [1, p_alpha1, p_alpha0]),
np.poly(integrator_pole_lat))
des_char_poly_lon = np.convolve([1, th_alpha1, th_alpha0],
np.poly(integrator_pole_lon))
des_poles_lat = np.roots(des_char_poly_lat)
des_poles_lon = np.roots(des_char_poly_lon)
# Latitudinal Controllability Matrix
if np.linalg.matrix_rank(cnt.ctrb(A1_lat, B1_lat)) != 5:
print("The Latitudinal system is not controllable")
else:
K1_lat = cnt.acker(A1_lat, B1_lat, des_poles_lat)
K_lat = K1_lat[0, 0:4]
ki_lat = K1_lat[0, 4]
# Longitudinal Controllability Matrix
if np.linalg.matrix_rank(cnt.ctrb(A1_lon, B1_lon)) != 3:
print("The Longitudinal system is not controllable")
else:
K1_lon = cnt.acker(A1_lon, B1_lon, des_poles_lon)
K_lon = K1_lon[0, 0:2]
ki_lon = K1_lon[0, 2]
UNCERTAINTY_PARAMETERS = False
if UNCERTAINTY_PARAMETERS:
alpha = 0.2
l1 = 0.85 * (1 + 2 * alpha * np.random.rand() - alpha
des_poles = np.array([-5 + 0.1j, -5 - 0.1j])
polesI = np.append(des_poles, -5)
if np.linalg.matrix_rank(cnt.ctrb(A1, B1)) != 3:
print('The system is not controllable.')
else:
K1 = cnt.place(A1, B1, polesI)
K = np.array([K1.item(0), K1.item(1)])
ki2 = K1.item(2)
# observer calculation
obsv_poles = 10 * des_poles
# compute gains if the system is observable
if np.linalg.matrix_rank(cnt.ctrb(A.T, C.T)) != 2:
print('The system is not observable.')
else:
L = cnt.acker(A.T, C.T, obsv_poles).T
print('K: ', K)
print('ki2: ', ki2)
print('L^T: ', L.T)
# ----------------------------------------------
# simulation parameters
t_start = 0.0
t_end = 40.0
Ts = 0.01
t_plot = 0.1
sigma = 0.05
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
# Longitudinal Parameters
Alon = np.matrix([[0.0, 1.0], [0.0, 0.0]])
Blon = np.matrix([[0], [1 / (P.mc + 2 * P.mr)]])
Clon = np.matrix([[1.0, 0.0]])
# gain calculation
des_char_poly = [1, 2 * zeta_h * wn_h, wn_h**2]
des_poles = np.roots(des_char_poly)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(Alon, Blon)) != 2:
print("The system is not controllable")
else:
K_lon = cnt.acker(Alon, Blon, des_poles)
kr_lon = -1.0 / (Clon[0] * np.linalg.inv(Alon - Blon * K_lon) * Blon)
# Lateral parameters
# State Space Equations
# xdot = A*x + B*u
# y = C*x
Fe = P.m * P.g
Alat = np.matrix(
[[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0],
[0.0, -Fe / (P.mc + 2 * P.mr), -P.mu / (P.mc + 2 * P.mr), 0.0],
[0.0, 0.0, 0.0, 0.0]])
Blat = np.matrix([[0.0], [0.0], [0.0], [1 / (P.Jc + 2 * P.mr * P.d**2)]])
# tuning parameters
tr = 0.4
zeta = 0.707
# State Space Equations
# xdot = A*x + B*u
# y = C*x
A = np.array([[0.0, 1.0], [0.0, -1.0 * P.b / P.m / (P.ell**2)]])
B = np.array([[0.0], [3.0 / P.m / (P.ell**2)]])
C = np.array([[1.0, 0.0]])
# gain calculation
wn = 2.2 / tr # natural frequency
des_char_poly = [1, 2 * zeta * wn, wn**2]
des_poles = np.roots(des_char_poly)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A, B)) != 2:
print("The system is not controllable")
else:
#.A just turns K matrix into a numpy array
K = (cnt.acker(A, B, des_poles)).A
kr = -1.0 / (C @ np.linalg.inv(A - B @ K) @ B)
print('K: ', K)
print('kr: ', kr)
wn_th = 2.2 / tr_theta # natural frequency for angle
wn_z = 2.2 / tr_z # natural frequency for latral
wn_h = 2.2 / tr_h # natural frequency for longitudinal
#-----------------------------latral-----------------------------------------
des_char_poly_z = np.convolve(
np.convolve([1, 2 * zeta_z * wn_z, wn_z**2],
[1, 2 * zeta_th * wn_th, wn_th**2]), [1, integrator_pole_z])
des_poles_z = np.roots(des_char_poly_z)
# Compute the gains if the system is controllable for latral
if np.linalg.matrix_rank(cnt.ctrb(A_z, B_z)) != 4:
print("The system is not controllable")
else:
K1_z = cnt.acker(A_z_1, B_z_1, des_poles_z)
K_z = np.matrix([K1_z.item(0), K1_z.item(1), K1_z.item(2), K1_z.item(3)])
ki_z = K1_z.item(4)
print('K_z: ', K_z)
print('ki_z: ', ki_z)
#----------------------------longi----------------------------------------
des_char_poly_h = np.convolve(
[1, 2 * zeta_h * wn_h, wn_h**2],
[1, integrator_pole_h]) #np.poly(integrator_pole_h))
des_poles_h = np.roots(des_char_poly_h)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A_h, B_h)) != 2:
print("The system is not controllable")
# gain calculation
wn_th = 2.2 / tr_theta # natural frequency for angle
wn_z = 2.2 / tr_z # natural frequency for latral
wn_h = 2.2 / tr_h # natural frequency for longitudinal
#-----------------------------latral-----------------------------------------
des_char_poly_z = np.convolve([1, 2 * zeta_z * wn_z, wn_z**2],
[1, 2 * zeta_th * wn_th, wn_th**2])
des_poles_z = np.roots(des_char_poly_z)
# Compute the gains if the system is controllable for latral
if np.linalg.matrix_rank(cnt.ctrb(A_z, B_z)) != 4:
print("The system is not controllable")
else:
K_z = cnt.acker(A_z, B_z, des_poles_z)
Cr_z = np.array([[1.0, 0.0, 0.0, 0.0]])
kr_z = -1.0 / (Cr_z * np.linalg.inv(A_z - B_z @ K_z) @ B_z)
print('K_z: ', K_z)
print('kr_z: ', kr_z)
#----------------------------longi----------------------------------------
des_char_poly_h = [1, 2 * zeta_h * wn_h, wn_h**2]
des_poles_h = np.roots(des_char_poly_h)
# Compute the gains if the system is controllable
if np.linalg.matrix_rank(cnt.ctrb(A_h, B_h)) != 2:
print("The system is not controllable")
else:
K_h = cnt.acker(A_h, B_h, des_poles_h)
# full state
# ------------------------------ #
d_0 = (1.0 / 3 * m2 * l**2 + m1 * z0**2)
a_0 = (-m1 * g) / d_0
b_0 = l / d_0
A = np.matrix([[0, 0, 1, 0], [0, 0, 0, 1], [0, -g, 0, 0], [a_0, 0, 0, 0]])
B = np.matrix([[0], [0], [0], [b_0]])
C = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0]])
Cr = C[0, :]
Cab = ctrl.ctrb(A, B)
cl_poles = list(np.roots([1, 2 * zeta_z * wn_z, wn_z**2])) + list(
np.roots([1, 2 * zeta_th * wn_th, wn_th**2]))
K = ctrl.acker(A, B, cl_poles)
kr = -1.0 / np.dot(C, np.dot(np.linalg.inv(A - np.dot(B, K)), B)).item(0)
# ------------------------------ #
# full state with integrator
# ------------------------------ #
USE_FULL_STATE_INTEGRATOR = True
if USE_FULL_STATE_INTEGRATOR:
A1 = np.concatenate((A, -Cr), 0)
A1 = np.concatenate((A1, np.zeros((A1.shape[0], Cr.shape[0]))), 1)
B1 = np.concatenate((B, np.zeros((Cr.shape[0], B.shape[1]))), 0)
int_pole = -4.0 * wn_th
cl_poles.append(int_pole)
