|
| 1 | +from numpy import * |
| 2 | +from numpy.linalg import * |
| 3 | +from numpy.testing import * |
| 4 | +from matplotlib.pyplot import * |
| 5 | +from scipy.integrate import * |
| 6 | +# Python 3.x Standard Library |
| 7 | +import gc |
| 8 | +import os |
| 9 | + |
| 10 | +# Third-Party Packages |
| 11 | +import numpy as np; np.seterr(all="ignore") |
| 12 | +import numpy.linalg as la |
| 13 | +import scipy.misc |
| 14 | +import matplotlib as mpl; mpl.use("Agg") |
| 15 | +import matplotlib.pyplot as pp |
| 16 | +import matplotlib.axes as ax |
| 17 | +import matplotlib.patches as pa |
| 18 | + |
| 19 | + |
| 20 | +# |
| 21 | +# Matplotlib Configuration & Helper Functions |
| 22 | +# -------------------------------------------------------------------------- |
| 23 | + |
| 24 | +# TODO: also reconsider line width and markersize stuff "for the web |
| 25 | +# settings". |
| 26 | +fontsize = 35 |
| 27 | + |
| 28 | +rc = { |
| 29 | + "text.usetex": True, |
| 30 | + "pgf.preamble": [r"\usepackage{amsmath,amsfonts,amssymb}"], |
| 31 | + #"font.family": "serif", |
| 32 | + "font.serif": [], |
| 33 | + #"font.sans-serif": [], |
| 34 | + "legend.fontsize": fontsize, |
| 35 | + "axes.titlesize": fontsize, |
| 36 | + "axes.labelsize": fontsize, |
| 37 | + "xtick.labelsize": fontsize, |
| 38 | + "ytick.labelsize": fontsize, |
| 39 | + #"savefig.dpi": 300, |
| 40 | + #"figure.dpi": 300, |
| 41 | +} |
| 42 | +mpl.rcParams.update(rc) |
| 43 | + |
| 44 | +# Web target: 160 / 9 inches (that's ~45 cm, this is huge) at 90 dpi |
| 45 | +# (the "standard" dpi for Web computations) gives 1600 px. |
| 46 | +width_in = 160 / 9 |
| 47 | + |
| 48 | +def save(name): |
| 49 | + cwd = os.getcwd() |
| 50 | + root = os.path.dirname(os.path.realpath(__file__)) |
| 51 | + os.chdir(root) |
| 52 | + pp.savefig(name + ".svg") |
| 53 | + os.chdir(cwd) |
| 54 | + |
| 55 | +def set_ratio(ratio=1.0, bottom=0.1, top=0.1, left=0.1, right=0.1): |
| 56 | + height_in = (1.0 - left - right)/(1.0 - bottom - top) * width_in / ratio |
| 57 | + pp.gcf().set_size_inches((width_in, height_in)) |
| 58 | + pp.gcf().subplots_adjust(bottom=bottom, top=1.0-top, left=left, right=1.0-right) |
| 59 | +from scipy.signal import place_poles |
| 60 | +A = array([[0, 1], [0, 0]]) |
| 61 | +C = array([[1, 0]]) |
| 62 | +poles = [-1, -2] |
| 63 | +K = place_poles(A.T, C.T, poles).gain_matrix |
| 64 | +L = K.T |
| 65 | +assert_almost_equal(K, [[3.0, 2.0]]) |
| 66 | +def fun(t, X_Xhat): |
| 67 | + x, x_hat = X_Xhat[0:2], X_Xhat[2:4] |
| 68 | + y, y_hat = C.dot(x), C.dot(x_hat) |
| 69 | + dx = A.dot(x) |
| 70 | + dx_hat = A.dot(x_hat) - L.dot(y_hat - y) |
| 71 | + return r_[dx, dx_hat] |
| 72 | +y0 = [-2.0, 1.0, 0.0, 0.0] |
| 73 | +result = solve_ivp(fun=fun, t_span=[0.0, 5.0], y0=y0, max_step=0.1) |
| 74 | +figure() |
| 75 | +t = result["t"] |
| 76 | +y = result["y"] |
| 77 | +plot(t, y[0], "b", label="$x_1$") |
| 78 | +plot(t, y[2], "b:", alpha=0.5, label=r"$\hat{x}_1$") |
| 79 | +plot(t, y[1], "g", label="$x_2$") |
| 80 | +plot(t, y[3], "g:", alpha=0.5, label=r"$\hat{x}_2$") |
| 81 | +grid(); legend() |
| 82 | +save("images/observer-trajectories") |
| 83 | +from scipy.linalg import solve_continuous_are |
| 84 | +A = array([[0, 1], [0, 0]]) |
| 85 | +B = array([[0], [1]]) |
| 86 | +Q = array([[1, 0], [0, 1]]); R = array([[1]]) |
| 87 | +Sigma = solve_continuous_are(A.T, C.T, inv(Q), inv(R)) |
| 88 | +L = Sigma @ C.T @ R |
| 89 | +eigenvalues, _ = eig(A - L @ C) |
| 90 | +assert all([real(s) < 0 for s in eigenvalues]) |
| 91 | +figure() |
| 92 | +x = [real(s) for s in eigenvalues] |
| 93 | +y = [imag(s) for s in eigenvalues] |
| 94 | +plot(x, y, "kx", ms=12.0) |
| 95 | +xticks([-2, -1, 0, 1, 2]) |
| 96 | +yticks([-2, -1, 0, 1, 2]) |
| 97 | +plot([0, 0], [-2, 2], "k") |
| 98 | +plot([-2, 2], [0, 0], "k") |
| 99 | +grid(True) |
| 100 | +title("Eigenvalues") |
| 101 | +axis("square") |
| 102 | +axis([-2, 2, -2, 2]) |
| 103 | +save("images/poles-Kalman") |
| 104 | +def fun(t, X_Xhat): |
| 105 | + x, x_hat = X_Xhat[0:2], X_Xhat[2:4] |
| 106 | + y, y_hat = C.dot(x), C.dot(x_hat) |
| 107 | + dx = A.dot(x) |
| 108 | + dx_hat = A.dot(x_hat) - L.dot(y_hat - y) |
| 109 | + return r_[dx, dx_hat] |
| 110 | +y0 = [-2.0, 1.0, 0.0, 0.0] |
| 111 | +result = solve_ivp(fun=fun, t_span=[0.0, 5.0], y0=y0, max_step=0.1) |
| 112 | +figure() |
| 113 | +t = result["t"] |
| 114 | +y = result["y"] |
| 115 | +plot(t, y[0], "b", label="$x_1$") |
| 116 | +plot(t, y[2], "b:", alpha=0.5, label=r"$\hat{x}_1$") |
| 117 | +plot(t, y[1], "g", label="$x_2$") |
| 118 | +plot(t, y[3], "g:", alpha=0.5, label=r"$\hat{x}_2$") |
| 119 | +grid(); legend() |
| 120 | +save("images/observer-Kalman-trajectories") |
0 commit comments