状態フィードバック

.py

# %%
import matplotlib.pyplot as plt
import matplotlib.patches as p
from scipy import signal
import sympy
sympy.init_printing(use_latex='png')


def sys2expandExpr(sys):
    num, den = sympy.fraction(sympy.simplify(sys))
    return sympy.expand(num) / sympy.expand(den)


def sys2coeff(sys):
    num, den = sympy.fraction(sys2expandExpr(sys))
    dim = (sympy.degree(num, s), sympy.degree(den, s))
    num_coeff = [num.coeff(s, i) for i in range(dim[0], -1, -1)]
    den_coeff = [den.coeff(s, i) for i in range(dim[1], -1, -1)]
    return (num_coeff, den_coeff)


def coeff2tf(num_coeff, den_coeff, values):
    num_coeffval = [float(elem.subs(values).evalf()) for elem in num_coeff]
    den_coeffval = [float(elem.subs(values).evalf()) for elem in den_coeff]
    return signal.TransferFunction(num_coeffval, den_coeffval)


def Lya2x2(A, C):
    p11, p12, p22 = sympy.symbols('p_{11},p_{12},p_{22}')
    P = sympy.Matrix([[p11, p12], [p12, p22]])
    M1, M2 = (A.T * P + P * A, -(C.T * C + K.T * K))  # A.T*P+P*A=-(C^T*C+K.T*K)
    eqs = [sympy.Eq(M1[i, j], M2[i, j]) for i in range(2) for j in range(2)]
    ans = sympy.solve(eqs, [p11, p12, p22])
    P = sympy.Matrix([[ans[p11], ans[p12]], [ans[p12], ans[p22]]])
    return P


def minJ(J, init, K):
    eqs = []
    Jinit = J.subs(init)
    for param in K:
        eq = sympy.diff(Jinit, param)
        eqs.append(sympy.Eq(sympy.simplify(eq[0]), 0))
    opts = sympy.solve(eqs, K)
    return [opt for opt in opts if all([sympy.im(param) == 0 for param in opt])]


def label(p):
    return f'$k_1,k_2=({sympy.latex(p[0])},{sympy.latex(p[1])})$'


def plot0(ax, sys, K, opts):
    ax.set_title('Step / Impulse response')
    ax.axhline(1, 0, 1, color='gray', linestyle='dotted')
    ax.axhline(0, 0, 1, color='black')
    ax.grid()
    for idx, vals in enumerate(opts):
        args = [[var, val] for var, val in zip(K, vals)]
        tf = coeff2tf(*sys2coeff(sys), args)
        if any([v.real > 0 for v in tf.poles]):
            continue
        c = f'C{idx}'
        l = label(vals)
        arg_t = [x*0.00020 for x in range(0, 100000)]
        ax.plot(
            *signal.step2(tf, T=arg_t),
            label=f'{l} step',
            color=c)
        ax.plot(
            *signal.impulse2(tf, T=arg_t),
            label=f'{l} impulse',
            color=c,
            linestyle='dashed')
    ax.legend()


def arrowW1(ax, c, w, H):
    pos = [i for i in range(len(w-1)) if w[i] < 1 and 1 < w[i+1]][0]
    b, d = (H[pos], H[pos+1] - H[pos])
    ary = (b.real, b.imag, d.real, d.imag)
    ax.arrow(*ary, color=c, head_width=.02, head_length=.02)


def plot1(ax, sys, K, opts):
    ax.set_title("Nyquist diagram")
    ax.set_xlabel("Re")
    ax.set_ylabel("Im")
    ax.set_aspect("equal")
    ax.axvline(0, 0, 1, color='black')
    ax.axhline(0, 0, 1, color='black')
    ax.grid()
    for idx, vals in enumerate(opts):
        args = [[var, val] for var, val in zip(K, vals)]
        tf = coeff2tf(*sys2coeff(sys), args)
        if any([v.real > 0 for v in tf.poles]):
            continue
        c = f'C{idx}'
        l = label(vals)
        w, H = signal.freqresp(tf)
        ax.plot(H.real, H.imag, label=l, color=c)
        arrowW1(ax, c, w, H)
    ax.add_artist(p.Circle((0, 0), radius=1, fill=False, linestyle="dotted"))
    ax.legend()


s, u, x1, x2, k1, k2 = sympy.symbols('s,u,x_{1},x_{2},k_{1},k_{2}')
X = sympy.Matrix([x1, x2])
A = sympy.Matrix([[0, 1], [0, -1]])
B = sympy.Matrix([0, 1])
C = sympy.Matrix([1, 0]).T
K = sympy.Matrix([k1, k2]).T
n = max(A.shape)
I = sympy.Matrix([[1 if i == j else 0 for j in range(n)]for i in range(n)])

init = [[x1, 1], [x2, 0]]

A1 = A - B * K
P = Lya2x2(A1, C)
J = X.T * P * X
opts = minJ(J, init, K)

eq = sympy.Eq((((s*I-A1)*X).subs(x2, s*x1))[-1], (B*u)[-1])
sys = sympy.solve(eq, x1)[-1] * 1 / u

fig = plt.figure(figsize=(8.27, 11.69), dpi=300)
fig.suptitle(f'${sympy.latex(sys2expandExpr(sys))}$', fontsize=20)
ax0 = plt.subplot2grid((2, 1), (0, 0))
ax1 = plt.subplot2grid((2, 1), (1, 0))
plot0(ax0, sys, K, opts)
plot1(ax1, sys, K, opts)
plt.tight_layout()