with(linalg):
#---------------------------------------------------------------------
#Package for Maple to be used in a linear systems course
#following for instance Kailath ch6.
#Author Per Hagander 911115, 
#latest modification 921130
#
#Contains the following functions:
#tf2rmfd    Transformation from matrix transfer function to right MFD 
#tf2lmfd    Transformation from matrix transfer function to left MFD
#isrmfd     Test if mfd is a valid right MFD
#islmfd     Test if mfd is a valid left MFD
#rightcf    Returns the greatest common right divisor of 
#           two matrix polynomials in 's' or 'q' of the same column dimension
#minrmfd    Transformation from arbitrary right MFD to minimal right MFD
#leftcf     Returns the greatest common right divisor of 
#           two matrix polynomials in 's' or 'q' of the same row dimension
#minlmfd    Transformation from arbitrary left MFD to minimal left MFD
#rmfd2lmfd  Transformation from arbitrary right MFD to minimal left MFD
#lmfd2rmfd  Transformation from arbitrary left MFD to minimal right MFD
#colind     Returns the maximal degree in each column
#Dhc        Returns the leading column coefficient matrix
#rowind     Returns the maximal degree in each row
#Dhr        Returns the leading row coefficient matrix
#colred     Transforms from arbitrary right MFD to column reduced MFD
#issp       Test if column reduced MFD is strictly proper
#controller Transforms an arbitrary right MFD to controller state space form
#SmithMcMillan Transforms a matrix transfer function to Smith-McMillan form
#ss2tf      Transformation from state space form to matrix transfer function
#rmfd2tf    Transforms right MFD to matrix transfer function
#lmfd2tf    Transforms left MFD to matrix transfer function
#pmd2ss     Transformation from PMD to state space
#Id         The identity matrix of dimension n
#---------------------------------------------------------------------
#Missing functions:
#Division algorithm to extract strictly proper part of MFD 
#---------------------------------------------------------------------
#
tf2rmfd := proc(g)
local i, localg, m, v, Den:
#Transformation from matrix transfer function to right MFD
#Author Per Hagander 911101
if not type(g,'matrix') then localg := evalm(g) else localg := g fi: 
if not type(localg,('matrix')(ratpoly(anything,s))) then 
  ERROR(`matrix entries must be  a quotient of two polynomials`) 
fi: 
m:=coldim(g):
Den:=array(1..m,1..m,diagonal):
for i to m do
  v:=map(denom,col(g,i)):
  Den[i,i]:=factor(lcm(convert(v,set)[])):
#converts first the vector to a set, then the set to a sequence
od:
table([nr=evalm(g&*Den),dr=eval(Den)]):
end:
#
tf2lmfd := proc(g)
local i, localg, p, v, Den:
#Transformation from matrix transfer function to left MFD
#Author Per Hagander 911101
if not type(g,'matrix') then localg := evalm(g) else localg := g fi: 
if not type(localg,('matrix')(ratpoly(anything,s))) then 
  ERROR(`matrix entries must be  a quotient of two polynomials`) 
fi: 
p:=rowdim(localg):
Den:=array(1..p,1..p,diagonal):
for i to p do
  v:=map(denom,row(localg,i)):
  Den[i,i]:=factor(lcm(convert(v,set)[])):
#converts first the vector to a set, then the set to a sequence
od:
table([nl=evalm(Den&*localg),dl=eval(Den)]):
end:
#
#----------------------------------------------------------------------
#
isrmfd:=proc(mfd)
 local tableok, dimok:
#Test if mfd is a valid right MFD
#Author Per Hagander 911101
if type(mfd,table) then 
  tableok:=evalb({indices(mfd)} minus {[nr],[dr]} = {}):
 else
  tableok:=false:
fi: 
if tableok and type(mfd[dr],'matrix') and type(mfd[nr],'matrix') then
  dimok:=evalb(
       (coldim(mfd[dr])=coldim(mfd[nr])) 
   and (coldim(mfd[dr])=rowdim(mfd[dr])) ):
 else
  tableok:=false: 
fi:
end:
#
islmfd:=proc(mfd)
 local tableok, dimok:
