root/applications/dual/vahala/kim/basic_main_lq4.m @ 1444

Revision 1436, 10.0 kB (checked in by vahalam, 13 years ago)

pridani a uprava lqg s hyperstavem viz clanek Kim2006

Line 
1function [loss] = basic_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)
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);
17    end
18    %%%%%
19    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
20
21    % KONSTANTY
22%     T = 120000; %horizont
23    % T = 40000;
24    dt = 0.000125; %casovy krok
25
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;
33
34    % Lq = 1.05*Ls;
35    % Ld = 0.95*Ls;
36
37    a = 0.9898;
38    b = 0.0072;
39    c = 0.0361;
40    d = 1.0;
41    e = 0.0149;
42       
43    Rs = 0.28;
44    Ls = 0.003465;
45    psi = 0.1989;
46    B = 0;   
47    kp = 1.5;
48    pp = 4.0;
49    J = 0.04;
50    Lq = 1.0*Ls;
51    Ld = 0.9*Ls;
52    kpp = kp*pp*pp;
53    kppj = kpp/J;
54
55    % ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0, 0,-3, -6, -3];%/9*200;
56    % ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
57%     ref_profile = [0, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 0];
58    % ref_profile = [1, 10, 50, 200, 200, 30, 0, 0, -1, -10, -50, -200, -200, -30, 0];
59    % ref_profile = [20, 20, 20, 50, 50, 50, -10, -10, -10, 0, 0, 0, 20, 20, 20];
60
61    %kovariance EKF na stavu, ktery vytvari hyperstav
62%     Q_k = diag([0.01, 0.01, 0.001, 0.00001]);
63%     R_k = diag([0.005, 0.005]);
64    Q_k = diag([0.1, 0.1, 0.01, 0.0001]);
65    R_k = diag([0.05, 0.05]);
66   
67    %hodnoty sumu v systemu
68    nQ = diag([0.0013, 0.0013, 5.0e-6, 1.0e-10]);
69    nR = diag([0.0006, 0.0006]);
70
71    iter_l = 10;% pocet iteraci ve vypoctu rizeni
72
73    B_l = zeros(5,2);
74    B_l(1,1) = c;
75    B_l(2,2) = c;
76
77    Q_l = zeros(5);
78    Q_l(3,3) = 1; %ome
79%     Q_l(10,10) = 1; %Var ome
80%     Q_l(14,14) = 1; %Var the
81    %           o t Po Pot Pt
82    % Q_l = diag([1 0 1 0 0 0]); % asi spravne z teoretickeho hlediska
83    % Q_l = diag([1 0 1 0 1 0]);
84    % Q_l = diag([1 0 0 0 0 0]);
85    r = 0.0001;
86    R_l = diag([r, r]);
87
88
89
90    % PROMENNE
91    x_s = zeros(4,T); %stav
92    y_s = zeros(2,T); %mereni
93    x_k = zeros(4,T); %odhad hyperstavu
94%     P_k = zeros(14); %kovariance hyperstavu
95    u_l = zeros(2,T); %rizeni
96%     S_l = zeros(15); %jadro ztraty
97%     pre_k = zeros(10,1); %predikce stavu
98
99
100    % POCATECNI HODNOTY
101    noise = 1; %prepinac sumu
102    % noise = 0;
103
104%     theta0 = 0;%1.5;%1.7; %pocatecni poloha
105    % Ps0 = eye(4); %odhad pocatecni kovariance stavu (apriorni)
106    Pk0 = eye(4); %pocatecni kovariance stavu
107    ST = eye(5);
108   
109
110
111    % INICIALIZACE
112    x_k(4,1) = theta0;
113
114    P_k = Pk0;   
115
116    ref_ome = zeros(1, T); 
117        for k = 1:T,
118               index = floor(k*dt);
119               if(index>0)
120                   lower = ref_profile(index);
121               else
122                   lower = 0;
123               end
124               if(index<T*dt)
125                   upper = ref_profile(index+1);
126               else
127                   upper = 0;
128               end
129               ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt);
130        end
131
132    % Derivace pro prvni EKF
133    ia = x_k(1,1);
134    ib = x_k(2,1);
135    ome = x_k(3,1);
136    the = x_k(4,1);
137    A_k = [a, 0, b*sin(the), b*ome*cos(the);...
138         0, a, -b*cos(the), b*ome*sin(the);...
139         -e*sin(the), e*cos(the), d, -e*(ib*sin(the)+ia*cos(the));...
140         0, 0, dt, 1.0];
141    C_k = [1, 0, 0, 0;...
142         0, 1, 0, 0];
143   
144     %pro LQ - QR rozklad rizeni
145    S_l = ST;
146    sqQ_l = Q_l;%sqrtm(Q_l);
147    sqR_l = sqrtm(R_l);
148
149    % HLAVNI SMYCKA
150    for t = 1:T-1,
151        % EKF           
152        Pp = A_k*P_k*A_k' + Q_k;
153        S = C_k*Pp*C_k' + R_k;
154        K = Pp*C_k'/S;
155        P_k = Pp - K*C_k*Pp;   
156
157        xp = zeros(4,1);
158        xp(1) = a*x_k(1,t) + b*x_k(3,t)*sin(x_k(4,t)) + c*u_l(1,t);
159        xp(2) = a*x_k(2,t) - b*x_k(3,t)*cos(x_k(4,t)) + c*u_l(2,t);
160        xp(3) = d*x_k(3,t) + e*(x_k(2,t)*cos(x_k(4,t)) - x_k(1,t)*sin(x_k(4,t)));
161        xp(4) = x_k(4,t) + dt*x_k(3,t);
162       
163        x_k(:,t+1) = xp + K*(y_s(:,t) - xp(1:2));
164   
165
166        % Derivace       
167        ia = x_k(1,t+1);
168        ib = x_k(2,t+1);
169        ome = x_k(3,t+1);
170        the = x_k(4,t+1);
171            %stejne indukcnosti
172        if(inddq == 0)
173            A_k = [a, 0, b*sin(the), b*ome*cos(the);...
174                   0, a, -b*cos(the), b*ome*sin(the);...
175                   -e*sin(the), e*cos(the), d, -e*(ib*sin(the)+ia*cos(the));...
176                   0, 0, dt, 1.0];
177            %ruzne indukcnosti
178        else
179            A_k = [[ (Lq - Rs*dt*sin(the)^2)/Lq - (dt*ome*sin(the)*Lq^2*cos(the) + Rs*dt*Lq*cos(the)^2)/(Ld*Lq) + (Ld*dt*ome*cos(the)*sin(the))/Lq,                                       (dt*(Ld - Lq)*(- Lq*ome*cos(the)^2 + Rs*cos(the)*sin(the) + Ld*ome*sin(the)^2))/(Ld*Lq),    dt*cos(the)*(ia*sin(the) - ib*cos(the) + (Lq*(ib*cos(the) - ia*sin(the)))/Ld) + dt*sin(the)*(psi/Lq - ia*cos(the) - ib*sin(the) + (Ld*(ia*cos(the) + ib*sin(the)))/Lq),                                               (dt*(ome*psi*cos(the) + Rs*ib*cos(2*the) - Rs*ia*sin(2*the)))/Lq + (Ld*dt*(ia*ome*cos(2*the) + ib*ome*sin(2*the)))/Lq - (dt*(Lq^2*ia*ome*cos(2*the) + Lq^2*ib*ome*sin(2*the) + Lq*Rs*ib*cos(2*the) - Lq*Rs*ia*sin(2*the)))/(Ld*Lq)];...
180                    [                                       (dt*(Ld - Lq)*(- Ld*ome*cos(the)^2 + Rs*cos(the)*sin(the) + Lq*ome*sin(the)^2))/(Ld*Lq), (Lq - Rs*dt*cos(the)^2)/Lq - (Lq*Rs*dt*sin(the)^2 - Lq^2*dt*ome*cos(the)*sin(the))/(Ld*Lq) - (Ld*dt*ome*cos(the)*sin(the))/Lq, (dt*(Lq*ia - psi*cos(the)))/Lq + (dt*((Lq^2*ia*cos(2*the))/2 - (Lq^2*ia)/2 + (Lq^2*ib*sin(2*the))/2))/(Ld*Lq) - (Ld*dt*(ia/2 + (ia*cos(2*the))/2 + (ib*sin(2*the))/2))/Lq, (dt*ome*psi*sin(the) - Rs*dt*ia*(2*sin(the)^2 - 1) + Rs*dt*ib*sin(2*the))/Lq + (Ld*(dt*ib*ome*(2*sin(the)^2 - 1) + dt*ia*ome*sin(2*the)))/Lq - (Lq*Rs*dt*ib*sin(2*the) + Lq^2*dt*ib*ome*(2*sin(the)^2 - 1) + Lq^2*dt*ia*ome*sin(2*the) - Lq*Rs*dt*ia*(2*sin(the)^2 - 1))/(Ld*Lq)];...
181                    [     -dt*kppj*(psi*sin(the) - cos(the)*(Ld - Lq)*(ib*cos(the) - ia*sin(the)) + sin(the)*(Ld - Lq)*(ia*cos(the) + ib*sin(the))),      dt*kppj*(psi*cos(the) + cos(the)*(Ld - Lq)*(ia*cos(the) + ib*sin(the)) + sin(the)*(Ld - Lq)*(ib*cos(the) - ia*sin(the))),                                                                                                                                                                         1.0,                                                                                                                                                   -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)];...
182                    [                                                                                                                             0.0,                                                                                                                             0.0,                                                                                                                                                                        dt,                                                                                                                                                                                                                                                                                1.0]];
183        end
184        %korekce nechavam stejne, ale muze to delat problemy
185        A_l = zeros(5);
186        A_l(1:4,1:4) = A_k;
187        A_l(1,5) = b*sin(the)*ref_ome(t) - b*ome*the*cos(the);
188        A_l(2,5) = -b*cos(the)*ref_ome(t) - b*ome*the*sin(the);
189        A_l(3,5) = (d - 1)*ref_ome(t) + e*the*(ia*cos(the)+ib*sin(the));
190        A_l(4,5) = dt*ref_ome(t);       
191        A_l(5,5) = 1;
192       
193        % LQ
194        %1 - Riccati
195%         S_l = ST;
196%         for i = 1:iter_l
197%            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;
198%         end
199%         L = (B_l'*S_l*B_l + R_l)\B_l'*S_l*A_l;
200       
201%         2 - QR rozklad
202        S_l = sqrtm(ST);
203        for i = 1:iter_l
204           preQR = [sqQ_l*B_l, sqQ_l*A_l;...
205                    sqR_l, zeros(2,5);...
206                    S_l*B_l, S_l*A_l];
207           [~, postR] = qr(preQR);
208           AA = postR(1:2,1:2);
209           BB = postR(1:2,3:end);
210           S_l = postR(3:7,3:end);
211        end
212        L = AA\BB;       
213% %         sum(L(:))
214        %%%%
215       
216        y = x_k(:,t+1);
217        y(3) = y(3) - ref_ome(t);
218        u_l(:,t+1) = -L*[y;1];
219
220        if u_l(1,t+1) > 100
221            u_l(1,t+1) = 100;
222        elseif u_l(1,t+1) < -100
223            u_l(1,t+1) = -100;
224        end
225        if u_l(2,t+1) > 100
226            u_l(2,t+1) = 100;
227        elseif u_l(2,t+1) < -100
228            u_l(2,t+1) = -100;
229        end
230
231        % Vyvoj systemu
232        [x_s(:,t+1), y_s(:,t+1)] = evolSys(x_s(:,t), u_l(:,t+1), nQ, nR, noise, simulator);       
233    end
234
235    if(graf == 1)
236        %vykresleni
237        cas = (1:T)*dt;
238        figure;
239        subplot(2,1,1);
240        plot(cas,x_k(3,:),cas,x_s(3,:),cas,ref_ome);
241        title('Prubeh otacek v case');
242        xlabel('cas [s]');
243        ylabel('otacky [rad/s]');
244        legend('odhad','skutecne','pozadovane');
245        subplot(2,1,2);
246        plot(cas,atan2(sin(x_k(4,:)),cos(x_k(4,:))),cas,atan2(sin(x_s(4,:)),cos(x_s(4,:))));
247        title('Prubeh polohy v case');
248        xlabel('cas [s]');
249        ylabel('poloha [rad]');
250       
251%         figure;
252%         subplot(2,1,1);
253%         plot(cas,x_k(1,:),cas,x_s(1,:));       
254%         subplot(2,1,2);
255%         plot(cas,x_k(2,:),cas,x_s(2,:));       
256
257        figure;
258        plot(cas,x_s(3,:)-ref_ome);
259        title('Prubeh chyby (skutecne - pozadovane otacky v case)');
260        xlabel('cas [s]');
261        ylabel('chyba [rad/s]');
262    end
263   
264    loss = sum((x_s(3,:)-ref_ome).^2);
265end
Note: See TracBrowser for help on using the browser.