clear all
close all
% Inverted cart - pendulum
% http://ctms.engin.umich.edu/CTMS/index.php?example=InvertedPendulum&section=ControlStateSpace
% Let phi represent the deviation of the pedulum's position from equilibrium, that is, theta = phi + pi.

M = 0.5;
m = 0.2;
b = 0.1;
I = 0.005;
g = 9.8;
l = 0.3;

p = I*(M+m)+M*m*l^2; %denominator for the A and B matrices

% Linearized at the upward position (unstable equilibrium)
A = [0      1              0           0;
     0 -(I+m*l^2)*b/p  (m^2*g*l^2)/p   0;
     0      0              0           1;
     0 -(m*l*b)/p       m*g*l*(M+m)/p  0];
B = [     0;
     (I+m*l^2)/p;
          0;
        m*l/p];

% Linearized system at downward position (stable equilibrium)
A2 = [0      1              0           0;
     0 -(I+m*l^2)*b/p  (m^2*g*l^2)/p   0;
     0      0              0           1;
     0 (m*l*b)/p       -m*g*l*(M+m)/p  0];
B2 = [     0;
     (I+m*l^2)/p;
          0;
        -m*l/p];
    
C = [1 0 0 0;
     0 0 1 0];
D = [0;
     0];

states = {'x' 'x_dot' 'phi' 'phi_dot'};
inputs = {'u'};
outputs = {'x'; 'phi'};

sys_ss = ss(A,B,C,D,'statename',states,'inputname',inputs,'outputname',outputs)

% Eigenvalues of open-loop system
poles = eig(A)

% Check controllability
co = ctrb(sys_ss);
controllability = rank(co)

% % LQR
% Q = C'*C
% 
% Q = 100*Q
% 
% R = 1;
% K = lqr(A,B,Q,R)
% 
% Ac = [(A-B*K)];
% Bc = [B];
% Cc = [C];
% Dc = [D];
% 
% states = {'x' 'x_dot' 'phi' 'phi_dot'};
% inputs = {'r'};
% outputs = {'x'; 'phi'};
% 
% sys_cl = ss(Ac,Bc,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);
% 
% t = 0:0.01:10;
% r =0.4*ones(size(t));
% [y,t,x]=lsim(sys_cl,r,t);
% [AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
% set(get(AX(1),'Ylabel'),'String','cart position (m)')
% set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')
% title('Step Response with LQR Control')

% Test observability

% O = obsv(A,C)
% rank(O)

C2 = [1 0 0 0]
D2 = zeros(size(C2,1), size(B2,2))
O2 = obsv(A2,C2)
rank(O2)

% C3 = [0 0 1 0]
% O3 = obsv(A,C3)
% rank(O3)


% Augment the system with disturbance and noise
Vd = 0.1*eye(4)
Vn = 1

BF = [B2 Vd 0*B2]

sys2 = ss(A2, BF, C2, [D2 zeros(size(C2)) Vn])

sys2full = ss(A2, BF, eye(4), zeros(4, size(BF,2)))

[L,P,E] = lqe(A2, eye(4), C2, Vd, Vn)

% or:
% L = (lqr(A2', C2', Vd, Vn))'

sysKF = ss(A2-L*C2, [B2 L], eye(4), 0*[B2 L])

% Test estimation

dt = 0.01
t = 0:dt:50;

uD = randn(4, size(t,2));
uN = randn(size(t));

u = 0*t;
u(100:200) = 100; % impulse
u(1500:1520) = -100; % impulse

uAUG = [u; Vd*uD; Vn*uN];

[y, t ] = lsim(sys2,uAUG, t);
plot(t,y,'b')

[xTrue,t] = lsim(sys2full, uAUG, t)
hold on
plot(t, xTrue(:,1),'r')

[xEst,t] = lsim(sysKF, [u; y'], t)
plot(t, xEst(:,1),'--k')
legend('noised', 'true', 'estimated')

% For the system linearized at the unstable equilibrium:
% + Simulation: Repeat the code above, use A and B instead of A2 and B2
% + Result: the closed-loop system is unstable and the linearization will
% soon be invalid; if we keep using one linearized model, the simulated
% system is blown up.
