Changeset 866
- Timestamp:
- 03/16/10 08:24:30 (15 years ago)
- Files:
-
- 1 modified
-
applications/dual/IterativeLocal/pmsm_ildp3.m (modified) (26 diffs)
Legend:
- Unmodified
- Added
- Removed
-
applications/dual/IterativeLocal/pmsm_ildp3.m
r846 r866 7 7 %pocatecni konstanty 8 8 9 Iterace = 4; %iterace10 K = 20; %casy9 Iterace = 5; %iterace 10 K = 50; %casy 11 11 N = 50; %vzorky 12 12 … … 32 32 % cUb = 50; 33 33 34 %presnost mereni proudu 35 deltaI = 0.085; 36 34 37 %matice 35 38 %kovariancni matice Q a R … … 37 40 mR = diag([0.0006 0.0006]); 38 41 39 mSigma = mR*mR';40 41 42 %matice pro vypocet 42 43 %matice A zavisla na parametrech … … 52 53 53 54 %pozadovana hodnota otacek 54 omega_t_stripe = 1.0015; 55 omega_t_stripe = 1.0015; 56 57 %penalizace za vstupy 58 cPenPsi = 0;%0.000009; 55 59 56 60 %pocatecni hodnoty … … 60 64 61 65 h_bel = 0; 62 h_beldx = [0; 0; 0; 0; 0; 0; 0; 0]; 66 h_beldx = [0; 0; 0; 0; 0; 0]; 67 h_beldxdx = zeros(4); 63 68 64 69 %velikost okoli pro lokalni metodu 65 rho = 0.1; 66 70 % rhoi = 0.0001; 71 % rhoo = 0.00015; 72 % rhot = 0.00005; 73 % rhop = 0.0001; 74 rhoi = 1.5; 75 rhoo = 1.5; 76 rhot = 1.5; 77 rhop = 1.5; 78 79 %zvetseni hamiltonianu pro minimalizace 80 % mag = 1000; 81 mag = 5; 82 83 %prepinac sumu on/off 84 noise = 1; 67 85 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 68 86 %globalni promenne 69 87 70 Kpi_alfa = ones(9, K); %konstanty aproximace slozky rizeni u_alfa 71 Kpi_alfa(1, :) = (Cd - Cb*Ce)*ones(1, K); 72 Kpi_alfa(2, :) = Ca*Ce*ones(1, K); 73 Kpi_alfa(3, :) = Ca*Ce*ones(1, K); 74 Kpi_alfa(4, :) = Cc*Ce*ones(1, K); 75 76 Kpi_beta = ones(9, K); %konstanty aproximace slozky rizeni u_beta 77 Kpi_beta(1, :) = (Cd - Cb*Ce)*ones(1, K); 78 Kpi_beta(2, :) = Ca*Ce*ones(1, K); 79 Kpi_beta(3, :) = Ca*Ce*ones(1, K); 80 Kpi_beta(4, :) = Cc*Ce*ones(1, K); 88 Kpi_alfa = -0.2*ones(1, K); %konstanty aproximace slozky rizeni u_alfa 89 Kpi_beta = 0.01*ones(1, K); %konstanty aproximace slozky rizeni u_beta 90 % Kpi_alfa = ones(1, K); %konstanty aproximace slozky rizeni u_alfa 91 % Kpi_beta = ones(1, K); %konstanty aproximace slozky rizeni u_beta 92 % Kpi_alfa = zeros(4, K); %konstanty aproximace slozky rizeni u_alfa 93 % Kpi_alfa(1, :) = 1000*Cc*Ce*ones(1, K); 94 % Kpi_alfa(2, :) = 1000*Cc*Ce*ones(1, K); 95 % 96 % Kpi_beta = zeros(4, K); %konstanty aproximace slozky rizeni u_beta 97 % Kpi_beta(1, :) = Cc*Ce*ones(1, K); 98 % Kpi_beta(2, :) = Cc*Ce*ones(1, K); 99 Kpi = ones(9, K); 81 100 82 Wv = zeros(35, K); %konstanty aproximace Bellmanovy fce 101 102 Wv = zeros(25, K); %konstanty aproximace Bellmanovy fce 83 103 84 104 Xkn = zeros(4, K, N); % X = [i_alfa, i_beta, omega, theta] … … 90 110 91 111 Xstripe = zeros(4, K); 92 Ystripe = zeros(4, K);93 112 Pstripe = zeros(4, 4, K); 94 113 95 Epsilon = zeros( 20, N); %globalni promena pro vypocet Bellmanovy fce z odchylek (X - Xprum)114 Epsilon = zeros(6, N); %globalni promena pro vypocet Bellmanovy fce z odchylek (X - Xprum) 96 115 97 116 gka = 0; %globalni promenna pro prenos casu k … … 103 122 for i = 1:Iterace, 104 123 105 disp('Iterace: '); 106 i 124 disp(['Iterace: ', num2str(i)]); 107 125 %generovani stavu 108 126 for n = 1:N, 109 127 Xkn(:, 1, n) = X0; 110 Ykn(:, 1, n) = Y0 + mR * [randn(); randn()];128 Ykn(:, 1, n) = Y0; 111 129 Pkn(:, :, 1, n) = P0; 112 130 … … 116 134 mRy = mC * Pkn(:, :, k, n) * mC' + mR; 117 135 mKy = Pkn(:, :, k, n) * mC' / mRy; 118 Xkn(:, k+1, n) = fceG(Xkn(:, k, n), Uk) - mKy * (Ykn(:, k, n) - fceH(Xkn(:, k, n))) ;119 Ykn(:, k+1, n) = Xkn(1:2, k+1, n) + mR * [randn(); randn()]; %X kopie do Y + sum136 Xkn(:, k+1, n) = fceG(Xkn(:, k, n), Uk) - mKy * (Ykn(:, k, n) - fceH(Xkn(:, k, n))) + noise * sqrtm(mQ) * randn(4, 1);%+gauss sum s rozptylem odmocnina mQ 137 Ykn(:, k+1, n) = round(Xkn(1:2, k+1, n) / deltaI) * deltaI; %X kopie do Y se vzorkovanim 0.085 120 138 mA(1, 3) = Cb * sin(Xkn(4, k, n)); 121 139 mA(1, 4) = Cb * Xkn(3, k, n) * cos(Xkn(4, k, n)); … … 125 143 mA(3, 2) = Ce * cos(Xkn(4, k, n)); 126 144 mA(3, 4) = - Ce * (Xkn(2, k, n) * sin(Xkn(4, k, n)) + Xkn(1, k, n) * cos(Xkn(4, k, n))); 127 Pkn(:, :, k+1, n) = mA * (Pkn(:, :, k, n) - Pkn(:, :, k, n) * mC' * inv(mRy) * mC * Pkn(:, :, k, n)) * mA + mQ;145 Pkn(:, :, k+1, n) = mA * (Pkn(:, :, k, n) - Pkn(:, :, k, n) * mC' * inv(mRy) * mC * Pkn(:, :, k, n)) * mA' + mQ; 128 146 end 129 147 end 130 Xstripe = mean(Xkn, 3); 131 Ystripe = mean(Ykn, 3); 148 Xstripe = mean(Xkn, 3); 132 149 Pstripe = mean(Pkn, 4); 133 150 … … 137 154 % 1] 138 155 for n = 1:N, 139 %krive okoli 140 Ykn(1, k, n) = Ykn(1, k, n) - Xkn(1, k, n); 141 Ykn(2, k, n) = Ykn(2, k, n) - Xkn(2, k, n); 142 143 Xkn(1, k, n) = Xstripe(1, k) + rho*randn(); 144 Xkn(2, k, n) = Xstripe(2, k) + rho*randn(); 145 Xkn(3, k, n) = Xstripe(3, k) + rho*randn(); 146 Xkn(4, k, n) = Xstripe(4, k) + rho*randn(); 147 148 Ykn(1, k, n) = Ykn(1, k, n) + Xkn(1, k, n); 149 Ykn(2, k, n) = Ykn(2, k, n) + Xkn(2, k, n); 150 151 Pkn(:, :, k, n) = Pstripe(:, :, k) .* exp(rho*randn(4)); 156 %krive okoli 157 Xkn(1, k, n) = Xstripe(1, k) + rhoi*randn(); 158 Xkn(2, k, n) = Xstripe(2, k) + rhoi*randn(); 159 Xkn(3, k, n) = Xstripe(3, k) + rhoo*randn(); 160 Xkn(4, k, n) = Xstripe(4, k) + rhot*randn(); 161 162 Ykn(:, k, n) = round(Xkn(1:2, k, n) / deltaI) * deltaI; 163 164 Pkn(:, :, k, n) = Pstripe(:, :, k) .* exp(rhop*randn(4)); 152 165 end 153 166 … … 156 169 gnu = n; 157 170 [Uopt2(:, n), Hmin(n)] = fmincon(@Hamilt, uPi(k, Xkn(:, k, n),Pkn(:, :, k, n)), [], [], [], [], [], [], @Cond2, optimset('GradConstr','on','Display','notify','Algorithm','active-set')); 171 % Uopt2(1,n)=sin(2*pi/20*k); 172 % Z = zeros(101,101); 173 % ii = 0; 174 % jj = 0; 175 % for ii = -50:50, 176 % for jj = -50:50, 177 % Z(ii+51,jj+51) = Hamilt([ii,jj]); 178 % end 179 % end 180 % surf(Z); 158 181 end 159 182 160 183 % 3] 161 184 for n = 1:N, 162 Vn(n) = DELTAt*Hmin(n) + Vtilde(k+1, Xkn(:, k, n), Pkn(:, :, k, n)); 185 Vn(n) = DELTAt*Hmin(n)/mag + Vtilde(k+1, Xkn(:, k, n), Pkn(:, :, k, n)); 186 163 187 end 164 188 165 189 % 4] 166 Epsilon = zeros(8, N);190 %urceni aproximace V Bellmanovy funkce 167 191 for n = 1:N, 168 192 Epsilon(1:4, n) = Xkn(1:4, k, n) - Xstripe(1:4, k); 169 Epsilon(5:8, n) = diag(Pkn(:, :, k, n) ./ Pstripe(:, :, k)); 193 tpdiag = diag(Pkn(:, :, k, n) ./ Pstripe(:, :, k)); 194 Epsilon(5:6, n) = tpdiag(3:4); 170 195 end 171 196 mFi = matrixFi(Epsilon); … … 173 198 Wv(:,k) = FiFiTInvFi * Vn'; 174 199 175 for n = 1:N, 176 tialfa(n) = Xkn(1, k, n); 177 tibeta(n) = Xkn(2, k, n); 178 tomega(n) = Xkn(3, k, n); 179 ttheta(n) = Xkn(4, k, n); 180 tp3(n) = Pkn(3, 3, k, n); 181 tp4(n) = Pkn(4, 4, k, n); 182 end 183 184 mPsi = [tomega',...1 185 -tialfa'.*sin(ttheta)',...2 186 tibeta'.*cos(ttheta)',...3 187 -Uopt2(1,:)'.*sin(ttheta)',...4 188 log(tp3)',...5 189 -tialfa'.*log(tp4)',...6 190 tibeta'.*log(tp4)',...7 191 -Uopt2(1,:)'.*log(tp4)',...8 192 -Uopt2(1,:)'];%9 193 PsiPsiTInvPsi = (mPsi'*mPsi)\mPsi'; 194 Kpi_alfa(:, k) = PsiPsiTInvPsi * (omega_t_stripe * ones(N, 1)); 195 196 mPsi = [tomega',...1 197 -tialfa'.*sin(ttheta)',...2 198 tibeta'.*cos(ttheta)',...3 199 Uopt2(2,:)'.*cos(ttheta)',...4 200 -log(tp3)',...5 201 tialfa'.*log(tp4)',...6 202 -tibeta'.*log(tp4)',...7 203 Uopt2(2,:)'.*log(tp4)',...8 204 Uopt2(2,:)'];%9 205 PsiPsiTInvPsi = (mPsi'*mPsi)\mPsi'; 206 Kpi_beta(:, k) = PsiPsiTInvPsi * (omega_t_stripe * ones(N, 1)); 200 %urceni aproximace pi rizeni u 201 Kpi_alfa(k) = mean(Uopt2(1,:)); 202 Kpi_beta(k) = mean(Uopt2(2,:)); 203 % mPsi = ones(N,1); 204 % PsiPsiTInvPsi = (mPsi'*mPsi)\mPsi'; 205 % Kpi_alfa(:, k) = PsiPsiTInvPsi * Uopt2(1,:)'; 206 % 207 % mPsi = ones(N,1); 208 % PsiPsiTInvPsi = (mPsi'*mPsi)\mPsi'; 209 % Kpi_beta(:, k) = PsiPsiTInvPsi * Uopt2(2,:)'; 210 % mPsi = [sin(squeeze(Xkn(4, k, :))),...1 211 % cos(squeeze(Xkn(4, k, :))),...2 212 % (sin(squeeze(Xkn(4, k, :))).^2),...3 213 % (cos(squeeze(Xkn(4, k, :))).^2)];%4 214 % PsiPsiTInvPsi = (mPsi'*mPsi)\mPsi'; 215 % Kpi_alfa(:, k) = PsiPsiTInvPsi * Uopt2(1,:)'; 216 % 217 % mPsi = [sin(squeeze(Xkn(4, k, :))),...1 218 % cos(squeeze(Xkn(4, k, :))),...2 219 % (sin(squeeze(Xkn(4, k, :))).^2),...3 220 % (cos(squeeze(Xkn(4, k, :))).^2)];%4 221 % PsiPsiTInvPsi = (mPsi'*mPsi)\mPsi'; 222 % Kpi_beta(:, k) = PsiPsiTInvPsi * Uopt2(2,:)'; 223 % tmpUfi = squeeze(Xkn(4, k, :) + DELTAt*Xkn(3, k, :)); 224 % tmpUamp = sqrt(Uopt2(1,:).^2 + Uopt2(2,:).^2)'; 225 % mPsi = [Cd*Cd*squeeze(Xkn(3, k, :)),...1 226 % Cd*Ce*squeeze(Xkn(2, k, :).*cos(Xkn(4, k, :))),...2 227 % - Cd*Ce*squeeze(Xkn(1, k, :).*sin(Xkn(4, k, :))),...3 228 % Ce*Ca*squeeze(Xkn(2, k, :)).*cos(tmpUfi),...4 229 % - Ce*Cb*squeeze(Xkn(3, k, :).*cos(Xkn(4, k, :))).*cos(tmpUfi),...5 230 % Ce*Ca*squeeze(Xkn(1, k, :)).*sin(tmpUfi),...6 231 % Ce*Cb*squeeze(Xkn(3, k, :).*sin(Xkn(4, k, :))).*sin(tmpUfi),...7 232 % Ce*Cc*squeeze( (cos(tmpUfi)).^2 - (sin(tmpUfi)).^2 ).*tmpUamp,...8 233 % tmpUamp];%9 234 % PsiPsiTInvPsi = (mPsi'*mPsi)\mPsi'; 235 % Kpi(:, k) = PsiPsiTInvPsi * (omega_t_stripe*ones(N, 1)); 207 236 end 208 237 end … … 221 250 for n = 1:N, 222 251 Xkn(:, 1, n) = X0; 223 Ykn(:, 1, n) = Y0 + mR * [randn(); randn()];252 Ykn(:, 1, n) = Y0; 224 253 Pkn(:, :, 1, n) = P0; 225 254 for k = 1:K-1, … … 227 256 mRy = mC * Pkn(:, :, k, n) * mC' + mR; 228 257 mKy = Pkn(:, :, k, n) * mC' / mRy; 229 Xkn(:, k+1, n) = fceG(Xkn(:, k, n), Ukn(:, k, n)) - mKy * (Ykn(:, k, n) - fceH(Xkn(:, k, n))) ;230 Ykn(:, k+1, n) = Xkn(1:2, k+1, n) + mR * [randn(); randn()]; %X kopie do Y + sum258 Xkn(:, k+1, n) = fceG(Xkn(:, k, n), Ukn(:, k, n)) - mKy * (Ykn(:, k, n) - fceH(Xkn(:, k, n))) + noise * sqrtm(mQ) * randn(4, 1);%+gauss sum s rozptylem odmocnina mQ 259 Ykn(:, k+1, n) = round(Xkn(1:2, k+1, n) / deltaI) * deltaI; %X kopie do Y se vzorkovanim 0.085 231 260 mA(1, 3) = Cb * sin(Xkn(4, k, n)); 232 261 mA(1, 4) = Cb * Xkn(3, k, n) * cos(Xkn(4, k, n)); … … 236 265 mA(3, 2) = Ce * cos(Xkn(4, k, n)); 237 266 mA(3, 4) = - Ce * (Xkn(2, k, n) * sin(Xkn(4, k, n)) + Xkn(1, k, n) * cos(Xkn(4, k, n))); 238 Pkn(:, :, k+1, n) = mA * (Pkn(:, :, k, n) - Pkn(:, :, k, n) * mC' * inv(mRy) * mC * Pkn(:, :, k, n)) * mA + mQ;267 Pkn(:, :, k+1, n) = mA * (Pkn(:, :, k, n) - Pkn(:, :, k, n) * mC' * inv(mRy) * mC * Pkn(:, :, k, n)) * mA' + mQ; 239 268 end 240 269 … … 299 328 plot(1:K,squeeze(Pkn(4, 4, :, n))) 300 329 end 330 331 figure 332 333 for n = 1:N, 334 Xkn(:, 1, n) = X0; 335 for k = 1:K-1, 336 Ukn(:, k, n) = uPi(k, Xkn(:, k, n), zeros(4)); 337 Xkn(:, k+1, n) = fceG(Xkn(:, k, n), Ukn(:, k, n)) + noise * sqrtm(mQ) * randn(4, 1); 338 end 339 340 hold all 341 subplot(2,3,1); 342 title('X:i_{\alpha}') 343 plot(1:K,Xkn(1,:,n)) 344 345 hold all 346 subplot(2,3,2); 347 title('X:i_{\beta}') 348 plot(1:K,Xkn(2,:,n)) 349 350 hold all 351 subplot(2,3,3); 352 title('X:\omega') 353 plot(1:K,Xkn(3,:,n)) 354 355 hold all 356 subplot(2,3,4); 357 title('X:\theta') 358 plot(1:K,Xkn(4,:,n)) 359 360 hold all 361 subplot(2,3,5); 362 title('u_{\alpha}') 363 plot(1:K,Ukn(1,:,n)) 364 365 hold all 366 subplot(2,3,6); 367 title('u_{\beta}') 368 plot(1:K,Ukn(2,:,n)) 369 370 end 301 371 302 372 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% … … 305 375 function [val_uPi] = uPi(k_uPi, x_uPi, p_uPi) 306 376 val_uPi = zeros(2, 1); 307 val_uPi(1) = (-omega_t_stripe + Kpi_alfa(1, k_uPi)*x_uPi(3)+Kpi_alfa(5, k_uPi)*log(p_uPi(3,3)) - Kpi_alfa(2, k_uPi)*x_uPi(1)*sin(x_uPi(4))-Kpi_alfa(6, k_uPi)*x_uPi(1)*log(p_uPi(4,4)) + Kpi_alfa(3, k_uPi)*x_uPi(2)*cos(x_uPi(4))+Kpi_alfa(7, k_uPi)*x_uPi(2)*log(p_uPi(4,4)) + Kpi_alfa(5)) / (Kpi_alfa(4)*sin(x_uPi(4))+Kpi_alfa(8)*log(p_uPi(4,4))+Kpi_alfa(9)); 308 val_uPi(2) = ( omega_t_stripe - Kpi_beta(1, k_uPi)*x_uPi(3)-Kpi_beta(5, k_uPi)*log(p_uPi(3,3)) + Kpi_beta(2, k_uPi)*x_uPi(1)*sin(x_uPi(4))+Kpi_beta(6, k_uPi)*x_uPi(1)*log(p_uPi(4,4)) - Kpi_beta(3, k_uPi)*x_uPi(2)*cos(x_uPi(4))-Kpi_beta(7, k_uPi)*x_uPi(2)*log(p_uPi(4,4)) - Kpi_beta(5)) / (Kpi_beta(4)*cos(x_uPi(4))+Kpi_beta(8)*log(p_uPi(4,4))+Kpi_beta(9)); 309 310 if ( (val_uPi(1)^2 + val_uPi(2)^2) > cC1^2 )%nesplnena podminka - presune pod stejnym uhlem na hranici 311 tmpfi = atan2(val_uPi(2), val_uPi(1)); 312 val_uPi(1) = cC1*cos(tmpfi); 313 val_uPi(2) = cC1*sin(tmpfi); 314 end 377 val_uPi(1) = Kpi_alfa(k_uPi); 378 val_uPi(2) = Kpi_beta(k_uPi); 379 % val_uPi(1) = Kpi_alfa(1, k_uPi)*sin(x_uPi(4)) + Kpi_alfa(2, k_uPi)*cos(x_uPi(4)) + Kpi_alfa(3, k_uPi)*(sin(x_uPi(4)))^2 + Kpi_alfa(4, k_uPi)*(cos(x_uPi(4)))^2; 380 % val_uPi(2) = Kpi_beta(1, k_uPi)*sin(x_uPi(4)) + Kpi_beta(2, k_uPi)*cos(x_uPi(4)) + Kpi_beta(3, k_uPi)*(sin(x_uPi(4)))^2 + Kpi_beta(4, k_uPi)*(cos(x_uPi(4)))^2; 381 % tmpfi = x_uPi(4) + DELTAt*x_uPi(3); 382 % tmpw = ( omega_t_stripe - Cd*(Kpi(1, k_uPi)*Cd*x_uPi(3) + Kpi(2, k_uPi)*Ce*x_uPi(2)*cos(x_uPi(4)) - Kpi(3, k_uPi)*Ce*x_uPi(1)*sin(x_uPi(4)))... 383 % - Ce*( (Kpi(4, k_uPi)*Ca*x_uPi(2) - Kpi(5, k_uPi)*Cb*x_uPi(3)*cos(x_uPi(4)))*cos(tmpfi)... 384 % -(Kpi(6, k_uPi)*Ca*x_uPi(1) + Kpi(7, k_uPi)*Cb*x_uPi(3)*sin(x_uPi(4)))*sin(tmpfi) ) ) /... 385 % (Kpi(8, k_uPi)*Ce*Cc*( (cos(tmpfi))^2 - (sin(tmpfi))^2 ) + Kpi(9, k_uPi)); 386 % 387 % if(tmpw > cC1) 388 % tmpw = cC1; 389 % elseif(tmpw < - cC1) 390 % tmpw = -cC1; 391 % end 392 % val_uPi(1) = tmpw*sin(tmpfi); 393 % val_uPi(2) = tmpw*cos(tmpfi); 315 394 end 316 395 … … 326 405 mA(3, 2) = Ce * cos(Xkn(4, gka, gnu)); 327 406 mA(3, 4) = - Ce * (Xkn(2, gka, gnu) * sin(Xkn(4, gka, gnu)) + Xkn(1, gka, gnu) * cos(Xkn(4, gka, gnu))); 328 tPkn = mA * (Pkn(:, :, gka, gnu) - Pkn(:, :, gka, gnu) * mC' * inv(mRy) * mC * Pkn(:, :, gka, gnu)) * mA + mQ;329 tf = zeros( 8,1);407 tPkn = mA * (Pkn(:, :, gka, gnu) - Pkn(:, :, gka, gnu) * mC' * inv(mRy) * mC * Pkn(:, :, gka, gnu)) * mA' + mQ; 408 tf = zeros(6,1); 330 409 tf(1:4) = tXkn; 331 tf(5:8) = diag(tPkn); 410 tfpdiag = diag(tPkn); 411 tf(5:6) = tfpdiag(3:4); 332 412 333 loss = (tXkn(3) - omega_t_stripe)^2; 413 % loss = (tXkn(3) - omega_t_stripe)^2; 414 % loss = (Cd * Xkn(3, gka, gnu) + Ce * ((Ca * Xkn(2, gka, gnu) - Cb * Xkn(3, gka, gnu) * cos(Xkn(4, gka, gnu)) + Cc * u_ham(2)) * cos(Xkn(4, gka, gnu)) - (Ca * Xkn(1, gka, gnu) + Cb * Xkn(3, gka, gnu) * sin(Xkn(4, gka, gnu)) + Cc * u_ham(1)) * sin(Xkn(4, gka, gnu))) - omega_t_stripe)^2; 415 % if(gka == 1) 416 % loss = (tXkn(3) - omega_t_stripe)^2; 417 % else 418 % loss = (Cd * Xkn(3, gka, gnu) + Ce * ((Ca * Xkn(2, gka-1, gnu) - Cb * Xkn(3, gka-1, gnu) * cos(Xkn(4, gka-1, gnu)) + Cc * u_ham(2)) * cos(Xkn(4, gka, gnu)) - (Ca * Xkn(1, gka-1, gnu) + Cb * Xkn(3, gka-1, gnu) * sin(Xkn(4, gka-1, gnu)) + Cc * u_ham(1)) * sin(Xkn(4, gka, gnu))) - omega_t_stripe)^2; 419 % end 420 % loss = (Cd * tXkn(3) + Ce * ((Ca * Xkn(2, gka, gnu) - Cb * Xkn(3, gka, gnu) * cos(Xkn(4, gka, gnu)) + Cc * u_ham(2)) * cos(tXkn(4)) - (Ca * Xkn(1, gka, gnu) + Cb * Xkn(3, gka, gnu) * sin(Xkn(4, gka, gnu)) + Cc * u_ham(1)) * sin(tXkn(4))) - omega_t_stripe)^2; 421 422 423 % loss = loss + cPenPsi*(u_ham(1)^2 + u_ham(2)^2); 424 loss = ( Cd*(Cd*Xkn(3, gka, gnu)... 425 + Ce*Xkn(2, gka, gnu)*cos(Xkn(4, gka, gnu))... 426 - Ce*Xkn(1, gka, gnu)*sin(Xkn(4, gka, gnu)))... 427 + Ce*( (Ca*Xkn(2, gka, gnu) - Cb*Xkn(3, gka, gnu)*cos(Xkn(4, gka, gnu)) )*cos(Xkn(4, gka, gnu)+DELTAt*Xkn(3, gka, gnu))... 428 -(Ca*Xkn(1, gka, gnu) + Cb*Xkn(3, gka, gnu)*sin(Xkn(4, gka, gnu)) )*sin(Xkn(4, gka, gnu)+DELTAt*Xkn(3, gka, gnu)))... 429 + Ce*Cc*u_ham(2)*cos(Xkn(4, gka, gnu)+DELTAt*Xkn(3, gka, gnu))... 430 - Ce*Cc*u_ham(1)*sin(Xkn(4, gka, gnu)+DELTAt*Xkn(3, gka, gnu))... 431 - omega_t_stripe )^2; 334 432 335 val_ham = loss + tf' * Vtilde_dx(gka+1, Xkn(:, gka, gnu), Pkn(:, :, gka, gnu)) + 1/2 * trace(mSigma * ( Wv(10, gka+1)*[2 0; 0 0] + Wv(18, gka+1)*[0 0; 0 2]));433 val_ham = mag*(loss + tf' * Vtilde_dx(gka+1, Xkn(:, gka, gnu), Pkn(:, :, gka, gnu)) + 1/2 * trace(mQ * Vtilde_dx_dx(gka+1))); 336 434 337 435 end … … 341 439 val_Vt = h_bel; 342 440 else 343 Epsl = zeros( 8, 1);441 Epsl = zeros(6, 1); 344 442 Epsl(1:4) = x_Vt(1:4) - Xstripe(1:4, k_Vt); 345 Epsl(5:8) = diag(p_Vt ./ Pstripe(:, :, k)); 443 tpdiag = diag(p_Vt ./ Pstripe(:, :, k)); 444 Epsl(5:6) = tpdiag(3:4); 346 445 347 446 val_Vt = vectFi(Epsl)' * Wv(:,k_Vt); … … 353 452 val_Vt = h_beldx; 354 453 else 355 Epsl = zeros( 8, 1);454 Epsl = zeros(6, 1); 356 455 Epsl(1:4) = x_Vt(1:4) - Xstripe(1:4, k_Vt); 357 Epsl(5:8) = diag(p_Vt ./ Pstripe(:, :, k)); 456 tpdiag = diag(p_Vt ./ Pstripe(:, :, k)); 457 Epsl(5:6) = tpdiag(3:4); 358 458 359 459 val_Vt = difFi(Epsl)' * Wv(:,k_Vt); … … 369 469 x_Fi(4); ... 370 470 log(x_Fi(5)); ... %ln Xi pro 5-8 tj diagonala matice Pt 4x4 371 log(x_Fi(6)); ... 372 log(x_Fi(7)); ... 373 log(x_Fi(8)); ... 471 log(x_Fi(6)); ... 374 472 x_Fi(1)^2; ... %kvadraticke cleny jen v Xi 1-4 a kombinovane 375 473 x_Fi(1)*x_Fi(2); ... … … 377 475 x_Fi(1)*x_Fi(4); ... 378 476 x_Fi(1)*log(x_Fi(5)); ... 379 x_Fi(1)*log(x_Fi(6)); ... 380 x_Fi(1)*log(x_Fi(7)); ... 381 x_Fi(1)*log(x_Fi(8)); ... 477 x_Fi(1)*log(x_Fi(6)); ... 382 478 x_Fi(2)^2; ... 383 479 x_Fi(2)*x_Fi(3); ... 384 480 x_Fi(2)*x_Fi(4); ... 385 481 x_Fi(2)*log(x_Fi(5)); ... 386 x_Fi(2)*log(x_Fi(6)); ... 387 x_Fi(2)*log(x_Fi(7)); ... 388 x_Fi(2)*log(x_Fi(8)); ... 482 x_Fi(2)*log(x_Fi(6)); ... 389 483 x_Fi(3)^2; ... 390 484 x_Fi(3)*x_Fi(4); ... 391 485 x_Fi(3)*log(x_Fi(5)); ... 392 x_Fi(3)*log(x_Fi(6)); ... 393 x_Fi(3)*log(x_Fi(7)); ... 394 x_Fi(3)*log(x_Fi(8)); ... 486 x_Fi(3)*log(x_Fi(6)); ... 395 487 x_Fi(4)^2; ... 396 488 x_Fi(4)*log(x_Fi(5)); ... 397 x_Fi(4)*log(x_Fi(6)); ... 398 x_Fi(4)*log(x_Fi(7)); ... 399 x_Fi(4)*log(x_Fi(8)); ... 489 x_Fi(4)*log(x_Fi(6)); ... 400 490 ]; 401 491 end … … 409 499 x_Fi(4, :); ... 410 500 log(x_Fi(5, :)); ... %ln Xi pro 5-8 tj diagonala matice Pt 4x4 411 log(x_Fi(6, :)); ... 412 log(x_Fi(7, :)); ... 413 log(x_Fi(8, :)); ... 501 log(x_Fi(6, :)); ... 414 502 x_Fi(1, :).^2; ... %kvadraticke cleny jen v Xi 1-4 a kombinovane 415 503 x_Fi(1, :).*x_Fi(2, :); ... … … 418 506 x_Fi(1, :).*log(x_Fi(5, :)); ... 419 507 x_Fi(1, :).*log(x_Fi(6, :)); ... 420 x_Fi(1, :).*log(x_Fi(7, :)); ...421 x_Fi(1, :).*log(x_Fi(8, :)); ...422 508 x_Fi(2, :).^2; ... 423 509 x_Fi(2, :).*x_Fi(3, :); ... 424 510 x_Fi(2, :).*x_Fi(4, :); ... 425 511 x_Fi(2, :).*log(x_Fi(5, :)); ... 426 x_Fi(2, :).*log(x_Fi(6, :)); ... 427 x_Fi(2, :).*log(x_Fi(7, :)); ... 428 x_Fi(2, :).*log(x_Fi(8, :)); ... 512 x_Fi(2, :).*log(x_Fi(6, :)); ... 429 513 x_Fi(3, :).^2; ... 430 514 x_Fi(3, :).*x_Fi(4, :); ... 431 515 x_Fi(3, :).*log(x_Fi(5, :)); ... 432 516 x_Fi(3, :).*log(x_Fi(6, :)); ... 433 x_Fi(3, :).*log(x_Fi(7, :)); ...434 x_Fi(3, :).*log(x_Fi(8, :)); ...435 517 x_Fi(4, :).^2; ... 436 518 x_Fi(4, :).*log(x_Fi(5, :)); ... 437 x_Fi(4, :).*log(x_Fi(6, :)); ... 438 x_Fi(4, :).*log(x_Fi(7, :)); ... 439 x_Fi(4, :).*log(x_Fi(8, :)); ... 519 x_Fi(4, :).*log(x_Fi(6, :)); ... 440 520 ]; 441 521 … … 444 524 function [val_Fi] = difFi(x_Fi) 445 525 val_Fi = [ ... 446 0 0 0 0 0 0 0 0; ... 447 1 0 0 0 0 0 0 0; ... 448 0 1 0 0 0 0 0 0; ... 449 0 0 1 0 0 0 0 0; ... 450 0 0 0 1 0 0 0 0; ... 451 0 0 0 0 1/x_Fi(5) 0 0 0; ... 452 0 0 0 0 0 1/x_Fi(6) 0 0; ... 453 0 0 0 0 0 0 1/x_Fi(7) 0; ... 454 0 0 0 0 0 0 0 1/x_Fi(8); ... 455 2*x_Fi(1) 0 0 0 0 0 0 0; ... 456 x_Fi(2) x_Fi(1) 0 0 0 0 0 0; ... 457 x_Fi(3) 0 x_Fi(1) 0 0 0 0 0; ... 458 x_Fi(4) 0 0 x_Fi(1) 0 0 0 0; ... 459 log(x_Fi(5)) 0 0 0 x_Fi(1)/x_Fi(5) 0 0 0; ... 460 log(x_Fi(6)) 0 0 0 0 x_Fi(1)/x_Fi(6) 0 0; ... 461 log(x_Fi(7)) 0 0 0 0 0 x_Fi(1)/x_Fi(7) 0; ... 462 log(x_Fi(8)) 0 0 0 0 0 0 x_Fi(1)/x_Fi(8); ... 463 0 2*x_Fi(2) 0 0 0 0 0 0; ... 464 0 x_Fi(3) x_Fi(2) 0 0 0 0 0; ... 465 0 x_Fi(4) 0 x_Fi(2) 0 0 0 0; ... 466 0 log(x_Fi(5)) 0 0 x_Fi(2)/x_Fi(5) 0 0 0; ... 467 0 log(x_Fi(6)) 0 0 0 x_Fi(2)/x_Fi(6) 0 0; ... 468 0 log(x_Fi(7)) 0 0 0 0 x_Fi(2)/x_Fi(7) 0; ... 469 0 log(x_Fi(8)) 0 0 0 0 0 x_Fi(2)/x_Fi(8); ... 470 0 0 2*x_Fi(3) 0 0 0 0 0; ... 471 0 0 x_Fi(4) x_Fi(3) 0 0 0 0; ... 472 0 0 log(x_Fi(5)) 0 x_Fi(3)/x_Fi(5) 0 0 0; ... 473 0 0 log(x_Fi(6)) 0 0 x_Fi(3)/x_Fi(6) 0 0; ... 474 0 0 log(x_Fi(7)) 0 0 0 x_Fi(3)/x_Fi(7) 0; ... 475 0 0 log(x_Fi(8)) 0 0 0 0 x_Fi(3)/x_Fi(8); ... 476 0 0 0 2*x_Fi(4) 0 0 0 0; ... 477 0 0 0 log(x_Fi(5)) x_Fi(4)/x_Fi(5) 0 0 0; ... 478 0 0 0 log(x_Fi(6)) 0 x_Fi(4)/x_Fi(6) 0 0; ... 479 0 0 0 log(x_Fi(7)) 0 0 x_Fi(4)/x_Fi(7) 0; ... 480 0 0 0 log(x_Fi(8)) 0 0 0 x_Fi(4)/x_Fi(8); ... 526 0 0 0 0 0 0; ...1 527 1 0 0 0 0 0; ...2 528 0 1 0 0 0 0; ...3 529 0 0 1 0 0 0; ...4 530 0 0 0 1 0 0; ...5 531 0 0 0 0 1/x_Fi(5) 0; ...6 532 0 0 0 0 0 1/x_Fi(6); ...7 533 2*x_Fi(1) 0 0 0 0 0; ...8 534 x_Fi(2) x_Fi(1) 0 0 0 0; ...9 535 x_Fi(3) 0 x_Fi(1) 0 0 0; ...10 536 x_Fi(4) 0 0 x_Fi(1) 0 0; ...11 537 log(x_Fi(5)) 0 0 0 x_Fi(1)/x_Fi(5) 0; ...12 538 log(x_Fi(6)) 0 0 0 0 x_Fi(1)/x_Fi(6); ...13 539 0 2*x_Fi(2) 0 0 0 0; ...14 540 0 x_Fi(3) x_Fi(2) 0 0 0; ...15 541 0 x_Fi(4) 0 x_Fi(2) 0 0; ...16 542 0 log(x_Fi(5)) 0 0 x_Fi(2)/x_Fi(5) 0; ...17 543 0 log(x_Fi(6)) 0 0 0 x_Fi(2)/x_Fi(6); ...18 544 0 0 2*x_Fi(3) 0 0 0; ...19 545 0 0 x_Fi(4) x_Fi(3) 0 0; ...20 546 0 0 log(x_Fi(5)) 0 x_Fi(3)/x_Fi(5) 0; ...21 547 0 0 log(x_Fi(6)) 0 0 x_Fi(3)/x_Fi(6); ...22 548 0 0 0 2*x_Fi(4) 0 0; ...23 549 0 0 0 log(x_Fi(5)) x_Fi(4)/x_Fi(5) 0; ...24 550 0 0 0 log(x_Fi(6)) 0 x_Fi(4)/x_Fi(6); ...25 481 551 ]; 552 end 553 554 function [valvt] = Vtilde_dx_dx(kin) 555 if(kin == K) 556 valvt = h_beldxdx; 557 else 558 valvt = Wv(8, kin)*diag([2 0 0 0]) +... 559 Wv(9, kin)*[0 1 0 0; 1 0 0 0; 0 0 0 0; 0 0 0 0] +... 560 Wv(10, kin)*[0 0 1 0; 0 0 0 0; 1 0 0 0; 0 0 0 0] +... 561 Wv(11, kin)*[0 0 0 1; 0 0 0 0; 0 0 0 0; 1 0 0 0] +... 562 Wv(14, kin)*diag([0 2 0 0]) +... 563 Wv(15, kin)*[0 0 0 0; 0 0 1 0; 0 1 0 0; 0 0 0 0] +... 564 Wv(16, kin)*[0 0 0 0; 0 0 0 1; 0 0 0 0; 0 1 0 0] +... 565 Wv(19, kin)*diag([0 2 0 0]) +... 566 Wv(20, kin)*[0 0 0 0; 0 0 0 0; 0 0 0 1; 0 0 1 0] +... 567 Wv(23, kin)*diag([0 2 0 0]); 568 end 482 569 end 483 570 … … 497 584 end 498 585 499 function [x_ret] = fceG_du(x_in, u_in)500 x_ret = zeros(4, 2);501 x_ret(1, 1) = Cc;502 x_ret(2, 2) = Cc;503 end504 505 586 function [y_ret] = fceH(x_in) 506 587 y_ret = zeros(2, 1);
