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

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

global ObserverGain;

# Reference signals
u(1) =	 pi + pi*(t>10) ;			# TwoLinkGMV (Ref_1)
u(2) =	 pi*(t>15) ;			# TwoLinkGMV (Ref_2)

# Control signals
w = u(1:2);			# Reference signal
x_model = x(9:12);		# Observed states
[O_o, O_h] = TwoLinkP_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)]; 
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;		# TwoLinkGMV (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