def __init__(self, plant, x_eq, u_eq, y_eq, controller_poles,
estimator_poles):
"""Creates a new inverted pendulum controller.
Args:
plant: The plant being controlled.
x_eq: The equilibrium state (or operating point).
u_eq: The input required to maintain equilibrium.
y_eq: The sensed output in the equilibrium state.
controller_poles: The poles to use for the controller.
estimator_poles: The poles to use for the estimator.
"""
self.plant = plant
self.u_eq = u_eq
self.y_eq = y_eq
self.z_hat = [0, 0]
# Create system matrices.
self.A = [[0, 1],
[(plant.m * constants.g * np.cos(x_eq[0]) -
u_eq * np.sin(x_eq[0])) / (plant.m * plant.l),
-plant.gamma / (plant.l**2 * plant.m)]]
self.B = [[0], [np.cos(x_eq[0]) / (plant.m * plant.l)]]
self.C = [[plant.l * np.cos(x_eq[0]), 0]]
self.D = [[0]]
self.K = control.place(self.A, self.B, controller_poles)
self.L = np.transpose(
control.place(np.transpose(self.A), np.transpose(self.C),
estimator_poles))
# Precompute frequently used matrices.
self.A_BK = np.subtract(self.A, np.matmul(self.B, self.K))
self.C_DK = np.subtract(self.C, np.matmul(self.D, self.K))
def set_poles(self, poles):
poles = np.asarray(poles)
self.K_cont = np.asarray(control.place(self.A, self.B, poles))
self.K_disc = np.asarray(control.place(self.A_d, self.B_d, np.exp(poles * self.Ts)))
self.A_hat = self.A - self.B @ self.K_cont
self.sys_cont = control.StateSpace(self.A_hat, self.B, self.C, self.D, remove_useless=False)
self.A_hat_d = self.A_d - self.B_d @ self.K_disc
self.sys_disc = control.StateSpace(self.A_hat_d, self.B_d, self.C_d, self.D_d, self.Ts, remove_useless=False)
return self.K_disc
def cal_feedback(self, param):
theta_0 = 0
b_theta = param.lT / (param.m1 * param.l1**2 + param.m2 * param.l2**2 +
param.J1y + param.J2y)
A_lon = np.zeros((2, 2))
A_lon[0, 1] = 1
B_lon = np.zeros((2, 1))
B_lon[1, 0] = b_theta
C_lon = np.zeros((1, 2))
C_lon[0, 0] = 1
self.Feq = (self.param.m1 * self.param.l1 +
self.param.m2 * self.param.l2
) * self.param.g * np.cos(theta_0) / self.param.lT
JT = self.param.m1 * self.param.l1**2 + self.param.m2 * self.param.l2**2 + self.param.J2z + self.param.m3 * (
self.param.l3x**2 + self.param.l3y**2)
A_lat = np.zeros((4, 4))
A_lat[0, 2] = 1
A_lat[1, 3] = 1
A_lat[3, 0] = self.param.lT * self.Feq / (JT + self.param.J1z)
B_lat = np.zeros((4, 1))
B_lat[2, 0] = 1 / (self.param.J1x)
C_lat = np.zeros((2, 4))
C_lat[0, 0] = 1
C_lat[1, 1] = 1
Cr_yaw = np.zeros((1, 4))
Cr_yaw[0, 1] = 1
des_char_lon = np.array(
[1, 2 * self.z_theta * self.wn_theta, self.wn_theta**2])
#print(ct.ctrb(A_lon,B_lon))
des_pole_lon = np.roots(des_char_lon)
self.K_lon = ct.place(A_lon, B_lon, des_pole_lon)
self.kr_lon = -1 / (np.dot(
np.dot(C_lon, np.linalg.inv(A_lon - B_lon * self.K_lon)), B_lon))
print(self.K_lon)
print(self.kr_lon)
des_char_lat = np.convolve(
[1, 2 * self.z_phi * self.wn_phi, self.wn_phi**2],
[1, 2 * self.z_psi * self.wn_psi, self.wn_psi**2])
des_pole_lat = np.roots(des_char_lat)
#(ct.ctrb(A_lat,B_lat))
self.K_lat = ct.place(A_lat, B_lat, des_pole_lat)
self.kr_lat = -1 / (np.dot(
np.dot(Cr_yaw, np.linalg.inv(A_lat - B_lat * self.K_lat)), B_lat))
print(self.K_lat)
print(self.kr_lat)
def __init__(self, slow_d):
self.fast_d = 0
#stay still
# self.slow_d = -5 + 5*0.2
#move
self.slow_d = slow_d
self.desired_states = np.matrix([[0], [slow_d], [0], [self.fast_d]])
#define our matrices for the linearized model, which we have
M = 0.5 #cart mass
m = 0.2 #pedulum mass
b = 0.1 #coefficient of friction
I = 0.006 #inertia
g = 9.8 #gravity
l = 0.3 #length of the pendulum
k = I * (M + m) + M * m * l**2.0
U2 = (I + m * l**2.0) / k
U4 = -m * l / k
TF_2_3 = -(g * m**2.0 * l**2.0) / k
TF_4_3 = (g * (m + M)) / k
# TF_4_3 = (m*l*(M+m))*g/k;
self.B = [[0], [U2], [0], [U4]]
self.A = [[0, 1, 0, 0], [0, 0, TF_2_3, 0], [0, 0, 0, 1],
[0, 0, TF_4_3, 0]]
self.C = [[1, 0, 0, 0], [0, 0, 1, 0]]
self.D = [[0], [0]]
self.sys = control.StateSpace(self.A, self.B, self.C, self.D)
# R = 1
# Q = [[20, 0, 0, 0], [0, 0, 0, 0], [0, 0, 3, 0], [0, 0, 0, 0,]]
#develop our fast dynamic controller (Using pole placement)
self.K = control.place(self.A, self.B, [-5.1, -5.2, -5.3, -5.4])
self.K = np.matrix(self.K.tolist()[0])
print("K = ", np.matrix(self.K))
print("A = ", np.matrix(self.A))
# print("B = ",np.matrix(self.B))
# print(np.linalg.eig(np.subtract(self.A, np.matmul(self.B,self.K))))
# print(np.subtract(self.A, np.matmul(self.B,self.K)))
exit()
self.closed_loop = np.subtract(self.A, np.matmul(self.B, self.K))
self.K2 = control.place(self.closed_loop, self.B,
[-5.1, -5.2, -5.3, -5.4])
#deevelop our slow dynamic controller (PID)
self.pid = PID(2, 1, 0.0)
def full_obs(sys,poles):
"""
Full order observer of the system sys
Call:
obs = full_obs(sys,poles)
Parameters
----------
sys : System in State Space form
poles: desired observer poles
Returns
-------
obs: ss
Observer
"""
if isinstance(sys, ct.TransferFunction):
"System must be in state space form"
return
a = np.mat(sys.A)
b = np.mat(sys.B)
c = np.mat(sys.C)
d = np.mat(sys.D)
L = ct.place(a.T,c.T,poles)
L = np.mat(L).T
Ao = a-L*c
Bo = np.hstack((b-L*d,L))
n = np.shape(Ao)
m = np.shape(Bo)
Co = np.eye(n[0],n[1])
Do = np.zeros((n[0],m[1]))
obs = ct.StateSpace(Ao,Bo,Co,Do,sys.dt)
return obs
def red_obs(sys, T, poles):
"""Reduced order observer of the system sys
Call:
obs=red_obs(sys,T,poles)
Parameters
----------
sys : System in State Space form
T: Complement matrix
poles: desired observer poles
Returns
-------
obs: ss
Reduced order Observer
"""
if isinstance(sys, TransferFunction):
"System must be in state space form"
return
a = np.mat(sys.A)
b = np.mat(sys.B)
c = np.mat(sys.C)
d = np.mat(sys.D)
T = np.mat(T)
P = np.mat(np.vstack((c, T)))
invP = np.inv(P)
AA = P * a * invP
ny = np.shape(c)[0]
nx = np.shape(a)[0]
nu = np.shape(b)[1]
A11 = AA[0:ny, 0:ny]
A12 = AA[0:ny, ny:nx]
A21 = AA[ny:nx, 0:ny]
A22 = AA[ny:nx, ny:nx]
L1 = place(A22.T, A12.T, poles)
L1 = np.mat(L1).T
nn = nx - ny
tmp1 = np.mat(np.hstack((-L1, np.eye(nn, nn))))
tmp2 = np.mat(np.vstack((np.zeros((ny, nn)), np.eye(nn, nn))))
Ar = tmp1 * P * a * invP * tmp2
tmp3 = np.vstack((np.eye(ny, ny), L1))
tmp3 = np.mat(np.hstack((P * b, P * a * invP * tmp3)))
tmp4 = np.hstack((np.eye(nu, nu), np.zeros((nu, ny))))
tmp5 = np.hstack((-d, np.eye(ny, ny)))
tmp4 = np.mat(np.vstack((tmp4, tmp5)))
Br = tmp1 * tmp3 * tmp4
Cr = invP * tmp2
tmp5 = np.hstack((np.zeros((ny, nu)), np.eye(ny, ny)))
tmp6 = np.hstack((np.zeros((nn, nu)), L1))
tmp5 = np.mat(np.vstack((tmp5, tmp6)))
Dr = invP * tmp5 * tmp4
obs = StateSpace(Ar, Br, Cr, Dr, sys.dt)
return obs
def initialize(self, param_value_dict):
self.k1 = param_value_dict["k1"]
self.k2 = param_value_dict["k2"]
self.luenberger_poles = param_value_dict["Luenberger poles"]
AB = np.array([[0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 0]])
BB = np.array([[0, 0],
[0, 0],
[0, 0],
[1, 0],
[0, 0],
[0, 1]])
CB = np.array([[1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]])
L = place(AB.T, CB.T, self.luenberger_poles).T
cont_observer_system = ss(AB - L @ CB, np.hstack((BB, L)), np.zeros((1, 6)), np.zeros((1, 6)))
# TODO: Remove hardcoded sample time
discrete_observer_system = sample_system(cont_observer_system, 1/60)
self.luenberger_Ad, self.luenberger_Bd, _, _ = ssdata(discrete_observer_system)
self.run_once = False
def __init__(self):
self.values = None
self.boostPercent = 0.0 #boost Percentage 0 - 100
self.lastz = None #the tick priors value of position
self.lastvz = None #the tick priors value of velocity
self.lastError = 0.0 #prior error
self.errorzI = 0.0 #summation of values for integral of error
self.errorzICOUNTER = 1.0
#Rocket League physics (using unreal units (uu))
self.gravity = 650 #uu/s^2
#State Space matrix coefficients
self.mass = 14.2
self.M = 1 / 14.2
self.A = np.array([[0.0, 1.0], [0.0, 0.0]])
self.B = np.array([[0.0], [1.0]])
self.C = np.array([[1, 0], [0, 0]])
self.D = np.array([[0], [0]])
self.system = control.ss(self.A, self.B, self.C, self.D, None)
#print(self.B)
self.poles = np.array([-2.5, -2.8])
self.K = control.place(self.A, self.B, self.poles)
self.k1 = self.K[0, 0]
self.k2 = self.K[0, 1]
self.kr = 1.0
def __init__(self):
self.fast_d = 0
self.slow_d = 2
#define our matrices for the linearized model, which we have
M = 0.5 #cart mass
m = 0.2 #pedulum mass
b = 0.1 #coefficient of friction
I = 0.006 #inertia
g = 9.8 #gravity
l = 0.3 #length of the pendulum
k = I * (M + m) + M * m * l**2.0
U2 = (I + m * l**2.0) / k
U4 = -m * l / k
TF_2_3 = -(g * m**2.0 * l**2.0) / k
TF_4_3 = (g * (m + M)) / k
# TF_4_3 = (m*l*(M+m))*g/k;
self.B = [[0], [U2], [0], [U4]]
self.A = [[0, 1, 0, 0], [0, 0, TF_2_3, 0], [0, 0, 0, 1],
[0, 0, TF_4_3, 0]]
self.C = [[1, 0, 0, 0], [0, 0, 1, 0]]
self.D = [[0], [0]]
self.sys = control.StateSpace(self.A, self.B, self.C, self.D)
# R = 1
# Q = [[20, 0, 0, 0], [0, 0, 0, 0], [0, 0, 3, 0], [0, 0, 0, 0,]]
#develop our fast dynamic controller (Using pole placement)
self.K = control.place(self.A, self.B, [-5.1, -5.2, -5.3, -5.4])
self.K = np.matrix(self.K.tolist()[0])
#deevelop our slow dynamic controller (PID)
self.pid = PID(5, 0.1, 0.05)
def fsf(t, e, v):
if t == 0:
start = time.time()
A = np.concatenate(
(np.concatenate((np.zeros((2, 2)),
np.dot(-1 * model.m_inv,
(model.k0 + model.k2 * (v**2)))),
axis=0),
np.concatenate(
(np.eye(2), np.dot(-1 * model.m_inv, model.c1 * v)),
axis=0)),
axis=1)
B = (np.concatenate((np.zeros((2, 2)), model.m_inv),
axis=0)[:, 1])[..., None]
self.K = control.place(
A, B, [self.eval1, self.eval2, self.eval3, self.eval4])
end = time.time()
print(self.K)
# print(end - start)
e_transpose = np.array([[e[0]], [e[1]], [e[2]], [e[3]]])
ans = (-self.K * e_transpose)
# print(ans[0,0])
return ans[0, 0]
def compute_feedback_matrix(self, e_op):
A, B = compute_linear_ss(self.model_type, e_op)
try:
K = ctr.place(A, B, self.poles)
return K
except Exception as e:
print("Error during pole placement: " + str(e))
def place_controller_poles(self, poles):
"""Design a controller that places the closed-loop system poles at the
given locations.
Most users should just use design_dlqr_controller(). Only use this if
you know what you're doing.
Keyword arguments:
poles -- a list of compex numbers which are the desired pole locations.
Complex conjugate poles must be in pairs.
"""
self.K = ct.place(self.sysd.A, self.sysd.B, poles)
def __init__(self, settings):
file = settings["config file"]
assert os.path.isfile(file)
with open(file, "rb") as f:
data = pickle.load(f)
if "system" not in data:
raise ValueError("Config file lacks mandatory settings.")
self.ss = data["system"]
self.input_offset = data.get("op_inputs", None)
self.output_offset = data.get("op_outputs", None)
if self.input_offset is None:
self.input_offset = np.zeros((self.ss.B.shape[1], ))
if len(self.input_offset) != self.ss.B.shape[1]:
raise ValueError("Provided input offset does not match input "
"dimensions.")
if self.output_offset is None:
self.output_offset = np.zeros((self.ss.C.shape[0], ))
if len(self.output_offset) != self.ss.C.shape[0]:
raise ValueError("Length of provided output offset does not match "
"output dimensions ({} != {}).".format(
len(self.output_offset),
self.ss.C.shape[0]
))
# add specific "private" settings
settings.update(input_order=0)
settings.update(output_dim=self.ss.C.shape[0])
settings.update(input_type=settings["input source"])
super().__init__(settings)
if settings.get("poles", None) is None:
# pretty useless but hey why not.
self.feedback = np.zeros((self.ss.B.shape[1], self.ss.A.shape[0]))
else:
if self.ss.B.shape[1] == 1:
# save the control/slycot dependency
self.feedback = place_siso(self.ss.A,
self.ss.B,
self.settings["poles"])
else:
import control
self.feedback = control.place(self.ss.A,
self.ss.B,
self.settings["poles"])
self.prefilter = calc_prefilter(self.ss.A, self.ss.B, self.ss.C,
self.feedback)
def place_observer_poles(self, poles):
"""Design a controller that places the closed-loop system poles at the
given locations.
Most users should just use design_kalman_filter(). Only use this if you
know what you're doing.
Keyword arguments:
poles -- a list of compex numbers which are the desired pole locations.
Complex conjugate poles must be in pairs.
"""
L = ct.place(self.sysd.A.T, self.sysd.C.T, poles).T
self.kalman_gain = np.linalg.inv(self.sysd.A) @ L
def __init__(self, settings):
file = settings["config file"]
assert os.path.isfile(file)
with open(file, "rb") as f:
data = pickle.load(f)
if "system" not in data:
raise ValueError("Config file lacks mandatory settings.")
self.ss = data["system"]
self.input_offset = data.get("op_inputs", None)
self.output_offset = data.get("op_outputs", None)
if self.input_offset is None:
self.input_offset = np.zeros((self.ss.B.shape[1], ))
if len(self.input_offset) != self.ss.B.shape[1]:
raise ValueError("Provided input offset does not match input "
"dimensions.")
if self.output_offset is None:
self.output_offset = np.zeros((self.ss.C.shape[0], ))
if len(self.output_offset) != self.ss.C.shape[0]:
raise ValueError("Length of provided output offset does not match "
"output dimensions ({} != {}).".format(
len(self.output_offset), self.ss.C.shape[0]))
# add specific "private" settings
settings.update(input_order=0)
settings.update(output_dim=self.ss.C.shape[0])
settings.update(input_type=settings["input source"])
super().__init__(settings)
if settings.get("poles", None) is None:
# pretty useless but hey why not.
self.feedback = np.zeros((self.ss.B.shape[1], self.ss.A.shape[0]))
else:
if self.ss.B.shape[1] == 1:
# save the control/slycot dependency
self.feedback = place_siso(self.ss.A, self.ss.B,
self.settings["poles"])
else:
import control
self.feedback = control.place(self.ss.A, self.ss.B,
self.settings["poles"])
self.prefilter = calc_prefilter(self.ss.A, self.ss.B, self.ss.C,
self.feedback)
def calc_control_parameters(M, m, b, l, I):
"""Given a system description, calculate a control matrix"""
p = I * (M + m) + M * m * l**2 # denominator for the A and B matrices
g = gravity
A = array([[0, 1, 0, 0],
[0, -(I + m * l**2) * b / p, (m**2 * g * l**2) / p, 0],
[0, 0, 0, 1], [0, -(m * l * b) / p, m * g * l * (M + m) / p,
0]])
B = array([[0], [(I + m * l**2) / p], [0], [m * l / p]])
# Place arbitrary negative poles in the control matrix as described.
K = control.place(A, B, [-1, -1, -1, -1])
return K[0]
def __init__(self):
self.values = None
self.boostPercent = 0.0 #boost Percentage 0 - 100
self.pitchPercent = 0 #pitch percentage
self.lastz = None #the tick priors value of position
self.lastvz = None #the tick priors value of velocity
self.lastError = 0.0 #prior error
self.errorzI = 0.0 #summation of values for integral of error
self.errorzICOUNTER = 1.0
#Rocket League physics (using unreal units (uu))
self.g = 650 #uu/s^2
self.Dp = -2.798194258050845 #Drag coeff for pitch
self.Tp = 12.14599781908070
T_r = -36.07956616966136
# torque coefficient for roll
T_p = -12.14599781908070
# torque coefficient for pitch
T_y = 8.91962804287785
# torque coefficient for yaw
D_r = -4.47166302201591
# drag coefficient for roll
D_p = -2.798194258050845
# drag coefficient for pitch
D_y = -1.886491900437232
# drag coefficient for yaw
self.I = 1
self.m = 180 #mass of the car arbitrary units
#State Space matrix coefficients
self.A = np.matrix([[0, 1], [0, 0]])
self.B = np.matrix([[0], [self.Tp]])
self.C = np.matrix([[1, 0], [0, 1]])
self.D = np.matrix([[0], [0]])
self.system = control.ss(self.A, self.B, self.C, self.D, None)
self.controllability = control.ctrb(self.A, self.B)
print("ctrb:", self.controllability)
#print(self.B)
#print(control.ctrb(self.A, self.B))
self.poles = np.array([-1000, -5])
print('poles: ', self.poles)
#self.eigen= control.pole(self.system)
self.eigen = self.system.pole()
print("Eigen: ", self.eigen)
self.K = control.place(self.A, self.B, self.poles)
print("\nK: ", self.K)
def calc_control_parameters(M,m,b,l,I):
"""Given a system description, calculate a control matrix"""
p = I*(M+m)+M*m*l**2 # denominator for the A and B matrices
g = gravity
A = array([[0, 1, 0, 0],
[0, -(I+m*l**2)*b/p, (m**2*g*l**2)/p, 0],
[0, 0, 0, 1],
[0, -(m*l*b)/p, m*g*l*(M+m)/p, 0]])
B = array([[0],
[(I+m*l**2)/p],
[0],
[m*l/p]])
# Place arbitrary negative poles in the control matrix as described.
K = control.place(A,B, [-1,-1,-1,-1])
return K[0]
def _find_act(self):
ral_sys = self.fsf_dict['ral_sys']
ral_poles = ral_sys.pole()
print(' '.join(('OL RAL poles:', str(ral_poles))))
new_poles = ral_poles
for i in range(len(new_poles)):
if new_poles[i].real > 0:
new_poles[i] = - new_poles[i].real + 1j * new_poles[i].imag
elif new_poles[i].real < 0:
new_poles[i] = new_poles[i].real + 1j * new_poles[i].imag
elif new_poles[i].real == 0:
new_poles[i] = - 10 * abs(np.random.random(1)) + 1j * new_poles[i].imag
print(' '.join(('CL RAL poles:', str(new_poles))))
K = control.place(ral_sys.A, ral_sys.B, new_poles)
#Closed-loop state space
cl_ral_sys = control.StateSpace(ral_sys.A - ral_sys.B @ K, np.zeros(ral_sys.B.shape), ral_sys.C, np.zeros(ral_sys.D.shape))
cl_ral_poles = cl_ral_sys.pole()
#Update FSF dictionary
self.fsf_dict.update(cl_ral_sys = cl_ral_sys)
self.fsf_dict.update(K = K)
def simulate(dt=0.1):
import control
time = np.arange(0, 20, dt)
vehicle = BicycleVehicle(road=None, position=[0, 5], velocity=8.3)
xx, uu = [], []
from highway_env.interval import LPV
A, B = vehicle.full_lateral_lpv_dynamics()
K = -np.asarray(control.place(A, B, -np.arange(1, 5)))
lpv = LPV(x0=vehicle.state[[1, 2, 4, 5]].squeeze(),
a0=A,
da=[np.zeros(A.shape)],
b=B,
d=[[0], [0], [0], [1]],
omega_i=[[0], [0]],
u=None,
k=K,
center=None,
x_i=None)
for t in time:
# Act
u = K @ vehicle.state[[1, 2, 4, 5]]
omega = 2 * np.pi / 20
u_p = 0 * np.array([[-20 * omega * np.sin(omega * t) * dt]])
u += u_p
# Record
xx.append(
np.array(
[vehicle.position[0], vehicle.position[1],
vehicle.heading])[:, np.newaxis])
uu.append(u)
# Interval
lpv.set_control(u, state=vehicle.state[[1, 2, 4, 5]])
lpv.step(dt)
x_i_t = lpv.change_coordinates(lpv.x_i_t, back=True, interval=True)
# Step
vehicle.act({"acceleration": 0, "steering": u})
vehicle.step(dt)
xx, uu = np.array(xx), np.array(uu)
plot(time, xx, uu)
def control(self, t, x, e_traj, lambda_traj):
# delete front and back speed because we dont need it here
x = x[:6]
# p, e, lamb, dp, de, dlamb = x
# u_op = np.array([self.Vf_op, self.Vb_op])
# x_op = np.array([0, self.operating_point[1], self.operating_point[0], 0, 0, 0])
operating_point, Vf_op, Vb_op = get_current_operating_point(
self.flatness_model_type, e_traj, lambda_traj)
A, B, Vf_op, Vb_op = getLinearizedMatrices(
self.linearization_model_type, operating_point, Vf_op, Vb_op)
linearized_state = x - operating_point
# compute K for this time step
try:
K_now = ctr.place(A, B, self.poles)
except Exception as e:
print("Error during pole placement: " + str(e))
u = -K_now @ linearized_state
return u
def __init__(self, model):
# Bind model
self.model = model
# Desired x_pos
self.xd = 0.0
# Compute desired K
if self.model.name == 'Pendulum':
desired_eigenvalues = [-2, -8, -9, -10]
else:
desired_eigenvalues = [-9, -10]
self.K = control.place(self.model.A_cont, self.model.B_cont,
desired_eigenvalues)
# Closed loop system matrix
Acl = self.model.A_cont - self.model.B_cont * self.K
# DC gain
Kdc = self.model.C * np.linalg.inv(Acl) * self.model.B_cont
# Reference gain
self.Kr = -1 / Kdc[0]
############ ODE solver for Observer ########
def obser(x_hat, t, L_, y_):
u = -np.matmul(K[0], x_hat.reshape(-1, 1))
dxdt = np.matmul(
(A - np.matmul(L_, C)), x_hat.reshape(-1, 1)) + B * u + np.matmul(
L_, y_.reshape((-1, 1)))
return dxdt.reshape(-1)
############# Solving non linear equation ############################
x0 = np.array([[0], [0], [-0.2], [0], [0.3], [0]])
t = np.arange(0, 100, 0.1)
####################################################################
L = control.place(np.matrix.transpose(A), np.matrix.transpose(C), K[2])
L_ = np.matrix.transpose(L)
x_hat0 = x0.reshape(6)
u = -np.matmul(K[0], x_hat0)
observed = []
observed.append(x_hat0)
for i in range(len(t) - 1):
print("solving " + str(i))
X = odeint(pend_non_linear, x_hat0, t[i:i + 2], args=(A, B, K[0]))
y = np.matmul(C, (X[1]).reshape(-1, 1)).reshape(-1) + np.random.normal(
mean, std_dev)
x_hat = odeint(obser, x_hat0, t[i:i + 2], args=(L_, y))
x_hat0 = x_hat[1]
u = -np.matmul(K[0], x_hat0)
observed.append(x_hat0)
def __init__(self):
# get parameters
try:
param_namespace = '/whirlybird'
self.param = rospy.get_param(param_namespace)
except KeyError:
rospy.logfatal('Parameters not set in ~/whirlybird namespace')
rospy.signal_shutdown('Parameters not set')
g = self.param['g']
l1 = self.param['l1']
l2 = self.param['l2']
m1 = self.param['m1']
m2 = self.param['m2']
d = self.param['d']
h = self.param['h']
r = self.param['r']
Jx = self.param['Jx']
Jy = self.param['Jy']
Jz = self.param['Jz']
km = self.param['km']
Jm = m1*l1**2 + m2*l2**2
self.Fe = (m1*l1 - m2*l2)*g/l1 #Note this is not the correct value for Fe, you will have to find that yourself
# Controller
self.controller = 'SS'
if self.controller == 'SS':
self.A_lat = [[0,0,1,0,0],[0,0,0,1,0],[0,0,0,0,0],[l1*self.Fe/(Jm+Jz),0,0,0,0],[0,-1,0,0,0]]
self.B_lat = [[0],[0],[1/Jx],[0],[0]]
self.C_lat = [[1,0,0,0],[0,1,0,0]]
if 5 != np.linalg.matrix_rank(control.ctrb(self.A_lat,self.B_lat)):
print 'Not controlable!!!!!!!!!!!!!!!!!!!!!!!!!!!!!'
self.A_lon = [[0,1,0],[(m1*l1-m2*l2)*g*np.sin(0)/(Jm + Jy),0,0],[-1,0,0]]
self.B_lon = [[0],[l1/(Jm + Jy)],[0]]
self.C_lon = [[1,0]]
if 3 != np.linalg.matrix_rank(control.ctrb(self.A_lon,self.B_lon)):
print 'Not controlable!!!!!!!!!!!!!!!!!!!!!!!!!!!!!'
# Pitch Gains
t_r_theta = 1.4
zeta_theta = 0.707
w_n_theta = 2.2/t_r_theta
p_i_lon = -5#-w_n_theta/2.0
theta_poles = np.roots([1,2*zeta_theta*w_n_theta,w_n_theta**2])
poles_lon = []
for pole in theta_poles:
poles_lon.append(pole)
poles_lon.append(p_i_lon)
K = control.place(self.A_lon,self.B_lon,poles_lon)
self.kr_lon = K[0,2]
self.K_lon = K[0,0:2]
#self.kr_lon = -1/(np.matmul(self.C_lon,np.matmul(np.linalg.inv(np.subtract(self.A_lon,np.matmul(self.B_lon,self.K_lon))),self.B_lon)))
# Yaw Gains
t_r_phi = 0.3
zeta_phi = 0.707
M = 5
t_r_psi = M*t_r_phi
zeta_psi = 0.707
w_n_phi = 2.2/t_r_phi
w_n_psi = 2.2/t_r_psi
p_i_lat = -5#-w_n_psi/2.0
phi_poles = np.roots([1,2*zeta_phi*w_n_phi,w_n_phi**2])
psi_poles = np.roots([1,2*zeta_psi*w_n_psi,w_n_psi**2])
poles_lat = []
for pole in phi_poles:
poles_lat.append(pole)
for pole in psi_poles:
poles_lat.append(pole)
poles_lat.append(p_i_lat)
K = control.place(self.A_lat,self.B_lat,poles_lat)
self.kr_lat = K[0,4]
self.K_lat = K[0,0:4]
#self.kr_lat = -1/(np.matmul(self.C_lat[1],np.matmul(np.linalg.inv(np.subtract(self.A_lat,np.matmul(self.B_lat,self.K_lat))),self.B_lat)))
# Dirty Derivative gains
self.sigma_theta = 0.05
self.sigma_phi = 0.05
self.sigma_psi = 0.05
# Initialize
self.thetao = 0
self.phio = 0
self.psio = 0
self.theta_doto = 0
self.phi_doto = 0
self.psi_doto = 0
self.Int_theta = 0.0
self.error_theta = 0.0
self.Int_psi = 0.0
self.error_psi = 0.0
self.psi_vlim = 0.05
self.theta_vlim = 0.05
self.theta_r = 0;
self.psi_r = 0;
elif self.controller == 'PID':
# Roll Gains
t_r_phi = 0.3
zeta_phi = 0.707
self.sigma_phi = 0.05
self.phi_r = 0.0
self.P_phi_ = (2.2/t_r_phi)**2*Jx
self.I_phi_ = 0.0
self.D_phi_ = 2*zeta_phi*(2.2/t_r_phi)*Jx
self.prev_phi = 0.0
self.Int_phi = 0.0
self.Dir_phi = 0.0
self.error_phi = 0.0
self.phi_vlim = 0.05
# Pitch Gains
b_theta = l1/(m1*l1**2 + m2*l2**2 + Jy)
t_r_theta = 1.0
zeta_theta = 0.707
self.sigma_theta = 0.05
self.theta_r = 0.0
self.P_theta_ = (2.2/t_r_theta)**2/(b_theta)
self.I_theta_ = 0.3
self.D_theta_ = 2*zeta_theta*(2.2/t_r_theta)/(b_theta)
self.prev_theta = 0.0
self.Int_theta = 0.0
self.Dir_theta = 0.0
self.error_theta = 0.0
self.theta_vlim = 0.05
# self.theta_ddot_old = [0.0];
# Yaw Gains
M = 10
t_r_psi = M*t_r_phi
zeta_psi = 0.707
Fe = (m1*l1 - m2*l2)*g/l1
b_psi = l1*Fe/(m1*l1**2 + m2*l2 **2 + Jz)
self.sigma_psi = 0.05
self.psi_r = 0.0
self.P_psi_ = (2.2/t_r_psi)**2/(b_psi)
self.I_psi_ = 0.01
self.D_psi_ = 2*zeta_psi*(2.2/t_r_psi)/(b_psi)
self.prev_psi = 0.0
self.Int_psi = 0.0
self.Dir_psi = 0.0
self.error_psi = 0.0
self.psi_vlim = 0.05
self.prev_time = rospy.Time.now()
self.sat_low = 0.0
self.sat_high = 0.7
self.command_sub_ = rospy.Subscriber('whirlybird', Whirlybird, self.whirlybirdCallback, queue_size=5)
self.psi_r_sub_ = rospy.Subscriber('psi_r', Float32, self.psiRCallback, queue_size=5)
self.theta_r_sub_ = rospy.Subscriber('theta_r', Float32, self.thetaRCallback, queue_size=5)
self.command_pub_ = rospy.Publisher('command', Command, queue_size=5)
while not rospy.is_shutdown():
# wait for new messages and call the callback when they arrive
rospy.spin()
zero_padder = np.array([[0, 0, 0, 0, 0], [0, 0, 0, 0, 0]]) AB = np.concatenate((A, B), axis=1) mat = np.concatenate((AB, zero_padder), axis=0) exmat = sp_expm(ts * mat) Ad = exmat[0:n_states, 0:n_states] Bd = exmat[0:n_states, n_states:n_states + n_inputs] print "\n -------------Closed-Loop Control------------- " eigval, eigvec = np.linalg.eig(A) print "\neigen values of A = " + str(eigval) # -12.87243308 -3.64492168 10.86290231 K_matrix = control.place(A, B, [-12.87243308, -3.64492168, -10.86290231]) print "\nK matrix = \n" + str(K_matrix) A_BK = A - np.dot(B, K_matrix) print "\nA - BK = \n" + str(A_BK) eigval_newC, eigvec_newC = np.linalg.eig(A_BK) print "\neigen values of (A-BK) = " + str(eigval_newC) print "\n now lets do some simulations:" t_ = np.zeros((1, iterations)) x_ = np.zeros((n_states, iterations + 1)) z_ = np.zeros((1, iterations + 1)) u_ = np.zeros((n_inputs, iterations + 1)) x0 = np.array([[Xo[0][0]], [Xo[1][0]], [Xo[2][0]]])
[0, -(I+m*l**2)*b/p, (m**2*g*l**2)/p, 0],
[0, 0, 0, 1],
[0, -(m*l*b)/p, m*g*l*(M+m)/p, 0]])
B = array([[0],
[(I+m*l**2)/p],
[0],
[m*l/p]])
C = array([[1, 0, 0, 0],
[0, 0, 1, 0]])
D = array([[0],
[0]])
poles,vect = eig(A)
# Print vectors to verify that the system is unstable
print poles
K = control.place(A,B, [-1,-1,-1,-1])
print 'K(place)=',K
Q = C.transpose().dot(C)
Q[1,1]=5000
Q[3,3] = 100
R = 1
K,S,E = control.lqr(A,B,Q,R)
print 'K(lqr)=',K
# New control matrix
Ac = A-B.dot(K)
poles,vect = eig(Ac)
print "Ac poles=", poles
theta = 0 # pendulum angle from vertical down
g = 9.8 # gravitational constant
p = I * (M + m) + M * m * l**2 # denominator for the A and B matrices
A = array([[0, 1, 0,
0], [0, -(I + m * l**2) * b / p, (m**2 * g * l**2) / p, 0],
[0, 0, 0, 1], [0, -(m * l * b) / p, m * g * l * (M + m) / p, 0]])
B = array([[0], [(I + m * l**2) / p], [0], [m * l / p]])
C = array([[1, 0, 0, 0], [0, 0, 1, 0]])
D = array([[0], [0]])
poles, vect = eig(A)
# Print vectors to verify that the system is unstable
print poles
K = control.place(A, B, [-1, -1, -1, -1])
print 'K(place)=', K
Q = C.transpose().dot(C)
Q[1, 1] = 5000
Q[3, 3] = 100
R = 1
K, S, E = control.lqr(A, B, Q, R)
print 'K(lqr)=', K
# New control matrix
Ac = A - B.dot(K)
poles, vect = eig(Ac)
print "Ac poles=", poles
# Add labels to the figure
normalized_label(inpfig, outfig)
plt.legend(('$\zeta_c = 0.5$', '$\zeta_c = 0.7$',
'$\zeta_c = 1$')) # Place a legend on the Axes.
plt.tight_layout() # Adjust the padding between and around subplots.
# ## Eigenvalue placement observer design (Example 8.3)
# ##
# Find the eigenvalue from the characteristic polynomial
wo = 1 # bandwidth for the observer
zo = 0.7 # damping ratio for the observer
eigs = np.roots([1, 2 * zo * wo, wo**2])
# Compute the estimator gain using eigenvalue placement利用特征值布局计算估计增益
L = np.transpose(ct.place(np.transpose(A), np.transpose(C), eigs))
print("L = ", L)
# Create a linear model of the lateral dynamics driving the estimator
est = ct.StateSpace(A - L @ C, np.block([[B, L]]), np.eye(2), np.zeros((2, 2)))
#
#
#
# ## Linear observer applied to nonlinear system output
# ##
# Convert the curvy trajectory into normalized coordinates
x_ref = x_curvy[0] / wheelbase
y_ref = x_curvy[1] / wheelbase
theta_ref = x_curvy[2]
def synthesize_controller(self,
pole_placement=False,
ensure_stability=True):
"""
Synthesize a controller the interval predictor via an LMI
:param pole_placement: use pole placement to synthesize the controller instead
:param ensure_stability: whether we need to check stability when the pole placement method is used
:return: whether we found stabilising controls (True if ensure_stability is False)
"""
# Input matrices
A0 = np.array(self.config["A0"])
dA = np.array(self.config["dA"])
B = np.array(self.config["B"])
logger.debug("A0:\n{}".format(A0))
logger.debug("dA:\n{}".format(dA))
logger.debug("B:\n{}".format(B))
dAp = sum(pos(dAi) for dAi in dA)
dAn = sum(neg(dAi) for dAi in dA)
DA = sum(dAi for dAi in dA)
p, q = int(B.shape[0]), int(B.shape[1])
# Extended matrices
zero = np.zeros((p, p))
cA0 = np.concatenate((np.concatenate(
(A0, zero), axis=1), np.concatenate((zero, A0), axis=1)))
cA1 = np.concatenate((np.concatenate(
(zero, -dAn), axis=1), np.concatenate((zero, dAp), axis=1)))
cA2 = np.concatenate((np.concatenate(
(-dAp, zero), axis=1), np.concatenate((dAn, zero), axis=1)))
cB = np.concatenate((B, B))
# Pole placement
if pole_placement:
import control
logger.debug(
"The eigenvalues of the matrix A0 = {}, Uncontrollable states = {}"
.format(np.linalg.eigvals(A0),
p - np.linalg.matrix_rank(control.ctrb(A0, B))))
poles = self.config.get(
"poles", np.minimum(np.linalg.eigvals(A0),
-np.arange(1, p + 1)))
K = -control.place(A0, B, poles)
logger.debug("The eigenvalues of the matrix A0+BK = {}".format(
np.linalg.eigvals(A0 + B * K)))
logger.debug("The eigenvalues of the matrix A0+BK+DA = {}".format(
np.linalg.eigvals(A0 + B * K + DA)))
logger.debug("The eigenvalues of the matrix A0+BK-DA = {}".format(
np.linalg.eigvals(A0 + B * K - DA)))
self.K0 = 0.5 * np.concatenate((K, K), axis=1)
self.K1 = self.K2 = np.zeros(self.K0.shape)
cA0 += cB @ self.K0
if not ensure_stability:
return True
# Solve LMI
success = self.stability_lmi(cA0,
cA1,
cA2,
cB,
synthesize_control=not pole_placement)
# If control synthesis via LMI fails, try pole placement instead
if not success and not pole_placement:
success = self.synthesize_controller(
pole_placement=True, ensure_stability=ensure_stability)
return success
# Statespace equations
A = np.array([[0.0, 1.0], [(-k1 / m), (-b / m)]])
B = np.array([[0.0], [(1 / (m))]])
C = np.array([[1.0, 0.0]])
# Augmented Statespace system for use with integrator full state feedback
A1 = np.array([[0.0, 1.0, 0.0], [(-k1 / m), (-b / m), 0.0], [-1.0, 0.0, 0.0]])
B1 = np.array([[0.0], [(1 / m)], [0.0]])
# gain calculation
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)
[0,0,0,1.],\
[0,d1/L,_q,-d2] ] )
B = np.expand_dims(np.array([0, 1.0 / M, 0., -1 / (M * L)]), 1)
# Pendulum Down - Verified correct.
# Eigen Values of this: array([ 0.00+0.j , -1.00+0.j , -0.25+2.78881695j, -0.25-2.78881695j])
# A = np.array([\
# [0,1,0,0], \
# [0,-d1, -g*m/(2*m+M),0],\
# [0,0,0,1],\
# [0,0,-(M+m)*g/(M*L),-d2] ] )
#B = np.array( [] )
print 'A\n', A
print 'B\n', B
# Controllability
print '---Controllability'
print 'rank of ctrb(A,b)', np.linalg.matrix_rank(control.ctrb(A, B))
print 'Eigenvalues of A ', np.linalg.eig(A)
# Pole Placement
K = control.place(A, B, [-1, -2, -4, -5])
print '---Pole Placement\nK=\n', K
# Verification of Eigen values of A-BK
print '---Verification of Eigen values of A-BK'
print 'Eigenvalues of A-BK', np.linalg.eig(A - np.matmul(B, K))
[2, -1]])
B = np.array([[1],
[0]])
C = np.array([1, 0])
D = np.array([0])
sys = c.ss(A, B, C, D) # Create state-space model of sample system
stepU = c.step_response(sys)
E = LA.eigvals(A)
print(E)# One of the eigenvalues is >0 so the energy of the system will blow up to infty
P = np.array([-2, -1]) # Place poles at -2, -1
# Note: You can change the aggressiveness by changing the poles
K = c.place(A, B, P) # Place new poles
Acl = np.array(A - B * K) # Create a closed loop A matrix
Ecl = LA.eigvals(Acl) # Calculate new eigenvalues (this evaluates to the poles that I placed!)
syscl = c.ss(Acl, B, C, D) # Closed loop state space model
step = c.step_response(syscl)
Kdc = c.dcgain(syscl) # Calculating a dc gain in order to reach intended output goal
Kr = 1 / Kdc
sysclNew = c.ss(Acl, B * Kr, C, D)
sysclNewDiscrete = matlab.c2d(sysclNew, 0.01)
def main():
J = 7.7500e-05
b = 8.9100e-05
Kt = 0.0184
Ke = 0.0211
R = 0.0916
L = 5.9000e-05
# fmt: off
A = np.array([[-b / J, Kt / J], [-Ke / L, -R / L]])
B = np.array([[0], [1 / L]])
C = np.array([[1, 0]])
D = np.array([[0]])
# fmt: on
sysc = cnt.StateSpace(A, B, C, D)
dt = 0.0001
tmax = 0.025
sysd = sysc.sample(dt)
# fmt: off
Q = np.array([[1 / 20**2, 0], [0, 1 / 40**2]])
R = np.array([[1 / 12**2]])
# fmt: on
K_pp1 = cnt.place(sysd.A, sysd.B, [0.1, 0.9])
K_pp2 = cnt.place(sysd.A, sysd.B, [0.1, 0.8])
K_lqr = frccnt.lqr(sysd, Q, R)
t = np.arange(0, tmax, dt)
r = np.array([[2000 * 0.1047], [0]])
r_rec = np.zeros((2, 1, len(t)))
# Pole placement 1
x_pp1 = np.array([[0], [0]])
x_pp1_rec = np.zeros((2, 1, len(t)))
u_pp1_rec = np.zeros((1, 1, len(t)))
# Pole placement 2
x_pp2 = np.array([[0], [0]])
x_pp2_rec = np.zeros((2, 1, len(t)))
u_pp2_rec = np.zeros((1, 1, len(t)))
# LQR
x_lqr = np.array([[0], [0]])
x_lqr_rec = np.zeros((2, 1, len(t)))
u_lqr_rec = np.zeros((1, 1, len(t)))
u_min = np.asarray(-12)
u_max = np.asarray(12)
for k in range(len(t)):
# Pole placement 1
u_pp1 = K_pp1 @ (r - x_pp1)
# Pole placement 2
u_pp2 = K_pp2 @ (r - x_pp2)
# LQR
u_lqr = K_lqr @ (r - x_lqr)
u_pp1 = np.clip(u_pp1, u_min, u_max)
x_pp1 = sysd.A @ x_pp1 + sysd.B @ u_pp1
u_pp2 = np.clip(u_pp2, u_min, u_max)
x_pp2 = sysd.A @ x_pp2 + sysd.B @ u_pp2
u_lqr = np.clip(u_lqr, u_min, u_max)
x_lqr = sysd.A @ x_lqr + sysd.B @ u_lqr
r_rec[:, :, k] = r
x_pp1_rec[:, :, k] = x_pp1
u_pp1_rec[:, :, k] = u_pp1
x_pp2_rec[:, :, k] = x_pp2
u_pp2_rec[:, :, k] = u_pp2
x_lqr_rec[:, :, k] = x_lqr
u_lqr_rec[:, :, k] = u_lqr
plt.figure(1)
plt.subplot(3, 1, 1)
plt.plot(t, r_rec[0, 0, :], label="Reference")
plt.ylabel("$\omega$ (rad/s)")
plt.plot(t,
x_pp1_rec[0, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.9, 0)$")
plt.plot(t,
x_pp2_rec[0, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.8, 0)$")
plt.plot(t, x_lqr_rec[0, 0, :], label="LQR")
plt.legend()
plt.subplot(3, 1, 2)
plt.plot(t, r_rec[1, 0, :], label="Reference")
plt.ylabel("Current (A)")
plt.plot(t,
x_pp1_rec[1, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.9, 0)$")
plt.plot(t,
x_pp2_rec[1, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.8, 0)$")
plt.plot(t, x_lqr_rec[1, 0, :], label="LQR")
plt.legend()
plt.subplot(3, 1, 3)
plt.plot(t,
u_pp1_rec[0, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.9, 0)$")
plt.plot(t,
u_pp2_rec[0, 0, :],
label="Pole placement at $(0.1, 0)$ and $(0.8, 0)$")
plt.plot(t, u_lqr_rec[0, 0, :], label="LQR")
plt.legend()
plt.ylabel("Control effort (V)")
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latexutils.savefig("case_study_pp_lqr")
else:
plt.show()
