root/applications/dual/vahala/pcrb/pcrb.m @ 1434

Revision 1416, 24.6 kB (checked in by vahalam, 14 years ago)

pridani slozky pcrb s cramer-rao mezi

Line 
1%simulator PMSM pro PCRB
2% zatim zaver:
3%   v d-q vzdy roste neomezene
4%   v al-be se omezuje a roste jen pri nulovych otackach
5%   zda se, ze al-be not equal L vubec nepomuze
6%   model d-q mereni al-be termer nepomuze
7%   zadne bikriterialni vylepseni vicemene nemeni PCRB
8%   znacne snizeni PCRB je nasledkem drzeni konst. nenul. id, ale otazka,
9%       co kdyz budu drzet esimovane id...funguje i pro estimovane id a
10%       zlepsuje nejen PCRB ale i odhad kalmana; jen konstanta, zadny sin ani
11%       obdelniky, jedine hodne mala frekvence, tj. skoro konstanta
12%   maximova norma rozdilu DF pro al-be pri stejnych a ruznych
13%       indukcnostech je stejneho radu (popr. o rad nizsi) nez samotna PCRB
14%       pro theta, nicmene ale vetsi rozdil je pri vyssim omega, pri
15%       nizkych otackach je rozdil maly a tedy tam, kde mel pomoct nepomuze
16%       a matice jsou prakticky stejne
17%     
18%   zjistit otacky omega, ktere kdyz to drzi konst. tak je konst. i
19%       chyba: pro kazde nenulove otacky pri N rozdeleni to nakonec dojde
20%       ke konstantni chybe, ale muze byt neunosne vysoka, otazka tedy nema
21%       smysl, po dostatecne dlouhem case je pak chyba vzdy konstantni
22%
23%   dalsi zpusob, jak ziskat lepsi mez je pocitat model sc5, ale stejne
24%       nezastavuje rust chyby pri nulovych otackach - v realne aplikaci je
25%       treba pridat korekce pri opusteni intervalu <-1;1>, jinak to casem
26%       diverguje v dusledku diskretizace derivace; dale PCRB nabyvaji pro
27%       sc "temer" nuly, kdyz odpovidajici skutecna hodnota sin(the) resp.
28%       cos(the) je extremni, tj. +-1, takze hodnoty +-1 dokazi urcit s
29%       temer nulovou minimalni chybou, pro hodnoty 0 chyba (PCRB) odpovida
30%       hodnote PCRB theta pro model se 4 stavovymi velicinami (horsi tedy
31%       nejsem nikdy)
32% TODO:
33%           von Mises na theta
34clear all;
35%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
36%   parametry stroje
37%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
38Rs = 0.28;
39Ls = 0.003465;
40psipm = 0.1989;
41B = 0;
42TL = 0;
43kp = 1.5;
44pp = 4.0;
45J = 0.04;
46dt = 0.000125;
47
48kpp = kp*pp*pp;
49psi = psipm;
50
51a = 0.9898;
52b = 0.0072;
53c = 0.0361;
54d = 1.0;
55e = 0.0149;
56
57Lq = 1.0*Ls;
58Ld = 0.9*Ls;
59
60Q = diag([0.0013 0.0013 5.0e-6 1.0e-10]);
61R = diag([0.0006 0.0006]);
62
63
64T = 120000; %horizont
65
66ref_ome = zeros(1, T); %referencni signal
67% ref_profile = [1, 10, 50, 200, 200, 30, 0, 0, -1, -10, -50, -200, -200, -30, 0];
68ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0,0,-3, -6, -3];
69% ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
70% ref_profile = ones(1,16);
71
72for k = 1:T,
73       index = floor(k*dt);
74       if(index>0)
75           lower = ref_profile(index);
76       else
77           lower = 0;
78       end
79       if(index<T*dt)
80           upper = ref_profile(index+1);
81       else
82           upper = 0;
83       end
84       ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt);
85end
86
87
88
89%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
90%   promenne stavu systemu
91%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
92
93x_sys = zeros(4, T); %vnitrni stav systemu
94u_ab = zeros(2, T); %rizeni systemu v alfa-beta
95
96Q6 = zeros(5);
97Q6(1:4,1:4) = Q;
98Q6(5,5) = Q(4,4);
99
100iJn = zeros(4,4,T);
101iJn1 = zeros(4,4,T);
102iJn2 = zeros(4,4,T);
103iJn3 = zeros(4,4,T);
104iJn4 = zeros(4,4,T);
105iJn5 = zeros(4,4,T);
106iJn6 = zeros(5,5,T);
107iJn7 = zeros(5,5,T);
108
109Jj = inv(Q);
110Jj1 = inv(Q);
111Jj2 = inv(Q);
112Jj3 = inv(Q);
113Jj4 = inv(Q);
114Jj5 = inv(Q);
115Jj6 = inv(Q6);
116Jj7 = inv(Q6);
117
118
119%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
120%   promenne rizeni
121%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
122
123u_dq = zeros(2, T);
124
125%PI vektorove
126% kon_pi = 3.0;
127% kon_ii = 0.00375;
128% kon_pu = 20.0;
129% kon_iu = 0.05;
130sum_iq = 0;
131sum_ud = 0;
132sum_uq = 0;
133
134kon_pi = 30.0;
135kon_ii = 0.0;
136kon_pu = 20.0;
137kon_iu = 0.0;
138
139%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
140%   inicializace
141%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
142
143%alpha-beta, equal Ls
144DF = zeros(4);
145DF(1,1) = a;
146DF(2,2) = a;
147DF(3,3) = d;
148DF(4,3) = dt;
149DF(4,4) = 1.0;
150DH = zeros(2,4);
151DH(1,1) = 1.0;
152DH(2,2) = 1.0;
153
154DF1 = DF;
155
156%d-q red, equal Ls
157DF2 = zeros(4);
158DF2(1,1) = a;
159DF2(2,2) = a;
160DF2(2,3) = -b;
161DF2(3,2) = e;
162DF2(3,3) = d;
163DF2(4,3) = dt;
164DF2(4,4) = 1.0;
165DH2 = DH;
166%
167%d-q full, equal Ls
168DF3 = DF2;
169
170%d-q, not equal Ld, Lq
171DF4 = zeros(4);
172DF4(1,1) = 1.0 - Rs*dt/Ld;
173DF4(2,2) = 1.0 - Rs*dt/Lq;
174DF4(3,3) = 1-B*dt/J;
175DF4(4,3) = dt;
176DF4(4,4) = 1.0;
177   
178%alpha-beta, not equal Ld, Lq
179DF5 = zeros(4);
180
181%alpha-beta, equal Ls sc theta
182DF6 = zeros(5);
183DF6(1,1) = a;
184DF6(2,2) = a;
185DF6(3,3) = d;
186DF6(4,4) = 1.0;
187DF6(5,5) = 1.0;
188DH6 = zeros(2,5);
189DH6(1,1) = 1.0;
190DH6(2,2) = 1.0;
191
192q1 = Q6(1,1);
193q2 = Q6(2,2);
194q3 = Q6(3,3);
195q4 = Q6(4,4);
196q5 = Q6(5,5);
197opr_D11 = diag([e^2*q4/q3,...
198                e^2*q5/q3,...
199                b^2*q5/q2 + dt^2*q5/q4 + b^2*q4/q1 + dt^2*q4/q5,...
200                e^2*q1/q3 + b^2*q3/q1 + dt^2*q3/q5,...
201                e^2*q2/q3 + b^2*q3/q2 + dt^2*q3/q4]);
202           
203%%%%
204opr_3_D11 = diag([dt^2*q3/q2, dt^2*q3/q1, dt^2*q1/q2 + dt^2*q2/q1, 0.0]);
205opr_4_D11 = diag([Ld^2/Lq^2*dt^2*q3/q2 + dt^2*kp^2*pp^4*(Ld-Lq)^2*q2/J^2/q3,...
206                  Lq^2/Ld^2*dt^2*q3/q1 + dt^2*kp^2*pp^4*(Ld-Lq)^2*q1/J^2/q3,...
207                  Lq^2/Ld^2*dt^2*q2/q1 + Ld^2/Lq^2*dt^2*q1/q2, 0.0]);
208
209             
210% D11_a = zeros(4,4,T);
211% D11_b = zeros(4,4,T);   
212
213DF7 = DF6;
214             
215figure;
216% nrm = zeros(1,T);
217%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
218%   hlavni smycka
219%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
220
221for t = 1:T-1,
222       
223   
224    %%%%% rizeni %%%% (estxt -> ut)
225    ial = x_sys(1, t);
226    ibe = x_sys(2, t);
227    ome = x_sys(3, t);
228    the = x_sys(4, t); 
229   
230    id = ial*cos(the) + ibe*sin(the);
231    iq = ibe*cos(the) - ial*sin(the);
232   
233    %%%%%%%%%%%%%%%%%%%%%%%%%%%%
234   
235%     %alpha-beta, equal Ls
236%     DF(1,3) = b*sin(the);
237%     DF(1,4) = b*ome*cos(the);
238%     DF(2,3) = -b*cos(the);
239%     DF(2,4) = b*ome*sin(the);
240%     DF(3,1) = -e*sin(the);
241%     DF(3,2) = e*cos(the);
242%     DF(3,4) = -e*(ial*cos(the)+ibe*sin(the));
243%     
244%     D11 = DF'*inv(Q)*DF;
245%     D12 = -DF'*inv(Q);
246%     D21 = D12';
247%     D22 = inv(Q) + DH'*inv(R)*DH;
248%     
249%     Jj = D22 - D21*inv(Jj + D11)*D12;
250%         
251%     iJn(:,:,t) = inv(Jj);
252   
253%     D11_a(:,:,t) = D11;
254   
255%     %alpha-beta, equal Ls - oprava!!!!!       
256%     D11(1, 1) = a^2/q1 + e^2/q3*(1/2 - 1/2*exp(-2*q4)*cos(2*the));
257%     D11(1, 2) = -e^2/q3*sin(the)*cos(the)*exp(-2*q4);
258%     D11(1, 3) = (a*b/q1 - d*e/q3)*sin(the)*exp(-1/2*q4);
259%     D11(1, 4) = e^2/q3*(ial*sin(the)*cos(the)*exp(-2*q4) + ibe*(1/2 - 1/2*exp(-2*q4)*cos(2*the)))...
260%                 + a*b/q1*ome*cos(the)*exp(-1/2*q4);
261%     
262%     D11(2, 1) = D11(1, 2);
263%     D11(2, 2) = a^2/q2 + e^2/q3*(1/2 + 1/2*exp(-2*q4)*cos(2*the));
264%     D11(2, 3) = (d*e/q3 - a*b/q2)*cos(the)*exp(-1/2*q4);
265%     D11(2, 4) = a*b/q2*ome*sin(the)*exp(-1/2*q4)...
266%                 - e^2/q3*(ial*(1/2 + 1/2*exp(-2*q4)*cos(2*the)) + ibe*sin(the)*cos(the)*exp(-2*q4));
267%     
268%     D11(3, 1) = D11(1, 3);
269%     D11(3, 2) = D11(2, 3);
270%     D11(3, 3) = d^2/q3 + dt^2/q4 + b^2/q2*(1/2 + 1/2*exp(-2*q4)*cos(2*the))...
271%                 + b^2/q1*(1/2 - 1/2*exp(-2*q4)*cos(2*the));
272%     D11(3, 4) = dt/q4 - d*e/q3*(ial*cos(the)*exp(-1/2*q4) + ibe*sin(the)*exp(-1/2*q4))...
273%                 + b^2/q1*ome*sin(the)*cos(the)*exp(-2*q4) - b^2/q2*ome*sin(the)*cos(the)*exp(-2*q4);
274%     
275%     D11(4, 1) = D11(1, 4);
276%     D11(4, 2) = D11(2, 4);
277%     D11(4, 3) = D11(3, 4);
278%     D11(4, 4) = 1/q4 + e^2/q3*((q1 + ial^2)*(1/2 + 1/2*exp(-2*q4)*cos(2*the))...
279%                 + 2*ial*ibe*sin(the)*cos(the)*exp(-2*q4)...
280%                 + (q2 + ibe^2)*(1/2 - 1/2*exp(-2*q4)*cos(2*the)))...
281%                 + b^2/q2*(q3 + ome^2)*(1/2 - 1/2*exp(-2*q4)*cos(2*the))...
282%                 + b^2/q1*(q3 + ome^2)*(1/2 + 1/2*exp(-2*q4)*cos(2*the));
283%             
284%     DF1(1,1) = a;
285%     DF1(1,2) = 0.0;
286%     DF1(1,3) = b*sin(the)*exp(-1/2*q4);
287%     DF1(1,4) = b*ome*cos(the)*exp(-1/2*q4);
288%     
289%     DF1(2,1) = 0.0;
290%     DF1(2,2) = a;
291%     DF1(2,3) = -b*cos(the)*exp(-1/2*q4);
292%     DF1(2,4) = b*ome*sin(the)*exp(-1/2*q4);
293%     
294%     DF1(3,1) = -e*sin(the)*exp(-1/2*q4);
295%     DF1(3,2) = e*cos(the)*exp(-1/2*q4);
296%     DF1(3,3) = d;
297%     DF1(3,4) = -e*(ial*cos(the)*exp(-1/2*q4) + ibe*sin(the)*exp(-1/2*q4));
298%     
299%     DF1(4,1) = 0.0;
300%     DF1(4,2) = 0.0;
301%     DF1(4,3) = dt;
302%     DF1(4,4) = 1.0;
303%     
304%     D12 = -DF1'*inv(Q);
305%     D21 = D12';
306%     D22 = inv(Q) + DH'*inv(R)*DH;
307%     
308%     Jj1 = D22 - D21*inv(Jj1 + D11)*D12;
309%         
310%     iJn1(:,:,t) = inv(Jj1);
311   
312%     D11_b(:,:,t) = D11;
313   
314    %d-q red, equal Ls
315%     DH2(1,1) = cos(the);
316%     DH2(1,2) = -sin(the);
317%     DH2(1,4) = -id*sin(the)-iq*cos(the);
318%     DH2(2,1) = sin(the);
319%     DH2(2,2) = cos(the);
320%     DH2(2,4) = id*cos(the)-iq*sin(the);
321%     D11 = DF2'*inv(Q)*DF2;
322%     D12 = -DF2'*inv(Q);
323%     D21 = D12';
324% %     D22 = inv(Q) + DH'*inv(R)*DH;
325%     D22 = inv(Q) + DH2'*inv(R)*DH2;
326%     
327%     Jj2 = D22 - D21*inv(Jj2 + D11)*D12;
328%         
329%     iJn2(:,:,t) = inv(Jj2);
330   
331%     d-q full, equal Ls
332%     DF3(1,2) = dt*ome;
333%     DF3(1,3) = dt*iq;
334%     DF3(2,1) = -dt*ome;
335%     DF3(2,3) = -dt*id-b;
336%     D11 = DF3'*inv(Q)*DF3 + opr_3_D11;
337%     D12 = -DF3'*inv(Q);
338%     D21 = D12';
339%     D22 = inv(Q) + DH'*inv(R)*DH;
340% %     D22 = inv(Q) + DH2'*inv(R)*DH2;
341%     
342%     Jj3 = D22 - D21*inv(Jj3 + D11)*D12;
343%         
344%     iJn3(:,:,t) = inv(Jj3);
345   
346    %d-q, not equal Ld, Lq
347%     DF4(1,2) = Lq*dt/Ld*ome;
348%     DF4(1,3) = Lq*dt/Ld*iq;
349%     DF4(2,1) = -Ld*dt/Lq*ome;
350%     DF4(2,3) = -Ld*dt/Lq*id-psipm*dt/Lq;
351%     DF4(3,1) = kp*pp*pp*dt*(Ld-Lq)/J*iq;
352%     DF4(3,2) = kp*pp*pp*dt/J*((Ld-Lq)*id+psipm);   
353%     D11 = DF4'*inv(Q)*DF4 + opr_4_D11;
354%     D12 = -DF4'*inv(Q);
355%     D21 = D12';
356%     D22 = inv(Q) + DH'*inv(R)*DH;
357% %     D22 = inv(Q) + DH2'*inv(R)*DH2;
358%     
359%     Jj4 = D22 - D21*inv(Jj4 + D11)*D12;
360%         
361%     iJn4(:,:,t) = inv(Jj4);
362   
363    %alpha-beta, not equal Ld, Lq
364%     ia = ial;
365%     ib = ibe;
366%     psi = psipm;
367%     kppj = kp*pp*pp/J;
368%     DF5 = [[ (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)];...
369%             [                                       (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)];...
370%             [     -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)];...
371%             [                                                                                                                             0.0,                                                                                                                             0.0,                                                                                                                                                                        dt,                                                                                                                                                                                                                                                                                1.0]];
372%     D11 = DF5'*inv(Q)*DF5;
373%     D12 = -DF5'*inv(Q);
374%     D21 = D12';
375%     D22 = inv(Q) + DH'*inv(R)*DH;
376%     
377%     Jj5 = D22 - D21*inv(Jj5 + D11)*D12;
378%         
379%     iJn5(:,:,t) = inv(Jj5);
380   
381%     delma = abs(DF - DF5);
382%     nrm(t) = max(delma(:));
383   
384%     alpha-beta, equal Ls sc theta
385    DF6(1,3) = b*sin(the);
386    DF6(1,4) = b*ome;
387    DF6(2,3) = -b*cos(the);
388    DF6(2,5) = -b*ome;
389    DF6(3,1) = -e*sin(the);
390    DF6(3,2) = e*cos(the);
391    DF6(3,4) = -e*ial;
392    DF6(3,5) = e*ibe;
393    DF6(4,3) = dt*cos(the);
394    DF6(4,5) = dt*ome;
395    DF6(5,3) = -dt*sin(the);
396    DF6(5,4) = -dt*ome;
397       
398    D11 = DF6'*inv(Q6)*DF6;
399%     D11 = DF6'*inv(Q6)*DF6 + opr_D11;
400    D12 = -DF6'*inv(Q6);
401    D21 = D12';
402    D22 = inv(Q6) + DH6'*inv(R)*DH6;
403   
404    Jj6 = D22 - D21*inv(Jj6 + D11)*D12;
405       
406    iJn6(:,:,t) = inv(Jj6);
407
408   
409%     %     alpha-beta, not equal Ld Lq sc theta, bad E 
410%     st = sin(the);
411%     ct = cos(the);
412%     DF7(1, 1) = 1.0 + dt*(-Rs*(st^2/Lq + ct^2/Ld) + ome*st*ct*(Ld/Lq - Lq/Ld));
413%     DF7(1, 2) = dt*(-ome + Rs*st*ct*(1/Lq - 1/Ld) + ome*(Ld*st^2/Lq + Lq*ct^2/Ld));
414%     DF7(1, 3) = dt*psi*st/Lq + dt*ial*st*ct*(Ld/Lq - Lq/Ld) + dt*ibe*(-1 + Ld/Lq*st^2 + Lq/Ld*ct^2);
415%     DF7(1, 4) = dt*psi*ome/Lq + dt*ial*(-Rs*2*st/Lq + ome*ct*(Ld/Lq - Lq/Ld)) + dt*ibe*(Rs*ct*(1/Lq - 1/Ld) + ome*2*Ld*st/Lq);
416%     DF7(1, 5) = dt*ial*(-Rs*2*ct/Ld + ome*st*(Ld/Lq - Lq/Ld)) + dt*ibe*(Rs*st*(1/Lq - 1/Ld) + ome*2*Lq*ct/Ld);
417%
418%     DF7(2, 1) = dt*(ome + Rs*st*ct*(1/Lq - 1/Ld) - ome*(Lq*st^2/Ld + Ld*ct^2/Lq));
419%     DF7(2, 2) = 1 + dt*(-Rs*(st^2/Ld + ct^2/Lq) + ome*st*ct*(Lq/Ld - Ld/Lq));
420%     DF7(2, 3) = -dt*psi*ct/Lq + dt*ial*(1 - Lq*st^2/Ld - Ld*ct^2/Lq) + dt*ibe*st*ct*(Lq/Ld - Ld/Lq);
421%     DF7(2, 4) = dt*ial*(Rs*ct*(1/Lq - 1/Ld) - ome*2*Lq*st/Ld) + dt*ibe*(-Rs*2*st/Ld + ome*ct*(Lq/Ld - Ld/Lq));
422%     DF7(2, 5) = -dt*psi*ome/Lq + dt*ial*(Rs*st*(1/Lq - 1/Ld) -ome*2*Ld*ct/Lq) + dt*ibe*(-Rs*2*ct/Lq + ome*st*(Lq/Ld - Ld/Lq));
423%
424%     DF7(3, 1) = dt*kpp/J*((Ld - Lq)*(-2*ial*st*ct + ibe*ct^2 - ibe*st^2) - psi*st);
425%     DF7(3, 2) = dt*kpp/J*((Ld - Lq)*(ial*ct^2 - ial*st^2 + 2*ibe*st*ct) + psi*ct);
426%     DF7(3, 3) = 1;
427%     DF7(3, 4) = dt*kpp/J*((Ld - Lq)*(-ial^2*ct - ial*ibe*2*st + ibe^2*ct) - psi*ial);
428%     DF7(3, 5) = dt*kpp/J*((Ld - Lq)*(-ial^2*st + ial*ibe*2*ct + ibe^2*st) + psi*ibe);
429%     
430%     DF7(4,3) = dt*cos(the);
431%     DF7(4,5) = dt*ome;
432%     DF7(5,3) = -dt*sin(the);
433%     DF7(5,4) = -dt*ome;
434%         
435% %     D11(1,1) = (dt*(Rs*(ct^2/Ld + st^2/Lq) - ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)^2/q1 + (dt^2*(ome*((Ld*ct^2)/Lq + (Lq*st^2)/Ld) - ome + Rs*ct*st*(1/Ld - 1/Lq))^2)/q2 + (dt^2*kpp^2*(psi*st + (Ld - Lq)*(- ibe*ct^2 + 2*ial*ct*st + ibe*st^2))^2)/(J^2*q3);
436% %     D11(1,2) = (dt*(dt*(Rs*(ct^2/Ld + st^2/Lq) - ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)*(ome - ome*((Lq*ct^2)/Ld + (Ld*st^2)/Lq) + Rs*ct*st*(1/Ld - 1/Lq)))/q1 + (dt*(dt*(Rs*(ct^2/Lq + st^2/Ld) + ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)*(ome*((Ld*ct^2)/Lq + (Lq*st^2)/Ld) - ome + Rs*ct*st*(1/Ld - 1/Lq)))/q2 - (dt^2*kpp^2*(ct*psi + (Ld - Lq)*(ial*ct^2 + 2*ibe*ct*st - ial*st^2))*(psi*st + (Ld - Lq)*(- ibe*ct^2 + 2*ial*ct*st + ibe*st^2)))/(J^2*q3);
437% %     D11(1,3) = (dt*(ome*((Ld*ct^2)/Lq + (Lq*st^2)/Ld) - ome + Rs*ct*st*(1/Ld - 1/Lq))*(dt*ial*((Ld*ct^2)/Lq + (Lq*st^2)/Ld - 1) + (ct*dt*psi)/Lq + ct*dt*ibe*st*(Ld/Lq - Lq/Ld)))/q2 - ((dt*(Rs*(ct^2/Ld + st^2/Lq) - ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)*(dt*ibe*((Lq*ct^2)/Ld + (Ld*st^2)/Lq - 1) + (dt*psi*st)/Lq + ct*dt*ial*st*(Ld/Lq - Lq/Ld)))/q1 - (dt*kpp*(psi*st + (Ld - Lq)*(- ibe*ct^2 + 2*ial*ct*st + ibe*st^2)))/(J*q3);
438% %     D11(1,4) = (dt*(dt*ibe*(ct*ome*(Ld/Lq - Lq/Ld) + (2*Rs*st)/Ld) + dt*ial*(Rs*ct*(1/Ld - 1/Lq) + (2*Lq*ome*st)/Ld))*(ome*((Ld*ct^2)/Lq + (Lq*st^2)/Ld) - ome + Rs*ct*st*(1/Ld - 1/Lq)))/q2 - ((dt*(Rs*(ct^2/Ld + st^2/Lq) - ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)*(dt*ial*(ct*ome*(Ld/Lq - Lq/Ld) - (2*Rs*st)/Lq) - dt*ibe*(Rs*ct*(1/Ld - 1/Lq) - (2*Ld*ome*st)/Lq) + (dt*ome*psi)/Lq))/q1 + (dt^2*kpp^2*(ial*psi + (Ld - Lq)*(ct*ial^2 + 2*st*ial*ibe - ct*ibe^2))*(psi*st + (Ld - Lq)*(- ibe*ct^2 + 2*ial*ct*st + ibe*st^2)))/(J^2*q3);
439% %     D11(1,5) = (dt*(ome*((Ld*ct^2)/Lq + (Lq*st^2)/Ld) - ome + Rs*ct*st*(1/Ld - 1/Lq))*(dt*ibe*(ome*st*(Ld/Lq - Lq/Ld) + (2*Rs*ct)/Lq) + dt*ial*(Rs*st*(1/Ld - 1/Lq) + (2*Ld*ct*ome)/Lq) + (dt*ome*psi)/Lq))/q2 - ((dt*ial*(ome*st*(Ld/Lq - Lq/Ld) - (2*Rs*ct)/Ld) - dt*ibe*(Rs*st*(1/Ld - 1/Lq) - (2*Lq*ct*ome)/Ld))*(dt*(Rs*(ct^2/Ld + st^2/Lq) - ct*ome*st*(Ld/Lq - Lq/Ld)) - 1))/q1 - (dt^2*kpp^2*(ibe*psi + (Ld - Lq)*(- st*ial^2 + 2*ct*ial*ibe + st*ibe^2))*(psi*st + (Ld - Lq)*(- ibe*ct^2 + 2*ial*ct*st + ibe*st^2)))/(J^2*q3);
440% %     
441% %     D11(2,1) = D11(1,2);
442% %     D11(2,2) = (dt*(Rs*(ct^2/Lq + st^2/Ld) + ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)^2/q2 + (dt^2*(ome - ome*((Lq*ct^2)/Ld + (Ld*st^2)/Lq) + Rs*ct*st*(1/Ld - 1/Lq))^2)/q1 + (dt^2*kpp^2*(ct*psi + (Ld - Lq)*(ial*ct^2 + 2*ibe*ct*st - ial*st^2))^2)/(J^2*q3);
443% %     D11(2,3) = ((dt*(Rs*(ct^2/Lq + st^2/Ld) + ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)*(dt*ial*((Ld*ct^2)/Lq + (Lq*st^2)/Ld - 1) + (ct*dt*psi)/Lq + ct*dt*ibe*st*(Ld/Lq - Lq/Ld)))/q2 - (dt*(dt*ibe*((Lq*ct^2)/Ld + (Ld*st^2)/Lq - 1) + (dt*psi*st)/Lq + ct*dt*ial*st*(Ld/Lq - Lq/Ld))*(ome - ome*((Lq*ct^2)/Ld + (Ld*st^2)/Lq) + Rs*ct*st*(1/Ld - 1/Lq)))/q1 + (dt*kpp*(ct*psi + (Ld - Lq)*(ial*ct^2 + 2*ibe*ct*st - ial*st^2)))/(J*q3);
444% %     D11(2,4) = ((dt*ibe*(ct*ome*(Ld/Lq - Lq/Ld) + (2*Rs*st)/Ld) + dt*ial*(Rs*ct*(1/Ld - 1/Lq) + (2*Lq*ome*st)/Ld))*(dt*(Rs*(ct^2/Lq + st^2/Ld) + ct*ome*st*(Ld/Lq - Lq/Ld)) - 1))/q2 - (dt*(dt*ial*(ct*ome*(Ld/Lq - Lq/Ld) - (2*Rs*st)/Lq) - dt*ibe*(Rs*ct*(1/Ld - 1/Lq) - (2*Ld*ome*st)/Lq) + (dt*ome*psi)/Lq)*(ome - ome*((Lq*ct^2)/Ld + (Ld*st^2)/Lq) + Rs*ct*st*(1/Ld - 1/Lq)))/q1 - (dt^2*kpp^2*(ial*psi + (Ld - Lq)*(ct*ial^2 + 2*st*ial*ibe - ct*ibe^2))*(ct*psi + (Ld - Lq)*(ial*ct^2 + 2*ibe*ct*st - ial*st^2)))/(J^2*q3);
445% %     D11(2,5) = ((dt*(Rs*(ct^2/Lq + st^2/Ld) + ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)*(dt*ibe*(ome*st*(Ld/Lq - Lq/Ld) + (2*Rs*ct)/Lq) + dt*ial*(Rs*st*(1/Ld - 1/Lq) + (2*Ld*ct*ome)/Lq) + (dt*ome*psi)/Lq))/q2 - (dt*(dt*ial*(ome*st*(Ld/Lq - Lq/Ld) - (2*Rs*ct)/Ld) - dt*ibe*(Rs*st*(1/Ld - 1/Lq) - (2*Lq*ct*ome)/Ld))*(ome - ome*((Lq*ct^2)/Ld + (Ld*st^2)/Lq) + Rs*ct*st*(1/Ld - 1/Lq)))/q1 + (dt^2*kpp^2*(ct*psi + (Ld - Lq)*(ial*ct^2 + 2*ibe*ct*st - ial*st^2))*(ibe*psi + (Ld - Lq)*(- st*ial^2 + 2*ct*ial*ibe + st*ibe^2)))/(J^2*q3);
446% %     
447% %     D11(3,1) = D11(1,3);
448% %     D11(3,2) = D11(2,3);
449% %     D11(3,3) = (dt*ial*((Ld*ct^2)/Lq + (Lq*st^2)/Ld - 1) + (ct*dt*psi)/Lq + ct*dt*ibe*st*(Ld/Lq - Lq/Ld))^2/q2 + (dt*ibe*((Lq*ct^2)/Ld + (Ld*st^2)/Lq - 1) + (dt*psi*st)/Lq + ct*dt*ial*st*(Ld/Lq - Lq/Ld))^2/q1 + 1/q3 + (ct^2*dt^2)/q4 + (dt^2*st^2)/q5;
450% %     D11(3,4) = ((dt*ial*(ct*ome*(Ld/Lq - Lq/Ld) - (2*Rs*st)/Lq) - dt*ibe*(Rs*ct*(1/Ld - 1/Lq) - (2*Ld*ome*st)/Lq) + (dt*ome*psi)/Lq)*(dt*ibe*((Lq*ct^2)/Ld + (Ld*st^2)/Lq - 1) + (dt*psi*st)/Lq + ct*dt*ial*st*(Ld/Lq - Lq/Ld)))/q1 + ((dt*ibe*(ct*ome*(Ld/Lq - Lq/Ld) + (2*Rs*st)/Ld) + dt*ial*(Rs*ct*(1/Ld - 1/Lq) + (2*Lq*ome*st)/Ld))*(dt*ial*((Ld*ct^2)/Lq + (Lq*st^2)/Ld - 1) + (ct*dt*psi)/Lq + ct*dt*ibe*st*(Ld/Lq - Lq/Ld)))/q2 + (ct*dt)/q4 + (dt^2*ome*st)/q5 - (dt*kpp*(ial*psi + (Ld - Lq)*(ct*ial^2 + 2*st*ial*ibe - ct*ibe^2)))/(J*q3);
451% %     D11(3,5) = ((dt*ial*(ome*st*(Ld/Lq - Lq/Ld) - (2*Rs*ct)/Ld) - dt*ibe*(Rs*st*(1/Ld - 1/Lq) - (2*Lq*ct*ome)/Ld))*(dt*ibe*((Lq*ct^2)/Ld + (Ld*st^2)/Lq - 1) + (dt*psi*st)/Lq + ct*dt*ial*st*(Ld/Lq - Lq/Ld)))/q1 - (dt*st)/q5 + ((dt*ibe*(ome*st*(Ld/Lq - Lq/Ld) + (2*Rs*ct)/Lq) + dt*ial*(Rs*st*(1/Ld - 1/Lq) + (2*Ld*ct*ome)/Lq) + (dt*ome*psi)/Lq)*(dt*ial*((Ld*ct^2)/Lq + (Lq*st^2)/Ld - 1) + (ct*dt*psi)/Lq + ct*dt*ibe*st*(Ld/Lq - Lq/Ld)))/q2 + (ct*dt^2*ome)/q4 + (dt*kpp*(ibe*psi + (Ld - Lq)*(- st*ial^2 + 2*ct*ial*ibe + st*ibe^2)))/(J*q3);
452% %     
453% %     D11(4,1) = D11(1,4);
454% %     D11(4,2) = D11(2,4);
455% %     D11(4,3) = D11(3,4);
456% %     D11(4,4) = (dt*ibe*(ct*ome*(Ld/Lq - Lq/Ld) + (2*Rs*st)/Ld) + dt*ial*(Rs*ct*(1/Ld - 1/Lq) + (2*Lq*ome*st)/Ld))^2/q2 + 1/q4 + (dt*ial*(ct*ome*(Ld/Lq - Lq/Ld) - (2*Rs*st)/Lq) - dt*ibe*(Rs*ct*(1/Ld - 1/Lq) - (2*Ld*ome*st)/Lq) + (dt*ome*psi)/Lq)^2/q1 + (dt^2*ome^2)/q5 + (dt^2*kpp^2*(ial*psi + (Ld - Lq)*(ct*ial^2 + 2*st*ial*ibe - ct*ibe^2))^2)/(J^2*q3);
457% %     D11(4,5) = (dt*ome)/q4 - (dt*ome)/q5 + ((dt*ial*(ome*st*(Ld/Lq - Lq/Ld) - (2*Rs*ct)/Ld) - dt*ibe*(Rs*st*(1/Ld - 1/Lq) - (2*Lq*ct*ome)/Ld))*(dt*ial*(ct*ome*(Ld/Lq - Lq/Ld) - (2*Rs*st)/Lq) - dt*ibe*(Rs*ct*(1/Ld - 1/Lq) - (2*Ld*ome*st)/Lq) + (dt*ome*psi)/Lq))/q1 + ((dt*ibe*(ct*ome*(Ld/Lq - Lq/Ld) + (2*Rs*st)/Ld) + dt*ial*(Rs*ct*(1/Ld - 1/Lq) + (2*Lq*ome*st)/Ld))*(dt*ibe*(ome*st*(Ld/Lq - Lq/Ld) + (2*Rs*ct)/Lq) + dt*ial*(Rs*st*(1/Ld - 1/Lq) + (2*Ld*ct*ome)/Lq) + (dt*ome*psi)/Lq))/q2 - (dt^2*kpp^2*(ial*psi + (Ld - Lq)*(ct*ial^2 + 2*st*ial*ibe - ct*ibe^2))*(ibe*psi + (Ld - Lq)*(- st*ial^2 + 2*ct*ial*ibe + st*ibe^2)))/(J^2*q3);
458% %     
459% %     D11(5,1) = D11(1,5);
460% %     D11(5,2) = D11(2,5);
461% %     D11(5,3) = D11(3,5);
462% %     D11(5,4) = D11(4,5);
463% %     D11(5,5) = (dt*ial*(ome*st*(Ld/Lq - Lq/Ld) - (2*Rs*ct)/Ld) - dt*ibe*(Rs*st*(1/Ld - 1/Lq) - (2*Lq*ct*ome)/Ld))^2/q1 + 1/q5 + (dt*ibe*(ome*st*(Ld/Lq - Lq/Ld) + (2*Rs*ct)/Lq) + dt*ial*(Rs*st*(1/Ld - 1/Lq) + (2*Ld*ct*ome)/Lq) + (dt*ome*psi)/Lq)^2/q2 + (dt^2*ome^2)/q4 + (dt^2*kpp^2*(ibe*psi + (Ld - Lq)*(- st*ial^2 + 2*ct*ial*ibe + st*ibe^2))^2)/(J^2*q3);   
464%     
465%     D11 = DF7'*inv(Q6)*DF7;
466%
467%     D12 = -DF7'*inv(Q6);
468%     D21 = D12';
469%     D22 = inv(Q6) + DH6'*inv(R)*DH6;
470%     
471%     Jj7 = D22 - D21*inv(Jj7 + D11)*D12;
472%         
473%     iJn7(:,:,t) = inv(Jj7);
474   
475    %%%%%%%%%%%%%%%%%%%%%%%%%%%       
476               
477       
478        sum_iq = sum_iq + ref_ome(t) - ome;
479        ref_iq = kon_pi*(ref_ome(t) - ome) + kon_ii*sum_iq;
480        sum_ud = sum_ud - id;
481        u_dq(1, t) = kon_pu*(-id) + kon_iu*sum_ud;
482        sum_uq = sum_uq + ref_iq - iq;
483        u_dq(2, t) = kon_pu*(ref_iq - iq) + kon_iu*sum_uq;
484        u_dq(1, t) = u_dq(1, t) - Ls*ome*ref_iq;
485        u_dq(2, t) = u_dq(2, t) + psipm*ome;       
486               
487       
488%         u_dq(2,t) = u_dq(2,t) + 10.0*cos(100*dt*t);   
489       
490%         ual = u_dq(1,t)*cos(the) - u_dq(2,t)*sin(the);
491%         ube = u_dq(1,t)*sin(the) + u_dq(2,t)*cos(the);
492%
493% %         ual = ual + 10.0*cos(100*dt*t);
494% %         ube = ube + 10.0*sin(100*dt*t);
495%
496% %         duab = sign(rand(2,1)-0.5)*10;
497% %         ual = ual + duab(1);
498% %         ube = ube + duab(2);
499%         
500%         ud = ual*cos(the) + ube*sin(the);
501%         uq = ube*cos(the) - ual*sin(the);
502   
503%     u_dq(1,t) = 0.1*Ld/dt - (1.0*Ld/dt - Rs)*id - Lq*ome*iq;
504   
505    %%%% simulace %%% (xt + ut -> xt+1; xt+1 -> yt+1)
506    idpl = (1.0 - Rs*dt/Ld)*id + Lq*dt/Ld*ome*iq + dt/Ld*u_dq(1,t);
507    iqpl = (1.0 - Rs*dt/Lq)*iq - psipm*dt/Lq*ome - Ld*dt/Lq*ome*id + dt/Lq*u_dq(2,t);
508   
509    x_sys(1, t+1) = idpl*cos(the) - iqpl*sin(the);
510    x_sys(2, t+1) = idpl*sin(the) + iqpl*cos(the);
511   
512    x_sys(3, t+1) = ref_ome(t);%(1.0-B*dt/J)*ome + kp*pp*pp*dt/J*((Ld-Lq)*id*iq + psipm*iq);
513    x_sys(4, t+1) = the + dt*ome;
514       
515end
516
517%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
518%   zaznam vysledku
519%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
520
521xax = 1:T-1;
522timex = (xax)*dt;
523
524% subplot(2, 2, 1);
525% plot(timex, x_sys(1, xax));
526% subplot(2, 2, 2);
527% plot(timex, x_sys(2, xax));
528% subplot(2, 2, 3);
529% plot(timex, x_sys(3, xax), timex, ref_ome(xax));
530% subplot(2, 2, 4);
531% plot(timex, atan2(sin(x_sys(4, xax)),cos(x_sys(4, xax))));
532
533% subplot(2,1,1);
534% plot(timex, x_sys(3, xax), timex, ref_ome(xax));
535% subplot(2,1,2);
536%  plot(timex,nrm(xax),'g')
537% hold on;
538% plot(timex, squeeze(iJn(4,4,xax)),timex, squeeze(iJn1(4,4,xax)),timex, squeeze(iJn6(4,4,xax)),timex, squeeze(iJn6(5,5,xax)));
539plot(timex, squeeze(iJn6(4,4,xax)),timex, squeeze(iJn6(5,5,xax)),timex, squeeze(iJn7(4,4,xax)),timex, squeeze(iJn7(5,5,xax)));
540
541% figure;
542% plot(timex, x_sys(3, xax)-ref_ome(xax));
Note: See TracBrowser for help on using the browser.