# Numerical parameter file (TwoLinkGPC_input.txt)
# Generated by MTT at Mon Apr 13 11:02:37 BST 1998

# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
# %% Version control history
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
# %% $Id$
# %% $Log$
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global ObserverGain;

if t<10
  tau = 0;
else
  tau = 0.1;
end

# Reference signals
Period = 20;
u(1) =	 -pi*sign(cos(2*pi*t/Period))*(t>5); # TwoLinkGPC (Ref_1)
u(2) =	 pi*sign(sin(2*pi*t/Period)); # TwoLinkGPC (Ref_2)
w = u(1:2);			      # Reference signal

# Control signals
x_model = x(9:12);		# Observed states
[O_o, O_h] = TwoLinkPS_obs(x_model) #GPC O function (affine case)
[spec_n,spec_d] = msdP_tf;	# Spec transfer function
P = spec_d(3:-1:1);		# P poly with ascending coeffs
PP = [eye(2)*P(1) eye(2)*P(2) eye(2)*P(3)]; # Matrix version
PO_o = PP*O_o;			# Compute Py with u zero
PO_h = PP*O_h;			# Coefficient of u
u_control =  PO_h\(w - PO_o);	# GMV control signal

u(3:4) = u_control;		# TwoLinkGPC (Tau_1)

# Observer
x_system = x(5:8);		# System states
error = x_system-x_model;	# Error in states
ObserverInput = ObserverGain*error;
u(5:8) = ObserverInput;		# Set the state-input to the observer