% Pro ukázku zvýšení robustnosti nelinearitou: Q=diag([200 110 110 0 0 0]);
% th1i  = deg2rad(0); th2i  = deg2rad(13); k1=1.2;

% Nejrobustnější kontroler bez nelinearity:  %Q =diag([5 50 50 700 700 700]);

%clc;
run ../../parametry.m
dt=0.001;
tf= 2.5;
maxpow=20;

%% pocatecni podminky
th1i  = deg2rad(0); 
th2i  = deg2rad(8);
th0i  = 0;

%% mereni
%filtry
tc0=0.02;
tc1=tc0;
tc2=tc1;

%kvantizace
qa0=0.00036;
qa1=2*pi/2048;
qa2=2*pi/2000;

%% definice statespace
%systemove matice kolem [0 0 0 0 0 0]

d0=[d1 d2 d3; d2 d4 d5; d3 d5 d6];
dg=[0 0 0;0 -f1 0; 0 0 -f2];

a=[zeros(3),eye(3);-inv(d0)*dg,zeros(3)];
b=[zeros(3,1);inv(d0)*[1;0;0]];
c=eye(3,6);
d=[0;0;0];
a_aug=[a b;zeros(1,7)];
b_aug=zeros(7,1);
c_aug=[c d;zeros(1,6) 1];
d_aug=[0;0;0;0];
sys=ss(a,b,c,d);
sys_aug=ss(a_aug,b_aug,c_aug,d_aug);
sysd_aug=c2d(sys_aug,dt);
sysd=c2d(sys,dt);

%% návrh LQR
%Q =diag([5 50 50 700 700 700]); %velmi pomalý, jezdí velmi daleko, nejrobustnější
%Q =diag([100 50 50 10 10 10]); %pomalý, jezdí daleko,
%Q =diag([700, 0, 0, 0, 0, 0]); %rychly, jezdí blízko,
Q=diag([200 110 110 0 0 0]);
% Q=diag([500 50 100 100 100 100]);
% Q=diag([1000 0 0 0 1000 0]);
R =1;
k=dlqr(sysd.a,sysd.b,Q,R);
k1=1.2;

%%
Qk=eye(6)*1e-4;
Qk(1,1)=1e-3;
Qk(2,2)=1e-3;
Qk(3,3)=1e-3;
Qk(4,4)=100;
Qk(5,5)=100;
Qk(6,6)=100;
Rk=eye(3)*1;
Rk(1,1)=1;
Rk(2,2)=10;
Rk(3,3)=10;

%% simulace

run('doublePendulumSim_model_LQR_kalman.slx');
sim('doublePendulumSim_model_LQR_kalman.slx');