close all clear all clc %% Aufgabe 1a dt = 0.01; N = 10/dt; x0 = [0;0;0;0]; u = 0.3*ones(1,N); [X, Y, T] = nlsim(@tractor_trailer_ode_v_pos, x0, u, dt, @tractor_trailer_out); %% Aufgabe 1b % Plotten der Ausgaenge figure; plot(T,Y) legend('xT','yT','xA','yA') grid on % Plotten der Zustaende figure; plot(T,X) legend('xT','yT','\beta','\gamma') grid on % Plotten der Positionen figure; plot(Y(1,:),Y(2,:)) hold on plot(Y(3,:),Y(4,:)) axis equal legend('Traktor','Anhaenger') grid on %% Aufgabe 1c % animation(X,u,dt) %% Aufgabe 1d [Xbwd, Ybwd, Tbwd] = nlsim(@tractor_trailer_ode_v_neg, x0, u, dt, @tractor_trailer_out); % animation(Xbwd,u,dt) %% Aufgabe 2a V = 5; L1 = 6; L2 = 4; x_ss = [0;0]; u_ss = [0]; A = [0 0; V/L2*(cos(x_ss(1)-x_ss(2))) -V/L2*(cos(x_ss(1)-x_ss(2)))]; B = [V/L1/cos(u_ss)^2; 0]; C = [0 1]; D = 0; %% Aufgabe 2b ss_fwd = ss(A,B,C,D); %% Aufgabe 2c eig(A) %% Aufgabe 2d V = -5; A = [0 0; V/L2*(cos(x_ss(1)-x_ss(2))) -V/L2*(cos(x_ss(1)-x_ss(2)))]; B = [V/L1/cos(u_ss)^2; 0]; C = [0 1]; D = 0; ss_bwd = ss(A,B,C,D); eig(A) %% Aufgabe 2e [y,t_fwd,x_fwd] = lsim(ss_fwd, u, T(1:end-1), x_ss); [y,t_bwd,x_bwd] = lsim(ss_bwd, u, T(1:end-1), x_ss); %% Aufgabe 2f figure; plot(t_fwd,x_fwd(:,1),'-r'); hold on plot(T,X(3,:),'--r') plot(t_fwd,x_fwd(:,2),'-b'); plot(T,X(4,:),'--b') legend({'$\beta$','$\beta_{\mathrm{nl}}$', '$\gamma$','$\gamma_{\mathrm{nl}}$'}, 'Interpreter', 'latex', ... 'Location', 'southeast'); grid on figure; plot(t_bwd,x_bwd(:,1),'-r'); hold on plot(Tbwd,Xbwd(3,:),'--r') plot(t_bwd,x_bwd(:,2),'-b'); plot(Tbwd,Xbwd(4,:),'--b') legend({'$\beta$','$\beta_{\mathrm{nl}}$', '$\gamma$','$\gamma_{\mathrm{nl}}$'}, 'Interpreter', 'latex', ... 'Location', 'southeast'); ylim([-4 4]) grid on %% Aufgabe 2g animation([zeros(N,2) x_bwd]',u,dt)