%
% Elo folks!
%
%**************************************************************************
%*                                                                        *
%*  Rozszerzony Obserwator Luenbergera z adaptacja predkosci typu PI      *
%*                            - odtwarzanie predkosci silnika klatkowego  *
%*                                                                        *
%*                                       Bartlomiej Ufnalski, 09.03.2017  *
%*                                                                        *
%**************************************************************************
%
% Kolejnosc uruchamiania plikow:
% 1. run_me_DRFOC_for_EKF_end_ELO.slx
% 2. ELO_speed.m --> algorytm rozszerzonego obserwatora Luenbergera (ELO)
%
% For mor details please consult http://ufnalski.edu.pl/dissertation/
%
an=Ls*Lr-Lm*Lm;
ar=Lr/an;
a=-Rs*ar;
b=-Lm*Lm/an;
c=Lm/an;
ror=Rr/Lr;
clear x_e x
x_e(:,1)=[0 0 0 0 0]';
z_e(:,1)=[0 0]';
clear uu
uu(1,:)=usa.signals.values';
uu(2,:)=usb.signals.values';
x(:,1)=[0 0 0 0 0]';
omegas=zeros(1,numel(usa.signals.values)); % stationary reference frame is assumed for the observer
% Nastawy korektora PI
k1=1; k2=100;
% ROZSZERZONY OBSERWATOR LUENBERGERA
for k=2:round(numel(usa.signals.values)),
    % Measurement
    zz(1,k)=isa.signals.values(k);
    zz(2,k)=isb.signals.values(k);
    % ELO
    A=[ ((a+b*ror)*Ts+1)    omegas(k-1)*Ts      c*ror*Ts                        c*Ts*x_e(5,k-1) ;
        -omegas(k-1)*Ts     ((a+b*ror)*Ts+1)    -c*Ts*x_e(5,k-1)                c*ror*Ts ;
        Lm*ror*Ts           0                   (-ror*Ts+1)                     (omegas(k-1)-x_e(5,k-1))*Ts ;
        0                   Lm*ror*Ts           (x_e(5,k-1)-omegas(k-1))*Ts     (-ror*Ts+1)] ;
    
    C=[ 1 0 ;
        0 1 ;
        0 0 ;
        0 0 ]';
    
    B=[ ar*Ts 0;
        0 ar*Ts;
        0 0;
        0 0];
    % Wyznaczenie sprzezenia
    sigma=1-Lm*Lm/Ls/Lr;
    tal=Lr/Rr;
    ar11=-(Rs/sigma/Ls+(1-sigma)/sigma/tal);
    ar12=Lm/sigma/Ls/Lr/tal;
    ai12=-Lm/sigma/Ls/Lr*x_e(5,k-1); % predkosc jako parametr macierzy stanu
    ar21=Lm/tal;
    ar22=-1/tal;
    ai22=x_e(5,k-1);
    c2=sigma*Ls*Lr/Lm;
    % Wspolczynnik proporcjonalnosci biegunow obiektu i obserwatora
    k_bigun=1.2;
    g1=(k_bigun-1)*(ar11+ar22);
    g2=(k_bigun-1)*ai22;
    g3=(k_bigun*k_bigun-1)*(c2*ar11+ar21)-c2*(k_bigun-1)*(ar11+ar22);
    g4=-c2*(k_bigun-1)*ai22;
    % Macierz sprzezenia
    L=[g1 g2 g3 g4; -g2 g1 -g4 g3]'*Ts;
    x_e(1:4,k)=A*x_e(1:4,k-1)+B*uu(:,k-1)+L*(z_e(:,k-1)-zz(:,k-1));
    z_e(:,k)=C*x_e(1:4,k);
    eid(k-1)=zz(1,k-1)-z_e(1,k-1);
    eiq(k-1)=zz(2,k-1)-z_e(2,k-1);
    % Adaptacja predkosci (korektor PI)
    x_e(5,k)=k2*(eid(k-1)*x_e(4,k-1)-eiq(k-1)*x_e(3,k-1))*Ts+x_e(5,k-1)+k1*(eid(k-1)*x_e(4,k-1)-eiq(k-1)*x_e(3,k-1));
end
% Wykres
try close(1), catch wiad, end;
figure(1);
plot(omega.time,omega.signals.values,omega.time,x_e(5,:)/pb)
ylabel('\omega_{est} [rad/s]')
xlabel('time [s]')
grid
legend('rotor speed','estimated speed (ELO, inertia not known)','Location','best');
% The second you put numbers into a computer, you move from reality to fantasy.