% LINIERISASI KENDALIAN PENDULUM TERBALIK
clear
clc
g = 10  % percepatan gravitasi 10 m/sec2
l = 0.6  % panjang batang 60 cm
M = 0.25  % Massa kereta 250 gram
m = 0.025  % massa pendulum 25 gram
A = [0 1 0 0; 0 0 -(m*g)/M 0; 0 0 0 1; 0 0 ((M+m)*g)/(M*l) 0] % Matrix A
B = [0; 1/M; 0; -1/(M*l)] % Matrix B
% Nilai Eigen Matrix A:
lambda_A = eig(A)
% Tidak semua ada di sebelah kiri sumbu khayal pada bidang kompleks
% Sistem tidak stabil
%
% Memeriksa KETERKENDALIAN
% Matrix Keterkendalan P:
P = [B  A*B A*A*B A*A*A*B]
% Periksa rank matrix P:
rank_P = rank(P)
% Jika rank P = 4 (full rank) maka kendalian sepenuhnya terkendali
% Bisa dirancangkan pengendali dengan umpan balik peubah keadaan
%      untuk menstabilkan sistem
%
% Penempatan NILAI EIGEN
% Supaya stabil, misalnya nilai eigen matrix [A - BK[ ditempatkan di
% (dari keempat nilai eigen) dua REAL NEGATIF dan sepasang KOMPLEKS
%         KONYUGASI:
j = sqrt(-1);
lambda_Abaru = [-10; -20; (-0.5 + (j*4)); (-0.5 - (j*4))]
%
% Menggunakan perintah "place" untuk menghitung gain-matrix K:
K = place(A,B,lambda_Abaru)
%
% Memeriksa kembali apakah nilai eigen matrix Abaru = [A - BK[ sudah
%     pada tempat yang diinginkan:
Abaru = A - B*K
% Nilai Eigen Abaru:
lambda_act = eig(Abaru)
%
% Selanjutnya nilai gain matrix K di-implementasikan pada pendulum