def main():
x, y = np.mgrid[-1.0:1.0:30j, 0.0:2.0:30j]
# Need an (N, 2) array of (x, y) pairs.
xy = np.column_stack([x.flat, y.flat])
mu = np.array([0.0, 1.0])
sigma = np.array([0.5, 0.5])
covariance = np.diag(sigma**2)
z = multivariate_normal.pdf(xy, mean=mu, cov=covariance).reshape(x.shape)
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
ax.plot_surface(x, y, z)
ax.set_xlabel("$x$")
ax.set_ylabel("$y$")
ax.set_zlabel("$p(x, y)$")
if "--noninteractive" in sys.argv:
latex.savefig("joint_pdf")
else:
plt.show()
def main():
xlim = [0, 3]
x = np.arange(xlim[0], xlim[1], 0.001)
plt.xlim(xlim)
y1 = np.sin(2 * math.pi * x)
plt.plot(x, y1, label="Signal at $1$ Hz")
y2 = np.sin(2 * math.pi * x / 2 + math.pi / 4)
plt.plot(x, y2, color="k", linestyle="--", label="Signal at $2$ Hz")
# Draw intersection points
idx = np.argwhere(np.diff(np.sign(y1 - y2)) != 0)
plt.plot(x[idx], y1[idx], "ko", label="Samples at $3$ Hz")
plt.xlabel("$t$")
plt.ylabel("$f(t)$")
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("nyquist")
else:
plt.show()
def main():
elevator = Elevator(0.1)
plt.figure(1)
plt.xlabel("Time (s)")
plt.ylabel("Position (m)")
simulate(elevator, 0.1, "zoh")
simulate(elevator, 0.1, "euler")
simulate(elevator, 0.1, "backward_diff")
simulate(elevator, 0.1, "bilinear")
plt.ylim([-2, 3])
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("sampling_simulation_010")
plt.figure(2)
plt.xlabel("Time (s)")
plt.ylabel("Position (m)")
simulate(elevator, 0.05, "zoh")
simulate(elevator, 0.05, "euler")
simulate(elevator, 0.05, "backward_diff")
simulate(elevator, 0.05, "bilinear")
plt.ylim([-2, 3])
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("sampling_simulation_005")
plt.figure(3)
plt.xlabel("Time (s)")
plt.ylabel("Position (m)")
simulate(elevator, 0.01, "zoh")
simulate(elevator, 0.01, "euler")
simulate(elevator, 0.01, "backward_diff")
simulate(elevator, 0.01, "bilinear")
plt.ylim([-0.25, 2])
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("sampling_simulation_004")
else:
plt.show()
def main():
dt = 0.00505
drivetrain = Drivetrain(dt)
t, xprof, vprof, aprof = fct.generate_s_curve_profile(
max_v=4.0, max_a=3.5, time_to_max_a=1.0, dt=dt, goal=10.0
)
# Generate references for LQR
refs = []
for i in range(len(t)):
r = np.array([[vprof[i]], [vprof[i]]])
refs.append(r)
# Run LQR
state_rec, ref_rec, u_rec, y_rec = drivetrain.generate_time_responses(t, refs)
plt.figure(1)
drivetrain.plot_time_responses(t, state_rec, ref_rec, u_rec)
if "--noninteractive" in sys.argv:
latex.savefig("ramsete_decoupled_vel_lqr_profile")
# Initial robot pose
pose = Pose2d(2, 0, np.pi / 2.0)
desired_pose = Pose2d()
# Ramsete tuning constants
b = 2
zeta = 0.7
vl = float("inf")
vr = float("inf")
x_rec = []
y_rec = []
vref_rec = []
omegaref_rec = []
v_rec = []
omega_rec = []
ul_rec = []
ur_rec = []
# Log initial data for plots
vref_rec.append(0)
omegaref_rec.append(0)
x_rec.append(pose.x)
y_rec.append(pose.y)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
v_rec.append(0)
omega_rec.append(0)
# Run Ramsete
i = 0
while i < len(t) - 1:
desired_pose.x = 0
desired_pose.y = xprof[i]
desired_pose.theta = np.pi / 2.0
# pose_desired, v_desired, omega_desired, pose, b, zeta
vref, omegaref = ramsete(desired_pose, vprof[i], 0, pose, b, zeta)
vl, vr = get_diff_vels(vref, omegaref, drivetrain.rb * 2.0)
next_r = np.array([[vl], [vr]])
drivetrain.update(next_r)
vc = (drivetrain.x[0, 0] + drivetrain.x[1, 0]) / 2.0
omega = (drivetrain.x[1, 0] - drivetrain.x[0, 0]) / (2.0 * drivetrain.rb)
# Log data for plots
vref_rec.append(vref)
omegaref_rec.append(omegaref)
x_rec.append(pose.x)
y_rec.append(pose.y)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
v_rec.append(vc)
omega_rec.append(omega)
# Update nonlinear observer
pose.x += vc * math.cos(pose.theta) * dt
pose.y += vc * math.sin(pose.theta) * dt
pose.theta += omega * dt
if i < len(t) - 1:
i += 1
plt.figure(2)
plt.plot([0] * len(t), xprof, label="Reference trajectory")
plt.plot(x_rec, y_rec, label="Ramsete controller")
plt.xlabel("x (m)")
plt.ylabel("y (m)")
plt.legend()
# Equalize aspect ratio
xlim = plt.xlim()
width = abs(xlim[0]) + abs(xlim[1])
ylim = plt.ylim()
height = abs(ylim[0]) + abs(ylim[1])
if width > height:
plt.ylim([-width / 2, width / 2])
else:
plt.xlim([-height / 2, height / 2])
if "--noninteractive" in sys.argv:
latex.savefig("ramsete_decoupled_response")
plt.figure(3)
num_plots = 4
plt.subplot(num_plots, 1, 1)
plt.title("Time domain responses")
plt.ylabel(
"Velocity (m/s)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, vref_rec, label="Reference")
plt.plot(t, v_rec, label="Estimated state")
plt.legend()
plt.subplot(num_plots, 1, 2)
plt.ylabel(
"Angular rate (rad/s)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, omegaref_rec, label="Reference")
plt.plot(t, omega_rec, label="Estimated state")
plt.legend()
plt.subplot(num_plots, 1, 3)
plt.ylabel(
"Left voltage (V)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, ul_rec, label="Control effort")
plt.legend()
plt.subplot(num_plots, 1, 4)
plt.ylabel(
"Right voltage (V)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, ur_rec, label="Control effort")
plt.legend()
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("ramsete_decoupled_vel_lqr_response")
else:
plt.show()
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 = ct.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 = fct.lqr(sysd, Q, R)
# Steady-state feedforward
tmp1 = concatenate(
(
concatenate((sysd.A - np.eye(sysd.A.shape[0]), sysd.B), axis=1),
concatenate((sysd.C, sysd.D), axis=1),
),
axis=0,
)
tmp2 = concatenate(
(np.zeros((sysd.A.shape[0], 1)), np.ones((sysd.C.shape[0], 1))), axis=0
)
NxNu = np.linalg.pinv(tmp1) * tmp2
Nx = NxNu[0 : sysd.A.shape[0], 0]
Nu = NxNu[sysd.C.shape[0] + 1 :, 0]
Kff_ss = Nu * np.linalg.pinv(Nx)
# Plant inversions
Kff_ts1 = np.linalg.inv(sysd.B.T * Q * sysd.B + R) * (sysd.B.T * Q)
Kff_ts2 = np.linalg.pinv(sysd.B)
t = np.arange(0, tmax, dt)
r = np.array([[2000 * 0.1047], [0]])
r_rec = np.zeros((2, 1, len(t)))
# No feedforward
x = np.array([[0], [0]])
x_rec = np.zeros((2, 1, len(t)))
u_rec = np.zeros((1, 1, len(t)))
# Steady-state feedforward
x_ss = np.array([[0], [0]])
x_ss_rec = np.zeros((2, 1, len(t)))
u_ss_rec = np.zeros((1, 1, len(t)))
# Plant inversion (Q and R cost)
x_ts1 = np.array([[0], [0]])
x_ts1_rec = np.zeros((2, 1, len(t)))
u_ts1_rec = np.zeros((1, 1, len(t)))
# Plant inversion (Q cost only)
x_ts2 = np.array([[0], [0]])
x_ts2_rec = np.zeros((2, 1, len(t)))
u_ts2_rec = np.zeros((1, 1, len(t)))
u_min = np.asarray(-12)
u_max = np.asarray(12)
for k in range(len(t)):
r_rec[:, :, k] = r
# Without feedforward
u = K @ (r - x)
u = np.clip(u, u_min, u_max)
x = sysd.A @ x + sysd.B @ u
x_rec[:, :, k] = x
u_rec[:, :, k] = u
# With steady-state feedforward
u_ss = K @ (r - x_ss) + Kff_ss @ r
u_ss = np.clip(u_ss, u_min, u_max)
x_ss = sysd.A @ x_ss + sysd.B @ u_ss
x_ss_rec[:, :, k] = x_ss
u_ss_rec[:, :, k] = u_ss
# Plant inversion (Q and R cost)
u_ts1 = K @ (r - x_ts1) + Kff_ts1 @ (r - sysd.A @ r)
u_ts1 = np.clip(u_ts1, u_min, u_max)
x_ts1 = sysd.A @ x_ts1 + sysd.B @ u_ts1
x_ts1_rec[:, :, k] = x_ts1
u_ts1_rec[:, :, k] = u_ts1
# Plant inversion
u_ts2 = K @ (r - x_ts2) + Kff_ts2 @ (r - sysd.A @ r)
u_ts2 = np.clip(u_ts2, u_min, u_max)
x_ts2 = sysd.A @ x_ts2 + sysd.B @ u_ts2
x_ts2_rec[:, :, k] = x_ts2
u_ts2_rec[:, :, k] = u_ts2
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_rec[0, 0, :], label="No FF")
plt.plot(t, x_ss_rec[0, 0, :], label="Steady-state FF")
plt.plot(t, x_ts1_rec[0, 0, :], label="Plant inversion (Q and R cost)")
plt.legend()
plt.subplot(3, 1, 2)
plt.plot(t, r_rec[1, 0, :], label="Reference")
plt.ylabel("Current (A)")
plt.plot(t, x_rec[1, 0, :], label="No FF")
plt.plot(t, x_ss_rec[1, 0, :], label="Steady-state FF")
plt.plot(t, x_ts1_rec[1, 0, :], label="Plant inversion (Q and R cost)")
plt.legend()
plt.subplot(3, 1, 3)
plt.plot(t, u_rec[0, 0, :], label="No FF")
plt.plot(t, u_ss_rec[0, 0, :], label="Steady-state FF")
plt.plot(t, u_ts1_rec[0, 0, :], label="Plant inversion (Q and R cost)")
plt.legend()
plt.ylabel("Control effort (V)")
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("case_study_ss_ff1")
plt.figure(2)
plt.subplot(3, 1, 1)
plt.plot(t, r_rec[0, 0, :], label="Reference")
plt.ylabel("$\omega$ (rad/s)")
plt.plot(t, x_ts1_rec[0, 0, :], label="Plant inversion (Q and R cost)")
plt.plot(t, x_ts2_rec[0, 0, :], label="Plant inversion (Q cost only)")
plt.legend()
plt.subplot(3, 1, 2)
plt.plot(t, r_rec[1, 0, :], label="Reference")
plt.ylabel("Current (A)")
plt.plot(t, x_ts1_rec[1, 0, :], label="Plant inversion (Q and R cost)")
plt.plot(t, x_ts2_rec[1, 0, :], label="Plant inversion (Q cost only)")
plt.legend()
plt.subplot(3, 1, 3)
plt.plot(t, u_ts1_rec[0, 0, :], label="Plant inversion (Q and R cost)")
plt.plot(t, u_ts2_rec[0, 0, :], label="Plant inversion (Q cost only)")
plt.legend()
plt.ylabel("Control effort (V)")
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("case_study_ss_ff2")
else:
plt.show()
def main():
dt = 0.00505
flywheel = Flywheel(dt)
# Set up graphing
l0 = 0.1
l1 = l0 + 5.0
l2 = l1 + 0.1
t = np.arange(0, l2 + 5.0, dt)
refs = []
# Generate references for simulation
for i in range(len(t)):
if t[i] < l0:
r = np.array([[0]])
elif t[i] < l1:
r = np.array([[9000 / 60 * 2 * math.pi]])
else:
r = np.array([[0]])
refs.append(r)
plt.figure(1)
x_rec, ref_rec, u_rec, y_rec = flywheel.generate_time_responses(t, refs)
plt.ylabel(flywheel.state_labels[0])
plt.plot(t, flywheel.extract_row(x_rec, 0), label="Output")
plt.plot(t, flywheel.extract_row(ref_rec, 0), label="Setpoint")
fill_end = int(3.0 / dt)
plt.fill_between(
t[:fill_end],
np.ravel(x_rec)[:fill_end],
np.ravel(ref_rec)[:fill_end],
color=(0.5, 0.5, 0.5, 0.5),
label="Error area for integral term",
)
plt.legend()
plt.xlabel("Time (s)")
annotate_t = 3.25
time = int(annotate_t / dt)
bottom = np.ravel(x_rec[0, time])
top = np.ravel(ref_rec[0, time])
plt.annotate(
"",
xy=(annotate_t, bottom - 5), # Start coord of arrow
xycoords="data",
xytext=(annotate_t, top + 5), # End coord of arrow
textcoords="data",
arrowprops=dict(arrowstyle="<->", connectionstyle="arc3,rad=0"),
ha="center",
va="center",
)
plt.annotate(
"steady-state error",
xy=(annotate_t + 0.125, (top + bottom) / 2.0), # Start coord of arrow
xycoords="data",
ha="left",
va="center",
)
if "--noninteractive" in sys.argv:
latex.savefig("p_controller_ss_error")
else:
plt.show()
def main():
i_stall = 134
i_free = 0.7
rpm_free = 18730
p_max = 347
t_stall = 0.71
# 775pro data
fig, ax_left = plt.subplots()
ax_left.set_xlabel("Speed (RPM)")
rpm = np.arange(0, rpm_free, 50)
line1 = ax_left.plot(rpm, [i_stall - i_stall / rpm_free * x for x in rpm],
"b",
label="Current (A)")
ax_left.annotate(
"Stall current: " + str(i_stall) + " A",
xy=(0, i_stall),
xytext=(0, 160),
arrowprops=dict(arrowstyle="->"),
)
line2 = ax_left.plot(
rpm,
[
-p_max / (rpm_free / 2.0)**2 * (x - rpm_free / 2.0)**2 + p_max
for x in rpm
],
"g",
label="Output power (W)",
)
ax_left.annotate(
"Peak power: " + str(p_max) + " W" + os.linesep + "(at " +
str(rpm_free / 2.0) + " RPM)",
xy=(rpm_free / 2.0, p_max),
xytext=(7000, 365),
arrowprops=dict(arrowstyle="->"),
)
ax_left.set_ylabel("Current (A), Power (W)")
ax_left.set_ylim([0, 400])
plt.legend(loc=3)
ax_right = ax_left.twinx()
line3 = ax_right.plot(rpm, [t_stall - t_stall / rpm_free * x for x in rpm],
"y",
label="Torque (N-m)")
ax_right.annotate(
"Stall torque: " + str(t_stall) + " N-m",
xy=(0, t_stall),
xytext=(0, 0.75),
arrowprops=dict(arrowstyle="->"),
)
ax_right.annotate(
"Free speed: " + str(rpm_free) + " RPM" + os.linesep +
"Free current: " + str(i_free) + " A",
xy=(rpm_free, 0),
xytext=(11000, 0.3),
arrowprops=dict(arrowstyle="->"),
)
ax_right.set_ylabel("Torque (N-m)")
ax_right.set_ylim([0, 0.8])
plt.legend(loc=1)
if "--noninteractive" in sys.argv:
latex.savefig("motor_data")
else:
plt.show()
def main():
f_f = 349.23
f_a = 440
f_c = 261.63
T = 0.000001
xlim = [0, 0.05]
ylim = [-3, 3]
x = np.arange(xlim[0], xlim[1], T)
plt.xlim(xlim)
plt.ylim(ylim)
yf = np.sin(f_f * 2 * math.pi * x)
ya = np.sin(f_a * 2 * math.pi * x)
yc = np.sin(f_c * 2 * math.pi * x)
ysum = yf + ya + yc
ysum_attenuating = ysum * np.exp(-25 * x)
num_plots = 2
plt.subplot(num_plots, 1, 1)
plt.ylim(ylim)
plt.ylabel("Fmaj4 ($\sigma = 0$)")
plt.plot(x, ysum)
plt.gca().axes.get_xaxis().set_ticks([])
plt.subplot(num_plots, 1, 2)
plt.ylim(ylim)
plt.ylabel("Attenuating Fmaj4 ($\sigma = -25$)")
plt.plot(x, ysum_attenuating)
plt.gca().axes.get_xaxis().set_ticks([])
plt.xlabel("$t$")
if "--noninteractive" in sys.argv:
latex.savefig("laplace_chord_attenuating")
x, y = np.mgrid[-150.0:150.0:500j, 200.0:500.0:500j]
# Need an (N, 2) array of (x, y) pairs.
xy = np.column_stack([x.flat, y.flat])
z = np.zeros(xy.shape[0])
for i, pair in enumerate(xy):
s = pair[0] + pair[1] * 1j
h = sin_tf(f_f, s) * sin_tf(f_a, s) * sin_tf(f_c, s)
z[i] = clamp(math.sqrt(h.real**2 + h.imag**2), -30, 30)
z = z.reshape(x.shape)
fig = plt.figure(2)
ax = fig.add_subplot(111, projection="3d")
ax.plot_surface(x, y, z, cmap=cm.coolwarm)
ax.set_xlabel("$Re(\sigma)$")
ax.set_ylabel("$Im(j\omega)$")
ax.set_zlabel("$H(s)$")
ax.set_zticks([])
if "--noninteractive" in sys.argv:
latex.savefig("laplace_chord_3d")
else:
plt.show()
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 = ct.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 = fct.lqr(sysd, Q, R)
# Plant inversions
Kff_ts1 = np.linalg.pinv(sysd.B)
Kff_ts2 = np.linalg.inv(sysd.B.T * Q * sysd.B + R) * (sysd.B.T * Q)
t = np.arange(0, tmax, dt)
r = np.array([[2000 * 0.1047], [0]])
r_rec = np.zeros((2, 1, len(t)))
# No feedforward
x = np.array([[0], [0]])
x_rec = np.zeros((2, 1, len(t)))
u_rec = np.zeros((1, 1, len(t)))
# Plant inversion (Q cost only)
x_ts1 = np.array([[0], [0]])
x_ts1_rec = np.zeros((2, 1, len(t)))
u_ts1_rec = np.zeros((1, 1, len(t)))
# Plant inversion (Q and R cost)
x_ts2 = np.array([[0], [0]])
x_ts2_rec = np.zeros((2, 1, len(t)))
u_ts2_rec = np.zeros((1, 1, len(t)))
u_min = np.asarray(-12)
u_max = np.asarray(12)
for k in range(len(t)):
r_rec[:, :, k] = r
# Without feedforward
u = K @ (r - x)
u = np.clip(u, u_min, u_max)
x = sysd.A @ x + sysd.B @ u
x_rec[:, :, k] = x
u_rec[:, :, k] = u
# Plant inversion (Q and R cost)
u_ts1 = K @ (r - x_ts1) + Kff_ts1 @ (r - sysd.A @ r)
u_ts1 = np.clip(u_ts1, u_min, u_max)
x_ts1 = sysd.A @ x_ts1 + sysd.B @ u_ts1
x_ts1_rec[:, :, k] = x_ts1
u_ts1_rec[:, :, k] = u_ts1
# Plant inversion
u_ts2 = K @ (r - x_ts2) + Kff_ts2 @ (r - sysd.A @ r)
u_ts2 = np.clip(u_ts2, u_min, u_max)
x_ts2 = sysd.A @ x_ts2 + sysd.B @ u_ts2
x_ts2_rec[:, :, k] = x_ts2
u_ts2_rec[:, :, k] = u_ts2
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_rec[0, 0, :], label="No feedforward")
plt.plot(t, x_ts2_rec[0, 0, :], label="Plant inversion")
plt.plot(t, x_ts1_rec[0, 0, :], label="Plant inversion (Q and R cost)")
plt.legend()
plt.subplot(3, 1, 2)
plt.plot(t, r_rec[1, 0, :], label="Reference")
plt.ylabel("Current (A)")
plt.plot(t, x_rec[1, 0, :], label="No feedforward")
plt.plot(t, x_ts2_rec[1, 0, :], label="Plant inversion")
plt.plot(t, x_ts1_rec[1, 0, :], label="Plant inversion (Q and R cost)")
plt.legend()
plt.subplot(3, 1, 3)
plt.plot(t, u_rec[0, 0, :], label="No feedforward")
plt.plot(t, u_ts2_rec[0, 0, :], label="Plant inversion")
plt.plot(t, u_ts1_rec[0, 0, :], label="Plant inversion (Q and R cost)")
plt.legend()
plt.ylabel("Control effort (V)")
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("case_study_ff")
else:
plt.show()
def main():
# x_1: robot position measured from corner
# x_2: robot velocity with positive direction toward wall
# x_3: wall position measured from corner
xhat = np.array([[0.0], [0.0], [0.0]])
# y_1: measurement of distance from robot to corner
# y_2: measurement of distance from robot to wall
y = np.genfromtxt("kalman_robot.csv", delimiter=",")
# fmt: off
phi = np.array([[1, 1, 0], [0, 1, 0], [0, 0, 1]])
gamma = np.array([[0], [0.1], [0]])
Q = np.array([[1]])
R = np.array([[10, 0], [0, 10]])
P = np.zeros((3, 3))
K = np.zeros((3, 2))
H = np.array([[1, 0, 0], [-1, 0, 1]])
# fmt: on
# Initialize matrix storage
xhat_pre_rec = np.zeros((3, 1, y.shape[1]))
xhat_post_rec = np.zeros((3, 1, y.shape[1]))
P_pre_rec = np.zeros((3, 3, y.shape[1]))
P_post_rec = np.zeros((3, 3, y.shape[1]))
xhat_smooth_rec = np.zeros((3, 1, y.shape[1]))
P_smooth_rec = np.zeros((3, 3, y.shape[1]))
t = [0] * (y.shape[1])
# Forward Kalman filter
# Set up initial conditions for first measurement
xhat[0] = y[0, 0]
xhat[1] = 0.2
xhat[2] = y[0, 0] + y[1, 0]
# fmt: off
P = np.array([[10, 0, 10], [0, 1, 0], [10, 0, 20]])
# fmt: on
xhat_pre_rec[:, :, 1] = xhat
P_pre_rec[:, :, 1] = P
xhat_post_rec[:, :, 1] = xhat
P_post_rec[:, :, 1] = P
t[1] = 1
for k in range(2, 100):
# Predict
xhat = phi @ xhat
P = phi @ P @ phi.T + gamma @ Q @ gamma.T
xhat_pre_rec[:, :, k] = xhat
P_pre_rec[:, :, k] = P
# Update
K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
xhat += K @ (y[:, k:k + 1] - H @ xhat)
P = (np.eye(3, 3) - K @ H) @ P
xhat_post_rec[:, :, k] = xhat
P_post_rec[:, :, k] = P
t[k] = k
# Kalman smoother
# Last estimate is already optional, so add it to the record
xhat_smooth_rec[:, :, -1] = xhat_post_rec[:, :, -1]
P_smooth_rec[:, :, -1] = P_post_rec[:, :, -1]
for k in range(y.shape[1] - 2, 0, -1):
A = P_post_rec[:, :, k] @ phi.T @ np.linalg.inv(P_pre_rec[:, :, k + 1])
xhat = xhat_post_rec[:, :, k] + A @ (xhat_smooth_rec[:, :, k + 1] -
xhat_pre_rec[:, :, k + 1])
P = P_post_rec[:, :, k] + A @ (P - P_pre_rec[:, :, k + 1]) @ A.T
xhat_smooth_rec[:, :, k] = xhat
P_smooth_rec[:, :, k] = P
# Robot position
plt.figure(1)
plt.xlabel("Time (s)")
plt.ylabel("Position (cm)")
plt.plot(t[1:], xhat_post_rec[0, 0, 1:], label="Kalman filter")
plt.plot(t[1:], xhat_smooth_rec[0, 0, 1:], label="Kalman smoother")
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("kalman_smoother_robot_pos")
# Robot velocity
plt.figure(2)
plt.xlabel("Time (s)")
plt.ylabel("Velocity (cm)")
plt.plot(t[1:], xhat_post_rec[1, 0, 1:], label="Kalman filter")
plt.plot(t[1:], xhat_smooth_rec[1, 0, 1:], label="Kalman smoother")
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("kalman_smoother_robot_vel")
# Wall position
plt.figure(3)
plt.xlabel("Time (s)")
plt.ylabel("Wall position (cm)")
plt.plot(t[1:], xhat_post_rec[2, 0, 1:], label="Kalman filter")
plt.plot(t[1:], xhat_smooth_rec[2, 0, 1:], label="Kalman smoother")
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("kalman_smoother_wall_pos")
# Robot position variance
plt.figure(4)
plt.xlabel("Time (s)")
plt.ylabel("Robot position variance ($cm^2$)")
plt.plot(t[1:], P_post_rec[1, 1, 1:], label="Kalman filter")
plt.plot(t[1:], P_smooth_rec[1, 1, 1:], label="Kalman smoother")
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("kalman_smoother_robot_pos_variance")
else:
plt.show()
def main():
dt = 0.05
drivetrain = Drivetrain(dt)
t, xprof, vprof, aprof = fct.generate_s_curve_profile(
max_v=4.0, max_a=3.5, time_to_max_a=1.0, dt=dt, goal=10.0
)
# Initial robot pose
pose = Pose2d(2, 0, np.pi / 2.0)
desired_pose = Pose2d()
twist_pose = Pose2d(2, 0, np.pi / 2.0)
# Ramsete tuning constants
b = 2
zeta = 0.7
vl = float("inf")
vr = float("inf")
x_rec = []
y_rec = []
twist_x_rec = []
twist_y_rec = []
vref_rec = []
omegaref_rec = []
v_rec = []
omega_rec = []
ul_rec = []
ur_rec = []
# Log initial data for plots
vref_rec.append(0)
omegaref_rec.append(0)
x_rec.append(pose.x)
y_rec.append(pose.y)
twist_x_rec.append(twist_pose.x)
twist_y_rec.append(twist_pose.y)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
v_rec.append(0)
omega_rec.append(0)
# Run Ramsete
i = 0
while i < len(t) - 1:
desired_pose.x = 0
desired_pose.y = xprof[i]
desired_pose.theta = np.pi / 2.0
# pose_desired, v_desired, omega_desired, pose, b, zeta
vref, omegaref = ramsete(desired_pose, vprof[i], 0, pose, b, zeta)
vl, vr = get_diff_vels(vref, omegaref, drivetrain.rb * 2.0)
next_r = np.array([[vl], [vr]])
drivetrain.update(next_r)
vc = (drivetrain.x[0, 0] + drivetrain.x[1, 0]) / 2.0
omega = (drivetrain.x[1, 0] - drivetrain.x[0, 0]) / (2.0 * drivetrain.rb)
# Log data for plots
vref_rec.append(vref)
omegaref_rec.append(omegaref)
x_rec.append(pose.x)
y_rec.append(pose.y)
twist_x_rec.append(twist_pose.x)
twist_y_rec.append(twist_pose.y)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
v_rec.append(vc)
omega_rec.append(omega)
# Update nonlinear observer
pose.x += vc * math.cos(pose.theta) * dt
pose.y += vc * math.sin(pose.theta) * dt
pose.theta += omega * dt
twist_pose.exp(Twist2d(vc, 0, omega), dt)
if i < len(t) - 1:
i += 1
plt.figure(1)
plt.ylabel("Odometry error (m)")
plt.plot(t, np.subtract(twist_x_rec, x_rec), label="Error in x")
plt.plot(t, np.subtract(twist_y_rec, y_rec), label="Error in y")
plt.legend()
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("ramsete_twist_odometry_error")
else:
plt.show()
def main():
# x_1: robot position measured from corner
# x_2: robot velocity with positive direction toward wall
# x_3: wall position measured from corner
xhat = np.array([[0], [0], [0]])
# y_1: measurement of distance from robot to corner
# y_2: measurement of distance from robot to wall
y = np.genfromtxt("kalman_robot.csv", delimiter=",")
# fmt: off
phi = np.array([[1, 1, 0],
[0, 0, 0],
[0, 0, 1]])
gamma = np.array([[0],
[0.1],
[0]])
Q = np.array([[1]])
R = np.array([[10, 0],
[0, 10]])
P = np.zeros((3, 3))
K = np.zeros((3, 2))
H = np.array([[1, 0, 0],
[-1, 0, 1]])
# fmt: on
num_points = y.shape[1]
xhat_rec = np.zeros((3, 1, num_points))
P_rec = np.zeros((3, 3, num_points))
t = np.linspace(0, num_points, num_points)
dt = 1
for k in range(100):
if k == 1:
xhat[0] = y[0, k]
xhat[1] = (y[0, 1] - y[0, 0]) / dt
xhat[2] = y[0, k] + y[1, k]
# fmt: off
P = np.array([[10, 10 / dt, 10],
[10 / dt, 20 / dt**2, 10 / dt],
[10, 10 / dt, 20]])
# fmt: on
xhat_rec[:, :, k] = xhat
P_rec[:, :, k] = np.array([P[0, 0], P[1, 1], P[2, 2]]).T
elif k > 1:
# Predict
xhat = phi @ xhat + np.array([[0, 0.8, 0]]).T
P = phi @ P @ phi.T + gamma @ Q @ gamma.T
# Update
K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
xhat += K @ (y[:, k : k + 1] - H @ xhat)
P = (np.eye(3, 3) - K @ H) @ P
xhat_rec[:, :, k] = xhat
P_rec[:, :, k] = np.array([P[0, 0], P[1, 1], P[2, 2]]).T
# State estimates and measurements
plt.figure(1)
plt.xlabel("Time (s)")
plt.plot(t[1:], xhat_rec[0, 0, 1:], label="Robot position estimate (cm)")
plt.plot(t[1:], xhat_rec[1, 0, 1:], label="Robot velocity estimate (cm/s)")
plt.plot(t[1:], xhat_rec[2, 0, 1:], label="Wall position estimate (cm)")
plt.plot(t[1:], y[0, 1:].T, label="Robot to corner measurement (cm)")
plt.plot(t[1:], y[1, 1:].T, label="Robot to wall measurement (cm)")
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("kalman_filter_all")
# Robot position estimate and variance
plt.figure(2)
plt.xlabel("Time (s)")
plt.plot(t[1:], xhat_rec[0, 0, 1:], label="Robot position estimate (cm)")
plt.plot(t[1:], P_rec[0, 0, 1:], label="Robot position variance ($cm^2$)")
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("kalman_filter_robot_pos")
# Wall position estimate and variance
plt.figure(3)
plt.xlabel("Time (s)")
plt.plot(t[1:], xhat_rec[2, 0, 1:], label="Wall position estimate (cm)")
plt.plot(t[1:], P_rec[2, 0, 1:], label="Wall position variance ($cm^2$)")
plt.legend()
if "--noninteractive" in sys.argv:
latex.savefig("kalman_filter_wall_pos")
else:
plt.show()
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 = ct.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 = ct.place(sysd.A, sysd.B, [0.1, 0.9])
K_pp2 = ct.place(sysd.A, sysd.B, [0.1, 0.8])
K_lqr = fct.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:
latex.savefig("case_study_pp_lqr")
else:
plt.show()
def main():
dt = 0.02
drivetrain = Drivetrain(dt)
data = np.genfromtxt("ramsete_traj.csv", delimiter=",")
t = data[1:, 0].T
xprof = data[1:, 1].T
yprof = data[1:, 2].T
thetaprof = data[1:, 3].T
vprof = data[1:, 4].T
omegaprof = data[1:, 5].T
# Initial robot pose
pose = Pose2d(xprof[0] + 0.5, yprof[0] + 0.5, np.pi)
desired_pose = Pose2d()
# Ramsete tuning constants
b = 2
zeta = 0.7
vl = float("inf")
vr = float("inf")
x_rec = []
y_rec = []
theta_rec = []
vl_rec = []
vlref_rec = []
vr_rec = []
vrref_rec = []
ul_rec = []
ur_rec = []
# Run Ramsete
i = 0
while i < len(t) - 1:
desired_pose.x = xprof[i]
desired_pose.y = yprof[i]
desired_pose.theta = thetaprof[i]
# pose_desired, v_desired, omega_desired, pose, b, zeta
vref, omegaref = ramsete(desired_pose, vprof[i], omegaprof[i], pose, b,
zeta)
vlref, vrref = get_diff_vels(vref, omegaref, drivetrain.rb * 2.0)
next_r = np.array([[vlref], [vrref]])
drivetrain.update(next_r)
vc = (drivetrain.x[0, 0] + drivetrain.x[1, 0]) / 2.0
omega = (drivetrain.x[1, 0] - drivetrain.x[0, 0]) / (2.0 *
drivetrain.rb)
vl, vr = get_diff_vels(vc, omega, drivetrain.rb * 2.0)
# Log data for plots
vlref_rec.append(vlref)
vrref_rec.append(vrref)
x_rec.append(pose.x)
y_rec.append(pose.y)
theta_rec.append(pose.theta)
vl_rec.append(vl)
vr_rec.append(vr)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
# Update nonlinear observer
pose.x += vc * math.cos(pose.theta) * dt
pose.y += vc * math.sin(pose.theta) * dt
pose.theta += omega * dt
if i < len(t) - 1:
i += 1
plt.figure(1)
plt.plot(x_rec, y_rec, label="Ramsete controller")
plt.plot(xprof, yprof, label="Reference trajectory")
plt.xlabel("x (m)")
plt.ylabel("y (m)")
plt.legend()
# Equalize aspect ratio
xlim = plt.xlim()
width = abs(xlim[0]) + abs(xlim[1])
ylim = plt.ylim()
height = abs(ylim[0]) + abs(ylim[1])
if width > height:
plt.ylim([-width / 2, width / 2])
else:
plt.xlim([-height / 2, height / 2])
if "--noninteractive" in sys.argv:
latex.savefig("ramsete_traj_xy")
plt.figure(2)
num_plots = 7
plt.subplot(num_plots, 1, 1)
plt.title("Time domain responses")
plt.ylabel(
"x position (m)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t[:-1], x_rec, label="Estimated state")
plt.plot(t, xprof, label="Reference")
plt.legend()
plt.subplot(num_plots, 1, 2)
plt.ylabel(
"y position (m)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t[:-1], y_rec, label="Estimated state")
plt.plot(t, yprof, label="Reference")
plt.legend()
plt.subplot(num_plots, 1, 3)
plt.ylabel(
"Theta (rad)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t[:-1], theta_rec, label="Estimated state")
plt.plot(t, thetaprof, label="Reference")
plt.legend()
t = t[:-1]
plt.subplot(num_plots, 1, 4)
plt.ylabel(
"Left velocity (m/s)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, vl_rec, label="Estimated state")
plt.plot(t, vlref_rec, label="Reference")
plt.legend()
plt.subplot(num_plots, 1, 5)
plt.ylabel(
"Right velocity (m/s)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, vr_rec, label="Estimated state")
plt.plot(t, vrref_rec, label="Reference")
plt.legend()
plt.subplot(num_plots, 1, 6)
plt.ylabel(
"Left voltage (V)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, ul_rec, label="Control effort")
plt.legend()
plt.subplot(num_plots, 1, 7)
plt.ylabel(
"Right voltage (V)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, ur_rec, label="Control effort")
plt.legend()
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("ramsete_traj_response")
else:
plt.show()
def main():
T = 0.000001
xlim = [0, 0.05]
ylim = [-3, 3]
x = np.arange(xlim[0], xlim[1], T)
plt.xlim(xlim)
plt.ylim(ylim)
f_f = 349.23
f_a = 440
f_c = 261.63
yf = np.sin(f_f * 2 * math.pi * x)
ya = np.sin(f_a * 2 * math.pi * x)
yc = np.sin(f_c * 2 * math.pi * x)
ysum = yf + ya + yc
num_plots = 4
plt.subplot(num_plots, 1, 1)
plt.ylim(ylim)
plt.ylabel("Fmaj4")
plt.plot(x, ysum)
plt.gca().axes.get_xaxis().set_ticks([])
plt.subplot(num_plots, 1, 2)
plt.ylim(ylim)
plt.ylabel("$F_4$")
plt.plot(x, yf)
plt.gca().axes.get_xaxis().set_ticks([])
plt.subplot(num_plots, 1, 3)
plt.ylim(ylim)
plt.ylabel("$A_4$")
plt.plot(x, ya)
plt.gca().axes.get_xaxis().set_ticks([])
plt.subplot(num_plots, 1, 4)
plt.ylim(ylim)
plt.ylabel("$C_4$")
plt.plot(x, yc)
plt.xlabel("$t$")
if "--noninteractive" in sys.argv:
latex.savefig("fourier_chord")
plt.figure(2)
N = 1000000 # Number of samples
T = 1.0 / 2000.0 # Sample spacing
x = np.arange(0.0, N * T, T)
yf = np.sin(f_f * 2 * math.pi * x)
ya = np.sin(f_a * 2 * math.pi * x)
yc = np.sin(f_c * 2 * math.pi * x)
ysum = yf + ya + yc
yf = scipy.fftpack.fft(ysum)
xf = np.linspace(0.0, 1.0 / (2.0 * T), N // 2)
plt.plot(xf, 2.0 / N * np.abs(yf[:N // 2]))
plt.xlabel("Frequency (Hz)")
plt.xlim([0, 700])
if "--noninteractive" in sys.argv:
latex.savefig("fourier_chord_fft")
else:
plt.show()