Post Reply 
Control System Programs
06-28-2020, 01:19 AM
Post: #1
Control System Programs
I created three programs that could potentially be useful for anyone in a control systems class. I plan to add more, hopefully, in the future. I tried to make sure everything is working properly and correctly but then I'll keep spending weeks checking and rechecking the code. I'd rather someone find it useful instead of me testing it.

The first program will take as input the system matrix (a) and the input matrix (b) to find the controllability matrix. With it there is another function that will check if the controllability matrix found is controllable or not.

Code:
EXPORT CTRB(a,b)
BEGIN
LOCAL k,Cm,Cmsolve;
IF (rowDim(a) == colDim(a) AND rowDim(a) == rowDim(b) AND SIZE(SIZE(b)) >1) THEN
Cm := MAKEMAT(0,rowDim(a),rowDim(a)*colDim(b));
 FOR k FROM 1 TO rowDim(a) DO
  Cmsolve := a^(k-1)*b;
  Cm := REPLACE(Cm,{1,k*colDim(b)-(colDim(b)-1)},Cmsolve);
 END;
ELSE
 RETURN "Matrix A should be square, and the rows of Matrices A and B should have the same amount of elements.";
END;
RETURN Cm;
END;

EXPORT isctrb(Cm)
BEGIN
 IF (abs(DET(Cm)) >= 1 AND RANK(Cm) == rowDim(Cm)) THEN
  RETURN "Controllable";
 ELSE
  RETURN "Uncontrollable";
 END;
END;

The next program will take the system matrix (a) and the output matrix (c) as inputs to find the observability matrix. Along with it is another function to check if the observability matrix is observable.

Code:
EXPORT OBSV(a,c)
BEGIN
LOCAL k,Om,Omsolve;
IF (SIZE(SIZE(c)) == 1) THEN
 c := TRN(list2mat(c,1));
END;
Om := MAKEMAT(0,rowDim(a)*rowDim(c),rowDim(a));
IF (rowDim(a) == colDim(a) AND rowDim(a) == colDim(c)) THEN
 FOR k FROM 1 TO rowDim(a) DO
  Omsolve := c*a^(k-1);
  Om := REPLACE(Om,{k*rowDim(c)-(rowDim(c)-1),1},Omsolve);
 END;
ELSE
 RETURN "Matrix A should be square, and the columns of Matrices A and C should have the same amount of elements.";
END;
RETURN Om;
END;

EXPORT isobsv(Om)
BEGIN
 IF (abs(DET(Om)) >= 1 AND RANK(Om) == rowDim(Om)) THEN
  RETURN "Observable";
 ELSE
  RETURN "Unobservable";
 END;
END;

This function below uses Ackermann's Formula to find the feedback constants. This function requires the use of the CTRB(a,b) function. It takes as input the system matrix (a), the input matrix (b), and the desired characteristic equation coefficients.

Code:
EXPORT ACKER(a,b,ce)
BEGIN
LOCAL c,cm,sc,sP,cemsolve,p,icm,k;
cm := CTRB(a,b);
 IF (abs(DET(cm)) >= 1 AND RANK(cm) == rowDim(a)) THEN
  sP := SIZE(ce);
  sP := sP(1);
  cemsolve := 0;
  p := revlist(ce);
  sc := SIZE(cm);
  sc := sc(1);
  c := [0];
  FOR W FROM 1 TO sc-2 DO
   c := append(c,0);
  END;
  c := append(c,1);
  FOR Q FROM sP DOWNTO 1 DO
    cemsolve := cemsolve+p(Q)*a^(Q-1);
  END;
  icm := inv(cm);
  k := c*icm*cemsolve;
  RETURN k;
 ELSE
  RETURN "System is uncontrollable, the determinate is zero and the rank does not match the system matrix";
 END;
END;

This program converts the system from a statespace model to a transfer function. It takes as arguments the system matrix (a), the input matrix (b), the output matrix (d) and the feedthrough matrix (d). This program requires the CAS but if someone knows how it would be cool to be able to output just the coefficients of the numerator and denominator of the transfer function. With it being in the CAS it may be nicer for someone studying though.

There is an area in the code that I added a bunch of commands to convert the denominator to a monic polynomial, and this was the way for me to output it correctly, but there is still issues with it, like it outputting a s+-K instead of s-K for example. I'm sure this could be improved.

Code:
#cas
ss2tf(a,b,c,d) :=
BEGIN
LOCAL tfnum,tfden,tfmonic,tftemp,tf,k,q;
IF TYPE(SIZE(c)) == 0 THEN
 c := TRN(list2mat(c,1));
