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

pridani a uprava lqg s hyperstavem viz clanek Kim2006

Location:
applications/dual/vahala/kim
Files:
42 added
6 modified

Legend:

Unmodified
Added
Removed

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

    r1435 r1436  
    1 function [A_k, C_k, pre_k, A_l] = assembDeriv(ksi, iab, ksi0, Q, R, ref_ome)     
     1function [A_k, C_k, pre_k, A_l] = assembDeriv(ksi, iab, ksi0, Q, R, ref_ome, inddq)     
    22    a = 0.9898; 
    33    b = 0.0072; 
    … …  
    66    e = 0.0149; 
    77    dt = 0.000125; 
     8 
     9    Rs = 0.28; 
     10    Ls = 0.003465; 
     11    psi = 0.1989; 
     12    B = 0;     
     13    kp = 1.5; 
     14    pp = 4.0; 
     15    J = 0.04; 
     16    Lq = 1.0*Ls; 
     17    Ld = 0.9*Ls; 
     18    kpp = kp*pp*pp; 
     19    kppj = kpp/J; 
    820     
    921    ome = ksi(1); 
    … …  
    1729     
    1830    %puvodni matice derivaci 
    19     A = [d, -e*(ia*cos(the)+ib*sin(the)); dt, 1.0]; 
     31    if(inddq == 0) 
     32        %stejne indukcnosti 
     33        A = [d, -e*(ia*cos(the)+ib*sin(the)); dt, 1.0]; 
     34    else 
     35        %ruzne indukcnosti 
     36        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]; 
     37    end 
    2038    C = [b*sin(the), b*ome*cos(the); -b*cos(the), b*ome*sin(the)]; 
    2139     
    … …  
    115133    pre_k(1) = Pnew(1,1); 
    116134    pre_k(2) = Pnew(1,2); 
    117     pre_k(3) = Pnew(2,2);     
     135    pre_k(3) = Pnew(2,2);  
     136    %max x(5) = pi^2/3 ... variance of uniform -pi,pi 
     137    if(pre_k(3) > pi^2/3) 
     138        pre_k(3) = pi^2/3; 
     139    end 
    118140end 

  • 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  

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

    r1435 r1436  
    1 function [x_s, y_s] = evolSys(x, u, nQ, nR, noise) 
    2     dt = 0.000125;  
    3     a = 0.9898; 
    4     b = 0.0072; 
    5     c = 0.0361; 
    6     d = 1.0; 
    7     e = 0.0149; 
     1function [x_s, y_s] = evolSys(x, u, nQ, nR, noise, simulator) 
     2    if ((simulator == 1)||(simulator == 10))%simulator 
     3        ua = 3*u(1); 
     4        ub = 3*u(2); 
     5        %kompenzace ubytku 
     6        if(simulator == 10) 
     7            ua = ua + comps(x(1)); 
     8            ub = ub + comps(x(2));            
     9        end 
     10         
     11        [tx, ty] = pmsm_sim(ua, ub, 0); 
     12         
     13        x_s = tx(1:4); %isa, isb, omega, theta         
     14        y_s = ty(3:4); %isa, isb         
     15         
     16    elseif (simulator == 0)%matlab - stejne indukcnosti 
     17        dt = 0.000125;  
     18        a = 0.9898; 
     19        b = 0.0072; 
     20        c = 0.0361; 
     21        d = 1.0; 
     22        e = 0.0149; 
    823 
    9     x_s(1) = a*x(1) + b*x(3)*sin(x(4)) + c*u(1) + noise*sqrt(nQ(1,1))*randn(); 
    10     x_s(2) = a*x(2) - b*x(3)*cos(x(4)) + c*u(2) + noise*sqrt(nQ(2,2))*randn(); 
    11     x_s(3) = d*x(3) + e*(x(2)*cos(x(4)) - x(1)*sin(x(4))) + noise*sqrt(nQ(3,3))*randn(); 
    12     x_s(4) = x(4) + dt*x(3) + noise*sqrt(nQ(4,4))*randn(); 
     24        x_s(1) = a*x(1) + b*x(3)*sin(x(4)) + c*u(1) + noise*sqrt(nQ(1,1))*randn(); 
     25        x_s(2) = a*x(2) - b*x(3)*cos(x(4)) + c*u(2) + noise*sqrt(nQ(2,2))*randn(); 
     26        x_s(3) = d*x(3) + e*(x(2)*cos(x(4)) - x(1)*sin(x(4))) + noise*sqrt(nQ(3,3))*randn(); 
     27        x_s(4) = x(4) + dt*x(3) + noise*sqrt(nQ(4,4))*randn(); 
    1328 
    14     y_s(1) = x_s(1) + noise*sqrt(nR(1,1))*randn(); 
    15     y_s(2) = x_s(2) + noise*sqrt(nR(2,2))*randn(); 
    16 end 
     29        y_s(1) = x_s(1) + noise*sqrt(nR(1,1))*randn(); 
     30        y_s(2) = x_s(2) + noise*sqrt(nR(2,2))*randn(); 
     31         
     32    else %matlab - ruzne indukcnosti 
     33        Rs = 0.28; 
     34        Ls = 0.003465; 
     35        psipm = 0.1989; 
     36        B = 0;     
     37        kp = 1.5; 
     38        pp = 4.0; 
     39        J = 0.04; 
     40        dt = 0.000125; 
     41        Lq = 1.0*Ls; 
     42        Ld = 0.9*Ls;  
     43         
     44        id = x(1)*cos(x(4)) + x(2)*sin(x(4)); 
     45        iq = x(2)*cos(x(4)) - x(1)*sin(x(4)); 
     46        om = x(3); 
     47        th = x(4); 
     48        ud = u(1)*cos(x(4)) + u(2)*sin(x(4)); 
     49        uq = u(2)*cos(x(4)) - u(1)*sin(x(4)); 
     50                 
     51        idp = (1 - Rs*dt/Ld)*id + Lq*dt/Ld*om*iq + dt/Ld*ud; 
     52        iqp = (1 - Rs*dt/Lq)*iq - psipm*dt/Lq*om - Ld*dt/Lq*om*id + dt/Lq*uq; 
     53        omp = (1 - B*dt/J)*om + kp*pp*pp*dt/J*((Ld-Lq)*id*iq + psipm*iq); 
     54        thp = th + dt*om; 
     55         
     56        x_s(1) = idp*cos(thp) - iqp*sin(thp) + noise*sqrt(nQ(1,1))*randn(); 
     57        x_s(2) = iqp*cos(thp) + idp*sin(thp) + noise*sqrt(nQ(2,2))*randn(); 
     58        x_s(3) = omp + noise*sqrt(nQ(3,3))*randn(); 
     59        x_s(4) = thp + noise*sqrt(nQ(4,4))*randn();         
     60 
     61        y_s(1) = x_s(1) + noise*sqrt(nR(1,1))*randn(); 
     62        y_s(2) = x_s(2) + noise*sqrt(nR(2,2))*randn(); 
     63         
     64    end 
     65end  

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

    r1435 r1436  
    2323     
    2424    x_k = xp + K*(y - yp); 
     25     
     26    %max x(5) = pi^2/3 ... variance of uniform -pi,pi 
     27%     if(x_k(5) > pi^2/3) 
     28%         x_k(5) = pi^2/3; 
     29%     end 
    2530end 

  • applications/dual/vahala/kim/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 % ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0, 0,-3, -6, -3];%/9*200; 
    29 ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; 
    30  
    31 %kovariance EKF na stavu, ktery vytvari hyperstav 
    32 % Q_k = diag([0.001, 0.00001]); 
    33 % R_k = diag([0.015, 0.015]); 
    34 Q_k = diag([0.01, 0.0001]); 
    35 R_k = diag([0.15, 0.15]); 
    36  
    37 %kovariance EKF na hyperstavu 
    38 % Qh_k = diag([0.001, 0.00001, 0.00001, 0.00001, 0.00001]); 
    39 % Rh_k = diag([0.015, 0.015]); 
    40 Qh_k = diag([0.01, 0.0001, 0.1, 0.1, 0.1]); 
    41 Rh_k = diag([0.15, 0.15]); 
    42  
    43 %hodnoty sumu v systemu 
    44 nQ = diag([0.0013, 0.0013, 5.0e-6, 1.0e-10]); 
    45 nR = diag([0.0006, 0.0006]); 
    46  
    47 iter_l = 10;% pocet iteraci ve vypoctu rizeni 
    48  
    49 B_l = zeros(6,2); 
    50 % B_l(1,1) = c; 
    51 % B_l(2,2) = c; 
    52  
    53 %           o t Po Pot Pt 
    54 Q_l = diag([1 0 300 0.0 300 0]); 
    55 % Q_l = diag([1 0 0 0 0 0]); 
    56 r = 0.0001; 
    57 R_l = diag([r, r]); 
    58  
    59  
    60  
    61 % PROMENNE 
    62 x_s = zeros(4,T); %stav 
    63 y_s = zeros(2,T); %mereni 
    64 x_k = zeros(5,T); %odhad hyperstavu 
    65 P_k = zeros(5); %kovariance hyperstavu 
    66 u_l = zeros(2,T); %rizeni 
    67 S_l = zeros(6); %jadro ztraty 
    68 pre_k = zeros(3,1); %predikce stavu 
    69  
    70  
    71 % POCATECNI HODNOTY 
    72 noise = 1; %prepinac sumu 
    73 % noise = 0; 
    74  
    75 theta0 = 1.5;%1.7; %pocatecni poloha 
    76 Ps0 = eye(2); %odhad pocatecni kovariance stavu (apriorni) 
    77 Pk0 = eye(5); %pocatecni kovariance hyperstavu 
    78 ST = zeros(6); %koncova ztrata 
    79  
    80  
    81 % INICIALIZACE 
    82 x_s(4,1) = theta0; 
    83 x_k(3,1) = Ps0(1,1); 
    84 x_k(4,1) = Ps0(1,2); 
    85 x_k(5,1) = Ps0(2,2); 
    86 P_k = Pk0; 
    87 S_l = ST; 
    88  
    89 ref_ome = zeros(1, T);   
    90     for k = 1:T, 
    91            index = floor(k*dt); 
    92            if(index>0) 
    93                lower = ref_profile(index); 
    94            else 
    95                lower = 0; 
    96            end 
    97            if(index<T*dt) 
    98                upper = ref_profile(index+1); 
    99            else 
    100                upper = 0; 
    101            end 
    102            ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt); 
     1function [loss] = main(T, ref_profile, theta0, simulator, graf, inddq)  
     2    % 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); 
    10317    end 
    104  
    105 % Derivace pro prvni EKF  
    106 [A_k, C_k, pre_k, A_l] = assembDeriv(x_k(:,1), y_s(:,1), x_k(:,1), Q_k, R_k, 0); 
    107    
    108 ri = 0.0001; 
    109 ai = (1-a*a)/c/c; 
    110 Si = (1 - ai*r + sqrt((ai*r-1)^2+4*r/c/c))/2; 
    111 Li = a*c*Si/(c*c*Si+ri); 
    112  
    113 Pia = 1; 
    114 Pib = 1; 
    115 qi = 0.1; 
    116 ri = 0.05; 
    117 y = [0;0]; 
    118  
    119 % HLAVNI SMYCKA 
    120 for t = 1:T-1, 
    121     % EKF    
    122     Pp = Pia*(a*a+b*b+b*b*cos(x_k(2,t))^2*(x_k(1,t)^2-1))+qi; 
    123     Pia = Pp-Pp*Pp/(Pp+ri); 
    124     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); 
    125     Pp = Pib*(a*a+b*b+b*b*sin(x_k(2,t))^2*(x_k(1,t)^2-1))+qi; 
    126     Pib = Pp-Pp*Pp/(Pp+ri); 
    127     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); 
    128     [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); 
    129 %     [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); 
    130      
    131 %     Q_l(1,1) = 1/(1+exp(-2*x_k(1,t+1)+6))+0.1; 
    132     Q_l(3,3) = x_k(5,t+1)^5*50; 
    133     Q_l(5,5) = Q_l(3,3); 
    134  
    135     % Derivace 
    136     [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)); 
    137      
    138     % LQ 
    139     B_l(1,1:2) = [-e*sin(x_k(2,t+1)), e*cos(x_k(2,t+1))]; 
    140 %     [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); 
    141 [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); 
    142     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); 
    143     if u_l(1,t+1) > 100 
    144         u_l(1,t+1) = 100; 
    145     elseif u_l(1,t+1) < -100 
    146         u_l(1,t+1) = -100; 
     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    % ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0, 0,-3, -6, -3];%/9*200; 
     43    % ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; 
     44%     ref_profile = [0, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 0]; 
     45    % ref_profile = [1, 10, 50, 200, 200, 30, 0, 0, -1, -10, -50, -200, -200, -30, 0]; 
     46    % ref_profile = [20, 20, 20, 50, 50, 50, -10, -10, -10, 0, 0, 0, 20, 20, 20]; 
     47 
     48    %kovariance EKF na stavu, ktery vytvari hyperstav 
     49    % Q_k = diag([0.001, 0.00001]); 
     50    % R_k = diag([0.015, 0.015]); 
     51    Q_k = diag([0.01, 0.0001]); 
     52    R_k = diag([0.15, 0.15]); 
     53 
     54    %kovariance EKF na hyperstavu 
     55    % Qh_k = diag([0.001, 0.00001, 0.00001, 0.00001, 0.00001]); 
     56    % Rh_k = diag([0.015, 0.015]); 
     57    Qh_k = diag([0.01, 0.0001, 0.1, 0.1, 0.1]); 
     58    Rh_k = diag([0.15, 0.15]); 
     59 
     60    %hodnoty sumu v systemu 
     61    nQ = diag([0.0013, 0.0013, 5.0e-6, 1.0e-10]); 
     62    nR = diag([0.0006, 0.0006]); 
     63 
     64    iter_l = 10;% pocet iteraci ve vypoctu rizeni 
     65 
     66    B_l = zeros(6,2); 
     67    % B_l(1,1) = c; 
     68    % B_l(2,2) = c; 
     69 
     70    %           o t Po Pot Pt 
     71    Q_l = diag([1 0 1 0 0 0]); % spravne z teoretickeho hlediska 
     72%     Q_l = diag([1 0 1 0 1 0]); 
     73    % Q_l = diag([1 0 0 0 0 0]); 
     74    r = 0.0001; 
     75    R_l = diag([r, r]); 
     76 
     77 
     78 
     79    % PROMENNE 
     80    x_s = zeros(4,T); %stav 
     81    y_s = zeros(2,T); %mereni 
     82    x_k = zeros(5,T); %odhad hyperstavu 
     83%     P_k = zeros(5); %kovariance hyperstavu 
     84    u_l = zeros(2,T); %rizeni 
     85%     S_l = zeros(6); %jadro ztraty 
     86%     pre_k = zeros(3,1); %predikce stavu 
     87 
     88 
     89    % POCATECNI HODNOTY 
     90    noise = 1; %prepinac sumu 
     91    % noise = 0; 
     92 
     93%     theta0 = 0; %pocatecni poloha odhadu (nejde pro stav kvuli simulatoru) 
     94    Ps0 = eye(2); %odhad pocatecni kovariance stavu (apriorni) 
     95    Pk0 = eye(5); %pocatecni kovariance hyperstavu 
     96    ST = zeros(6); %koncova ztrata 
     97 
     98 
     99    % INICIALIZACE 
     100    x_k(2,1) = theta0; 
     101    x_k(3,1) = Ps0(1,1); 
     102    x_k(4,1) = Ps0(1,2); 
     103    x_k(5,1) = Ps0(2,2); 
     104    P_k = Pk0; 
     105    S_l = ST; 
     106 
     107    ref_ome = zeros(1, T);   
     108        for k = 1:T, 
     109               index = floor(k*dt); 
     110               if(index>0) 
     111                   lower = ref_profile(index); 
     112               else 
     113                   lower = 0; 
     114               end 
     115               if(index<T*dt) 
     116                   upper = ref_profile(index+1); 
     117               else 
     118                   upper = 0; 
     119               end 
     120               ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt); 
     121        end 
     122 
     123    % Derivace pro prvni EKF  
     124    [A_k, C_k, pre_k, A_l] = assembDeriv(x_k(:,1), y_s(:,1), x_k(:,1), Q_k, R_k, 0, inddq); 
     125 
     126    ri = 0.0001; 
     127    ai = (1-a*a)/c/c; 
     128    Si = (1 - ai*r + sqrt((ai*r-1)^2+4*r/c/c))/2; 
     129    Li = a*c*Si/(c*c*Si+ri); 
     130 
     131    Pia = 1; 
     132    Pib = 1; 
     133    qi = 0.1; 
     134    ri = 0.05; 
     135    y = [0;0]; 
     136 
     137    % HLAVNI SMYCKA 
     138    for t = 1:T-1, 
     139        % EKF    
     140        Pp = Pia*(a*a+b*b+b*b*cos(x_k(2,t))^2*(x_k(1,t)^2-1))+qi; 
     141        Pia = Pp-Pp*Pp/(Pp+ri); 
     142        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); 
     143        Pp = Pib*(a*a+b*b+b*b*sin(x_k(2,t))^2*(x_k(1,t)^2-1))+qi; 
     144        Pib = Pp-Pp*Pp/(Pp+ri); 
     145        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); 
     146        [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); 
     147    %     [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); 
     148 
     149    %     Q_l(1,1) = 1/(1+exp(-2*x_k(1,t+1)+6))+0.1; 
     150    %     Q_l(3,3) = x_k(5,t+1)^5*50; 
     151    %     Q_l(5,5) = Q_l(3,3); 
     152 
     153        % Derivace 
     154        [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), inddq); 
     155 
     156        % LQ 
     157        B_l(1,1:2) = [-e*sin(x_k(2,t+1)), e*cos(x_k(2,t+1))]; 
     158    %     [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); 
     159        [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); 
     160        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); 
     161 
     162        if u_l(1,t+1) > 100 
     163            u_l(1,t+1) = 100; 
     164        elseif u_l(1,t+1) < -100 
     165            u_l(1,t+1) = -100; 
     166        end 
     167        if u_l(2,t+1) > 100 
     168            u_l(2,t+1) = 100; 
     169        elseif u_l(2,t+1) < -100 
     170            u_l(2,t+1) = -100; 
     171        end 
     172 
     173        % Vyvoj systemu 
     174        [x_s(:,t+1), y_s(:,t+1)] = evolSys(x_s(:,t), u_l(:,t+1), nQ, nR, noise, simulator);     
    147175    end 
    148     if u_l(2,t+1) > 100 
    149         u_l(2,t+1) = 100; 
    150     elseif u_l(2,t+1) < -100 
    151         u_l(2,t+1) = -100; 
     176 
     177    if(graf == 1) 
     178        %vykresleni 
     179        cas = (1:T)*dt; 
     180        figure; 
     181        subplot(2,1,1); 
     182        plot(cas,x_k(1,:),cas,x_s(3,:),cas,ref_ome); 
     183        title('Prubeh otacek v case'); 
     184        xlabel('cas [s]'); 
     185        ylabel('otacky [rad/s]'); 
     186        legend('odhad','skutecne','pozadovane'); 
     187        subplot(2,1,2); 
     188        plot(cas,atan2(sin(x_k(2,:)),cos(x_k(2,:))),cas,atan2(sin(x_s(4,:)),cos(x_s(4,:)))); 
     189        title('Prubeh polohy v case'); 
     190        xlabel('cas [s]'); 
     191        ylabel('poloha [rad]'); 
     192 
     193        figure; 
     194        plot(cas,x_s(3,:)-ref_ome); 
     195        title('Prubeh chyby (skutecne - pozadovane otacky v case)'); 
     196        xlabel('cas [s]'); 
     197        ylabel('chyba [rad/s]'); 
    152198    end 
    153199     
    154     % Vyvoj systemu 
    155     [x_s(:,t+1), y_s(:,t+1)] = evolSys(x_s(:,t), u_l(:,t+1), nQ, nR, noise); 
     200    loss = sum((x_s(3,:)-ref_ome).^2); 
    156201end 
    157  
    158 figure; 
    159 subplot(2,1,1); 
    160 plot(1:T,x_k(1,:),1:T,x_s(3,:),1:T,ref_ome); 
    161 subplot(2,1,2); 
    162 plot(1:T,atan2(sin(x_k(2,:)),cos(x_k(2,:))),1:T,atan2(sin(x_s(4,:)),cos(x_s(4,:)))); 

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

    r1435 r1436  
    1 % main - hlavni skript 
    2 clear all; 
    3 % oznaceni: s ... system 
    4 %           k ... kalman (EKF) 
    5 %           l ... rizeni (LQR) 
     1function [loss] = main_lq4(T, ref_profile, theta0, simulator, graf, inddq) 
     2    % main - hlavni skript >>>>>>>PLNY STAV<<<<<<< 
     3    % clear all; 
     4    % oznaceni: s ... system 
     5    %           k ... kalman (EKF) 
     6    %           l ... rizeni (LQR) 
    67 
    7 % KONSTANTY 
    8 T = 40000; %horizont 
    9 dt = 0.000125; %casovy krok 
     8    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
     9    %%%%%pouziti SIMULATORU 
     10%     simulator = 1; 
     11    % simulator = 0; 
    1012 
    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; 
     13    if((simulator == 1)||(simulator == 10)) 
     14        sim_param = pmsm_sim; 
     15    %     sim_param(9) = 0; %vypne dead-time 
     16        pmsm_sim(sim_param); 
     17    end 
     18    %%%%% 
     19    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
    1820 
    19 % Lq = 1.05*Ls; 
    20 % Ld = 0.95*Ls; 
     21    % KONSTANTY 
     22%     T = 120000; %horizont 
     23    % T = 40000; 
     24    dt = 0.000125; %casovy krok 
    2125 
    22 a = 0.9898; 
    23 b = 0.0072; 
    24 c = 0.0361; 
    25 d = 1.0; 
    26 e = 0.0149; 
     26    % Rs = 0.28; 
     27    % Ls = 0.003465; 
     28    % psipm = 0.1989; 
     29    % B = 0;     
     30    % kp = 1.5; 
     31    % pp = 4.0; 
     32    % J = 0.04; 
    2733 
    28 ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0, 0,-3, -6, -3];%/9*200; 
    29 % ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; 
     34    % Lq = 1.05*Ls; 
     35    % Ld = 0.95*Ls; 
    3036 
    31 %kovariance EKF na stavu, ktery vytvari hyperstav 
    32 % Q_k = diag([0.001, 0.00001]); 
    33 % R_k = diag([0.015, 0.015]); 
    34 Q_k = diag([0.01, 0.0001]); 
    35 R_k = diag([0.15, 0.15]); 
     37%     a = 0.9898; 
     38%     b = 0.0072; 
     39    c = 0.0361; 
     40%     d = 1.0; 
     41%     e = 0.0149; 
    3642 
    37 %kovariance EKF na hyperstavu 
    38 % Qh_k = diag([0.001, 0.00001, 0.00001, 0.00001, 0.00001]); 
    39 % Rh_k = diag([0.015, 0.015]); 
    40 Qh_k = diag([0.01, 0.0001, 10.1, 10.1, 10.1]); 
    41 Rh_k = diag([0.15, 0.15]); 
     43    % ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0, 0,-3, -6, -3];%/9*200; 
     44    % ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; 
     45%     ref_profile = [0, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 0]; 
     46    % ref_profile = [1, 10, 50, 200, 200, 30, 0, 0, -1, -10, -50, -200, -200, -30, 0]; 
     47    % ref_profile = [20, 20, 20, 50, 50, 50, -10, -10, -10, 0, 0, 0, 20, 20, 20]; 
    4248 
    43 %hodnoty sumu v systemu 
    44 nQ = diag([0.0013, 0.0013, 5.0e-6, 1.0e-10]); 
    45 nR = diag([0.0006, 0.0006]); 
     49    %kovariance EKF na stavu, ktery vytvari hyperstav 
     50    % Q_k = diag([0.001, 0.00001]); 
     51    % R_k = diag([0.015, 0.015]); 
     52    Q_k = diag([0.1, 0.1, 0.01, 0.0001]); 
     53    R_k = diag([0.05, 0.05]); 
    4654 
    47 iter_l = 10;% pocet iteraci ve vypoctu rizeni 
     55    %kovariance EKF na hyperstavu 
     56    % Qh_k = diag([0.001, 0.00001, 0.00001, 0.00001, 0.00001]); 
     57    % Rh_k = diag([0.015, 0.015]); 
     58    Qh_k = 0.1*eye(14); 
     59    Qh_k(1:4,1:4) = diag([0.1, 0.1, 0.01, 0.0001]); 
     60    Rh_k = diag([0.05, 0.05]); 
    4861 
    49 B_l = zeros(6,2); 
    50 % B_l(1,1) = c; 
    51 % B_l(2,2) = c; 
     62    %hodnoty sumu v systemu 
     63    nQ = diag([0.0013, 0.0013, 5.0e-6, 1.0e-10]); 
     64    nR = diag([0.0006, 0.0006]); 
    5265 
    53 %           o t Po Pot Pt 
    54 Q_l = diag([10 0 0.1 0.00001 0.1 0]); 
    55 % Q_l = diag([1 0 0 0 0 0]); 
    56 r = 0.0001; 
    57 R_l = diag([r, r]); 
     66    iter_l = 10;% pocet iteraci ve vypoctu rizeni 
     67 
     68    B_l = zeros(15,2); 
     69    B_l(1,1) = c; 
     70    B_l(2,2) = c; 
     71 
     72    Q_l = zeros(15); 
     73    Q_l(3,3) = 1; %ome 
     74    Q_l(10,10) = 1; %Var ome 
     75%     Q_l(14,14) = 1; %Var the 
     76    %           o t Po Pot Pt 
     77    % Q_l = diag([1 0 1 0 0 0]); % asi spravne z teoretickeho hlediska 
     78    % Q_l = diag([1 0 1 0 1 0]); 
     79    % Q_l = diag([1 0 0 0 0 0]); 
     80    r = 0.0001; 
     81    R_l = diag([r, r]); 
    5882 
    5983 
    6084 
    61 % PROMENNE 
    62 x_s = zeros(4,T); %stav 
    63 y_s = zeros(2,T); %mereni 
    64 x_k = zeros(5,T); %odhad hyperstavu 
    65 P_k = zeros(5); %kovariance hyperstavu 
    66 u_l = zeros(2,T); %rizeni 
    67 S_l = zeros(6); %jadro ztraty 
    68 pre_k = zeros(3,1); %predikce stavu 
     85    % PROMENNE 
     86    x_s = zeros(4,T); %stav 
     87    y_s = zeros(2,T); %mereni 
     88    x_k = zeros(14,T); %odhad hyperstavu 
     89%     P_k = zeros(14); %kovariance hyperstavu 
     90    u_l = zeros(2,T); %rizeni 
     91%     S_l = zeros(15); %jadro ztraty 
     92%     pre_k = zeros(10,1); %predikce stavu 
    6993 
    7094 
    71 % POCATECNI HODNOTY 
    72 noise = 1; %prepinac sumu 
    73 % noise = 0; 
     95    % POCATECNI HODNOTY 
     96    noise = 1; %prepinac sumu 
     97    % noise = 0; 
    7498 
    75 theta0 = 1.5;%1.7; %pocatecni poloha 
    76 Ps0 = eye(2); %odhad pocatecni kovariance stavu (apriorni) 
    77 Pk0 = eye(5); %pocatecni kovariance hyperstavu 
    78 ST = zeros(6); %koncova ztrata 
     99%     theta0 = 0;%1.5;%1.7; %pocatecni poloha 
     100    % Ps0 = eye(4); %odhad pocatecni kovariance stavu (apriorni) 
     101    Pk0 = eye(14); %pocatecni kovariance hyperstavu 
     102    ST = eye(15); %koncova ztrata 
    79103 
    80104 
    81 % INICIALIZACE 
    82 x_s(4,1) = theta0; 
    83 x_k(3,1) = Ps0(1,1); 
    84 x_k(4,1) = Ps0(1,2); 
    85 x_k(5,1) = Ps0(2,2); 
    86 P_k = Pk0; 
    87 S_l = ST; 
     105    % INICIALIZACE 
     106    x_k(4,1) = theta0; 
    88107 
    89 ref_ome = zeros(1, T);   
    90     for k = 1:T, 
    91            index = floor(k*dt); 
    92            if(index>0) 
    93                lower = ref_profile(index); 
    94            else 
    95                lower = 0; 
    96            end 
    97            if(index<T*dt) 
    98                upper = ref_profile(index+1); 
    99            else 
    100                upper = 0; 
    101            end 
    102            ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt); 
     108    %Ps0 = eye 
     109    x_k(5,1) = 1; 
     110    x_k(7,1) = 1; 
     111    x_k(11,1) = 1; 
     112    x_k(14,1) = 1; 
     113 
     114    P_k = Pk0; 
     115    S_l = ST; 
     116 
     117    ref_ome = zeros(1, T);   
     118        for k = 1:T, 
     119               index = floor(k*dt); 
     120               if(index>0) 
     121                   lower = ref_profile(index); 
     122               else 
     123                   lower = 0; 
     124               end 
     125               if(index<T*dt) 
     126                   upper = ref_profile(index+1); 
     127               else 
     128                   upper = 0; 
     129               end 
     130               ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt); 
     131        end 
     132 
     133    % Derivace pro prvni EKF  
     134    [A_k, C_k, pre_k, A_l] = assembDeriv4(x_k(:,1), u_l(:,1), x_k(:,1), Q_k, R_k, 0, inddq); 
     135 
     136    % HLAVNI SMYCKA 
     137    for t = 1:T-1, 
     138        % EKF          
     139        [x_k(:,t+1), P_k] = extKF4(x_k(:,t), y_s(:,t), u_l(:,t), pre_k, A_k, C_k, P_k, Qh_k, Rh_k); 
     140    %     x_k(1:4,t+1) = x_s(:,t); %cidlo 
     141 
     142        % Derivace 
     143        [A_k, C_k, pre_k, A_l] = assembDeriv4(x_k(:,t+1), u_l(:,t), x_k(:,t+1), Q_k, R_k, ref_ome(t), inddq); 
     144 
     145        % LQ  
     146        %!!! upraveno na QR verzi - tj. parametry jsou odmocniny 
     147        [u_l(:,t+1), S_l] = ctrlLQ4(x_k(:,t+1), ref_ome(t), A_l, B_l, sqrtm(ST), Q_l, sqrtm(R_l), iter_l); 
     148 
     149        if u_l(1,t+1) > 100 
     150            u_l(1,t+1) = 100; 
     151        elseif u_l(1,t+1) < -100 
     152            u_l(1,t+1) = -100; 
     153        end 
     154        if u_l(2,t+1) > 100 
     155            u_l(2,t+1) = 100; 
     156        elseif u_l(2,t+1) < -100 
     157            u_l(2,t+1) = -100; 
     158        end 
     159 
     160        % Vyvoj systemu 
     161        [x_s(:,t+1), y_s(:,t+1)] = evolSys(x_s(:,t), u_l(:,t+1), nQ, nR, noise, simulator); 
    103162    end 
    104163 
    105 % Derivace pro prvni EKF  
    106 [A_k, C_k, pre_k, A_l] = assembDeriv(x_k(:,1), y_s(:,1), x_k(:,1), Q_k, R_k, 0); 
    107    
    108 ri = 0.0001; 
    109 ai = (1-a*a)/c/c; 
    110 Si = (1 - ai*r + sqrt((ai*r-1)^2+4*r/c/c))/2; 
    111 Li = a*c*Si/(c*c*Si+ri); 
    112          
    113 % HLAVNI SMYCKA 
    114 for t = 1:T-1, 
    115     % EKF     
    116     [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); 
    117      
    118     % Derivace 
    119     [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)); 
    120      
    121     % LQ 
    122     B_l(1,1:2) = [-e*sin(x_k(2,t+1)), e*cos(x_k(2,t+1))]; 
    123     [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); 
    124     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); 
    125     if u_l(1,t+1) > 100 
    126         u_l(1,t+1) = 100; 
    127     elseif u_l(1,t+1) < -100 
    128         u_l(1,t+1) = -100; 
    129     end 
    130     if u_l(2,t+1) > 100 
    131         u_l(2,t+1) = 100; 
    132     elseif u_l(2,t+1) < -100 
    133         u_l(2,t+1) = -100; 
     164    if(graf == 1) 
     165        %vykresleni 
     166        cas = (1:T)*dt; 
     167        figure; 
     168        subplot(2,1,1); 
     169        plot(cas,x_k(3,:),cas,x_s(3,:),cas,ref_ome); 
     170        title('Prubeh otacek v case'); 
     171        xlabel('cas [s]'); 
     172        ylabel('otacky [rad/s]'); 
     173        legend('odhad','skutecne','pozadovane'); 
     174        subplot(2,1,2); 
     175        plot(cas,atan2(sin(x_k(4,:)),cos(x_k(4,:))),cas,atan2(sin(x_s(4,:)),cos(x_s(4,:)))); 
     176        title('Prubeh polohy v case'); 
     177        xlabel('cas [s]'); 
     178        ylabel('poloha [rad]'); 
     179 
     180        figure; 
     181        plot(cas,x_s(3,:)-ref_ome); 
     182        title('Prubeh chyby (skutecne - pozadovane otacky v case)'); 
     183        xlabel('cas [s]'); 
     184        ylabel('chyba [rad/s]'); 
    134185    end 
    135186     
    136     % Vyvoj systemu 
    137     [x_s(:,t+1), y_s(:,t+1)] = evolSys(x_s(:,t), u_l(:,t+1), nQ, nR, noise); 
     187    loss = sum((x_s(3,:)-ref_ome).^2); 
    138188end 
    139  
    140 figure; 
    141 subplot(2,1,1); 
    142 plot(1:T,x_k(1,:),1:T,x_s(3,:),1:T,ref_ome); 
    143 subplot(2,1,2); 
    144 plot(1:T,atan2(sin(x_k(2,:)),cos(x_k(2,:))),1:T,atan2(sin(x_s(4,:)),cos(x_s(4,:))));