def __init__(self, dt, name, num_stages_per_side, num_motors_per_side, stall_current, free_current, stall_torque,
free_speed, gear_ratio_per_side, moment_of_inertia, wheel_radius, drivebase_radius, mass,
C = numpy.matrix([[1 , 0, 0, 0], [0,1,0,0]])):
"""Instantiates the drive template. Units should be SI. Free speed
should be radians per second, gear ratio should be greater than one,
stall current, free current, stall torque just refer to your motor specs for one
of that type which should be found on Vex Motors. The dt refers to the robot's
loop speed which as of 2018, is 5 ms (so dt = 0.005), and name refers to what
you want to call the subsystem. Drivebase radius is a fudge factor for the
effects of the drivetrain on each side, a reasonable approximation is half
the length of the drivetrain base."""
state_space_controller.__init__(self, dt, name)
#Input values
self.N = num_stages_per_side
self.n = num_motors_per_side
self.I_stall = stall_current * self.n
self.I_free = free_current * self.n
self.tau_stall = stall_torque * self.n
self.omega_free = free_speed
self.J = moment_of_inertia
self.G = gear_ratio_per_side
self.r_w = wheel_radius
self.r_b = drivebase_radius
self.m = mass
#Derived Values
#efficiency
self.mu = 0.95 ** self.N
#resistance
self.R = 12.0 / self.I_stall
self.k_t = self.I_stall / self.tau_stall
self.k_v = (12.0 - self.I_free * self.R) / self.omega_free
self.gamma_1 = (1.0 / self.m + self.r_b * self.r_b / self.J)#just placeholder constant so I don't retype all the variables
self.gamma_2 = (1.0 / self.m - self.r_b * self.r_b / self.J)
#coefficient of velocity
self.k_vel = -self.k_v * self.G * self.G / (self.k_t * self.R * self.r_w * self.r_w)
#coefficient of input
self.k_u = self.mu * self.G / (self.k_t * self.R * self.r_w)
#State is [[pos_left],[pos_right],[vel_left],[vel_right]]
self.A_c = numpy.matrix([[0, 0, 1.0, 0],
[0, 0, 0, 1.0],
[0, 0, self.gamma_1 * self.k_vel, self.gamma_2 * self.k_vel],
[0 ,0, self.gamma_2 * self.k_vel, self.gamma_1 * self.k_vel]])
self.B_c = numpy.matrix([[0, 0],
[0, 0],
[self.gamma_1 * self.k_u, self.gamma_2 * self.k_u],
[self.gamma_2 * self.k_u, self.gamma_1 * self.k_u]])
self.C = C
self.D = numpy.matrix([[0, 0],
[0, 0]])
#converts continuous to discrete matrices
self.A_d, self.B_d = controls.c2d(self.A_c,self.B_c,self.dt)
self.minU = numpy.matrix([[-12.0], [-12.0]])
self.maxU = numpy.matrix([[12.0], [12.0]])
self.initialize_state()
def __init__(self,
dt,
name,
num_stages,
num_motors,
stall_current,
free_current,
stall_torque,
free_speed,
moment_of_inertia,
gear_ratio,
C=numpy.matrix([[1, 0]])):
state_space_controller.__init__(self, dt, name)
"""Instantiates the arm template. Units should be SI. Free speed
should be radians per second, gear ratio should be greater than one,
stall current, free current, stall torque just refer to your motor specs for one
of that type which should be found on Vex Motors. The dt refers to the robot's
loop speed which as of 2018, is 5 ms (so dt = 0.005), and name refers to what
you want to call the subsystem."""
#Input values
self.N = num_stages
self.n = num_motors
self.I_stall = stall_current * num_motors
self.I_free = free_current * num_motors
self.tau_stall = stall_torque * num_motors
self.omega_free = free_speed
self.J = moment_of_inertia
self.G = gear_ratio
#Derived Values
#efficiency
self.mu = 0.95**self.N
#resistance
self.R = 12.0 / self.I_stall
self.k_t = self.I_stall / self.tau_stall
self.k_v = (12.0 - self.I_free * self.R) / self.omega_free
#coefficient of velocity
self.k_omega = -self.k_v * self.G * self.G / (self.k_t * self.R *
self.J)
#coefficient of input
self.k_u = self.mu * self.G / (self.k_t * self.R * self.J)
#State is [[angle],[angular_velocity]]
self.A_c = numpy.matrix([[0, 1], [0, self.k_omega]])
self.B_c = numpy.matrix([[0], [self.k_u]])
self.C = C
self.D = numpy.matrix([[0]])
#converts continuous to discrete matrices
self.A_d, self.B_d = controls.c2d(self.A_c, self.B_c, self.dt)
self.minU = numpy.matrix([[-12.0]])
self.maxU = numpy.matrix([[12.0]])
self.time_elapsed = 0.0
self.initialize_state()
def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
"""Calculates the discrete time values for A and B.
Args:
A_continuous: numpy.matrix, The continuous time A matrix
B_continuous: numpy.matrix, The continuous time B matrix
dt: float, The time step of the control loop
Returns:
(A, B), numpy.matrix, the control matricies.
"""
return controls.c2d(A_continuous, B_continuous, dt)
def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
"""Calculates the discrete time values for A and B.
Args:
A_continuous: numpy.matrix, The continuous time A matrix
B_continuous: numpy.matrix, The continuous time B matrix
dt: float, The time step of the control loop
Returns:
(A, B), numpy.matrix, the control matricies.
"""
return controls.c2d(A_continuous, B_continuous, dt)
def error_u_kalman(plant, Q_c = None, R_c = None, Q_d = None, R_d = None):
augmented_system = _augment(plant)
if Q_c is not None and R_c is not None:
_, _, Q_d, R_d = controls.c2d(augmented_system.A_c, augmented_system.B_c, augmented_system.dt, Q_c, R_c)
elif Q_d is not None and R_d is not None:
pass
else:
Q_d = plant.Q_d
R_d = plant.R_d
augmented_system.Q_c = Q_c
augmented_system.R_c = R_c
augmented_system.Q_d = Q_d
augmented_system.R_d = R_d
return state_space_observer.kalman(augmented_system)
def error_u_lqr(plant, u_min = None, u_max = None, Q_c = None, R_c = None, Q_d = None, R_d = None):
augmented_system = _augment(plant)
if Q_c is not None and R_c is not None:
_, _, Q_d, R_d = controls.c2d(augmented_system.A_c, augmented_system.B_c, augmented_system.dt, Q_c, R_c)
elif Q_d is not None and R_d is not None:
pass
else:
Q_d = plant.Q_d
R_d = plant.R_d
augmented_system.Q_c = Q_c
augmented_system.R_c = R_c
augmented_system.Q_d = Q_d
augmented_system.R_d = R_d
return state_space_controller.lqr(augmented_system, u_min, u_max)
def set_system(self, dt, A, B, C, D=0, Q=None, R=None):
self.A_c = np.asmatrix(A)
self.B_c = np.asmatrix(B)
self.C = np.asmatrix(C)
self.D = np.asmatrix(D)
if Q is None:
Q = np.zeros(self.A_c.shape)
self.Q_c = np.asmatrix(Q)
if R is None:
R = mat.zeros((self.num_outputs, self.num_outputs))
self.R_c = np.asmatrix(R)
self.dt = dt
self.A_d, self.B_d, self.Q_d, self.R_d = controls.c2d(
self.A_c, self.B_c, dt, self.Q_c, self.R_c)
def main():
rod_weight = 4.0 * 0.293 * 0.453592
radius = 3.0 / 16.0 * 0.0254
pitch = 16.0/ (1 * 0.0254)
load = calculate_load_torque(40. * 4.4482, 2.0 * radius, pitch, 0.19)
test = linear_actuator(0.005, "test", 1.0, 1.0, 105.0, 1.8, 2.6, 5676 * 2.0 * math.pi / 60.0,
0.5 * rod_weight * radius * radius, 1.0, pitch, load)
#test.run_custom_test(numpy.matrix([[0.0],[0]]), numpy.matrix([[24.0],[0]]), max_func, True, False, True, 800)
#test.get_stats()
best_G = calculate_ideal_gear_ratio(load, 1.3)
print(best_G)
#Neo @ 1:3 overdrive running at 10 volts works best
drive_gear_ratio = 7.083 * 24.0 / 60.0 * 64.0 / 20.0;#1.13333#5.10#50.0 /34.0 2.16 ratio spread 54 : 30
j = 6.0
#2778 2000 2907 3269 3211 3099 3287 3573 2705
#drivetrain = drive(0.005, "drivetrain", 4.0, 2.0, 131.0, 2.7, 2.41, 5330 * 2.0 * math.pi / 60.0,
# drive_gear_ratio, j, 3.0 * 0.0254, 28.0 * 0.0254 * 0.5, 154.0 * 0.454)
#radius base might be 25.5 * 0.0254 * 0.5, but not likely, more like 0.45
drivetrain = drive(0.005, "drivetrain", 3.0, 2.0, 131.0, 2.7, 2.41, 5330 * 2.0 * math.pi / 60.0,
drive_gear_ratio, j, 3.0 * 0.0254, 25.5 * 0.0254 * 0.5 , (125.0) * 0.454)
#drivetrain.run_pid_test([1.0, 0.0,0],numpy.matrix([[0.0],[0],[0],[0]]), numpy.matrix([[55.0],[55.0], [0.0],[0.0]]), True, True, 4000)
#drivetrain.run_custom_test(numpy.matrix([[0.0],[0],[0],[0]]), numpy.matrix([[12.0],[12.0], [0.0],[0.0]]), full_volts, True, False, True, 2000)
drivetrain.get_stats(True)
driveQ_c = numpy.matrix([[1.0 / (0.14)**2.0 , 0, 0, 0],
[0, 1.0 / (0.14)**2.0, 0, 0],
[0, 0, 1.0 / (1.0)**2.0, 0],
[0, 0, 0, 1.0 / (1.0)**2.0]])
driveR_c = numpy.matrix([[0.0001**2, 0.0],
[0.0, 0.0001 **2]])
# [0 , 1.0 / (.1 * 0.0254)**2]])
driveA_d, driveB_d, driveQ_d, driveR_d = controls.c2d(drivetrain.A_c, drivetrain.B_c, 0.005, driveQ_c, driveR_c)
driveK = controls.dlqr(driveA_d, driveB_d, driveQ_d, driveR_d)
print(driveK)
elev = elevator(0.005, "elev", 2.0, 2.0, 134.0, 0.7, 0.71, 18730.0 * 2.0 * math.pi / 60.0,
63.0, 3.0 * (0.8755 - 0.16) * 0.0254, 18.8* 0.454)#11.59)
#elev.export("../python")
elevQ_c = numpy.matrix([[1.0 / (0.0254 * 0.01)**2.0 , 0],
[0 , 1.0 / (.1 * 0.0254)**2]])
elevR_c = numpy.matrix([[1.0 / 13.0**2]])
elevA_d, elevB_d, elevQ_d, elevR_d = controls.c2d(elev.A_c, elev.B_c, 0.005, elevQ_c, elevR_c)
elevK = controls.dlqr(elevA_d, elevB_d, elevQ_d, elevR_d)
elevL = controls.dkalman(elevA_d, elevB_d, elevQ_d, elevR_d)
#print(elevK)
elev.run_test([[0.],[0.]],[[0.0],[60.0]])
def __init__(self, dt, name):
ss.StateSpaceController.__init__(self, dt)
self.origin = (0, 0)
self.time_elapsed = 0.0
self.has_cube = False
self.free_speed = 18730.0 * 2.0 * math.pi / 60.0 #radians per second
self.free_current = 0.7 #amps
self.stall_torque = 0.71 # newton meters
self.stall_current = 134.0
self.resistence = 12.0 / self.stall_current
self.k_t = self.stall_torque / self.stall_current
self.k_v = (12.0 -
self.free_current * self.resistence) / self.free_speed
self.gear_ratio = 833.33
self.mass = 10.0 * 0.454 #kilograms
if (self.has_cube):
self.mass += 1.59
self.radius = 17.0 * 0.0254 #meters
self.moment_of_inertia = self.mass * self.radius * self.radius #10.0 #this is speculative
self.efficiency = 0.65
self.a = (-self.k_t * self.k_v * self.gear_ratio *
self.gear_ratio) / (self.moment_of_inertia * self.resistence)
self.b = (self.efficiency * self.k_t *
self.gear_ratio) / (self.moment_of_inertia * self.resistence)
#state is [angle,
# angular_velocity]
#input is [voltage]
self.A_c = numpy.matrix([[0, 1], [0, self.a]])
self.B_c = numpy.matrix([[0], [self.b]])
self.C = numpy.matrix([[1, 0]])
self.D = numpy.matrix([[0]])
q_1 = 1.0 / ((math.pi / 90.0)**2)
q_2 = 1.0 / ((math.pi / 90.0)**2)
self.Q_c = numpy.matrix([[q_1, 0], [0, q_2]])
r_1 = 1.0 / (0.1**2)
self.R_c = numpy.matrix([[r_1]])
self.temp = controls.clqr(self.A_c, self.B_c, self.Q_c, self.R_c)
print(controls.eig(self.A_c - self.B_c * self.temp))
self.A_d, self.B_d, self.Q_d, self.R_d = controls.c2d(
self.A_c, self.B_c, self.dt, self.Q_c, self.R_c)
#s = 0,-295.41341225 are open loop poles
#converting s to z is through e^(sT) where T is the discrete time step.
#(0.995 , 0.874) these are when s = -1, and s = -27
#tried with poles at 4.2, -295.41341225
poles = (cmath.exp(0.005 * (-27.0)), cmath.exp(0.005 * (-28.0)))
polesd = (-27.0 + 3.0j, -27.0 - 3.0j)
self.K = controls.place(self.A_d, self.B_d, poles)
self.Kff = controls.feedforwards(self.A_d, self.B_d, self.Q_d)
#These should be chosen 10 times faster in the s domain, and then converted
#to the z domain
#(0.951, 0.259) these are when s = -10, s= -270
self.L = controls.dkalman(self.A_d, self.C, self.Q_d, self.R_d)
self.minU = numpy.matrix([[-12.0]])
self.maxU = numpy.matrix([[12.0]])
self.initialize_state()
def __init__(self):
n = 2
tau_stall = 0.71 * n #N/m
I_stall = 134 * n# A
omega_free = 18730.0 * 2.0 * math.pi / 60.0 # rad / s
V_max = 12.0 #V
I_free = 0.7 * n
mu = 1.0
G = 63.0
#resistance
R = V_max / I_stall
#voltage constant
k_v = (V_max - I_free * R) / omega_free
#torque constant
k_t = I_stall / tau_stall
#pulley_radius
r_p = 0.0254
m = 10.0 * 0.454
A_11 = -(k_v * G * G) / (k_t * R * r_p * r_p * m)
B_01 = (G * mu) / (k_t * R * r_p * m)
A_c = np.asmatrix([[0.0, 1.0],
[0.0, A_11]])
B_c = np.asmatrix([[0.0],
[B_01]])
C = np.asmatrix([[1.0, 0.0]])
D = np.asmatrix([[0.0]])
Q_lqr = np.asmatrix([[3.0 ** 2, 0.0],
[0.0, 1.5 ** 2]])
Q_kal = np.asmatrix([[.01 ** 2, 0.0],
[0.0, .01 ** 2]])
R = np.asmatrix([[0.001 ** 2]])
self.minU = np.asmatrix([[-V_max]])
self.maxU = np.asmatrix([[V_max]])
A, B, Q_d, R_d = controls.c2d(A_c, B_c, 0.005, Q_lqr, R)
A, B, Q_kalman, R_d = controls.c2d(A_c, B_c, 0.005, Q_kal, R)
self.dt = 0.005
self.controller = ss.state_space_controller(A, B, C, D)
#pole placement
#self.K = controls.place(A, B, (0.7, 0.1))
#self.Kff = controls.feedforwards(A, B)
#self.controller.L = controls.place(A.T, C.T, (-0.61939275, 0.98497246)).T
#LQG
self.K = controls.dlqr(A, B, Q_d, R_d)
print(np.linalg.eig(A - B * self.K))
self.Kff = controls.feedforwards(A, B, Q_d)
self.controller.L = controls.dkalman(A, C, Q_kalman, R_d)
print(np.linalg.eig(A.T - C.T * self.controller.L.T))
def __init__(self, name="RawClaw"):
super(Claw, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 2.42
# Stall Current in Amps
self.stall_current = 133
# Free Speed in RPM
self.free_speed = 5500.0
# Free Current in Amps
self.free_current = 2.7
# Moment of inertia of the claw in kg m^2
self.J_top = 2.8
self.J_bottom = 3.0
# Resistance of the motor
self.R = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = (self.free_speed / 60.0 * 2.0 * numpy.pi) / (13.5 - self.R * self.free_current)
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratio
self.G = 14.0 / 48.0 * 18.0 / 32.0 * 18.0 / 66.0 * 12.0 / 60.0
# Control loop time step
self.dt = 0.01
# State is [bottom position, bottom velocity, top - bottom position,
# top - bottom velocity]
# Input is [bottom power, top power - bottom power * J_top / J_bottom]
# Motor time constants. difference_bottom refers to the constant for how the
# bottom velocity affects the difference of the top and bottom velocities.
self.common_motor_constant = -self.Kt / self.Kv / (self.G * self.G * self.R)
self.bottom_bottom = self.common_motor_constant / self.J_bottom
self.difference_bottom = -self.common_motor_constant * (1 / self.J_bottom - 1 / self.J_top)
self.difference_difference = self.common_motor_constant / self.J_top
# State feedback matrices
self.A_continuous = numpy.matrix(
[
[0, 0, 1, 0],
[0, 0, 0, 1],
[0, 0, self.bottom_bottom, 0],
[0, 0, self.difference_bottom, self.difference_difference],
]
)
self.A_bottom_cont = numpy.matrix([[0, 1], [0, self.bottom_bottom]])
self.A_diff_cont = numpy.matrix([[0, 1], [0, self.difference_difference]])
self.motor_feedforward = self.Kt / (self.G * self.R)
self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom
self.motor_feedforward_difference = self.motor_feedforward / self.J_top
self.motor_feedforward_difference_bottom = self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top)
self.B_continuous = numpy.matrix(
[
[0, 0],
[0, 0],
[self.motor_feedforward_bottom, 0],
[-self.motor_feedforward_bottom, self.motor_feedforward_difference],
]
)
print "Cont X_ss", self.motor_feedforward / -self.common_motor_constant
self.B_bottom_cont = numpy.matrix([[0], [self.motor_feedforward_bottom]])
self.B_diff_cont = numpy.matrix([[0], [self.motor_feedforward_difference]])
self.C = numpy.matrix([[1, 0, 0, 0], [1, 1, 0, 0]])
self.D = numpy.matrix([[0, 0], [0, 0]])
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
self.A_bottom, self.B_bottom = controls.c2d(self.A_bottom_cont, self.B_bottom_cont, self.dt)
self.A_diff, self.B_diff = controls.c2d(self.A_diff_cont, self.B_diff_cont, self.dt)
self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [0.75 + 0.1j, 0.75 - 0.1j])
self.K_diff = controls.dplace(self.A_diff, self.B_diff, [0.75 + 0.1j, 0.75 - 0.1j])
print "K_diff", self.K_diff
print "K_bottom", self.K_bottom
print "A"
print self.A
print "B"
print self.B
self.Q = numpy.matrix(
[
[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
[0.0, (1.0 / (0.06 ** 2.0)), 0.0, 0.0],
[0.0, 0.0, 0.10, 0.0],
[0.0, 0.0, 0.0, 0.1],
]
)
self.R = numpy.matrix([[(1.0 / (40.0 ** 2.0)), 0.0], [0.0, (1.0 / (5.0 ** 2.0))]])
# self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
self.K = numpy.matrix(
[[self.K_bottom[0, 0], 0.0, self.K_bottom[0, 1], 0.0], [0.0, self.K_diff[0, 0], 0.0, self.K_diff[0, 1]]]
)
# Compute the feed forwards aceleration term.
self.K[1, 0] = -self.B[1, 0] * self.K[0, 0] / self.B[1, 1]
lstsq_A = numpy.identity(2)
lstsq_A[0, :] = self.B[1, :]
lstsq_A[1, :] = self.B[3, :]
print "System of Equations coefficients:"
print lstsq_A
print "det", numpy.linalg.det(lstsq_A)
out_x = numpy.linalg.lstsq(lstsq_A, numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]
self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
print "K unaugmented"
print self.K
print "B * K unaugmented"
print self.B * self.K
F = self.A - self.B * self.K
print "A - B * K unaugmented"
print F
print "eigenvalues"
print numpy.linalg.eig(F)[0]
self.rpl = 0.05
self.ipl = 0.010
self.PlaceObserverPoles(
[self.rpl + 1j * self.ipl, self.rpl + 1j * self.ipl, self.rpl - 1j * self.ipl, self.rpl - 1j * self.ipl]
)
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
self.U_max = numpy.matrix([[12.0], [12.0]])
self.U_min = numpy.matrix([[-12.0], [-12.0]])
# For the tests that check the limits, these are (upper, lower) for both
# claws.
self.hard_pos_limits = None
self.pos_limits = None
# Compute the steady state velocities for a given applied voltage.
# The top and bottom of the claw should spin at the same rate if the
# physics is right.
X_ss = numpy.matrix([[0], [0], [0.0], [0]])
U = numpy.matrix([[1.0], [1.0]])
A = self.A
B = self.B
# X_ss[2, 0] = X_ss[2, 0] * A[2, 2] + B[2, 0] * U[0, 0]
X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0]
# X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
# X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
X_ss[3, 0] = 1 / (1 - A[3, 3]) * (X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
# X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
X_ss[1, 0] = A[1, 2] * X_ss[2, 0] + A[1, 3] * X_ss[3, 0] + B[1, 0] * U[0, 0] + B[1, 1] * U[1, 0]
print "X_ss", X_ss
self.InitializeState()
C = np.asmatrix([1, 0])
D = np.asmatrix([0])
dt = .01
x_initial = np.asmatrix([0, 0]).T
# Controller weighting factors
Q_c = np.asmatrix([[10, 0], [0, 100]])
R_c = np.asmatrix([[1]])
# Noise properties
Q_o = np.asmatrix([[1, 0], [0, 1]])
R_o = np.asmatrix([[0.3]])
# Calculate gain matrices and discrete-time matrices
A_d, B_d, Q_d, R_d = controls.c2d(A, B, dt, Q_o, R_o)
K = controls.clqr(A, B, Q_c, R_c)
Kff = controls.feedforwards(A_d, B_d)
L = controls.dkalman(A_d, C, Q_d, R_d)
# Create a single set of gains
gains = StateSpaceGains('test_gains', dt, A_d, B_d, C, D, Q_d, R_o, K, Kff, L)
gains.add_writable_constant('A_c', A)
gains.add_writable_constant('B_c', B)
u_max = np.asmatrix([[12]])
plant = StateSpacePlant(gains, x_initial)
controller = StateSpaceController(gains, -u_max, u_max)
observer = StateSpaceObserver(gains, x_initial)
