function pmsm_ildp2 %verze ildp algoritmu s uvazovanim P (v logaritmu) pro PMSM motor %rozsirena verze o diag P v pi aproximaci u tic %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %pocatecni konstanty Iterace = 2; %iterace K = 20; %casy N = 50; %vzorky %konstanty motoru %Rs = 0.28; %Ls = 0.003465; %PSIpm = 0.1989; %kp = 1.5; %p = 4.0; %J = 0.04; DELTAt = 0.000125; %upravene konstanty Ca = 0.9898; Cb = 0.0072; Cc = 0.0361; Cd = 1.0; Ce = 0.0149; %omezeni rizeni cC1 = 100*100; cLb = -50; cUb = 50; %matice %kovariancni matice Q a R mQ = diag([0.0013 0.0013 5.0e-6 1.0e-10]); mR = diag([0.0006 0.0006]); mSigma = mR*mR'; %matice pro vypocet %matice A zavisla na parametrech mA = zeros(4); mA(1,1) = Ca; mA(2,2) = Ca; mA(3,3) = Cd; mA(4,4) = 1; mA(4,3) = DELTAt; %macite C konstantni mC = [ 1 0 0 0; 0 1 0 0]; %pozadovana hodnota otacek omega_t_stripe = 1.0015; %pocatecni hodnoty X0 = [0; 0; 1; pi/2]; Y0 = [0; 0]; P0 = diag([0.01 0.01 0.01 0.01]); h_bel = 0; h_beldx = [0; 0; 0; 0; 0; 0; 0; 0]; %velikost okoli pro lokalni metodu rho = 0.01; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %globalni promenne % Kpi_alfa = ones(12, K); %konstanty aproximace slozky rizeni u_alfa Kpi_alfa = ones(11, K); %konstanty aproximace slozky rizeni u_alfa Kpi_alfa(1, :) = (Cd - Cb*Ce)*ones(1, K); Kpi_alfa(2, :) = Ca*Ce*ones(1, K); Kpi_alfa(3, :) = Ca*Ce*ones(1, K); Kpi_alfa(4, :) = Cc*Ce*ones(1, K); % Kpi_alfa(5, :) = zeros(1, K); % Kpi_beta = ones(12, K); %konstanty aproximace slozky rizeni u_beta Kpi_beta = ones(11, K); %konstanty aproximace slozky rizeni u_beta Kpi_beta(1, :) = (Cd - Cb*Ce)*ones(1, K); Kpi_beta(2, :) = Ca*Ce*ones(1, K); Kpi_beta(3, :) = Ca*Ce*ones(1, K); Kpi_beta(4, :) = Cc*Ce*ones(1, K); % Kpi_beta(5, :) = zeros(1, K); Wv = zeros(35, K); %konstanty aproximace Bellmanovy fce Xkn = zeros(4, K, N); % X = [i_alfa, i_beta, omega, theta] Ykn = zeros(2, K, N); % Y = [i_alfa, i_beta] Pkn = zeros(4, 4, K, N); % P = N vzorku posloupnosti K matic 4x4 mKy = zeros(4, 2); % K = pomocna matice pro vypocet mRy = zeros(2, 2); % R = pomocna matice pro vypocet Xstripe = zeros(4, K); Ystripe = zeros(4, K); Pstripe = zeros(4, 4, K); Epsilon = zeros(20, N); %globalni promena pro vypocet Bellmanovy fce z odchylek (X - Xprum) gka = 0; %globalni promenna pro prenos casu k gnu = 0; %globalni promenna pro prenos vzorku n Uopt2 = zeros(2, N); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %hlavni iteracni smycka for i = 1:Iterace, %generovani stavu for n = 1:N, Xkn(:, 1, n) = X0; Ykn(:, 1, n) = Y0 + mR * [randn(); randn()]; Pkn(:, :, 1, n) = P0; for k = 1:K-1, Uk = uPi(k, Xkn(:, k, n), Pkn(:, :, k, n)); mRy = mC * Pkn(:, :, k, n) * mC' + mR; mKy = Pkn(:, :, k, n) * mC' / mRy; Xkn(:, k+1, n) = fceG(Xkn(:, k, n), Uk) - mKy * (Ykn(:, k, n) - fceH(Xkn(:, k, n))); Ykn(:, k+1, n) = Xkn(1:2, k+1, n) + mR * [randn(); randn()]; %X kopie do Y + sum mA(1, 3) = Cb * sin(Xkn(4, k, n)); mA(1, 4) = Cb * Xkn(3, k, n) * cos(Xkn(4, k, n)); mA(2, 3) = - Cb * cos(Xkn(4, k, n)); mA(2, 4) = Cb * Xkn(3, k, n) * sin(Xkn(4, k, n)); mA(3, 1) = - Ce * sin(Xkn(4, k, n)); mA(3, 2) = Ce * cos(Xkn(4, k, n)); mA(3, 4) = - Ce * (Xkn(2, k, n) * sin(Xkn(4, k, n)) + Xkn(1, k, n) * cos(Xkn(4, k, n))); Pkn(:, :, k+1, n) = mA * (Pkn(:, :, k, n) - Pkn(:, :, k, n) * mC' * inv(mRy) * mC * Pkn(:, :, k, n)) * mA + mQ; end end Xstripe = mean(Xkn, 3); Ystripe = mean(Ykn, 3); Pstripe = mean(Pkn, 4); for k = K-1:-1:1, gka = k; % 1] for n = 1:N, %krive okoli Ykn(1, k, n) = Ykn(1, k, n) - Xkn(1, k, n); Ykn(2, k, n) = Ykn(2, k, n) - Xkn(2, k, n); Xkn(1, k, n) = Xstripe(1, k) + rho*randn(); Xkn(2, k, n) = Xstripe(2, k) + rho*randn(); Xkn(3, k, n) = Xstripe(3, k) + rho*randn(); Xkn(4, k, n) = Xstripe(4, k) + rho*randn(); Ykn(1, k, n) = Ykn(1, k, n) + Xkn(1, k, n); Ykn(2, k, n) = Ykn(2, k, n) + Xkn(2, k, n); Pkn(:, :, k, n) = Pstripe(:, :, k) .* exp(rho*randn(4)); end % 2] for n = 1:N, gnu = n; [Uopt2(:, n), Hmin(n)] = fmincon(@Hamilt, uPi(k, Xkn(:, k, n),Pkn(:, :, k, n)), [], [], [], [], cLb, cUb, @Cond2, optimset('GradConstr','on','Display','notify','Algorithm','active-set')); end % 3] for n = 1:N, Vn(n) = DELTAt*Hmin(n) + Vtilde(k+1, Xkn(:, k, n), Pkn(:, :, k, n)); end % 4] Epsilon = zeros(8, N); for n = 1:N, Epsilon(1:4, n) = Xkn(1:4, k, n) - Xstripe(1:4, k); Epsilon(5:8, n) = diag(Pkn(:, :, k, n) ./ Pstripe(:, :, k)); end mFi = matrixFi(Epsilon); FiFiTInvFi = (mFi*mFi')\mFi; Wv(:,k) = FiFiTInvFi * Vn'; for n = 1:N, tialfa(n) = Xkn(1, k, n); tibeta(n) = Xkn(2, k, n); tomega(n) = Xkn(3, k, n); ttheta(n) = Xkn(4, k, n); tp1(n) = Pkn(1, 1, k, n); tp2(n) = Pkn(2, 2, k, n); tp3(n) = Pkn(3, 3, k, n); tp4(n) = Pkn(4, 4, k, n); end % mPsi = [tomega',...1 % -tialfa'.*sin(ttheta)',...2 % tibeta'.*cos(ttheta)',...3 % -Uopt2(1,:)'.*sin(ttheta)',...4 % ones(N,1),...5 % log(tp3)',...6 % -tialfa'.*log(tp4)',...7 % -log(tp1)'.*sin(ttheta)',...8 % tibeta'.*log(tp4)',...9 % log(tp2)'.*cos(ttheta)',...10 % -Uopt2(1,:)'.*log(tp4)',...11 % -Uopt2(1,:)'];%12 mPsi = [tomega',...1 -tialfa'.*sin(ttheta)',...2 tibeta'.*cos(ttheta)',...3 -Uopt2(1,:)'.*sin(ttheta)',...4 log(tp3)',...6 -tialfa'.*log(tp4)',...7 -log(tp1)'.*sin(ttheta)',...8 tibeta'.*log(tp4)',...9 log(tp2)'.*cos(ttheta)',...10 -Uopt2(1,:)'.*log(tp4)',...11 -Uopt2(1,:)'];%12 PsiPsiTInvPsi = (mPsi'*mPsi)\mPsi'; Kpi_alfa(:, k) = PsiPsiTInvPsi * (omega_t_stripe * ones(N, 1)); % mPsi = [tomega',...1 % -tialfa'.*sin(ttheta)',...2 % tibeta'.*cos(ttheta)',...3 % Uopt2(2,:)'.*cos(ttheta)',...4 % ones(N,1),...5 % -log(tp3)',...6 % tialfa'.*log(tp4)',...7 % log(tp1)'.*sin(ttheta)',...8 % -tibeta'.*log(tp4)',...9 % -log(tp2)'.*cos(ttheta)',...10 % Uopt2(2,:)'.*log(tp4)',...11 % Uopt2(2,:)'];%12 mPsi = [tomega',...1 -tialfa'.*sin(ttheta)',...2 tibeta'.*cos(ttheta)',...3 Uopt2(2,:)'.*cos(ttheta)',...4 -log(tp3)',...6 tialfa'.*log(tp4)',...7 log(tp1)'.*sin(ttheta)',...8 -tibeta'.*log(tp4)',...9 -log(tp2)'.*cos(ttheta)',...10 Uopt2(2,:)'.*log(tp4)',...11 Uopt2(2,:)'];%12 PsiPsiTInvPsi = (mPsi'*mPsi)\mPsi'; Kpi_beta(:, k) = PsiPsiTInvPsi * (omega_t_stripe * ones(N, 1)); end end %%%%%%%%%%% toc % keyboard Kpi_alfa Kpi_beta %vykresleni grafu clf subplot(3,4,3); plot(1:K,omega_t_stripe*ones(1,K)); Ukn = zeros(2, K, N); for n = 1:N, Xkn(:, 1, n) = X0; Ykn(:, 1, n) = Y0 + mR * [randn(); randn()]; Pkn(:, :, 1, n) = P0; for k = 1:K-1, Ukn(:, k, n) = uPi(k, Xkn(:, k, n), Pkn(:, :, k, n)); mRy = mC * Pkn(:, :, k, n) * mC' + mR; mKy = Pkn(:, :, k, n) * mC' / mRy; Xkn(:, k+1, n) = fceG(Xkn(:, k, n), Ukn(:, k, n)) - mKy * (Ykn(:, k, n) - fceH(Xkn(:, k, n))); Ykn(:, k+1, n) = Xkn(1:2, k+1, n) + mR * [randn(); randn()]; %X kopie do Y + sum mA(1, 3) = Cb * sin(Xkn(4, k, n)); mA(1, 4) = Cb * Xkn(3, k, n) * cos(Xkn(4, k, n)); mA(2, 3) = - Cb * cos(Xkn(4, k, n)); mA(2, 4) = Cb * Xkn(3, k, n) * sin(Xkn(4, k, n)); mA(3, 1) = - Ce * sin(Xkn(4, k, n)); mA(3, 2) = Ce * cos(Xkn(4, k, n)); mA(3, 4) = - Ce * (Xkn(2, k, n) * sin(Xkn(4, k, n)) + Xkn(1, k, n) * cos(Xkn(4, k, n))); Pkn(:, :, k+1, n) = mA * (Pkn(:, :, k, n) - Pkn(:, :, k, n) * mC' * inv(mRy) * mC * Pkn(:, :, k, n)) * mA + mQ; end hold all subplot(3,4,1); title('X:i_{\alpha}') plot(1:K,Xkn(1,:,n)) hold all subplot(3,4,2); title('X:i_{\beta}') plot(1:K,Xkn(2,:,n)) hold all subplot(3,4,3); title('X:\omega') plot(1:K,Xkn(3,:,n)) hold all subplot(3,4,4); title('X:\theta') plot(1:K,Xkn(4,:,n)) hold all subplot(3,4,5); title('Y:i_{\alpha}') plot(1:K,Ykn(1,:,n)) hold all subplot(3,4,6); title('Y:i_{\beta}') plot(1:K,Ykn(2,:,n)) hold all subplot(3,4,7); title('u_{\alpha}') plot(1:K,Ukn(1,:,n)) hold all subplot(3,4,8); title('u_{\beta}') plot(1:K,Ukn(2,:,n)) hold all subplot(3,4,9); title('P(1, 1)') plot(1:K,squeeze(Pkn(1, 1, :, n))) hold all subplot(3,4,10); title('P(2, 2)') plot(1:K,squeeze(Pkn(2, 2, :, n))) hold all subplot(3,4,11); title('P(3, 3)') plot(1:K,squeeze(Pkn(3, 3, :, n))) hold all subplot(3,4,12); title('P(4, 4)') plot(1:K,squeeze(Pkn(4, 4, :, n))) end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %pomocne funkce function [val_uPi] = uPi(k_uPi, x_uPi, p_uPi) val_uPi = zeros(2, 1); % val_uPi(1) = (-omega_t_stripe + Kpi_alfa(1, k_uPi)*x_uPi(3)+Kpi_alfa(6, k_uPi)*log(p_uPi(3,3)) - Kpi_alfa(2, k_uPi)*x_uPi(1)*sin(x_uPi(4))-Kpi_alfa(7, k_uPi)*x_uPi(1)*log(p_uPi(4,4))-Kpi_alfa(8, k_uPi)*log(p_uPi(1,1))*sin(x_uPi(4)) + Kpi_alfa(3, k_uPi)*x_uPi(2)*cos(x_uPi(4))+Kpi_alfa(9, k_uPi)*x_uPi(2)*log(p_uPi(4,4))+Kpi_alfa(10, k_uPi)*log(p_uPi(2,2))*cos(x_uPi(4)) + Kpi_alfa(5)) / (Kpi_alfa(4)*sin(x_uPi(4))+Kpi_alfa(11)*log(p_uPi(4,4))+Kpi_alfa(12)); % val_uPi(2) = ( omega_t_stripe - Kpi_beta(1, k_uPi)*x_uPi(3)-Kpi_beta(6, k_uPi)*log(p_uPi(3,3)) + Kpi_beta(2, k_uPi)*x_uPi(1)*sin(x_uPi(4))+Kpi_beta(7, k_uPi)*x_uPi(1)*log(p_uPi(4,4))+Kpi_beta(8, k_uPi)*log(p_uPi(1,1))*sin(x_uPi(4)) - Kpi_beta(3, k_uPi)*x_uPi(2)*cos(x_uPi(4))-Kpi_beta(9, k_uPi)*x_uPi(2)*log(p_uPi(4,4))-Kpi_beta(10, k_uPi)*log(p_uPi(2,2))*cos(x_uPi(4)) - Kpi_beta(5)) / (Kpi_beta(4)*cos(x_uPi(4))+Kpi_beta(11)*log(p_uPi(4,4))+Kpi_beta(12)); 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(7, k_uPi)*log(p_uPi(1,1))*sin(x_uPi(4)) + Kpi_alfa(3, k_uPi)*x_uPi(2)*cos(x_uPi(4))+Kpi_alfa(8, k_uPi)*x_uPi(2)*log(p_uPi(4,4))+Kpi_alfa(9, k_uPi)*log(p_uPi(2,2))*cos(x_uPi(4)) + Kpi_alfa(5)) / (Kpi_alfa(4)*sin(x_uPi(4))+Kpi_alfa(10)*log(p_uPi(4,4))+Kpi_alfa(11)); 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(7, k_uPi)*log(p_uPi(1,1))*sin(x_uPi(4)) - Kpi_beta(3, k_uPi)*x_uPi(2)*cos(x_uPi(4))-Kpi_beta(8, k_uPi)*x_uPi(2)*log(p_uPi(4,4))-Kpi_beta(9, k_uPi)*log(p_uPi(2,2))*cos(x_uPi(4)) - Kpi_beta(5)) / (Kpi_beta(4)*cos(x_uPi(4))+Kpi_beta(10)*log(p_uPi(4,4))+Kpi_beta(11)); if (val_uPi(1)cUb) val_uPi(1) = cUb; end if (val_uPi(2)cUb) val_uPi(2) = cUb; end end function [val_ham] = Hamilt(u_ham) mRy = mC * Pkn(:, :, gka, gnu) * mC' + mR; mKy = Pkn(:, :, gka, gnu) * mC' / mRy; tXkn = fceG(Xkn(:, gka, gnu), u_ham) - mKy * (Ykn(:, gka, gnu) - fceH(Xkn(:, gka, gnu))); mA(1, 3) = Cb * sin(Xkn(4, gka, gnu)); mA(1, 4) = Cb * Xkn(3, gka, gnu) * cos(Xkn(4, gka, gnu)); mA(2, 3) = - Cb * cos(Xkn(4, gka, gnu)); mA(2, 4) = Cb * Xkn(3, gka, gnu) * sin(Xkn(4, gka, gnu)); mA(3, 1) = - Ce * sin(Xkn(4, gka, gnu)); mA(3, 2) = Ce * cos(Xkn(4, gka, gnu)); mA(3, 4) = - Ce * (Xkn(2, gka, gnu) * sin(Xkn(4, gka, gnu)) + Xkn(1, gka, gnu) * cos(Xkn(4, gka, gnu))); tPkn = mA * (Pkn(:, :, gka, gnu) - Pkn(:, :, gka, gnu) * mC' * inv(mRy) * mC * Pkn(:, :, gka, gnu)) * mA + mQ; tf = zeros(8,1); tf(1:4) = tXkn; tf(5:8) = diag(tPkn); loss = (tXkn(3) - omega_t_stripe)^2; 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] )); end function [val_Vt] = Vtilde(k_Vt, x_Vt, p_Vt) if(k_Vt == K) val_Vt = h_bel; else Epsl = zeros(8, 1); Epsl(1:4) = x_Vt(1:4) - Xstripe(1:4, k_Vt); Epsl(5:8) = diag(p_Vt ./ Pstripe(:, :, k)); val_Vt = vectFi(Epsl)' * Wv(:,k_Vt); end end function [val_Vt] = Vtilde_dx(k_Vt, x_Vt, p_Vt) if(k_Vt == K) val_Vt = h_beldx; else Epsl = zeros(8, 1); Epsl(1:4) = x_Vt(1:4) - Xstripe(1:4, k_Vt); Epsl(5:8) = diag(p_Vt ./ Pstripe(:, :, k)); val_Vt = difFi(Epsl)' * Wv(:,k_Vt); end end function [val_Fi] = vectFi(x_Fi) val_Fi = [ ... 1; ... %1 x_Fi(1); ... %Xi pro 1-4 x_Fi(2); ... x_Fi(3); ... x_Fi(4); ... log(x_Fi(5)); ... %ln Xi pro 5-8 tj diagonala matice Pt 4x4 log(x_Fi(6)); ... log(x_Fi(7)); ... log(x_Fi(8)); ... x_Fi(1)^2; ... %kvadraticke cleny jen v Xi 1-4 a kombinovane x_Fi(1)*x_Fi(2); ... x_Fi(1)*x_Fi(3); ... x_Fi(1)*x_Fi(4); ... x_Fi(1)*log(x_Fi(5)); ... x_Fi(1)*log(x_Fi(6)); ... x_Fi(1)*log(x_Fi(7)); ... x_Fi(1)*log(x_Fi(8)); ... x_Fi(2)^2; ... x_Fi(2)*x_Fi(3); ... x_Fi(2)*x_Fi(4); ... x_Fi(2)*log(x_Fi(5)); ... x_Fi(2)*log(x_Fi(6)); ... x_Fi(2)*log(x_Fi(7)); ... x_Fi(2)*log(x_Fi(8)); ... x_Fi(3)^2; ... x_Fi(3)*x_Fi(4); ... x_Fi(3)*log(x_Fi(5)); ... x_Fi(3)*log(x_Fi(6)); ... x_Fi(3)*log(x_Fi(7)); ... x_Fi(3)*log(x_Fi(8)); ... x_Fi(4)^2; ... x_Fi(4)*log(x_Fi(5)); ... x_Fi(4)*log(x_Fi(6)); ... x_Fi(4)*log(x_Fi(7)); ... x_Fi(4)*log(x_Fi(8)); ... ]; end function [val_Fi] = matrixFi(x_Fi) val_Fi = [ ... ones(1, N); ... %1 x_Fi(1, :); ... %Xi pro 1-4 x_Fi(2, :); ... x_Fi(3, :); ... x_Fi(4, :); ... log(x_Fi(5, :)); ... %ln Xi pro 5-8 tj diagonala matice Pt 4x4 log(x_Fi(6, :)); ... log(x_Fi(7, :)); ... log(x_Fi(8, :)); ... x_Fi(1, :).^2; ... %kvadraticke cleny jen v Xi 1-4 a kombinovane x_Fi(1, :).*x_Fi(2, :); ... x_Fi(1, :).*x_Fi(3, :); ... x_Fi(1, :).*x_Fi(4, :); ... x_Fi(1, :).*log(x_Fi(5, :)); ... x_Fi(1, :).*log(x_Fi(6, :)); ... x_Fi(1, :).*log(x_Fi(7, :)); ... x_Fi(1, :).*log(x_Fi(8, :)); ... x_Fi(2, :).^2; ... x_Fi(2, :).*x_Fi(3, :); ... x_Fi(2, :).*x_Fi(4, :); ... x_Fi(2, :).*log(x_Fi(5, :)); ... x_Fi(2, :).*log(x_Fi(6, :)); ... x_Fi(2, :).*log(x_Fi(7, :)); ... x_Fi(2, :).*log(x_Fi(8, :)); ... x_Fi(3, :).^2; ... x_Fi(3, :).*x_Fi(4, :); ... x_Fi(3, :).*log(x_Fi(5, :)); ... x_Fi(3, :).*log(x_Fi(6, :)); ... x_Fi(3, :).*log(x_Fi(7, :)); ... x_Fi(3, :).*log(x_Fi(8, :)); ... x_Fi(4, :).^2; ... x_Fi(4, :).*log(x_Fi(5, :)); ... x_Fi(4, :).*log(x_Fi(6, :)); ... x_Fi(4, :).*log(x_Fi(7, :)); ... x_Fi(4, :).*log(x_Fi(8, :)); ... ]; end function [val_Fi] = difFi(x_Fi) val_Fi = [ ... 0 0 0 0 0 0 0 0; ... 1 0 0 0 0 0 0 0; ... 0 1 0 0 0 0 0 0; ... 0 0 1 0 0 0 0 0; ... 0 0 0 1 0 0 0 0; ... 0 0 0 0 1/x_Fi(5) 0 0 0; ... 0 0 0 0 0 1/x_Fi(6) 0 0; ... 0 0 0 0 0 0 1/x_Fi(7) 0; ... 0 0 0 0 0 0 0 1/x_Fi(8); ... 2*x_Fi(1) 0 0 0 0 0 0 0; ... x_Fi(2) x_Fi(1) 0 0 0 0 0 0; ... x_Fi(3) 0 x_Fi(1) 0 0 0 0 0; ... x_Fi(4) 0 0 x_Fi(1) 0 0 0 0; ... log(x_Fi(5)) 0 0 0 x_Fi(1)/x_Fi(5) 0 0 0; ... log(x_Fi(6)) 0 0 0 0 x_Fi(1)/x_Fi(6) 0 0; ... log(x_Fi(7)) 0 0 0 0 0 x_Fi(1)/x_Fi(7) 0; ... log(x_Fi(8)) 0 0 0 0 0 0 x_Fi(1)/x_Fi(8); ... 0 2*x_Fi(2) 0 0 0 0 0 0; ... 0 x_Fi(3) x_Fi(2) 0 0 0 0 0; ... 0 x_Fi(4) 0 x_Fi(2) 0 0 0 0; ... 0 log(x_Fi(5)) 0 0 x_Fi(2)/x_Fi(5) 0 0 0; ... 0 log(x_Fi(6)) 0 0 0 x_Fi(2)/x_Fi(6) 0 0; ... 0 log(x_Fi(7)) 0 0 0 0 x_Fi(2)/x_Fi(7) 0; ... 0 log(x_Fi(8)) 0 0 0 0 0 x_Fi(2)/x_Fi(8); ... 0 0 2*x_Fi(3) 0 0 0 0 0; ... 0 0 x_Fi(4) x_Fi(3) 0 0 0 0; ... 0 0 log(x_Fi(5)) 0 x_Fi(3)/x_Fi(5) 0 0 0; ... 0 0 log(x_Fi(6)) 0 0 x_Fi(3)/x_Fi(6) 0 0; ... 0 0 log(x_Fi(7)) 0 0 0 x_Fi(3)/x_Fi(7) 0; ... 0 0 log(x_Fi(8)) 0 0 0 0 x_Fi(3)/x_Fi(8); ... 0 0 0 2*x_Fi(4) 0 0 0 0; ... 0 0 0 log(x_Fi(5)) x_Fi(4)/x_Fi(5) 0 0 0; ... 0 0 0 log(x_Fi(6)) 0 x_Fi(4)/x_Fi(6) 0 0; ... 0 0 0 log(x_Fi(7)) 0 0 x_Fi(4)/x_Fi(7) 0; ... 0 0 0 log(x_Fi(8)) 0 0 0 x_Fi(4)/x_Fi(8); ... ]; end function [c, ceq, GC, GCeq] = Cond2(x) c = x(1)*x(1) + x(2)*x(2) - cC1; ceq = []; GC = [2*x(1); 2*x(2)]; GCeq = []; end function [x_ret] = fceG(x_in, u_in) x_ret = zeros(4, 1); x_ret(1) = Ca * x_in(1) + Cb * x_in(3) * sin(x_in(4)) + Cc * u_in(1); x_ret(2) = Ca * x_in(2) - Cb * x_in(3) * cos(x_in(4)) + Cc * u_in(2); x_ret(3) = Cd * x_in(3) + Ce * (x_in(2) * cos(x_in(4)) - x_in(1) * sin(x_in(4))); x_ret(4) = x_in(4) + x_in(3) * DELTAt; end function [x_ret] = fceG_du(x_in, u_in) x_ret = zeros(4, 2); x_ret(1, 1) = Cc; x_ret(2, 2) = Cc; end function [y_ret] = fceH(x_in) y_ret = zeros(2, 1); y_ret(1) = x_in(1); y_ret(2) = x_in(2); end end