| 1 | function [var_ome, var_th] = fpcrb2(model, control, amp, injom, Q, R, T, ref_profile, Q0) |
|---|
| 2 | %function for computing PCRB of PMSM model |
|---|
| 3 | |
|---|
| 4 | |
|---|
| 5 | % machine parameters |
|---|
| 6 | Rs = 0.28; |
|---|
| 7 | Ls = 0.003465; |
|---|
| 8 | psipm = 0.1989; |
|---|
| 9 | B = 0; |
|---|
| 10 | kp = 1.5; |
|---|
| 11 | pp = 4.0; |
|---|
| 12 | J = 0.04; |
|---|
| 13 | |
|---|
| 14 | dt = 0.000125; |
|---|
| 15 | |
|---|
| 16 | Lq = 1.0*Ls; |
|---|
| 17 | Ld = 0.9*Ls; |
|---|
| 18 | |
|---|
| 19 | %aliases |
|---|
| 20 | kpp = kp*pp*pp; |
|---|
| 21 | kppj = kpp/J; |
|---|
| 22 | psi = psipm; |
|---|
| 23 | |
|---|
| 24 | a = 0.9898; |
|---|
| 25 | b = 0.0072; |
|---|
| 26 | c = 0.0361; |
|---|
| 27 | d = 1.0; |
|---|
| 28 | e = 0.0149; |
|---|
| 29 | |
|---|
| 30 | %reference signal |
|---|
| 31 | ref_ome = zeros(1, T); |
|---|
| 32 | for k = 1:T, |
|---|
| 33 | index = floor(k*dt); |
|---|
| 34 | if(index>0) |
|---|
| 35 | lower = ref_profile(index); |
|---|
| 36 | else |
|---|
| 37 | lower = 0; |
|---|
| 38 | end |
|---|
| 39 | if(index<T*dt) |
|---|
| 40 | upper = ref_profile(index+1); |
|---|
| 41 | else |
|---|
| 42 | upper = 0; |
|---|
| 43 | end |
|---|
| 44 | ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt); |
|---|
| 45 | end |
|---|
| 46 | |
|---|
| 47 | |
|---|
| 48 | |
|---|
| 49 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
|---|
| 50 | % system variables |
|---|
| 51 | |
|---|
| 52 | %system state and control |
|---|
| 53 | x_sys = zeros(4, T); |
|---|
| 54 | u_dq = zeros(2, T); |
|---|
| 55 | |
|---|
| 56 | %covariance alternatives |
|---|
| 57 | Q4 = Q; |
|---|
| 58 | Q2 = Q4(3:4,3:4); |
|---|
| 59 | iQ4 = inv(Q4); |
|---|
| 60 | iQ2 = inv(Q2); |
|---|
| 61 | iR = inv(R); |
|---|
| 62 | |
|---|
| 63 | |
|---|
| 64 | %PCRB J matrices |
|---|
| 65 | if(model(1) == 1) |
|---|
| 66 | iJn1 = zeros(2,T); |
|---|
| 67 | Jj1 = inv(Q0); |
|---|
| 68 | end |
|---|
| 69 | if(model(2) == 1) |
|---|
| 70 | iJn2 = zeros(2,T); |
|---|
| 71 | Jj2 = inv(Q0); |
|---|
| 72 | end |
|---|
| 73 | if(model(3) == 1) |
|---|
| 74 | iJn3 = zeros(2,T); |
|---|
| 75 | Jj3 = inv(Q0(3:4,3:4)); |
|---|
| 76 | end |
|---|
| 77 | if(model(4) == 1) |
|---|
| 78 | iJn4 = zeros(2,T); |
|---|
| 79 | Jj4 = inv(Q0(3:4,3:4)); |
|---|
| 80 | end |
|---|
| 81 | |
|---|
| 82 | |
|---|
| 83 | |
|---|
| 84 | %PI control |
|---|
| 85 | sum_iq = 0; |
|---|
| 86 | sum_ud = 0; |
|---|
| 87 | sum_uq = 0; |
|---|
| 88 | kon_pi = 30.0; |
|---|
| 89 | kon_ii = 0.0; |
|---|
| 90 | kon_pu = 20.0; |
|---|
| 91 | kon_iu = 0.0; |
|---|
| 92 | |
|---|
| 93 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
|---|
| 94 | % init |
|---|
| 95 | |
|---|
| 96 | % measurement function derivatives |
|---|
| 97 | C4 = zeros(2,4); |
|---|
| 98 | C4(1,1) = 1.0; |
|---|
| 99 | C4(2,2) = 1.0; |
|---|
| 100 | |
|---|
| 101 | % system function derivatives |
|---|
| 102 | %alpha-beta, equal Ls |
|---|
| 103 | A14 = zeros(4); |
|---|
| 104 | A14(1,1) = a; |
|---|
| 105 | A14(2,2) = a; |
|---|
| 106 | A14(3,3) = d; |
|---|
| 107 | A14(4,3) = dt; |
|---|
| 108 | A14(4,4) = 1.0; |
|---|
| 109 | |
|---|
| 110 | % new figure |
|---|
| 111 | % figure; |
|---|
| 112 | |
|---|
| 113 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
|---|
| 114 | % main loop |
|---|
| 115 | |
|---|
| 116 | for t = 1:T-1, |
|---|
| 117 | %aliases |
|---|
| 118 | ial = x_sys(1, t); |
|---|
| 119 | ibe = x_sys(2, t); |
|---|
| 120 | ome = x_sys(3, t); |
|---|
| 121 | the = x_sys(4, t); |
|---|
| 122 | |
|---|
| 123 | ia = ial; |
|---|
| 124 | ib = ibe; |
|---|
| 125 | |
|---|
| 126 | id = ial*cos(the) + ibe*sin(the); |
|---|
| 127 | iq = ibe*cos(the) - ial*sin(the); |
|---|
| 128 | %PCRB models |
|---|
| 129 | if(model(1) == 1) %al-be/Ls |
|---|
| 130 | A14(1,3) = b*sin(the); |
|---|
| 131 | A14(1,4) = b*ome*cos(the); |
|---|
| 132 | A14(2,3) = -b*cos(the); |
|---|
| 133 | A14(2,4) = b*ome*sin(the); |
|---|
| 134 | A14(3,1) = -e*sin(the); |
|---|
| 135 | A14(3,2) = e*cos(the); |
|---|
| 136 | A14(3,4) = -e*(ial*cos(the)+ibe*sin(the)); |
|---|
| 137 | |
|---|
| 138 | D11 = A14'*iQ4*A14; |
|---|
| 139 | D12 = -A14'*iQ4; |
|---|
| 140 | D21 = D12'; |
|---|
| 141 | D22 = iQ4 + C4'*iR*C4; |
|---|
| 142 | |
|---|
| 143 | Jj1 = D22 - D21/(Jj1 + D11)*D12; |
|---|
| 144 | tmp = inv(Jj1); |
|---|
| 145 | iJn1(1,t) = tmp(3,3); |
|---|
| 146 | iJn1(2,t) = tmp(4,4); |
|---|
| 147 | end |
|---|
| 148 | if(model(2) == 1) %al-be/Ldq |
|---|
| 149 | A70 = [[ (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)];... |
|---|
| 150 | [ (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)];... |
|---|
| 151 | [ -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)];... |
|---|
| 152 | [ 0.0, 0.0, dt, 1.0]]; |
|---|
| 153 | |
|---|
| 154 | D11 = A70'*iQ4*A70; |
|---|
| 155 | D12 = -A70'*iQ4; |
|---|
| 156 | D21 = D12'; |
|---|
| 157 | D22 = iQ4 + C4'*iR*C4; |
|---|
| 158 | |
|---|
| 159 | Jj2 = D22 - D21/(Jj2 + D11)*D12; |
|---|
| 160 | tmp = inv(Jj2); |
|---|
| 161 | iJn2(1,t) = tmp(3,3); |
|---|
| 162 | iJn2(2,t) = tmp(4,4); |
|---|
| 163 | end |
|---|
| 164 | if(model(3) == 1) %reduced al-be/Ls |
|---|
| 165 | A3 = [d, -e*(ibe*sin(the)+ial*cos(the)); dt, 1]; |
|---|
| 166 | |
|---|
| 167 | D11 = A3'*iQ2*A3; |
|---|
| 168 | D12 = -A3'*iQ2; |
|---|
| 169 | D21 = D12'; |
|---|
| 170 | D22 = iQ2 + [b^2, 0; 0, b^2*(ome^2 + Q4(3,3))]*iR; |
|---|
| 171 | % D22 = iQ2 + [b^2, 0; 0, b^2*(ome^2)]*iR; |
|---|
| 172 | % tmp = inv(Jj3); |
|---|
| 173 | % D22 = iQ2 + [b^2, 0; 0, b^2*(ome^2 + tmp(1,1))]*iR; |
|---|
| 174 | % tady nahore to dela extremni rozdil |
|---|
| 175 | |
|---|
| 176 | |
|---|
| 177 | Jj3 = D22 - D21/(Jj3 + D11)*D12; |
|---|
| 178 | tmp = inv(Jj3); |
|---|
| 179 | iJn3(1,t) = tmp(1,1); |
|---|
| 180 | iJn3(2,t) = tmp(2,2); |
|---|
| 181 | end |
|---|
| 182 | if(model(4) == 1) %reduced al-be/Ldq |
|---|
| 183 | A3 = [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]; |
|---|
| 184 | |
|---|
| 185 | D11 = A3'*iQ2*A3; |
|---|
| 186 | D12 = -A3'*iQ2; |
|---|
| 187 | D21 = D12'; |
|---|
| 188 | D22 = iQ2 + [b^2, 0; 0, b^2*(ome^2 + Q4(3,3))]*iR; |
|---|
| 189 | % D22 = iQ2 + [b^2, 0; 0, b^2*(ome^2)]*iR; |
|---|
| 190 | % tmp = inv(Jj3); |
|---|
| 191 | % D22 = iQ2 + [b^2, 0; 0, b^2*(ome^2 + tmp(1,1))]*iR; |
|---|
| 192 | % tady nahore to dela extremni rozdil |
|---|
| 193 | |
|---|
| 194 | |
|---|
| 195 | Jj4 = D22 - D21/(Jj4 + D11)*D12; |
|---|
| 196 | tmp = inv(Jj4); |
|---|
| 197 | iJn4(1,t) = tmp(1,1); |
|---|
| 198 | iJn4(2,t) = tmp(2,2); |
|---|
| 199 | end |
|---|
| 200 | |
|---|
| 201 | %control & simulation |
|---|
| 202 | if(control == 1) %ome = ref_ome, the = integral(ome), ial=ibe=0 |
|---|
| 203 | x_sys(1, t+1) = 0; |
|---|
| 204 | x_sys(2, t+1) = 0; |
|---|
| 205 | x_sys(3, t+1) = ref_ome(t); |
|---|
| 206 | x_sys(4, t+1) = the + dt*ome; |
|---|
| 207 | else |
|---|
| 208 | if(control == 6) %rnd. err. ref_ome |
|---|
| 209 | ref_ome(t) = ref_ome(t) + amp*randn(); |
|---|
| 210 | end |
|---|
| 211 | |
|---|
| 212 | %PI (only PI for control == 2) |
|---|
| 213 | sum_iq = sum_iq + ref_ome(t) - ome; |
|---|
| 214 | ref_iq = kon_pi*(ref_ome(t) - ome) + kon_ii*sum_iq; |
|---|
| 215 | sum_ud = sum_ud - id; |
|---|
| 216 | u_dq(1, t) = kon_pu*(-id) + kon_iu*sum_ud; |
|---|
| 217 | sum_uq = sum_uq + ref_iq - iq; |
|---|
| 218 | u_dq(2, t) = kon_pu*(ref_iq - iq) + kon_iu*sum_uq; |
|---|
| 219 | u_dq(1, t) = u_dq(1, t) - Ls*ome*ref_iq; |
|---|
| 220 | u_dq(2, t) = u_dq(2, t) + psipm*ome; |
|---|
| 221 | |
|---|
| 222 | if(control == 3) %inj. sin -> dq |
|---|
| 223 | u_dq(1,t) = u_dq(1,t) + amp*sin(injom*dt*t); |
|---|
| 224 | u_dq(2,t) = u_dq(2,t) + amp*cos(injom*dt*t); |
|---|
| 225 | elseif(control == 4) %inj. rect. -> dq |
|---|
| 226 | u_dq(1,t) = u_dq(1,t) + amp*sign(sin(injom*dt*t)); |
|---|
| 227 | u_dq(2,t) = u_dq(2,t) + amp*sign(cos(injom*dt*t)); |
|---|
| 228 | elseif(control == 5) %inj. const. -> d |
|---|
| 229 | % u_dq(1,t) = 0.1*Ld/dt - (1.0*Ld/dt - Rs)*id - Lq*ome*iq; |
|---|
| 230 | u_dq(1,t) = amp; |
|---|
| 231 | elseif(control == 9) %bic. sign. ome |
|---|
| 232 | u_dq(1,t) = u_dq(1,t) - amp*sign(ome); |
|---|
| 233 | u_dq(2,t) = u_dq(2,t) + amp*sign(ome); |
|---|
| 234 | end |
|---|
| 235 | |
|---|
| 236 | ual = u_dq(1,t)*cos(the) - u_dq(2,t)*sin(the); |
|---|
| 237 | ube = u_dq(1,t)*sin(the) + u_dq(2,t)*cos(the); |
|---|
| 238 | |
|---|
| 239 | if(control == 7) %inj. sin -> al-be |
|---|
| 240 | ual = ual + amp*cos(injom*dt*t); |
|---|
| 241 | ube = ube + amp*sin(injom*dt*t); |
|---|
| 242 | elseif(control == 8) %inj. rect. -> al-be |
|---|
| 243 | ual = ual + amp*sign(cos(injom*dt*t)); |
|---|
| 244 | ube = ube + amp*sign(sin(injom*dt*t)); |
|---|
| 245 | elseif((control == 10)&&(rand() > (1/5))) %bic rnd. 5 |
|---|
| 246 | duab = sign(rand(2,1)-0.5)*amp; |
|---|
| 247 | ual = ual + duab(1); |
|---|
| 248 | ube = ube + duab(2); |
|---|
| 249 | end |
|---|
| 250 | |
|---|
| 251 | ud = ual*cos(the) + ube*sin(the); |
|---|
| 252 | uq = ube*cos(the) - ual*sin(the); |
|---|
| 253 | |
|---|
| 254 | %simulation model Ldq |
|---|
| 255 | idpl = (1.0 - Rs*dt/Ld)*id + Lq*dt/Ld*ome*iq + dt/Ld*ud; |
|---|
| 256 | iqpl = (1.0 - Rs*dt/Lq)*iq - psipm*dt/Lq*ome - Ld*dt/Lq*ome*id + dt/Lq*uq; |
|---|
| 257 | |
|---|
| 258 | x_sys(1, t+1) = idpl*cos(the) - iqpl*sin(the); |
|---|
| 259 | x_sys(2, t+1) = idpl*sin(the) + iqpl*cos(the); |
|---|
| 260 | x_sys(3, t+1) = (1.0-B*dt/J)*ome + kp*pp*pp*dt/J*((Ld-Lq)*id*iq + psipm*iq); |
|---|
| 261 | x_sys(4, t+1) = the + dt*ome; |
|---|
| 262 | end |
|---|
| 263 | |
|---|
| 264 | end |
|---|
| 265 | |
|---|
| 266 | ssz = sum(model); |
|---|
| 267 | var_ome = zeros(T,ssz); |
|---|
| 268 | var_th = zeros(T,ssz); |
|---|
| 269 | ind = 1; |
|---|
| 270 | if(model(1) == 1) |
|---|
| 271 | var_ome(:,ind) = iJn1(1,:); |
|---|
| 272 | var_th(:,ind) = iJn1(2,:); |
|---|
| 273 | ind = ind+1; |
|---|
| 274 | end |
|---|
| 275 | if(model(2) == 1) |
|---|
| 276 | var_ome(:,ind) = iJn2(1,:); |
|---|
| 277 | var_th(:,ind) = iJn2(2,:); |
|---|
| 278 | ind = ind+1; |
|---|
| 279 | end |
|---|
| 280 | if(model(3) == 1) |
|---|
| 281 | var_ome(:,ind) = iJn3(1,:); |
|---|
| 282 | var_th(:,ind) = iJn3(2,:); |
|---|
| 283 | ind = ind+1; |
|---|
| 284 | end |
|---|
| 285 | if(model(4) == 1) |
|---|
| 286 | var_ome(:,ind) = iJn4(1,:); |
|---|
| 287 | var_th(:,ind) = iJn4(2,:); |
|---|
| 288 | ind = ind+1; |
|---|
| 289 | end |
|---|
| 290 | end |
|---|
