-
Notifications
You must be signed in to change notification settings - Fork 3
/
Routh_Criterion.py
198 lines (178 loc) · 6.99 KB
/
Routh_Criterion.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
import sympy as sp
def normalize_dict(dict):
"""Generates a modified dictionary for the coefficients of the characteristic eqn
in the form: s**4 s**3 s**2 s**1 s**0"""
modified_dict = {}
while dict:
item = dict.popitem()
if item[0] == 1:
modified_dict["s**0"] = item[1]
elif str(item[0]) == 's':
modified_dict["s**1"] = item[1]
else:
modified_dict[str(item[0])] = item[1]
return modified_dict
# check the powers of the polynomial
def check_powers(powers):
"""Check whether all the powers exist or not
NB: If the zero-th power doesn't exist it can't determine the result
and needs further routh calculations."""
# getting the p_list of powers
p_list = []
for pow in powers:
p_list.append(int(pow[-1]))
# checking the existance of all powers in the p_list
if len(p_list) == max(p_list) + 1:
return True
if 0 not in p_list and len(p_list) == max(p_list):
return True
return False
# check the signs of the coefficients
def check_signs(coeffs):
"""Checks whether the characterstic coeffiecients have the same sign or nor"""
negative = 0
positive = 0
# checking the sign of the coefficients
for coeff in coeffs:
if int(coeff) < 0:
negative += 1
if positive:
return False
elif int(coeff) > 0:
positive += 1
if negative:
return False
return True
# construct the routh array
def construct_routh_array(dict):
"""Initializes the Routh Array with the first 2 coefficienst Rows"""
sorted_keys=sorted(dict.keys(),reverse=True)
n = len(sorted_keys)
m = n//2 + n%2
x=0
routh_array = [[0 for i in range(m)] for j in range(n)]
for i in range(0, n, 2):
routh_array[0][x] = dict.get(sorted_keys[i])
x+=1
x=0
for i in range(1, n, 2):
routh_array[1][x] = dict.get(sorted_keys[i])
x+=1
return routh_array
def fill_routh_array(matrix):
"""Calculates the routh array for a given coeffiecients matrix
Exceptions:
case 0 row: handled if the auxilliary equation is even-ordered
case 0 in the first column: handled using 1e-10 instead of 0
Returns:
matrix: The new Routh Array
state: state identifying whether there is an exception."""
n = len(matrix)
state = 1
for i in range(2, n):
for j in range(0, len(matrix[0])-1):
try:
if matrix[i-1][0] == 0:
raise ZeroDivisionError
matrix[i][j] = (matrix[i-1][0]*matrix[i-2][j+1] - matrix[i-2][0]*matrix[i-1][j+1]) / matrix[i-1][0]
except ZeroDivisionError:
row = True
for j in range(0, len(matrix[0]) - 1):
if matrix[i-1][j] != 0:
row = False
if row:
state = -1
pow = n - (i - 2) - 1
if pow % 2 == 0:
for j in range(0, len(matrix[0])-1):
if pow >= 0:
matrix[i-1][j] = pow * matrix[i-2][j]
pow -= 2
matrix[i][j] = (matrix[i-1][0]*matrix[i-2][j+1] - matrix[i-2][0]*matrix[i-1][j+1]) / matrix[i-1][0]
else:
raise Exception("System can't be tested for stability: Odd-Ordered Auxiliary Equation")
else:
state = 0
matrix[i-1][0] = 1e-10
for j in range(0, len(matrix[0]) - 1):
matrix[i][j] = (matrix[i-1][0]*matrix[i-2][j+1] - matrix[i-2][0]*matrix[i-1][j+1]) / matrix[i-1][0]
return matrix, state
def check_stability(matrix):
"""Checks whether the system is stable after conducting the Routh Array"""
n = len(matrix)
sign_change = 0
pos = True
for i in range(0, n):
if matrix[i][0] > 0 and not pos:
pos = True
sign_change += 1
elif matrix[i][0] < 0 and pos:
pos = False
sign_change += 1
return sign_change
def parse_sympy_equation():
"""Parse the input string into a sympy expression and
return the symbols used in the expression.
Returns:
parsed_expression: sympy expression
symbols_dict: dictionary of symbols used in the expression
"""
expression_str = input("Enter the Characteristic Equation in the form a*s**n + b*s**n-1 ... c*s**2 + d*s**1 + e(*s**0)\n")
try:
parsed_expression = sp.sympify(expression_str)
symbols_used = parsed_expression.free_symbols
symbols_dict = {str(symbol): symbol for symbol in symbols_used}
return parsed_expression, symbols_dict
except Exception as e:
print("Error:", e)
return None, None
def solve_eqn(parsed_eqn, symbols_dict):
"""Solve the equation to find the values of the positive roots"""
solns = sp.solve(parsed_eqn, symbols_dict)
return solns
def main():
parsed_expression = None
while(not parsed_expression):
parsed_expression, symbols_dict = parse_sympy_equation()
if(not parsed_expression):
print("Please enter a valid expression.")
substituted_expression = parsed_expression.subs(symbols_dict)
print(sp.latex(substituted_expression))
dict = substituted_expression.as_coefficients_dict()
new_dict = normalize_dict(dict)
# checking the powers and signs
stability = 0
if not check_powers(new_dict.keys()):
print("System is Unstable: Missing powers of s.")
print("Finding the number of roots.")
print("________________________________________")
stability = -1
elif not check_signs(new_dict.values()):
print("System is Unstable: Coefficients alternate signs.")
print("Finding the number of roots.")
print("________________________________________")
stability = -1
else:
matrix = construct_routh_array(new_dict)
routh_matrix, state = fill_routh_array(matrix)
stability = check_stability(routh_matrix)
if(state == -1):
print("System has a zero row in the Routh array ... Checking for Marginal Stability.")
elif(state == 0):
print("System has a zero division error in the Routh array.")
else:
print("System has no zero division error in the Routh array.")
print("Final Matrix: ", routh_matrix)
if(stability == 0):
print("Stability Check: System is Stable.")
else:
print("Stability Check: System is Unstable and has {} roots in the positive side of the S-plane.".format(stability))
if stability:
count = 1
for i in solve_eqn(parsed_expression, symbols_dict):
i = sp.N(i)
if sp.re(i) > 0:
print("Root {}: ".format(count), i)
count += 1
if __name__ == "__main__":
main()