root/applications/dual/vahala/pcrb/pcrb_e.m @ 1458

Revision 1416, 11.9 kB (checked in by vahalam, 13 years ago)

pridani slozky pcrb s cramer-rao mezi

Line 
1%simulator PMSM pro PCRB s lepsi ocekavanou hodnotou E
2% zatim zaver:
3%   
4clear all;
5%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
6%   parametry stroje
7%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
8Rs = 0.28;
9Ls = 0.003465;
10psipm = 0.1989;
11B = 0;
12TL = 0;
13kp = 1.5;
14pp = 4.0;
15J = 0.04;
16dt = 0.000125;
17
18a = 0.9898;
19b = 0.0072;
20c = 0.0361;
21d = 1.0;
22e = 0.0149;
23
24Lq = 1.0*Ls;
25Ld = 0.9*Ls;
26
27Q = diag([0.0013 0.0013 5.0e-6 1.0e-10]);
28R = diag([0.0006 0.0006]);
29
30
31T = 120000; %horizont
32
33ref_ome = zeros(1, T); %referencni signal
34% ref_profile = [1, 10, 50, 200, 200, 30, 0, 0, -1, -10, -50, -200, -200, -30, 0];
35ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0,0,-3, -6, -3];
36% ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
37
38for k = 1:T,
39       index = floor(k*dt);
40       if(index>0)
41           lower = ref_profile(index);
42       else
43           lower = 0;
44       end
45       if(index<T*dt)
46           upper = ref_profile(index+1);
47       else
48           upper = 0;
49       end
50       ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt);
51end
52
53
54
55%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
56%   promenne stavu systemu
57%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
58
59x_sys = zeros(4, T); %vnitrni stav systemu
60u_ab = zeros(2, T); %rizeni systemu v alfa-beta
61
62iJn = zeros(4,4,T);
63iJn2 = zeros(4,4,T);
64iJn3 = zeros(4,4,T);
65iJn4 = zeros(4,4,T);
66iJn5 = zeros(4,4,T);
67Jj = inv(Q);
68Jj2 = inv(Q);
69Jj3 = inv(Q);
70Jj4 = inv(Q);
71Jj5 = inv(Q);
72
73%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
74%   promenne rizeni
75%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
76
77u_dq = zeros(2, T);
78
79%PI vektorove
80% kon_pi = 3.0;
81% kon_ii = 0.00375;
82% kon_pu = 20.0;
83% kon_iu = 0.05;
84sum_iq = 0;
85sum_ud = 0;
86sum_uq = 0;
87
88kon_pi = 30.0;
89kon_ii = 0.0;
90kon_pu = 20.0;
91kon_iu = 0.0;
92
93%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
94%   inicializace
95%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
96
97%alpha-beta, equal Ls
98DF = zeros(4);
99DF(1,1) = a;
100DF(2,2) = a;
101DF(3,3) = d;
102DF(4,3) = dt;
103DF(4,4) = 1.0;
104DH = zeros(2,4);
105DH(1,1) = 1.0;
106DH(2,2) = 1.0;
107
108%d-q red, equal Ls
109DF2 = zeros(4);
110DF2(1,1) = a;
111DF2(2,2) = a;
112DF2(2,3) = -b;
113DF2(3,2) = e;
114DF2(3,3) = d;
115DF2(4,3) = dt;
116DF2(4,4) = 1.0;
117DH2 = DH;
118
119%d-q full, equal Ls
120DF3 = DF2;
121
122%d-q, not equal Ld, Lq
123DF4 = zeros(4);
124DF4(1,1) = 1.0 - Rs*dt/Ld;
125DF4(2,2) = 1.0 - Rs*dt/Lq;
126DF4(3,3) = 1-B*dt/J;
127DF4(4,3) = dt;
128DF4(4,4) = 1.0;
129   
130%alpha-beta, not equal Ld, Lq
131DF5 = zeros(4);
132
133% figure;
134
135%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
136%   hlavni smycka
137%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
138
139for t = 1:T-1,
140       
141   
142    %%%%% rizeni %%%% (estxt -> ut)
143    ial = x_sys(1, t);
144    ibe = x_sys(2, t);
145    ome = x_sys(3, t);
146    the = x_sys(4, t); 
147   
148    id = ial*cos(the) + ibe*sin(the);
149    iq = ibe*cos(the) - ial*sin(the);
150   
151    %%%%%%%%%%%%%%%%%%%%%%%%%%%%
152   
153    %alpha-beta, equal Ls
154%     DF(1,3) = b*sin(the);
155%     DF(1,4) = b*ome*cos(the);
156%     DF(2,3) = -b*cos(the);
157%     DF(2,4) = b*ome*sin(the);
158%     DF(3,1) = -e*sin(the);
159%     DF(3,2) = e*cos(the);
160%     DF(3,4) = -e*(ial*cos(the)+ibe*sin(the));
161%     
162%     D11 = DF'*inv(Q)*DF;
163%     D12 = -DF'*inv(Q);
164%     D21 = D12';
165%     D22 = inv(Q) + DH'*inv(R)*DH;
166%     
167%     Jj = D22 - D21*inv(Jj + D11)*D12;
168%         
169%     iJn(:,:,t) = inv(Jj);
170   
171    %d-q red, equal Ls ... DF-OK
172%     DH2(1,1) = cos(the);
173%     DH2(1,2) = -sin(the);
174%     DH2(1,4) = -id*ome*sin(the)-iq*ome*cos(the);
175%     DH2(2,1) = sin(the);
176%     DH2(2,2) = cos(the);
177%     DH2(2,4) = id*ome*cos(the)-iq*ome*sin(the);
178%     D11 = DF2'*inv(Q)*DF2;
179%     D12 = -DF2'*inv(Q);
180%     D21 = D12';
181% %     D22 = inv(Q) + DH'*inv(R)*DH;
182%     D22 = inv(Q) + DH2'*inv(R)*DH2;
183%     
184%     Jj2 = D22 - D21*inv(Jj2 + D11)*D12;
185%         
186%     iJn2(:,:,t) = inv(Jj2);
187   
188    %d-q full, equal Ls
189%     DF3(1,2) = dt*ome;
190%     DF3(1,3) = dt*iq;
191%     DF3(2,1) = -dt*ome;
192%     DF3(2,3) = -dt*id-b;
193%     D11 = DF3'*inv(Q)*DF3; !!!!!!!!!!!!!!!!!BAD
194    iQ = inv(Q);
195    q11 = iQ(1,1);
196    q12 = iQ(1,2);
197    q13  = iQ(1,3);
198    q14  = iQ(1,4);
199    q21  = iQ(2,1);
200    q22  = iQ(2,2);
201    q23  = iQ(2,3);
202    q24  = iQ(2,4);
203    q31  = iQ(3,1);
204    q32  = iQ(3,2);
205    q33  = iQ(3,3);
206    q34  = iQ(3,4);
207    q41  = iQ(4,1);
208    q42  = iQ(4,2);
209    q43  = iQ(4,3);
210    q44 = iQ(4,4);
211    om = ome;
212    D11 = [[a*(a*iQ(1,1) - dt*om*iQ(2,1)) - dt*om*a*iQ(1,2) + dt^2*Q(3,3)*iQ(2,2), a*(a*iQ(1,2) - dt*om*iQ(2,2)) + e*(a*iQ(1,3) - dt*om*iQ(2,3)) + dt*om*a*iQ(1,1) - dt^2*Q(3,3)*iQ(2,1), d*(a*iQ(1,3) - dt*om*iQ(2,3)) - (a*iQ(1,2) - dt*om*iQ(2,2))*b - a*iQ(1,2)*dt*id - dt^2*Q(1,3)*iQ(2,2) + dt*(a*q14 - dt*om*q24) + dt*iq*a*q11 - dt^2*Q(2,3)*q21, a*q14 - dt*om*q24];...
213            [ a*(a*q21 + e*q31 + dt*om*q11) - dt*om*(a*q22 + e*q32) - dt^2*Q(3,3)*q12, a*(a*q22 + e*q32 + dt*om*q12) + e*(a*q23 + e*q33 + dt*om*q13) + dt*om*(a*q21 + e*q31) + dt^2*Q(3,3)*q11, d*(a*q23 + e*q33 + dt*om*q13) + dt*(a*q24 + e*q34 + dt*om*q14) - b*(a*q22 + e*q32 + dt*om*q12) - dt*id*(a*q22 + e*q32) - dt^2*Q(1,3)*q12 + dt*iq*(a*q21 + e*q31) + dt^2*Q(2,3)*q11,a*q24 + e*q34 + dt*om*q14];...
214            [ a*(d*q31 + dt*q41 - q21*(b + dt*id) + dt*iq*q11) - dt*om*(d*q32 + dt*q42 - q22*b) + dt*q22*(dt*Q(1,3)) - dt^2*Q(2,3)*q12, a*(d*q32 + dt*q42 - q22*(b + dt*id) + dt*iq*q12) + e*(d*q33 + dt*q43 - q23*(b + dt*id) + dt*iq*q13) + dt*om*(d*q31 + dt*q41 - q21*b) - q21*dt^2*Q(1,3) + dt^2*Q(2,3)*q11, d*(d*q33 + dt*q43 - q23*(b + dt*id) + dt*iq*q13) - (b + dt*id)*(d*q32 + dt*q42 - q22*b) + b*(q22*dt*id - dt*iq*q12) + dt*(q22*dt*Q(1,1) - dt*Q(1,2)*q12) + dt*(d*q34 + dt*q44 - q24*(b + dt*id) + dt*iq*q14) + dt*iq*(d*q31 + dt*q41 - q21*b) + dt*(- q21*dt*Q(1,2) + dt*Q(2,2)*q11), d*q34 + dt*q44 - q24*(b + dt*id) + dt*iq*q14];...
215            [ a*q41 - dt*om*q42,  a*q42 + e*q43 + dt*om*q41,  d*q43 + dt*q44 - q42*(b + dt*id) + dt*iq*q41,   q44]];
216 
217    D12 = -DF3'*iQ;
218    D21 = D12';
219% %     D22 = inv(Q) + DH'*inv(R)*DH;
220    D22 = inv(Q) + DH2'*inv(R)*DH2;
221   
222    Jj3 = D22 - D21*inv(Jj3 + D11)*D12;
223       
224    iJn3(:,:,t) = inv(Jj3);
225   
226    %d-q, not equal Ld, Lq
227%     DF4(1,2) = Lq*dt/Ld*ome;
228%     DF4(1,3) = Lq*dt/Ld*iq;
229%     DF4(2,1) = -Ld*dt/Lq*ome;
230%     DF4(2,3) = -Ld*dt/Lq*id-psipm*dt/Lq;
231%     DF4(3,1) = kp*pp*pp*dt*(Ld-Lq)/J*iq;
232%     DF4(3,2) = kp*pp*pp*dt/J*((Ld-Lq)*id+psipm);   
233%     D11 = DF4'*inv(Q)*DF4;
234%     D12 = -DF4'*inv(Q);
235%     D21 = D12';
236%     D22 = inv(Q) + DH'*inv(R)*DH;
237% %     D22 = inv(Q) + DH2'*inv(R)*DH2;
238%     
239%     Jj4 = D22 - D21*inv(Jj4 + D11)*D12;
240%         
241%     iJn4(:,:,t) = inv(Jj4);
242   
243    %alpha-beta, not equal Ld, Lq
244%     ia = ial;
245%     ib = ibe;
246%     psi = psipm;
247%     kppj = kp*pp*pp/J;
248%     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)];...
249%             [                                       (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)];...
250%             [     -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)];...
251%             [                                                                                                                             0.0,                                                                                                                             0.0,                                                                                                                                                                        dt,                                                                                                                                                                                                                                                                                1.0]];
252%     D11 = DF5'*inv(Q)*DF5;
253%     D12 = -DF5'*inv(Q);
254%     D21 = D12';
255%     D22 = inv(Q) + DH'*inv(R)*DH;
256%     
257%     Jj5 = D22 - D21*inv(Jj5 + D11)*D12;
258%         
259%     iJn5(:,:,t) = inv(Jj5);
260   
261   
262    %%%%%%%%%%%%%%%%%%%%%%%%%%%       
263               
264       
265        sum_iq = sum_iq + ref_ome(t) - ome;
266        ref_iq = kon_pi*(ref_ome(t) - ome) + kon_ii*sum_iq;
267        sum_ud = sum_ud - id;
268        u_dq(1, t) = kon_pu*(-id) + kon_iu*sum_ud;
269        sum_uq = sum_uq + ref_iq - iq;
270        u_dq(2, t) = kon_pu*(ref_iq - iq) + kon_iu*sum_uq;
271        u_dq(1, t) = u_dq(1, t) - Ls*ome*ref_iq;
272        u_dq(2, t) = u_dq(2, t) + psipm*ome;       
273               
274       
275%         u_dq(2,t) = u_dq(2,t) + 10.0*cos(100*dt*t);   
276       
277%         ual = u_dq(1,t)*cos(the) - u_dq(2,t)*sin(the);
278%         ube = u_dq(1,t)*sin(the) + u_dq(2,t)*cos(the);
279%
280% %         ual = ual + 10.0*cos(100*dt*t);
281% %         ube = ube + 10.0*sin(100*dt*t);
282%
283% %         duab = sign(rand(2,1)-0.5)*10;
284% %         ual = ual + duab(1);
285% %         ube = ube + duab(2);
286%         
287%         ud = ual*cos(the) + ube*sin(the);
288%         uq = ube*cos(the) - ual*sin(the);
289   
290%     u_dq(1,t) = 0.1*Ld/dt - (1.0*Ld/dt - Rs)*id - Lq*ome*iq;
291   
292    %%%% simulace %%% (xt + ut -> xt+1; xt+1 -> yt+1)
293    idpl = (1.0 - Rs*dt/Ld)*id + Lq*dt/Ld*ome*iq + dt/Ld*u_dq(1,t);
294    iqpl = (1.0 - Rs*dt/Lq)*iq - psipm*dt/Lq*ome - Ld*dt/Lq*ome*id + dt/Lq*u_dq(2,t);
295   
296    x_sys(1, t+1) = idpl*cos(the) - iqpl*sin(the);
297    x_sys(2, t+1) = idpl*sin(the) + iqpl*cos(the);
298   
299    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);
300    x_sys(4, t+1) = the + dt*ome;
301       
302end
303
304%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
305%   zaznam vysledku
306%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
307
308xax = 1:T-1;
309timex = (xax)*dt;
310
311% subplot(2, 2, 1);
312% plot(timex, x_sys(1, xax));
313% subplot(2, 2, 2);
314% plot(timex, x_sys(2, xax));
315% subplot(2, 2, 3);
316% plot(timex, x_sys(3, xax), timex, ref_ome(xax));
317% subplot(2, 2, 4);
318% plot(timex, atan2(sin(x_sys(4, xax)),cos(x_sys(4, xax))));
319
320% subplot(2,1,1);
321% plot(timex, x_sys(3, xax), timex, ref_ome(xax));
322% subplot(2,1,2);
323hold on;
324plot(timex, squeeze(iJn3(4,4,xax)),'r');%,timex, squeeze(iJn5(4,4,xax)),timex, squeeze(iJn4(4,4,xax)));
325
326% figure;
327% plot(timex, x_sys(3, xax)-ref_ome(xax));
Note: See TracBrowser for help on using the browser.