#Test if mfd is a valid left MFD
#Author Per Hagander 911101
if type(mfd,table) then 
  tableok:=evalb({indices(mfd)} minus {[nl],[dl]} = {}):
 else
  tableok:=false:
fi: 
if tableok and type(mfd[dl],'matrix') and type(mfd[nl],'matrix') then
  dimok:=evalb(
       (rowdim(mfd[dl])=rowdim(mfd[nl])) 
   and (coldim(mfd[dl])=rowdim(mfd[dl])) ):
 else
  tableok:=false: 
fi:
end:
#
#----------------------------------------------------------------------
#
rightcf := proc(Num,Den,sorq)
     local m, R:
#Returns the greatest common right divisor 
#of two matrix polynomials in 's' or 'q' of the same column dimension
#Author Per Hagander 911101
#Uses the linalg-function hermite
if not type(Num,'matrix') then ERROR(`1st argument must be a matrix`) fi: 
if not type(Den,'matrix') then ERROR(`2nd argument must be a matrix`) fi: 
if not type(sorq,name) then ERROR(`3nd argument must be a name`) fi: 
if not (type(Num,('matrix')(polynom(anything,s))) 
    and type(Den,('matrix')(polynom(anything,s))))  then 
  ERROR(`matrix entries must be  polynomials`) 
fi: 
m:=coldim(Num):
if not  m=coldim(Den) then 
  ERROR(`matrices must have the same number of columns`) 
fi: 
R:=hermite(stack(Den,Num),sorq):
map(factor,submatrix(R,1..m,1..m)):
end:
#
minrmfd:=proc(mfd)
  local Num, Den, R, Rinv:
#Transformation from arbitrary right MFD to minimal right MFD
#Author Per Hagander 911101
#Uses rightcf to extract right common factor of maximal determinantal degree
if not isrmfd(mfd) then ERROR(`The argument is not a valid right MFD`):fi:
Num:=eval(mfd[nr]):Den:=eval(mfd[dr]):
R:=rightcf(Num,Den,'s'):
Rinv:=inverse(R):
table([nr=evalm(Num &* Rinv),dr=evalm(Den &* Rinv)]):
end:
#
leftcf := proc(Num,Den,sorq)
     local p, R:
#Returns the greatest common left divisor of 
# two matrix polynomials in 's' or 'q' of the same row dimension
#Author Per Hagander 911107
#Uses the linalg-function hermite
if not type(Num,'matrix') then ERROR(`1st argument must be a matrix`) fi: 
if not type(Den,'matrix') then ERROR(`2nd argument must be a matrix`) fi: 
if not type(sorq,name) then ERROR(`3nd argument must be a name`) fi: 
if not (type(Num,('matrix')(polynom(anything,s))) 
    and type(Den,('matrix')(polynom(anything,s))))  then 
  ERROR(`matrix entries must be  polynomials`) 
fi: 
p:=rowdim(Num):
if not  p=rowdim(Den) then 
  ERROR(`matrices must have the same number of rows`) 
fi: 
R:=hermite(transpose(concat(Den,Num)),sorq):
R:=map(factor,transpose(submatrix(R,1..p,1..p))):
end:
#
minlmfd:=proc(mfd)
  local Num, Den, R, Rinv:
#Transformation from arbitrary left MFD to minimal left MFD
#Author Per Hagander 911101
#Uses leftcf to extract left common factor of maximal determinantal degree
if not islmfd(mfd) then ERROR(`The argument is not a valid left MFD`):fi:
Num:=eval(mfd[nl]):Den:=eval(mfd[dl]):
R:=leftcf(Num,Den,'s'):
Rinv:=inverse(R):
table([nl=evalm(Rinv &* Num),dl=evalm(Rinv &* Den)]):
end:
#
#----------------------------------------------------------------------
#
rmfd2lmfd:= proc(mfd)
  local m,p, dn, ru:
