% Lecture #19 % Gantry plus full-order observer X = [-1;0;0;0]; dX = zeros(4,1); Ref = 0; dt = 0.025; U = 0; t = 0; %Observer Ae = [0,0,1,0;0,0,0,1;0,4.9,0,0;0,-14.7,0,0]; Be = [0;0;0.5;-0.5]; Ce = [1,0,0,0]; Xe = X + 0.2*rand(4,1); H = [10, -19.7959, 20.3, -56]'; y = []; n = 0; while(t < 12) U = 2*cos(t); Ref = 0; dX = GantryDynamics(X, U); dXe = Ae*Xe + Be*U + H*(X(1) - Xe(1)); X = X + dX * dt; Xe = Xe + dXe * dt; t = t + dt; GantryDisplay3(X, Xe, Ref); plot([Ref, Ref],[-0.1,0.1],'b'); y = [y ; X(1), Xe(1) ]; end hold off t = [1:length(y)]' * dt; plot(t,y);