342 lines
10 KiB
Matlab
342 lines
10 KiB
Matlab
%% Routh-Hurwitz Stability Criterion Table Generator V1.1
|
|
|
|
% Author: Varun Bhatia [bhatiav16@berkeley.edu]
|
|
|
|
% Date: July 28, 2019
|
|
|
|
% This function goes through the process of setting up a Routh-Hurwitz
|
|
% table to determine information regarding the in/stability of a control
|
|
% system given a closed/open-loop transfer function. More specifically, this
|
|
% will solve for and output how many closed/open-loop poles are in the
|
|
% right-half plane, the left-half plane, and on the jw-axis. Additionally,
|
|
% this function will take into account and generate the Routh_Hurwitz table
|
|
% given two special cases:
|
|
% 1)First element of a row is 0.
|
|
% 2)Entire Row is 0.
|
|
|
|
%Recall that this function operates under the assumption that you are
|
|
%inputting a CLOSED LOOP TRANSFER FUNCTION. Later developments will
|
|
%incorporate inputs of open loop forward transfer functions with unity
|
|
%and/or nonunity feedback.
|
|
|
|
%Any usage of MATLAB functions is accredited to MathWorks.
|
|
|
|
%UPDATE 1.1: This program now takes into account UNITY feedback.
|
|
|
|
%%
|
|
|
|
function []= routh_hurwitz()
|
|
|
|
prompt_n = 'Enter highest power in numerator: ';
|
|
num_hp = input(prompt_n);
|
|
num_hp_added = num_hp + 1;
|
|
num = zeros(1, num_hp_added);
|
|
|
|
for i = 1:num_hp+1
|
|
num(i) = input('Enter coefficients, starting with the highest power: ');
|
|
end
|
|
|
|
prompt_d = 'Enter highest power in denominator: ';
|
|
den_hp = input(prompt_d);
|
|
den_hp_added = den_hp + 1;
|
|
den = zeros(1, den_hp_added);
|
|
|
|
for i = 1:den_hp+1
|
|
den(i) = input('Enter coefficients, starting with the highest power: ');
|
|
end
|
|
|
|
prompt_TF = 'Are you entering an open loop or closed loop TF? If closed loop type "C", if open loop type "O" with quotes around letter: ';
|
|
TF_type = input(prompt_TF);
|
|
|
|
if TF_type == "O"
|
|
|
|
H = tf(num,den);
|
|
K = 1;
|
|
G = feedback(H,K)
|
|
[~,d] = tfdata(G, 'v');
|
|
den = d; %to acquire the new denominator coefficients from post-feedback
|
|
|
|
elseif TF_type == "C"
|
|
|
|
G = tf(num,den)
|
|
|
|
end
|
|
|
|
|
|
|
|
if den_hp == 0
|
|
cols = 1;
|
|
elseif den_hp == 1 | den_hp == 2
|
|
cols = 2;
|
|
elseif den_hp == 3 | den_hp == 4
|
|
cols = 3;
|
|
elseif den_hp == 5 | den_hp == 6
|
|
cols = 4;
|
|
elseif den_hp == 7 | den_hp == 8
|
|
cols = 5;
|
|
elseif den_hp == 9 | den_hp == 10
|
|
cols = 6;
|
|
elseif den_hp == 11 | den_hp == 12
|
|
cols = 7;
|
|
elseif den_hp == 13 | den_hp == 14
|
|
cols = 8;
|
|
end
|
|
|
|
%the matrix with proper number of rows and columns
|
|
RH_Table = zeros(den_hp+1, cols);
|
|
|
|
|
|
%set up the first element in row 1 & row 2 since MATLAB doesn't do 0 indexing
|
|
if cols == 1
|
|
RH_Table(1,1) = den(1);
|
|
|
|
elseif cols > 1
|
|
RH_Table(1,1) = den(1);
|
|
RH_Table(2,1) = den(2);
|
|
|
|
end
|
|
|
|
if cols == 1
|
|
fprintf('\n *Only one element in the RH Table, thus, you have a stable system.* \n');
|
|
|
|
elseif cols > 1
|
|
first_count = 3; %since added first element (element 1) above so now skip one and start at 3rd element for first row
|
|
second_count = 4; %since added second element (element 2) above so now skip one and start at 4th element for second row
|
|
|
|
%filling in the first row of the RH table
|
|
for i = 2:cols %start at 2 because filled in first element above
|
|
if first_count <= den_hp_added
|
|
RH_Table(1,i) = den(first_count);
|
|
first_count = first_count + 2;
|
|
else
|
|
RH_Table(1,i) = 0;
|
|
end
|
|
end
|
|
|
|
%filling in the second row of the RH table
|
|
for i = 2:cols %start at 2 because filled in first element above
|
|
if second_count <= den_hp_added
|
|
RH_Table(2,i) = den(second_count);
|
|
second_count = second_count + 2;
|
|
else
|
|
RH_Table(2,i) = 0;
|
|
end
|
|
end
|
|
|
|
%now fill in remaining elements of RH Table with determinants
|
|
X = zeros(2,2);
|
|
|
|
for i = 1:(den_hp-1)
|
|
for j = 1:(cols-1)
|
|
X(1,1) = RH_Table(i,1);
|
|
X(2,1) = RH_Table(i+1,1);
|
|
X(1,2) = RH_Table(i,j+1);
|
|
X(2,2) = RH_Table(i+1,j+1);
|
|
|
|
RH_Table(i+2,j) = -det(X)/RH_Table(i+1,1);
|
|
end
|
|
end
|
|
|
|
RH_Table % to show full RH table
|
|
|
|
%now analyze the first column of the table for sign changes to
|
|
%determine how many poles in the right half-plane.
|
|
sign_changes = 0;
|
|
for i = 1:(den_hp)
|
|
if (RH_Table(i,1) * RH_Table(i+1,1) < 0)
|
|
sign_changes = sign_changes + 1;
|
|
end
|
|
end
|
|
|
|
if sign_changes > 0
|
|
fprintf(['\n *You have %d right-half poles and %d left-half poles.',...
|
|
' Thus, you have an unstable system.* \n'],sign_changes,den_hp-sign_changes)
|
|
else
|
|
fprintf(['\n *You have %d right-half poles and %d left-half poles.',...
|
|
' Thus, you have a stable system.* \n'],sign_changes,den_hp-sign_changes)
|
|
end
|
|
|
|
end
|
|
|
|
|
|
%Special Case #1: Check if there exists a full row of zeros. If so, the
|
|
%following code will be utilized.
|
|
|
|
%first check if any array contains a row of zeros
|
|
row_of_zeros = 0;
|
|
tol = 1.e-6;
|
|
for i = 1:(den_hp + 1)
|
|
if abs(RH_Table(i,:) - 0) < tol
|
|
row_of_zeros = 1;
|
|
row = i;
|
|
break; %break because you have identified a row of zeros
|
|
end
|
|
end
|
|
|
|
|
|
%If the check shows that you do indeed have a row of zeros, the following
|
|
%code will run
|
|
|
|
if (row_of_zeros == 1)
|
|
row_above = RH_Table(row-1,:);
|
|
aux_poly = poly2sym(row_above);
|
|
diff_poly = diff(aux_poly);
|
|
diff_poly_coeffs = flip(coeffs(diff_poly));
|
|
|
|
for j = 1:cols
|
|
if numel(diff_poly_coeffs) < cols
|
|
diff_poly_coeffs = [diff_poly_coeffs, 0]; %add zero to the end of coefficients to make sure dimensions match when adding back to table
|
|
else
|
|
break;
|
|
end
|
|
end
|
|
|
|
RH_Table(row,:) = diff_poly_coeffs; %replacing row in RH Table with the coefficients of row above, differentiated
|
|
|
|
X = zeros(2,2);
|
|
|
|
for i = row-1:(den_hp-1)
|
|
for j = 1:(cols-1)
|
|
X(1,1) = RH_Table(i,1);
|
|
X(2,1) = RH_Table(i+1,1);
|
|
X(1,2) = RH_Table(i,j+1);
|
|
X(2,2) = RH_Table(i+1,j+1);
|
|
|
|
RH_Table(i+2,j) = -det(X)/RH_Table(i+1,1);
|
|
end
|
|
end
|
|
|
|
%Split up the sign changes process into two:
|
|
%1) Check sign changes until two rows above row of zeros normally and
|
|
%store these sign changes.
|
|
%2) Check sign changes from one row above row of zeros down to bottom.
|
|
%By symmetry, the right half poles must equal the left half poles, and
|
|
%anything left over are jw-axis poles. Additionally, the row above the
|
|
%row of zeros is required to be an even polynomial.
|
|
|
|
%1)
|
|
first_sign_changes = 0;
|
|
second_sign_changes = 0;
|
|
for k = 1:(row-2)
|
|
if (RH_Table(k,1) * RH_Table(k+1,1) < 0)
|
|
first_sign_changes = first_sign_changes + 1;
|
|
end
|
|
end
|
|
|
|
%2)
|
|
for k = row-1:den_hp
|
|
if (RH_Table(k,1) * RH_Table(k+1,1) < 0)
|
|
second_sign_changes = second_sign_changes + 1;
|
|
end
|
|
end
|
|
|
|
total_sign_changes = first_sign_changes + second_sign_changes;
|
|
left_half_poles =((row-2)-first_sign_changes + second_sign_changes);
|
|
jw_axis_poles = den_hp - (row-2) - (2*second_sign_changes);
|
|
|
|
RH_Table
|
|
|
|
if (total_sign_changes) > 0
|
|
fprintf(['\n *Upon changes made to the RH Table, you now have %d right-half',...
|
|
' poles and %d left-half poles. Additionally, you have %d jw-axis poles. Thus, you have an unstable',...
|
|
' system.* \n'],total_sign_changes,left_half_poles,jw_axis_poles);
|
|
elseif (total_sign_changes == 0)
|
|
fprintf(['\n *Upon changes made to the RH Table, you now have %d',...
|
|
' right-half poles and %d left-half poles. Additionally, you have %d jw-axis poles.',...
|
|
' Thus, you have a marginally stable system.* \n'],total_sign_changes,left_half_poles, jw_axis_poles);
|
|
end
|
|
end
|
|
|
|
|
|
%Special Case #2: Check if zero exists in first column and if so, form reciprocal
|
|
%polynomial and redo process above to get determinants. Have a separate
|
|
%method for this portion, however, for the purposes of File Exchange, I've
|
|
%attached that portion below.
|
|
|
|
for i = 1:(den_hp+1)
|
|
if RH_Table(i,1) == 0
|
|
den = flip(den);
|
|
if cols == 1
|
|
RH_Table(1,1) = den(1);
|
|
elseif cols > 1
|
|
RH_Table(1,1) = den(1);
|
|
RH_Table(2,1) = den(2);
|
|
end
|
|
|
|
if cols == 1
|
|
fprintf('\n *Only one element in the RH Table, thus, you have a stable system.* \n');
|
|
elseif cols > 1
|
|
first_count = 3;
|
|
second_count = 4;
|
|
for k = 2:cols
|
|
if first_count <= den_hp_added
|
|
RH_Table(1,k) = den(first_count);
|
|
first_count = first_count + 2;
|
|
else
|
|
RH_Table(1,k) = 0;
|
|
end
|
|
end
|
|
|
|
for l = 2:cols
|
|
if second_count <= den_hp_added
|
|
RH_Table(2,l) = den(second_count);
|
|
second_count = second_count + 2;
|
|
else
|
|
RH_Table(2,l) = 0;
|
|
end
|
|
end
|
|
|
|
X = zeros(2,2);
|
|
for m = 1:(den_hp-1)
|
|
for j = 1:(cols-1)
|
|
X(1,1) = RH_Table(m,1);
|
|
X(2,1) = RH_Table(m+1,1);
|
|
X(1,2) = RH_Table(m,j+1);
|
|
X(2,2) = RH_Table(m+1,j+1);
|
|
|
|
RH_Table(m+2,j) = -det(X)/RH_Table(m+1,1);
|
|
end
|
|
end
|
|
|
|
sign_changes = 0;
|
|
for k = 1:(den_hp)
|
|
if (RH_Table(k,1) * RH_Table(k+1,1) < 0) %if the element * next element is negative, must be a sign change
|
|
sign_changes = sign_changes + 1;
|
|
end
|
|
end
|
|
|
|
RH_Table
|
|
if sign_changes > 0
|
|
fprintf(['\n *Upon changes made to the RH Table, you now have %d right-half',...
|
|
' poles and %d left-half poles. Thus, you have an unstable',...
|
|
' system.* \n'],sign_changes,den_hp-sign_changes)
|
|
else
|
|
fprintf(['\n *Upon changes made to the RH Table, you now have %d',...
|
|
' right-half poles and %d left-half poles.',...
|
|
' Thus, you have a stable system.* \n'],sign_changes,den_hp-sign_changes)
|
|
end
|
|
end
|
|
end
|
|
end %end special case #2%
|
|
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|