#Transformation from arbitrary right MFD to minimal left MFD
#Uses the linalg-function hermite
#Author Per Hagander 911114
if not isrmfd(mfd) then ERROR(`The argument is not a valid right MFD`):fi:
p:=rowdim(mfd[nr]):
m:=coldim(mfd[nr]):
n:=m+p:
dn:=concat(stack(mfd[dr],mfd[nr]),array(identity,1..m+p,1..m+p)):
ru:=hermite(dn,'s'):
table([dl=submatrix(ru,m+1..m+p,m+m+1..m+m+p),
       nl=evalm(-submatrix(ru,m+1..m+p,m+1..m+m))]):
end:
#
lmfd2rmfd:= proc(mfd)
  local Num, Den, rmfd, lmfd:
#Transformation from arbitrary left MFD to minimal right MFD
#Author Per Hagander 911114
if not islmfd(mfd) then ERROR(`The argument is not a valid left MFD`):fi:
Num:=transpose(eval(mfd[nl])):
Den:=transpose(eval(mfd[dl])):
rmfd:=table([nr=eval(Num),dr=eval(Den)]):
lmfd:=rmfd2lmfd(rmfd):
Num:=transpose(eval(lmfd[nl])):
Den:=transpose(eval(lmfd[dl])):
table([dr=eval(Den),nr=eval(Num)]):
end:
#
#----------------------------------------------------------------------
#
colind:=proc(Dr)
  local i, m, v, k:
#Returns the maximal degree in each column
#Author Per Hagander 911101
if not type(Dr,'matrix') then ERROR(`argument must be a matrix`) fi: 
if not type(Dr,('matrix')(polynom(anything,s)))   then 
  ERROR(`matrix entries must be  polynomials`) 
fi: 
m:=coldim(Dr):
k:=array(1..m):
for i to m do
  v:=map(collect,col(Dr,i),s):
  k[i]:=max(convert(map(degree,v,s),set)[]):
od:
eval(k):
end:
#
Dhc:=proc(Dr)
  local i, j, m, k, hc:
#Returns the leading column coefficient matrix
#Author Per Hagander 911101
if not type(Dr,'matrix') then ERROR(`argument must be a matrix`) fi: 
if not type(Dr,('matrix')(polynom(anything,s)))   then 
  ERROR(`matrix entries must be  polynomials`) 
fi: 
m:=coldim(Dr):
k:=colind(Dr):
hc:=array(1..m,1..m):
for j to m do
    for i to m do
    hc[i,j]:=coeff(collect(Dr[i,j],s),s,k[j]):
  od:
od:
eval(hc):
end:
#
rowind:=proc(Dl)
  local p, i, v, l:
#Returns the maximal degree in each row
#Author Per Hagander 911101, 
#Modified 921218, PH (- p included in list of locals)
if not type(Dl,'matrix') then ERROR(`argument must be a matrix`) fi: 
if not type(Dl,('matrix')(polynom(anything,s)))   then 
  ERROR(`matrix entries must be  polynomials`) 
fi: 
p:=rowdim(Dl):
l:=array(1..p):
for i to p do
  v:=map(collect,row(Dl,i),s):
  l[i]:=max(convert(map(degree,v,s),set)[]):
od:
eval(l):
end:
#
Dhr:=proc(Dl)
  local i, j, p, l, hc:
#Returns the leading row coefficient matrix
#Author Per Hagander 911101
if not type(Dl,'matrix') then ERROR(`argument must be a matrix`) fi: 
if not type(Dl,('matrix')(polynom(anything,s)))   then 
  ERROR(`matrix entries must be  polynomials`) 
fi: 
p:=rowdim(Dl):
l:=rowind(Dl):
hc:=array(1..p,1..p):
for i to p do
  for j to p do
    hc[i,j]:=coeff(collect(Dl[i,j],s),s,l[i]):
  od:
od:
eval(hc):
end:
#
#__________________________________________________________________________
#
colred:=proc(fmfd)
  local m, lmfd, nmfd:
#Transforms from arbitrary right MFD to column reduced MFD
#Author Per Hagander 911101
if not isrmfd(fmfd) then ERROR(`The argument is not a valid right MFD`):fi:
m:=coldim(fmfd[dr]):
if m=1 then return(fmfd) fi:
lmfd:=table([nr=eval(fmfd[nr]),dr=eval(fmfd[dr])]):
#needed because of one level only evaluation in procedures
#lmfd:=eval(fmfd):  would not evaluate the matrices
#and the value of the in argument of the function might thus change
while  rank(Dhc(lmfd[dr]))<m do
  lmfd:=colredstep(lmfd):
