import numpy as np
from pylab import *

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

def odpSkokowa(A,B,C):
    x = np.matrix([[0], [0], [0]])
    y = []
    for n in range(30):
        y.append((C*x).item(0))
        x = A*x + B
    return y

subplot(2,1,1)
plot(odpSkokowa(A,B,C))


def sterowanieOptymalne(A,B,C,c1,c2):
    Q = c1*np.eye(3)
    R = c2
    P = np.matrix([[0,0,0],[0,0,0],[0,0,0]])
    for n in range(30):
        P = Q + np.transpose(A)* (P - P*B* (R + np.transpose(B)*P*B)**(-1) * np.transpose(B)*P)*A
    F = (R + np.transpose(B)*P*B)**(-1)*np.transpose(B)*P*A
    return F

def odpSkokowaXXX(A,B,C):
    x = np.matrix([[0], [0], [0]])
    xk = []
    y = []
    for n in range(30):
        y.append((C*x).item(0))
        x = A*x + B
        xk.append((-sterowanieOptymalne(A,B,C,1,5)*x).item(0))
    return xk

subplot(2,1,2)
plot(odpSkokowa(A-B*sterowanieOptymalne(A,B,C,1,5), B, C), 'og')
plot(odpSkokowaXXX(A-B*sterowanieOptymalne(A,B,C,1,5), B, C), 'ob')
show()