Control System Programs
06-28-2020, 01:19 AM
Post: #1
 Major Junior Member Posts: 10 Joined: Jan 2020
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 := ;   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,);    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,);    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.
 « Next Oldest | Next Newest »

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