import numpy as np
import matplotlib.pyplot as plt

a0 = -0.675
a1 = 3.675
a2 = -4.15
u = 1

c1 = 100
c2 = 0.5

A = np.matrix([[-a2, -a1, -a0], [1, 0, 0], [0, 1, 0]])
B = np.matrix([[1], [0], [0]])
C = np.matrix([1, 1, 1])
D = np.matrix([0])

x = x2 = xk = np.matrix([[0], [0], [0]])
y = y2 = []


def odpowiedz_skokowa ():
    for i in range(0, 20):
        global x, y
        y = np.append(y, C * x + D * u)
        x = (A*x + B * u)

    n = np.arange(len(y))
    plt.figure(1)
    plt.subplot(421)
    plt.plot(n, y, 'rx')

    return x


def sterownik (c1, c2):
    Q = c1 * np.eye(3)
    R = c2
    P = np.eye(3)

    for i in range(0, 20):
        P = Q + np.transpose(A) * (P - P * B * (R + np.transpose(B) * P * B) ** (-1) * np.transpose(B)*P) * A


    global xk
    F = (R + np.transpose(B) * P * B)**(-1) * np.transpose(B) * P * A

    uk=[[0]]

    A_nowe = A - B * F

    global x2, y2

    for i in range(0, 20):
        x2 =( A_nowe * x2 + B * u)
        y2 = np.append(y2, C * x2 + D * u)
        uk = np.append(uk, - (F * x2))


    return [y2, uk]

odpowiedz_skokowa()

# Dla sterownika o c1 = 100, c2 = 0.5

sterownik_1 = sterownik(100, 0.5)

plt.figure(1)
plt.subplot(423)
plt.plot(sterownik_1[0], 'rx')
plt.subplot(424)
plt.plot(sterownik_1[1], 'rx')


# Dla sterownika o c1 = 100, c2 = 0.5

sterownik_2 = sterownik(20, 25)
plt.subplot(425)
plt.plot(sterownik_2[0], 'rx')
plt.subplot(426)
plt.plot(sterownik_2[1], 'rx')


# Dla sterownika o c1 = 100, c2 = 0.5

sterownik_3 = sterownik(50, 10)
plt.subplot(427)
plt.plot(sterownik_3[0], 'rx')
plt.subplot(428)
plt.plot(sterownik_3[1], 'rx')


plt.show()