Show
Ignore:
Timestamp:
03/04/12 19:28:08 (14 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/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,:))));