#! /bin/sh

     ###################################### 
     ##### Model Transformation Tools #####
     ######################################

# Bourne shell script: sm2can_r
# state matrices to various canonical forms.
# P.J.Gawthrop  12 Jan 1997
# Copyright (c) P.J.Gawthrop 1997

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


# Inform user
echo Creating $1_can.r -- NOTE this is for SISO systems only.

# Remove the old log file
rm -f sm2can_r.log

# Use reduce to accomplish the transformation
reduce >sm2can_r.log << EOF

in "$1_def.r";
in "$1_sm.r";
in "$1_tf.r";

%Read the formatting function
in "$MTTPATH/trans/reduce_matrix.r";


OFF Echo;
OFF Nat;

% Find the controllability and observibility matrices.
MATRIX MTTCon(MTTNx,MTTNX);
MTTAB := MTTB;
FOR j := 1:MTTNx DO
  BEGIN
   FOR i := 1:MTTNx DO 
      MTTCon(i,j) := MTTAB(i,1);
   MTTAB := MTTA*MTTAB;
  END;

MATRIX MTTObs(MTTNx,MTTNX);
MTTCA := MTTC;
FOR i := 1:MTTNx DO
  BEGIN
   FOR j := 1:MTTNx DO 
      MTTObs(i,j) := MTTCA(1,j);
   MTTCA := MTTCA*MTTA;
  END;

%Canonical forms:
% This statement makes Gs a scalar transfer function
Gs := MTTtf(1,1);

% Numerator and denominator polynomials
bs := num(gs);
as := den(gs);

% extract coeficients and divide by coeff of s^n
% reverse operator puts list with highest oder coeffs first
ai := reverse(coeff(as,s));
a0 := first(ai);
MTTn := length(ai) - 1;


% Normalised coeficients;
ai := reverse(coeff(as/a0,s));
bi := reverse(coeff(bs/a0,s));
MTTm := length(bi)-1;

% Zap the (unity) first element of ai list;
ai := rest(ai);

% System in controller form
% MTTA_c matrix
matrix MTTA_c(MTTn,MTTn);

% First row is ai coefficients
for i := 1:MTTn do
  MTTA_c(1,i) := -part(ai,i);

% (MTTn-1)x(MTTn-1) unit matrix in lower left-land corner (if n>1)
if MTTn>1 then
  for i := 1:MTTn-1 do
    MTTA_c(i+1,i) := 1;

% B_c vector;
matrix MTTB_c(MTTn,1);
MTTB_c(1,1) := 1;

% C_c vector;
matrix MTTC_c(1,MTTn);
for i := 1:MTTm+1 do
  MTTC_c(1,i+MTTn-MTTm-1) := part(bi,i);

% D_c
MTTD_c := MTTD;

%Observable form.
MTTA_o := tp(MTTA_c);
MTTB_o := tp(MTTC_c);
MTTC_o := tp(MTTB_c);
MTTD_o := MTTD;

%Controllability matrix of controllable form
MATRIX MTTCon_c(MTTNx,MTTNX);
MTTAB := MTTB_c;
FOR j := 1:MTTNx DO
  BEGIN
   FOR i := 1:MTTNx DO 
      MTTCon_c(i,j) := MTTAB(i,1);
   MTTAB := MTTA_c*MTTAB;
  END;

% Transformation matrix;
MTTT_c := MTTCon_c*MTTCon^(-1);

%Observability matrix of observer form
MATRIX MTTObs_o(MTTNx,MTTNX);
MTTCA := MTTC_o;
FOR i := 1:MTTNx DO
  BEGIN
   FOR j := 1:MTTNx DO 
      MTTObs_o(i,j) := MTTCA(1,j);
   MTTCA := MTTCA*MTTA_o;
  END;

% Transformation matrix;
MTTT_o := MTTObs^(-1)*MTTObs_o;

%%%%  Controller design %%%%%

