clear all
close all
clc

g = 9.81;
m_a = 0.3583;
l_a = 0.3 - 0.01074;
l_m_a = 0.05528;
i_a = 2.354e-3;
m_p = 0.0958;
l_m_p = 0.06279;
i_p = 4.503e-4; 
h = 0.01;

A_const = m_a * l_m_a^2 + i_a + m_p * l_a^2;
B_const = m_a * l_m_p^2 + i_p;
C_const = m_p * l_a * l_m_p;
D_const = m_p * g * l_m_p;

Q_c = diag([10, 10, 1, 1]);
R_c = 1e3 * eye(1);
N_c = [0;
    0;
    0;
    0];

A_c = [0 0 1 0;
         0 0 0 1;
         0 ((C_const * D_const)/(A_const * B_const - C_const^2)) 0 0;
         0 ((A_const * D_const)/(A_const * B_const - C_const^2)) 0 0];
B_c = [0;
         0;
        (B_const/(A_const * B_const - C_const^2));
        (C_const/(A_const * B_const - C_const^2))];
C_c = [1 0 0 0
         0 1 0 0];
 
A_d = expm(A_c * h);
B_eq = @(x) expm(A_c * x) * B_c;
B_d = integral(B_eq, 0, h, 'ArrayValued', 1);
C_d = C_c;

Q_d = Q_c * h;
R_d = R_c * h;
N_d = N_c * h;

[K, S, P] = lqr(A_c, B_c, Q_d, R_d, N_c);
[K2, S2, P2] = dlqr(A_d, B_d, Q_d, R_d, N_d);
[K3, S3, e] = lqrd(A_c, B_c, Q_c, R_c, N_c, h);

z = 100 * h;