od:
lmfd[dr]:=map(factor,lmfd[dr]):
lmfd[nr]:=map(factor,lmfd[nr]):
eval(lmfd):
end:
colredstep:=proc(mfd)
  local m, i, j, k, store, hc, W, v, x, leave:
  #performs one step in the column degree reduction of colred
  #changes the formal argument mfd
  #
  #First permute columns so that 
  #the column indices appear in increasing order
  m:=coldim(mfd[dr]):
  k:=colind(mfd[dr]):
  for i to m-1 do
    for j from i+1 to m do
      if k[j]<k[i] then 
        mfd[dr]:=swapcol(mfd[dr],i,j):
        mfd[nr]:=swapcol(mfd[nr],i,j):
        store:=k[i]: k[i]:=k[j]: k[j]:=store:
      fi:
    od:
  od:
  # If Dhc is not full rank
  # transform the mfd 
  # so that the first dependent column of Dhc is eliminated
  # i.e. the corresponding col degree of the mfd is reduced
  # The exponents of s in the transformation are nonnegative, 
  # because of the sorting of k[i]
  hc:=Dhc(mfd[dr]):
  W:=submatrix(hc,1..m,1..1):
  leave:=0: 
  for i from 2 to m while leave=0 do
    v:=col(hc,i):
    if rank(concat(W,v))=i then 
      W:=concat(W,v):
    else
      x:=linsolve(W,v):
      for j to i-1 do
        mfd[dr]:=addcol(mfd[dr],j,i,-x[j]*s^(k[i]-k[j])):
        mfd[nr]:=addcol(mfd[nr],j,i,-x[j]*s^(k[i]-k[j])):
      od:
      leave:=1:
    fi:
  od:
  eval(mfd):
end:
#
#
issp:=proc(mfd)
  local p, m, k, hc, bool, j, v, boolj:
#Test if column reduced right MFD is strictly proper
#Author Per Hagander 911108
if not isrmfd(mfd) then ERROR(`The argument is not a valid right MFD`):fi:
p:=rowdim(mfd[nr]):
m:=coldim(mfd[nr]):
k:=colind(mfd[dr]):
hc:= Dhc(mfd[dr]):
if not rank(hc)=m then ERROR(`The MFD is not column reduced`):fi:
bool:=true:
for j to m do
  v:=map(collect,col(mfd[nr],j),s):
  boolj:=(k[j] > max(convert(map(degree,v,s),set)[]))
          or (k[j]=0 and iszero(submatrix(mfd[nr],1..p,j..j))):
  bool:=bool and boolj:
od:
eval(bool):
end:
#
#
#________________________________________________________________________
#
#
controller:=proc(fmfd)
  local m, p, lmfd, k, hc, hcinv, ABcore, n, Dlc, Nlc, j, l, i, coeffcolj:
#Transforms an arbitrary right MFD to controller state space form
#provided it is strictly proper
#Author Per Hagander 911108
#
if not isrmfd(fmfd) then ERROR(`The argument is not a valid right MFD`):fi:
m:=coldim(fmfd[nr]):
p:=rowdim(fmfd[nr]):
lmfd:=colred(fmfd):
if not issp(lmfd) then ERROR(`The MFD is not strictly proper`): fi:
k:=colind(lmfd[dr]):
hc:=Dhc(lmfd[dr]):
hcinv:=inverse(hc):
ABcore:=blockcore(k):
n:=sum(k[j],j=1..m):
Dlc:=array(1..m,1..n,sparse):
Nlc:=array(1..p,1..n,sparse):
lmfd[nr]:=map(collect,lmfd[nr],s):
lmfd[dr]:=map(collect,lmfd[dr],s):
n:=0:
for j to m do
 if k[j]>0 then
  for l to k[j] do
    n:=n+1:
    for i to p do Nlc[i,n]:=coeff(lmfd[nr][i,j],s,k[j]-l):od:
    for i to m do Dlc[i,n]:=coeff(lmfd[dr][i,j],s,k[j]-l):od:
  od:
 fi:
