Show
Ignore:
Timestamp:
03/04/12 19:28:08 (12 years ago)
Author:
vahalam
Message:

pridani a uprava lqg s hyperstavem viz clanek Kim2006

Files:
1 modified

Legend:

Unmodified
Added
Removed

  • applications/dual/vahala/kim/basic_main.m

    r1435 r1436  
    1 % main - hlavni skript 
    2 clear all; 
    3 % oznaceni: s ... system 
    4 %           k ... kalman (EKF) 
    5 %           l ... rizeni (LQR) 
    6  
    7 % KONSTANTY 
    8 T = 40000; %horizont 
    9 dt = 0.000125; %casovy krok 
    10  
    11 % Rs = 0.28; 
    12 % Ls = 0.003465; 
    13 % psipm = 0.1989; 
    14 % B = 0;     
    15 % kp = 1.5; 
    16 % pp = 4.0; 
    17 % J = 0.04; 
    18  
    19 % Lq = 1.05*Ls; 
    20 % Ld = 0.95*Ls; 
    21  
    22 a = 0.9898; 
    23 b = 0.0072; 
    24 c = 0.0361; 
    25 d = 1.0; 
    26 e = 0.0149; 
    27  
    28 Ls = 0.003465; 
    29 psipm = 0.1989; 
    30  
    31 % ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0, 0,-3, -6, -3];%/9*200; 
    32 ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; 
    33  
    34 %kovariance EKF na stavu 
    35 % Q_k = diag([0.001, 0.00001]); 
    36 % R_k = diag([0.015, 0.015]); 
    37 Q_k = diag([0.01, 0.0001]); 
    38 R_k = diag([0.15, 0.15]); 
    39  
    40 %hodnoty sumu v systemu 
    41 nQ = diag([0.0013, 0.0013, 5.0e-6, 1.0e-10]); 
    42 nR = diag([0.0006, 0.0006]); 
    43  
    44 iter_l = 10;% pocet iteraci ve vypoctu rizeni 
    45  
    46 % B_l = zeros(3,2); 
    47 % B_l = zeros(2,2); 
    48 % B_l(1,1) = c; 
    49 % B_l(2,2) = c; 
    50  
    51 Q_l = diag([1 0 0]); 
    52 % % Q_l = diag([0 0 1 0 0]); 
    53 r = 0.01; 
    54 R_l = diag([r, r]); 
    55  
    56 % PROMENNE 
    57 x_s = zeros(4,T); %stav 
    58 y_s = zeros(2,T); %mereni 
    59 x_k = zeros(2,T); %odhad stavu 
    60 P_k = zeros(2); %kovariance stavu 
    61 u_l = zeros(2,T); %rizeni 
    62 % S_l = zeros(3); %jadro ztraty 
    63 S_l = zeros(2); 
    64  
    65 % POCATECNI HODNOTY 
    66 noise = 1; %prepinac sumu 
    67 % noise = 0; 
    68  
    69 theta0 = 1.5;%1.7; %pocatecni poloha 
    70 P0 = eye(2); %odhad pocatecni kovariance stavu (apriorni) 
    71 % ST = zeros(3); %koncova ztrata 
    72 ST = ones(3); 
    73  
    74  
    75 % INICIALIZACE 
    76 x_s(4,1) = theta0; 
    77 % x_s(3,1) = 5; 
    78 P_k = P0; 
    79 S_l = ST; 
    80  
    81 ref_ome = zeros(1, T);   
    82     for k = 1:T, 
    83            index = floor(k*dt); 
    84            if(index>0) 
    85                lower = ref_profile(index); 
    86            else 
    87                lower = 0; 
    88            end 
    89            if(index<T*dt) 
    90                upper = ref_profile(index+1); 
    91            else 
    92                upper = 0; 
    93            end 
    94            ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt); 
     1function [loss] = basic_main(T, ref_profile, theta0, simulator, graf, inddq) 
     2    % basic main - hlavni skript 
     3    % clear all; 
     4    % oznaceni: s ... system 
     5    %           k ... kalman (EKF) 
     6    %           l ... rizeni (LQR) 
     7 
     8    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
     9    %%%%%pouziti SIMULATORU 
     10    % simulator = 1; 
     11    % simulator = 0; 
     12 
     13    if((simulator == 1)||(simulator == 10)) 
     14        sim_param = pmsm_sim; 
     15    %     sim_param(9) = 0; %vypne dead-time 
     16        pmsm_sim(sim_param); 
    9517    end 
    96 % ref_ome = 0*ones(1, T); 
    97  
    98 % Derivace pro prvni EKF  
    99 ome = x_k(1,1); 
    100 the = x_k(2,1);     
    101 ia = y_s(1,1); 
    102 ib = y_s(2,1); 
    103 A = [d, -e*(ia*cos(the)+ib*sin(the)); dt, 1.0]; 
    104 C = [b*sin(the), b*ome*cos(the); -b*cos(the), b*ome*sin(the)]; 
    105  
    106  
    107 ri = 0.0001; 
    108 ai = (1-a*a)/c/c; 
    109 Si = (1 - ai*r + sqrt((ai*r-1)^2+4*r/c/c))/2; 
    110 Li = a*c*Si/(c*c*Si+ri); 
    111      
    112 A_l = [d,0,0;dt,1,dt;0,0,1]; 
    113 % B_l = zeros(2); 
    114 % % A_l = [a 0 0 0 0; 0 a 0 0 0; 0 0 d 0 (d-1); 0 0 dt 1 dt; 0 0 0 0 1]; 
    115 % % B_l = zeros(5,2); 
    116 % % B_l(1:2,1:2) = [c 0;0 c]; 
    117  
    118 %PI vektorove 
    119 % kon_pi = 3.0; 
    120 % kon_ii = 0.00375; 
    121 % kon_pu = 20.0; 
    122 % kon_iu = 0.05; 
    123 % sum_iq = 0; 
    124 % sum_ud = 0; 
    125 % sum_uq = 0; 
    126  
    127  
    128  
    129 % HLAVNI SMYCKA 
    130 for t = 1:T-1, 
    131     % EKF       
    132     Pp = A*P_k*A' + Q_k; 
    133     S = C*Pp*C' + R_k; 
    134     K = Pp*C'/S; 
    135     P_k = Pp - K*C*Pp;     
    136      
    137     xp = zeros(2,1); 
    138     xp(1) = d*x_k(1,t) + e*(y_s(2,t)*cos(x_k(2,t)) - y_s(1,t)*sin(x_k(2,t))); 
    139     xp(2) = x_k(2,t) + dt*x_k(1,t);     
    140     yp = zeros(2,1); 
    141     yp(1) = a*y_s(1,t) + b*x_k(1,t)*sin(x_k(2,t)) + c*u_l(1,t); 
    142     yp(2) = a*y_s(2,t) - b*x_k(1,t)*cos(x_k(2,t)) + c*u_l(2,t); 
    143      
    144     x_k(:,t+1) = xp + K*(y_s(:,t) - yp); 
    145      
    146     %!!! 
    147 %     tmp = x_k(:,t+1); 
    148 %     x_k(:,t+1) = x_s(3:4,t); 
    149      
    150     % Derivace     
    151     ome = x_k(1,t+1); 
    152     the = x_k(2,t+1);     
    153     ia = y_s(1,t); 
    154     ib = y_s(2,t); 
     18    %%%%% 
     19    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
     20 
     21    % KONSTANTY 
     22    % T = 120000; %horizont 
     23    dt = 0.000125; %casovy krok 
     24 
     25    % Rs = 0.28; 
     26    % Ls = 0.003465; 
     27    % psipm = 0.1989; 
     28    % B = 0;     
     29    % kp = 1.5; 
     30    % pp = 4.0; 
     31    % J = 0.04; 
     32 
     33    % Lq = 1.05*Ls; 
     34    % Ld = 0.95*Ls; 
     35 
     36    a = 0.9898; 
     37    b = 0.0072; 
     38    c = 0.0361; 
     39    d = 1.0; 
     40    e = 0.0149; 
     41 
     42    Rs = 0.28; 
     43    Ls = 0.003465; 
     44    psi = 0.1989; 
     45    B = 0;     
     46    kp = 1.5; 
     47    pp = 4.0; 
     48    J = 0.04; 
     49    Lq = 1.0*Ls; 
     50    Ld = 0.9*Ls; 
     51    kpp = kp*pp*pp; 
     52    kppj = kpp/J; 
     53 
     54%     Ls = 0.003465; 
     55%     psipm = 0.1989; 
     56 
     57    % ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0, 0,-3, -6, -3];%/9*200; 
     58    % ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; 
     59    % ref_profile = [0, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 0]; 
     60    % ref_profile = [1, 10, 50, 200, 200, 30, 0, 0, -1, -10, -50, -200, -200, -30, 0]; 
     61    % ref_profile = [20, 20, 20, 50, 50, 50, -10, -10, -10, 0, 0, 0, 20, 20, 20]; 
     62 
     63    %kovariance EKF na stavu 
     64    % Q_k = diag([0.001, 0.00001]); 
     65    % R_k = diag([0.015, 0.015]); 
     66    Q_k = diag([0.01, 0.0001]); 
     67    R_k = diag([0.15, 0.15]); 
     68 
     69    %hodnoty sumu v systemu 
     70    nQ = diag([0.0013, 0.0013, 5.0e-6, 1.0e-10]); 
     71    nR = diag([0.0006, 0.0006]); 
     72 
     73    iter_l = 10;% pocet iteraci ve vypoctu rizeni 
     74 
     75    % B_l = zeros(3,2); 
     76    % B_l = zeros(2,2); 
     77    % B_l(1,1) = c; 
     78    % B_l(2,2) = c; 
     79 
     80    Q_l = diag([1 0 0]); 
     81    % % Q_l = diag([0 0 1 0 0]); 
     82    r = 0.01; 
     83    R_l = diag([r, r]); 
     84 
     85    % PROMENNE 
     86    x_s = zeros(4,T); %stav 
     87    y_s = zeros(2,T); %mereni 
     88    x_k = zeros(2,T); %odhad stavu 
     89%     P_k = zeros(2); %kovariance stavu 
     90    u_l = zeros(2,T); %rizeni 
     91    % S_l = zeros(3); %jadro ztraty 
     92%     S_l = zeros(2); 
     93 
     94    % POCATECNI HODNOTY 
     95    noise = 1; %prepinac sumu 
     96    % noise = 0; 
     97 
     98    % theta0 = 0; %pocatecni poloha odhadu (nejde pro stav kvuli simulatoru) 
     99    P0 = eye(2); %odhad pocatecni kovariance stavu (apriorni) 
     100    % ST = zeros(3); %koncova ztrata 
     101    ST = ones(3); 
     102 
     103 
     104    % INICIALIZACE 
     105    x_k(2,1) = theta0; 
     106    % x_s(3,1) = 5; 
     107    P_k = P0; 
     108    S_l = ST; 
     109 
     110    ref_ome = zeros(1, T);   
     111        for k = 1:T, 
     112               index = floor(k*dt); 
     113               if(index>0) 
     114                   lower = ref_profile(index); 
     115               else 
     116                   lower = 0; 
     117               end 
     118               if(index<T*dt) 
     119                   upper = ref_profile(index+1); 
     120               else 
     121                   upper = 0; 
     122               end 
     123               ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt); 
     124        end 
     125    % ref_ome = 0*ones(1, T); 
     126 
     127    % Derivace pro prvni EKF  
     128    ome = x_k(1,1); 
     129    the = x_k(2,1);     
     130    ia = y_s(1,1); 
     131    ib = y_s(2,1); 
    155132    A = [d, -e*(ia*cos(the)+ib*sin(the)); dt, 1.0]; 
    156133    C = [b*sin(the), b*ome*cos(the); -b*cos(the), b*ome*sin(the)]; 
     134 
     135 
     136    ri = 0.0001; 
     137    ai = (1-a*a)/c/c; 
     138    Si = (1 - ai*r + sqrt((ai*r-1)^2+4*r/c/c))/2; 
     139    Li = a*c*Si/(c*c*Si+ri); 
     140 
     141    A_l = [d,0,0;dt,1,dt;0,0,1]; 
     142    % B_l = zeros(2); 
     143    % % A_l = [a 0 0 0 0; 0 a 0 0 0; 0 0 d 0 (d-1); 0 0 dt 1 dt; 0 0 0 0 1]; 
     144    % % B_l = zeros(5,2); 
     145    % % B_l(1:2,1:2) = [c 0;0 c]; 
     146 
     147    %PI vektorove 
     148    % kon_pi = 3.0; 
     149    % kon_ii = 0.00375; 
     150    % kon_pu = 20.0; 
     151    % kon_iu = 0.05; 
     152    % sum_iq = 0; 
     153    % sum_ud = 0; 
     154    % sum_uq = 0; 
     155 
     156 
     157 
     158    % HLAVNI SMYCKA 
     159    for t = 1:T-1, 
     160        % EKF       
     161        Pp = A*P_k*A' + Q_k; 
     162        S = C*Pp*C' + R_k; 
     163        K = Pp*C'/S; 
     164        P_k = Pp - K*C*Pp;     
     165 
     166        xp = zeros(2,1); 
     167        xp(1) = d*x_k(1,t) + e*(y_s(2,t)*cos(x_k(2,t)) - y_s(1,t)*sin(x_k(2,t))); 
     168        xp(2) = x_k(2,t) + dt*x_k(1,t);     
     169        yp = zeros(2,1); 
     170        yp(1) = a*y_s(1,t) + b*x_k(1,t)*sin(x_k(2,t)) + c*u_l(1,t); 
     171        yp(2) = a*y_s(2,t) - b*x_k(1,t)*cos(x_k(2,t)) + c*u_l(2,t); 
     172 
     173        x_k(:,t+1) = xp + K*(y_s(:,t) - yp); 
     174 
     175        %!!! 
     176    %     tmp = x_k(:,t+1); 
     177    %     x_k(:,t+1) = x_s(3:4,t); 
     178 
     179        % Derivace     
     180        ome = x_k(1,t+1); 
     181        the = x_k(2,t+1);     
     182        ia = y_s(1,t); 
     183        ib = y_s(2,t); 
     184        if(inddq == 0) 
     185            %stejne indukcnosti 
     186            A = [d, -e*(ia*cos(the)+ib*sin(the)); dt, 1.0]; 
     187        else 
     188            %ruzne indukcnosti         
     189            A = [d, -dt*kppj*(psi*(ia*cos(the) + ib*sin(the)) + (Ld - Lq)*(ia*cos(the) + ib*sin(the))^2 - (Ld - Lq)*(ib*cos(the) - ia*sin(the))^2); dt, 1.0]; 
     190        end 
     191        C = [b*sin(the), b*ome*cos(the); -b*cos(the), b*ome*sin(the)]; 
     192 
     193    %     id = ia*cos(the) + ib*sin(the); 
     194    %     iq = ib*cos(the) - ia*sin(the); 
     195 
     196        % LQ 
     197    %     phi = zeros(2,1); 
     198    %     phi(1) = d*x_k(1,t+1) + e*(y_s(2,t)*cos(x_k(2,t+1)) - y_s(1,t)*sin(x_k(2,t+1))); 
     199    %     phi(2) = x_k(2,t+1) + dt*x_k(1,t+1); 
     200    %     y = x_k(:,t+1); 
     201    %     y(1) = y(1) - ref_ome(t); 
     202    %     A_l = zeros(3); 
     203    %     A_l(1:2,1:2) = A; 
     204    %     A_l = A; 
     205    %     A_l(1,2) = 0; 
     206    %     A_l(1:2,3) = phi - A*y; 
     207    %     A_l(3,3) = 1; 
     208        B_l = [-e*sin(the), e*cos(the); 0, 0; 0,0]; 
     209        y = [(ome-ref_ome(t)); the; ref_ome(t)]; 
     210    % %     y = [ia; ib; (ome-ref_ome(t)); the; ref_ome(t)];     
     211    % %     A_l(1, 3) = b*sin(the); 
     212    % %     A_l(2, 3) = -b*cos(the); 
     213    % %     A_l(1, 5) = b*sin(the); 
     214    % %     A_l(2, 5) = -b*cos(the); 
     215    % %     A_l(3, 1) = -e*sin(the); 
     216    % %     A_l(3, 2) = e*cos(the); 
     217        for i = 1:iter_l 
     218           S_l = A_l'*(S_l - S_l*B_l/(B_l'*S_l*B_l + R_l)*B_l'*S_l)*A_l + Q_l; 
     219        end 
     220        L = (B_l'*S_l*B_l + R_l)\B_l'*S_l*A_l;     
     221    %     yref = -L*y;%referencni proudy 
     222    %     u_l(:,t+1) = b/c*ome*[-sin(the);cos(the)] + yref/c - Li*y_s(:,t); 
     223 
     224    %     sum_iq = sum_iq + ref_ome(t) - ome; 
     225    %     ref_iq = kon_pi*(ref_ome(t) - ome) + kon_ii*sum_iq; 
     226    %     sum_ud = sum_ud - id; 
     227    %     u_d = kon_pu*(-id) + kon_iu*sum_ud; 
     228    %     sum_uq = sum_uq + ref_iq - iq; 
     229    %     u_q = kon_pu*(ref_iq - iq) + kon_iu*sum_uq; 
     230    %     u_d = u_d - Ls*ome*ref_iq; 
     231    %     u_q = u_q + psipm*ome; 
     232    %         
     233    %     u_l(1, t+1) = u_d*cos(the) - u_q*sin(the); 
     234    %     u_l(2, t+1) = u_q*cos(the) + u_d*sin(the); 
     235    %     u_l(:,t+1) = b/c*ome*[-sin(the);cos(the)] + yref/c*[sin(the);-cos(the)] - Li*y_s(:,t); 
     236 
     237    %     u_l(:,t+1) = yref/c - Li*y_s(:,t); 
     238    %     u_l(:,t+1) = -L*[y;1]; 
     239        u_l(:,t+1) = -L*y + b/c*ome*[-sin(the);cos(the)] - Li*y_s(:,t); 
     240        if u_l(1,t+1) > 100 
     241            u_l(1,t+1) = 100; 
     242        elseif u_l(1,t+1) < -100 
     243            u_l(1,t+1) = -100; 
     244        end 
     245        if u_l(2,t+1) > 100 
     246            u_l(2,t+1) = 100; 
     247        elseif u_l(2,t+1) < -100 
     248            u_l(2,t+1) = -100; 
     249        end 
     250    %     u_l(:,t+1) = 0; 
     251        % Vyvoj systemu        
     252        [x_s(:,t+1), y_s(:,t+1)] = evolSys(x_s(:,t), u_l(:,t+1), nQ, nR, noise, simulator); 
     253 
     254 
     255        %!!! 
     256    %     x_k(:,t+1) = tmp; 
     257    end 
     258 
     259    if(graf == 1) 
     260        %vykresleni 
     261        cas = (1:T)*dt; 
     262        figure; 
     263        subplot(2,1,1); 
     264        plot(cas,x_k(1,:),cas,x_s(3,:),cas,ref_ome); 
     265        title('Prubeh otacek v case'); 
     266        xlabel('cas [s]'); 
     267        ylabel('otacky [rad/s]'); 
     268        legend('odhad','skutecne','pozadovane'); 
     269        subplot(2,1,2); 
     270        plot(cas,atan2(sin(x_k(2,:)),cos(x_k(2,:))),cas,atan2(sin(x_s(4,:)),cos(x_s(4,:)))); 
     271        title('Prubeh polohy v case'); 
     272        xlabel('cas [s]'); 
     273        ylabel('poloha [rad]'); 
     274 
     275        figure; 
     276        plot(cas,x_s(3,:)-ref_ome); 
     277        title('Prubeh chyby (skutecne - pozadovane otacky v case)'); 
     278        xlabel('cas [s]'); 
     279        ylabel('chyba [rad/s]'); 
     280    end 
    157281     
    158 %     id = ia*cos(the) + ib*sin(the); 
    159 %     iq = ib*cos(the) - ia*sin(the); 
    160      
    161     % LQ 
    162 %     phi = zeros(2,1); 
    163 %     phi(1) = d*x_k(1,t+1) + e*(y_s(2,t)*cos(x_k(2,t+1)) - y_s(1,t)*sin(x_k(2,t+1))); 
    164 %     phi(2) = x_k(2,t+1) + dt*x_k(1,t+1); 
    165 %     y = x_k(:,t+1); 
    166 %     y(1) = y(1) - ref_ome(t); 
    167 %     A_l = zeros(3); 
    168 %     A_l(1:2,1:2) = A; 
    169 %     A_l = A; 
    170 %     A_l(1,2) = 0; 
    171 %     A_l(1:2,3) = phi - A*y; 
    172 %     A_l(3,3) = 1; 
    173     B_l = [-e*sin(the), e*cos(the); 0, 0; 0,0]; 
    174     y = [(ome-ref_ome(t)); the; ref_ome(t)]; 
    175 % %     y = [ia; ib; (ome-ref_ome(t)); the; ref_ome(t)];     
    176 % %     A_l(1, 3) = b*sin(the); 
    177 % %     A_l(2, 3) = -b*cos(the); 
    178 % %     A_l(1, 5) = b*sin(the); 
    179 % %     A_l(2, 5) = -b*cos(the); 
    180 % %     A_l(3, 1) = -e*sin(the); 
    181 % %     A_l(3, 2) = e*cos(the); 
    182     for i = 1:iter_l 
    183        S_l = A_l'*(S_l - S_l*B_l/(B_l'*S_l*B_l + R_l)*B_l'*S_l)*A_l + Q_l; 
    184     end 
    185     L = (B_l'*S_l*B_l + R_l)\B_l'*S_l*A_l;     
    186 %     yref = -L*y;%referencni proudy 
    187 %     u_l(:,t+1) = b/c*ome*[-sin(the);cos(the)] + yref/c - Li*y_s(:,t); 
    188      
    189 %     sum_iq = sum_iq + ref_ome(t) - ome; 
    190 %     ref_iq = kon_pi*(ref_ome(t) - ome) + kon_ii*sum_iq; 
    191 %     sum_ud = sum_ud - id; 
    192 %     u_d = kon_pu*(-id) + kon_iu*sum_ud; 
    193 %     sum_uq = sum_uq + ref_iq - iq; 
    194 %     u_q = kon_pu*(ref_iq - iq) + kon_iu*sum_uq; 
    195 %     u_d = u_d - Ls*ome*ref_iq; 
    196 %     u_q = u_q + psipm*ome; 
    197 %         
    198 %     u_l(1, t+1) = u_d*cos(the) - u_q*sin(the); 
    199 %     u_l(2, t+1) = u_q*cos(the) + u_d*sin(the); 
    200 %     u_l(:,t+1) = b/c*ome*[-sin(the);cos(the)] + yref/c*[sin(the);-cos(the)] - Li*y_s(:,t); 
    201  
    202 %     u_l(:,t+1) = yref/c - Li*y_s(:,t); 
    203 %     u_l(:,t+1) = -L*[y;1]; 
    204     u_l(:,t+1) = -L*y + b/c*ome*[-sin(the);cos(the)] - Li*y_s(:,t); 
    205     if u_l(1,t+1) > 100 
    206         u_l(1,t+1) = 100; 
    207     elseif u_l(1,t+1) < -100 
    208         u_l(1,t+1) = -100; 
    209     end 
    210     if u_l(2,t+1) > 100 
    211         u_l(2,t+1) = 100; 
    212     elseif u_l(2,t+1) < -100 
    213         u_l(2,t+1) = -100; 
    214     end 
    215 %     u_l(:,t+1) = 0; 
    216     % Vyvoj systemu 
    217     [x_s(:,t+1), y_s(:,t+1)] = evolSys(x_s(:,t), u_l(:,t+1), nQ, nR, noise); 
    218      
    219     %!!! 
    220 %     x_k(:,t+1) = tmp; 
     282    loss = sum((x_s(3,:)-ref_ome).^2); 
    221283end 
    222  
    223 figure; 
    224 subplot(2,1,1); 
    225 plot(1:T,x_k(1,:),1:T,x_s(3,:),1:T,ref_ome); 
    226 subplot(2,1,2); 
    227 plot(1:T,atan2(sin(x_k(2,:)),cos(x_k(2,:))),1:T,atan2(sin(x_s(4,:)),cos(x_s(4,:)))); 
    228