% gain in controller form:
matrix MTTk_c(1,mttn);
matrix alpha_c(9,1);
alpha_c(1,1) := alpha_c1;
alpha_c(2,1) := alpha_c2;
alpha_c(3,1) := alpha_c3;
alpha_c(4,1) := alpha_c4;
alpha_c(5,1) := alpha_c5;
alpha_c(6,1) := alpha_c6;
alpha_c(7,1) := alpha_c7;
alpha_c(8,1) := alpha_c8;
alpha_c(9,1) := alpha_c9;


for i := 1:MTTNx do
 MTTk_c(1,i) := alpha_c(i,1) - part(ai,i);

% Gain in physical form
MTTk := MTTk_c*MTTT_c;

%%%%  Observer design %%%%%
% gain in Observer form:
matrix MTTl_o(MTTn,1);
matrix alpha_o(9,1);
alpha_o(1,1) := alpha_o1;
alpha_o(2,1) := alpha_o2;
alpha_o(3,1) := alpha_o3;
alpha_o(4,1) := alpha_o4;
alpha_o(5,1) := alpha_o5;
alpha_o(6,1) := alpha_o6;
alpha_o(7,1) := alpha_o7;
alpha_o(8,1) := alpha_o8;
alpha_o(9,1) := alpha_o9;

for i := 1:MTTNx DO
 MTTL_o(i,1) := alpha_o(i,1) - part(ai,i);

% Gain in physical form
MTTL := MTTT_o*MTTL_o;

% Steady-state stuff
% Create the matrix [A B; C D];
matrix ABCD(MTTn+1,MTTn+1);
for i := 1:MTTNx do
  for j := 1:MTTNx do
    ABCD(i,j) := MTTA(i,j);

for i :=1:MTTNx do
  ABCD(i,MTTNx+1) := MTTB(i,1);

for j := 1:MTTNx do
  ABCD(MTTNx+1,j) := MTTC(1,j);

ABCD(MTTNx+1,MTTNx+1) := MTTD(1,1);

matrix zero_one(MTTNx+1,1);
zero_one(MTTNx+1,1) := 1;

%Find N vector
Nxu := ABCD^(-1)*zero_one;

%Extract the parts
MATRIX MTTX_r(MTTNx,1);
FOR i := 1:MTTNx DO
  MTTX_r(i,1) := Nxu(i,1);

MTTu_r := Nxu(MTTNx+1,1);


% Compensator

matrix zero(MTTNx,1);
%State matrices
MTTA_comp := MTTA - MTTL*MTTC - MTTB*MTTK;
MTTB_comp := -MTTL;
MTTC_comp := -MTTK;

%Transfer function
%Ds := C_d*((s*MTTI - A_d)^(-1))*B_d;
%MTTTFC := Ds;

%Create the output file
OUT "$1_can.r";

%Write out the matrices.

% Controllable form
MTT_Matrix := MTTA_c$ 
MTT_Matrix_name := "MTTA_c"$
MTT_Matrix_n := MTTNx$
MTT_Matrix_m := MTTNx$
Reduce_Matrix()$

MTT_Matrix := MTTB_c$ 
MTT_Matrix_name := "MTTB_c"$
MTT_Matrix_n := MTTNx$
MTT_Matrix_m := MTTNu$
Reduce_Matrix()$

MTT_Matrix := MTTC_c$ 
MTT_Matrix_name := "MTTC_c"$
MTT_Matrix_n := MTTNy$
MTT_Matrix_m := MTTNx$
Reduce_Matrix()$


% Observable form
MTT_Matrix := MTTA_o$ 
MTT_Matrix_name := "MTTA_o"$
MTT_Matrix_n := MTTNx$
MTT_Matrix_m := MTTNx$
Reduce_Matrix()$

MTT_Matrix := MTTB_o$ 
MTT_Matrix_name := "MTTB_o"$
MTT_Matrix_n := MTTNx$
MTT_Matrix_m := MTTNu$
Reduce_Matrix()$

