function pmsm_ildp %verze ildp algoritmu s uvazovanim P (v logaritmu) pro PMSM motor tic %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %pocatecni konstanty Iterace = 1; %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.5; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %globalni promenne Kpi_alfa = zeros(5, 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_beta = zeros(5, 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); 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('GradObj','on','GradConstr','on','Display','notify')); 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); end mPsi = [tomega',... -tialfa'.*sin(ttheta)',... tibeta'.*cos(ttheta)',... -Uopt2(1,:)'.*sin(ttheta)',... ones(N,1)]; PsiPsiTInvPsi = (mPsi'*mPsi)\mPsi'; Kpi_alfa(:, k) = PsiPsiTInvPsi * (omega_t_stripe * ones(N, 1)); mPsi = [tomega',... -tialfa'.*sin(ttheta)',... tibeta'.*cos(ttheta)',... Uopt2(2,:)'.*cos(ttheta)',... ones(N,1)]; PsiPsiTInvPsi = (mPsi'*mPsi)\mPsi'; Kpi_beta(:, k) = PsiPsiTInvPsi * (omega_t_stripe * ones(N, 1)); end end %%%%%%%%%%% toc % keyboard %vykresleni grafu % clf 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,Xkn(1,:,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(2, k_uPi)*x_uPi(1)*sin(x_uPi(4)) + Kpi_alfa(3, k_uPi)*x_uPi(2)*cos(x_uPi(4)) + Kpi_alfa(5)) / (Kpi_alfa(4)*sin(x_uPi(4))); val_uPi(2) = ( omega_t_stripe - Kpi_beta(1, k_uPi)*x_uPi(3) + Kpi_beta(2, k_uPi)*x_uPi(1)*sin(x_uPi(4)) - Kpi_beta(3, k_uPi)*x_uPi(2)*cos(x_uPi(4)) - Kpi_beta(5)) / (Kpi_beta(4)*cos(x_uPi(4))); if (val_uPi(1)cUb) val_uPi(1) = cUb; end if (val_uPi(2)cUb) val_uPi(2) = cUb; end end function [val_ham, val_grad] = Hamilt(u_ham) %u_ham = vect(2,1) % val_ham = (Xkn(1, gka, gnu) + Xkn(2, gka, gnu)*u_ham - Rk(gka+1))^2 + Xkn(3, gka, gnu)*u_ham^2 ... + sigma^2 ... %ztrata l % + [Xkn(2, gka, gnu)*u_ham; ... % Xkn(3, gka, gnu)*u_ham*(Xkn(1, gka+1, gnu) - Xkn(1, gka, gnu) - Xkn(2, gka, gnu)*u_ham)/(Xkn(3, gka, gnu)*u_ham^2 + sigma^2); ... % -Xkn(3, gka, gnu)^2*u_ham^2/(Xkn(3, gka, gnu)*u_ham^2 + sigma^2)]' ... %fce f % *Vtilde_dx(gka+1, Xkn(:, gka, gnu)) ... % + Wv(5, gka+1)*sigma; loss = (Xkn(3, gka, gnu) - omega_t_stripe)^2; 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); 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] )); tfdu = zeros(8,2); tXkn = fceG_du(Xkn(:, gka, gnu), u_ham); tfdu(1:4,:) = tXkn; val_grad = tfdu' * Vtilde_dx(gka+1, Xkn(:, gka, gnu), Pkn(:, :, gka, gnu)); % val_grad = 2*(Xkn(1, gka, gnu) + Xkn(2, gka, gnu)*u_ham - Rk(gka+1))*Xkn(2, gka, gnu) + 2*Xkn(3, gka, gnu)*u_ham ... %ztrata du % + [Xkn(2, gka, gnu);... % (2*u_ham^2*Xkn(3, gka, gnu)^2*(Xkn(1, gka, gnu) - Xkn(1, gka+1, gnu) + u_ham*Xkn(2, gka, gnu)))/(sigma^2 + u_ham^2*Xkn(3, gka, gnu))^2 - (u_ham*Xkn(2, gka, gnu)*Xkn(3, gka, gnu))/(sigma^2 + u_ham^2*Xkn(3, gka, gnu)) - (Xkn(3, gka, gnu)*(Xkn(1, gka, gnu) - Xkn(1, gka+1, gnu) + u_ham*Xkn(2, gka, gnu)))/(sigma^2 + u_ham^2*Xkn(3, gka, gnu));... % (2*u_ham^3*Xkn(3, gka, gnu)^3)/(sigma^2 + u_ham^2*Xkn(3, gka, gnu))^2 - (2*u_ham*Xkn(3, gka, gnu)^2)/(sigma^2 + u_ham^2*Xkn(3, gka, gnu))]' ... %fce f du % * Vtilde_dx(gka+1, Xkn(:, gka, gnu)); %derivace Bellman fce 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