od:
table([a=evalm(ABcore[a]-ABcore[b]&*hcinv&*Dlc),
       b=evalm(ABcore[b]&*hcinv),
       c=evalm(Nlc)]):
end:
#
blockcore:=proc(k)
  local AB, ABn, first, m, i:
#Returns A and B matrices for a block core realization 
#Block sizes are given by the argument vector k
#Accepts k[i]=0, but there must exist at least one nonzero element
m:=vectdim(k):
first:=1:
if m>1 then
for i to m-1 while k[first]=0 do
  first:=first+1:
od:fi:
ABn:=core(k[first]):
if first>1 then 
  ABn[b]:=concat(matrix(rowdim(eval(ABn[b])),first-1,0),eval(ABn[b])):
fi:
if m>first then
 for i from first+1 to m do
   if k[i]>0 then
     AB:=core(k[i]):
     ABn:=table([a=diag2(ABn[a],AB[a]),b=diag2(ABn[b],AB[b])]):
   else
     ABn:=table([a=eval(ABn[a]),b=extend(eval(ABn[b]),0,1,0)]):
   fi:
  od:
fi:
eval(ABn):
end:
#
diag2:=proc(A1,A2)
  local big, m1, p1, m2, p2:
#Handles nonsquare matrices as well
m1:=coldim(A1):
p1:=rowdim(A1):
m2:=coldim(A2):
p2:=rowdim(A2):
big:=array(1..p1+p2,1..m1+m2,sparse):
big:=copyinto(A1,big,1,1):
big:=copyinto(A2,big,p1+1,m1+1):
end:
#
core:=proc(k)
  local A, B:
#requires k>0
A:=companion(s^k,s):
B:=array(1..k,1..1,sparse):
B[1,1]:=1:
table([a=eval(A),b=eval(B)]):
end:
#
#____________________________________________________________________________
#
SmithMcMillan := proc(g)
 local m, v, den, d, sm:
m:=coldim(g):
den:=array(1..m):
for i to m do
  v:=map(denom,col(g,i)):
  den[i]:=factor(lcm(convert(v,set)[])):
#converts first the vector to a set, then the set to a sequence
od:
d:=factor(lcm(convert(den,set)[])):
sm:=smith(map(normal,scalarmul(g,d)),s):
map(normal,scalarmul(sm,1/d)):
end:
#
#____________________________________________________________________________
#
ss2tf:=proc(abc):
evalm(abc[c]&*inverse(s*&*()-abc[a])&*abc[b]):
end:
#
rmfd2tf:=proc(mfd):
if not isrmfd(mfd) then ERROR(`The argument is not a valid right MFD`):fi:
evalm(mfd[nr]&*inverse(mfd[dr])):
end:
#
lmfd2tf:=proc(mfd):
if not islmfd(mfd) then ERROR(`The argument is not a valid left MFD`):fi:
evalm(inverse(mfd[dl])&*mfd[nl]):
end:
#
pmd2ss := proc(pmd)
local P, Q, R, W, g, gbar, Y, rmfd, abc, L, B:
#Transformation from polynomial matrix description (PMD) to state space
#Author Per Hagander 911115
P:=pmd[p]:
Q:=pmd[q]:
R:=pmd[r]:
W:=pmd[w]:
g:=evalm(R&*inverse(eval(P))):
Y:=polpart(g):
rmfd:=table([dr=eval(P),nr=evalm(R-Y&*P)]):
abc:=controller(rmfd):
g:=evalm(inverse(s*&*()-abc[a])&*abc[b]&*Q):
L:=polpart(g):
B:=map(factor,evalm(abc[b]&*Q-(s*&*()-abc[a])&*L)):
table([a=eval(abc[a]),b=eval(B),c=eval(abc[c]),d=evalm(W+Y&*Q+abc[c]&*L)]):
end:
#
polpart:=proc(g):
map(g->quo(numer(g),denom(g),s),g):
end:
sppart:=proc(g) :
map(factor,evalm(g-map(g->quo(numer(g),denom(g),s),g))):
end:
#
Id:=proc(n):
#Id         The identity matrix of dimension n
array(identity,1..n,1..n):
end: