root/applications/dual/vahala/kim/main.m @ 1435

% main - hlavni skript
clear all;
% oznaceni: s ... system
%           k ... kalman (EKF)
%           l ... rizeni (LQR)

% KONSTANTY
T = 40000; %horizont
dt = 0.000125; %casovy krok

% Rs = 0.28;
% Ls = 0.003465;
% psipm = 0.1989;
% B = 0;    
% kp = 1.5;
% pp = 4.0;
% J = 0.04;

% Lq = 1.05*Ls;
% Ld = 0.95*Ls;

a = 0.9898;
b = 0.0072;
c = 0.0361;
d = 1.0;
e = 0.0149;

% ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0, 0,-3, -6, -3];%/9*200;
ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];

%kovariance EKF na stavu, ktery vytvari hyperstav
% Q_k = diag([0.001, 0.00001]);
% R_k = diag([0.015, 0.015]);
Q_k = diag([0.01, 0.0001]);
R_k = diag([0.15, 0.15]);

%kovariance EKF na hyperstavu
% Qh_k = diag([0.001, 0.00001, 0.00001, 0.00001, 0.00001]);
% Rh_k = diag([0.015, 0.015]);
Qh_k = diag([0.01, 0.0001, 0.1, 0.1, 0.1]);
Rh_k = diag([0.15, 0.15]);

%hodnoty sumu v systemu
nQ = diag([0.0013, 0.0013, 5.0e-6, 1.0e-10]);
nR = diag([0.0006, 0.0006]);

iter_l = 10;% pocet iteraci ve vypoctu rizeni

B_l = zeros(6,2);
% B_l(1,1) = c;
% B_l(2,2) = c;

%           o t Po Pot Pt
Q_l = diag([1 0 300 0.0 300 0]);
% Q_l = diag([1 0 0 0 0 0]);
r = 0.0001;
R_l = diag([r, r]);



% PROMENNE
x_s = zeros(4,T); %stav
y_s = zeros(2,T); %mereni
x_k = zeros(5,T); %odhad hyperstavu
P_k = zeros(5); %kovariance hyperstavu
u_l = zeros(2,T); %rizeni
S_l = zeros(6); %jadro ztraty
pre_k = zeros(3,1); %predikce stavu


% POCATECNI HODNOTY
noise = 1; %prepinac sumu
% noise = 0;

theta0 = 1.5;%1.7; %pocatecni poloha
Ps0 = eye(2); %odhad pocatecni kovariance stavu (apriorni)
Pk0 = eye(5); %pocatecni kovariance hyperstavu
ST = zeros(6); %koncova ztrata


% INICIALIZACE
x_s(4,1) = theta0;
x_k(3,1) = Ps0(1,1);
x_k(4,1) = Ps0(1,2);
x_k(5,1) = Ps0(2,2);
P_k = Pk0;
S_l = ST;

ref_ome = zeros(1, T);  
    for k = 1:T,
           index = floor(k*dt);
           if(index>0)
               lower = ref_profile(index);
           else
               lower = 0;
           end
           if(index<T*dt)
               upper = ref_profile(index+1);
           else
               upper = 0;
           end
           ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt);
    end

% Derivace pro prvni EKF 
[A_k, C_k, pre_k, A_l] = assembDeriv(x_k(:,1), y_s(:,1), x_k(:,1), Q_k, R_k, 0);
  
ri = 0.0001;
ai = (1-a*a)/c/c;
Si = (1 - ai*r + sqrt((ai*r-1)^2+4*r/c/c))/2;
Li = a*c*Si/(c*c*Si+ri);

Pia = 1;
Pib = 1;
qi = 0.1;
ri = 0.05;
y = [0;0];

% HLAVNI SMYCKA
for t = 1:T-1,
    % EKF   
    Pp = Pia*(a*a+b*b+b*b*cos(x_k(2,t))^2*(x_k(1,t)^2-1))+qi;
    Pia = Pp-Pp*Pp/(Pp+ri);
    y(1) = (1-Pp/(Pp+ri))*(a*y(1)+b*x_k(1,t)*sin(x_k(2,t))+c*u_l(1,t)) + Pp/(Pp+ri)*y_s(1,t);
    Pp = Pib*(a*a+b*b+b*b*sin(x_k(2,t))^2*(x_k(1,t)^2-1))+qi;
    Pib = Pp-Pp*Pp/(Pp+ri);
    y(2) = (1-Pp/(Pp+ri))*(a*y(2)-b*x_k(1,t)*cos(x_k(2,t))+c*u_l(2,t)) + Pp/(Pp+ri)*y_s(2,t);
    [x_k(:,t+1), P_k] = extKF(x_k(:,t), y, u_l(:,t), pre_k, A_k, C_k, P_k, Qh_k, Rh_k);
%     [x_k(:,t+1), P_k] = extKF(x_k(:,t), y_s(:,t), u_l(:,t), pre_k, A_k, C_k, P_k, Qh_k, Rh_k);
    
%     Q_l(1,1) = 1/(1+exp(-2*x_k(1,t+1)+6))+0.1;
    Q_l(3,3) = x_k(5,t+1)^5*50;
    Q_l(5,5) = Q_l(3,3);

    % Derivace
    [A_k, C_k, pre_k, A_l] = assembDeriv(x_k(:,t+1), y_s(:,t), x_k(:,t+1), Q_k, R_k, ref_ome(t));
    
    % LQ
    B_l(1,1:2) = [-e*sin(x_k(2,t+1)), e*cos(x_k(2,t+1))];
%     [u_l(:,t+1), S_l] = ctrlLQ(x_k(:,t+1), ref_ome(t), A_l, B_l, S_l, Q_l, R_l, iter_l);
[u_l(:,t+1), S_l] = ctrlLQ(x_k(:,t+1), ref_ome(t), A_l, B_l, ST, Q_l, R_l, iter_l);
    u_l(:,t+1) = b/c*x_k(1,t+1)*[-sin(x_k(2,t+1));cos(x_k(2,t+1))] + u_l(:,t+1) - Li*y_s(:,t);
    if u_l(1,t+1) > 100
        u_l(1,t+1) = 100;
    elseif u_l(1,t+1) < -100
        u_l(1,t+1) = -100;
    end
    if u_l(2,t+1) > 100
        u_l(2,t+1) = 100;
    elseif u_l(2,t+1) < -100
        u_l(2,t+1) = -100;
    end
    
    % Vyvoj systemu
    [x_s(:,t+1), y_s(:,t+1)] = evolSys(x_s(:,t), u_l(:,t+1), nQ, nR, noise);
end

figure;
subplot(2,1,1);
plot(1:T,x_k(1,:),1:T,x_s(3,:),1:T,ref_ome);
subplot(2,1,2);
plot(1:T,atan2(sin(x_k(2,:)),cos(x_k(2,:))),1:T,atan2(sin(x_s(4,:)),cos(x_s(4,:))));