END;
CASE
  IF (TYPE(d) == 0 OR TYPE(d) == 3) THEN d := [[d]] END;
  IF (TYPE(SIZE(d)) == 0) THEN d := TRN(d) END;
  DEFAULT BREAK END;
 IF (rowDim(a) == colDim(a) AND rowDim(b) == colDim(c) AND rowDim(a) == rowDim(b) AND rowDim(a) == colDim(c) AND (SIZE(d) == [1, 1] OR (colDim(d) == colDim(b) AND rowDim(c) == rowDim(d)))) THEN
  tf := (c*(((s*IDENMAT(rowDim(a)))-a)^-1)*b)+d;
  FOR k FROM 1 TO rowDim(d) DO
   FOR q FROM 1 TO colDim(d) DO
    tftemp := tf(k,q);
    tfnum := numer(tftemp);
    tfden := denom(tftemp);
    tfmonic := coeff(tfden,s)(1);
    tfnum := approx((tfnum)/tfmonic);
    tfden := approx((tfden)/tfmonic);
    tfnum := expand(tfnum);
    tfden := expand(tfden);
    tftemp := [tfnum/tfden];
    tf := REPLACE(tf,{k,q},tftemp);
   END;
  END;
  tf := approx(exact(tf));
 ELSE
  RETURN "The number of rows and columns in Matrix A need to match those in B and C respectively. The rows and columns of the D Matrix need to match C and B respectively."
 END;
 IF (SIZE(d) == [1, 1]) THEN
  tf := tf(1,1);
 ELSE
  RETURN tf;
 END;
END;
#end

This program converts the system from a transfer function to statespace form. It takes as inputs the numerator and denominator either as a function or as a vector containing the coefficients. The third input (form) is either 0 for Phase-Variable form and 1 for observable canonical form. It only outputs the system matrix (a). To output the other matrices just insert b, c, and d into the CAS console. If you want to check the statespace with MATLAB's take the transpose of the output of form 1.

Code:
#cas
tf2ss(num,den,form) :=
BEGIN
LOCAL A1,A2,Ai2,sA1,Snum,Aden;
CASE
 IF (TYPE(SIZE(num)) == 4 OR TYPE(SIZE(den)) == 4) THEN RETURN "Numerator and Denominator must be vectors"; END;
 IF TYPE(den) == 8 OR TYPE(num) == 8 THEN
  Aden := expand(den);
  Aden := symb2poly(den,s);
  Snum := expand(num);
  Snum := symb2poly(num,s);
  Snum := EXPR(STRING(Snum));
  Aden := EXPR(STRING(Aden));
  
 END;
 IF TYPE(den) == 0 OR 1 OR 4 OR 6 OR TYPE(num) == 0 OR 1 OR 4 OR 6 THEN
  Aden := den;
  Snum := num;
 END;
 DEFAULT RETURN "Args should be poly/(s-p1)*(s-pn) or list/vector";
 END;

 IF (Snum == Aden) THEN RETURN "Numerator is the same as the denominator"; END;

WHILE SIZE(Snum) < SIZE(Aden) DO
  Snum := INSERT(Snum,1,0);
  IF SIZE(Snum) == SIZE(Aden) THEN BREAK; END;
END;

 Snum := approx(Snum./Aden(1));
 Aden := approx(Aden./Aden(1));

 CASE
  IF (form == 0) THEN
   A1 := -(REVERSE(Aden));
   sA1 := size(A1)-1;
   A2 := A1(1..(sA1));
   Ai2 := IDENMAT(sA1);
   a := ADDROW(Ai2,A2,sA1+1);
   a := DELROW(a,1);

   b := MAKEMAT(0,sA1,1);
   b := REPLACE(b,sA1,[1]);

   qrinfo := quorem(Snum,Aden);
   c := qrinfo(2);
   d := TRN(TRN(qrinfo(1)));
   IF (SIZE(qrinfo(2)) != rowDim(a)) THEN
    WHILE SIZE(qrinfo(2)) < rowDim(a) DO
     c := INSERT(c,1,0);
     IF SIZE(c) == rowDim(a) THEN BREAK; END;
    END;
   END;
  c := TRN(TRN(revlist(c)));
  RETURN a;
 END;

 IF (form == 1) THEN
   A1 := -(Aden);
   sA1 := size(A1)-1;
   A2 := A1(2..(sA1+1));
   Ai2 := IDENMAT(sA1);
   a := ADDCOL(Ai2,A2,1);
   a := DELCOL(a,sA1+1);

   qrinfo := quorem(Snum,Aden);
    b := qrinfo(2);
    d := TRN(TRN(qrinfo(1)));
    IF (SIZE(qrinfo(2)) != rowDim(A3)) THEN
     WHILE SIZE(qrinfo(2)) < rowDim(a) DO
      b := INSERT(b,1,0);
      IF SIZE(b) == rowDim(a) THEN BREAK; END;
     END;
    END;
   b := TRN(b);

   c := MAKEMAT(0,1,sA1);
   c := REPLACE(c,1,[1]);

   RETURN a;
  END;
   DEFAULT RETURN "Invalid form, choose 0 for Phase Var. Canonical or 1 for Obsv. Canon. Form"
  END;
END;
#end

I took the comments out of this code but I will upload the .hpprgm files with them if you want to understand it better. The code is pretty bad on some of the programs, and it is kind of a pain to work with matrices and vectors on the HP Prime. If anyone wants to try it out just remember I just took some classes on this, there may be times when the program is wrong and I haven't taken it into account.

Anyone is welcome to improve the code or provide suggestions to improve the code since it definitely needs work.
Find all posts by this user
Quote this message in a reply
Post Reply 




User(s) browsing this thread: 1 Guest(s)