MTT_Matrix := MTTC_o$ 
MTT_Matrix_name := "MTTC_o"$
MTT_Matrix_n := MTTNy$
MTT_Matrix_m := MTTNx$
Reduce_Matrix()$

write "%  - Controllability matrix";
MTT_Matrix := MTTCon$ 
MTT_Matrix_name := "MTTCon"$
MTT_Matrix_n := MTTNx$
MTT_Matrix_m := MTTNx$
Reduce_Matrix()$

write "%  -Observability matrix";
MTT_Matrix := MTTObs$ 
MTT_Matrix_name := "MTTObs"$
MTT_Matrix_n := MTTNx$
MTT_Matrix_m := MTTNx$
Reduce_Matrix()$

write "%  -Controllability matrix - controller form";
MTT_Matrix := MTTCon_c$ 
MTT_Matrix_name := "MTTCon_c"$
MTT_Matrix_n := MTTNx$
MTT_Matrix_m := MTTNx$
Reduce_Matrix()$

write "%  - Transformation matrix - controller form";
MTT_Matrix := MTTT_c$ 
MTT_Matrix_name := "MTTT_c"$
MTT_Matrix_n := MTTNx$
MTT_Matrix_m := MTTNx$
Reduce_Matrix()$

write "%  - Gain matrix - controller form";
MTT_Matrix := MTTK_c$ 
MTT_Matrix_name := "MTTK_c"$
MTT_Matrix_n := MTTNu$
MTT_Matrix_m := MTTNx$
Reduce_Matrix()$

write "%  - Gain matrix - physical form";
MTT_Matrix := MTTK$ 
MTT_Matrix_name := "MTTK"$
MTT_Matrix_n := MTTNu$
MTT_Matrix_m := MTTNx$
Reduce_Matrix()$


write "%  -Observability matrix - Observer form";
MTT_Matrix := MTTObs_o$ 
MTT_Matrix_name := "MTTObs_o"$
MTT_Matrix_n := MTTNx$
MTT_Matrix_m := MTTNx$
Reduce_Matrix()$

write "%  - Transformation matrix - Observer form";
MTT_Matrix := MTTT_o$ 
MTT_Matrix_name := "MTTT_o"$
MTT_Matrix_n := MTTNx$
MTT_Matrix_m := MTTNx$
Reduce_Matrix()$

write "%  - Observer Gain matrix - observer form";
MTT_Matrix := MTTL_o$ 
MTT_Matrix_name := "MTTL_o"$
MTT_Matrix_n := MTTNx$
MTT_Matrix_m := MTTNy$
Reduce_Matrix()$

write "%  - Gain matrix - physical form";
MTT_Matrix := MTTL$ 
MTT_Matrix_name := "MTTL"$
MTT_Matrix_n := MTTNx$
MTT_Matrix_m := MTTNy$
Reduce_Matrix()$


% Controllable form
MTT_Matrix := MTTA_comp$ 
MTT_Matrix_name := "MTTA_comp"$
MTT_Matrix_n := MTTNx$
MTT_Matrix_m := MTTNx$
Reduce_Matrix()$

MTT_Matrix := MTTB_comp$ 
MTT_Matrix_name := "MTTB_comp"$
MTT_Matrix_n := MTTNx$
MTT_Matrix_m := MTTNu$
Reduce_Matrix()$

MTT_Matrix := MTTC_comp$ 
MTT_Matrix_name := "MTTC_comp"$
MTT_Matrix_n := MTTNy$
MTT_Matrix_m := MTTNx$
Reduce_Matrix()$


KX := MTTK*MTTX_r;
MTTu_r := MTTu_r + KX(1,1);



MTTu_r := MTTu_r;
write "END;";

SHUT "$1_can.r";
quit;

EOF

# Now invoke the standard error handling.
mtt_error_r sm2can_r.log
