% Computes the penalty matrices for the state variable and the

% control input

Q = diag([10 300 0 0.5 2]);

R = 0.03;

% Specifies the sampling time for the digital controller

Ts = 0.001;

% Extracts the digital state space model from the continous state % space matrices

% Computes the digital Linear Quadratic Regulator Gains

[K,S,E] = dlqr(Ad,Bd,Q,R);

% Conversion from V/rad to V/deg

K(1) = K(1)*pi/180;

K(3) = K(3)*pi/180;

K(5) = K(5)*pi/180;

% Converstion from V/m to V/cm

K(2) = K(2)/100;

K(4) = K(4)/100;

% Defines cut-off frequency for encoder sensor

w_filter1 = 60;

% Defines the continuous time derivative and noise filter for

% encoder

filter1 = tf([w_filter1 0], [1 w_filter1]);

% Extracts the digital derivative and noise filter for encoder

dfilter1 = c2d(filter1,Ts);

% Extracts numerator and denominator for dfilter1

% Defines cut-off frequency for camera sensor

w_filter2 = 70;

% Defines the continuous time derivative and noise filter for

% encoder

filter2 = tf([w_filter2 0], [1 w_filter2]);

% Extracts the digital derivative and noise filter for encoder

dfilter2 = c2d(filter2,Ts);

% Extracts numerator and denominator for dfilter1

% End of m-file