clear all;
clc;
display('------------Linear Quadratic Regulator-----------------')
% Define the matrix A, B, C, D
A=[0,1,0,0;0,-42.44,212.24,0;0,0,0,1;0,0,0,-42.44];
B=[0,0;106.12,-5;0,0;78.6,-42.44];
C=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1];
D=[0];

%Detect the controllability of the  Given system
Qc= ctrb(A,B);
rank_of_Qc = rank(Qc)

%calculate the K to by using function K = lqr( A, B, Q, R)
x = 2;
Q = diag([x 0 0 0]);
R=[1,0;0,1];
K=lqr(A,B,Q,R)

%Find step response of LQR controller
LQRcontroller_sys=ss((A-B*K),B,C,D) %Generating the closed loop state space model
t=0:0.1:10;
step(LQRcontroller_sys,t)
grid on;