Проблемы конструирования устойчивой системы автоматического управления

W = RESULT.U

end

Листинг 2

stablility_degree.m

function [ degree, est1, est2 ] = stability_degree(eps)

if size(eps, 1) > 1

length = size(eps, 1);

degree = zeros(length, 1);

est1 = zeros(length, 1);

est2 = zeros(length, 1);

for i=1:1:size(eps)

fprintf('eps = %f\n', eps(i));

[degree(i, 1), est1(i, 1), est2(i,1)] = stability_degree(eps(i));

fprintf('degree = %f, est1 = %f, est2 = %f\n', degree(i, 1), est1(i, 1), est2(i,1));

end

return

end

% S = Q/P

s = sym('s');

W0Q = s^2;

W0P = (s^3+2*s^2+s+2);

S1Q = s^14 - 10*s^12 + 17*s^10 + 67*s^8 + s^6*(eps - 66) - s^4*(eps + 289) - 264*s^2 - 80;

S1P = s^4 * (1 - s^2)*(s^3 + 2*s^2 + s + 2)*(s^3 - 2*s^2 + s - 2);

S2Q = 4*s^6 - 8*s^4 + s^2*(eps - 28) - eps - 16;

S2P = (1 - s^2)*(s^3 + 2*s^2 + s + 2)*(s^3 - 2*s^2 + s - 2);

%distributing roots

S1Q_roots = solve(S1Q)

S1P_roots = solve(S1P)

[S1Q_left_roots, S1Q_right_roots] = distribute_roots(S1Q_roots)

[S1P_left_roots, S1P_right_roots] = distribute_roots(S1P_roots)

S1Q_mult = get_multiplier(S1Q)

S1P_mult = get_multiplier(S1P)

S1Q_right = sym_from_roots(S1Q_right_roots, S1Q_mult)

S1P_right = sym_from_roots(S1P_right_roots, S1P_mult)

fprintf('###S3 = S2 / S1-###\n');

S3Q = S2Q * S1P_right

S3P = S2P * S1Q_right

fprintf('simplified fraction:\n');

[S3Q, S3P] = simplify_sym_fraction(S3Q, S3P)

numerator = sym2poly(S3Q)

num_mult = numerator(1)

denominator = sym2poly(S3P)

den_mult = denominator(1)

numerator = num_mult^(-1) * numerator;

denominator = den_mult^(-1) * denominator;

[residues, poles, direct] = residue(numerator, denominator)

count = 1;

for i=1:1:size(poles)

real_part = real(poles(i));

if abs(real_part) < 10e-12

real_part = 0;

end

if real_part <= 0

new_poles(count) = poles(i);

new_residues(count) = residues(i);

count = count + 1;

end

end

new_poles

new_residues

[new_numerator, new_denominator] = residue(new_residues, new_poles, 0);

new_numerator = num_mult * new_numerator

new_denominator = den_mult * new_denominator

fprintf('###S4 = [S2 / S1-]+###\n');

S4Q = sym_from_poly(new_numerator)

S4P = sym_from_poly(new_denominator)

fprintf('###HV = S4/S1+###\n');

S1Q_left = sym_from_roots(S1Q_left_roots, 1)

S1P_left = sym_from_roots(S1P_left_roots, 1)

HV_Q = S4Q * S1P_left

HV_P = S4P * S1Q_left

fprintf('simplified fraction:\n');

[HV_Q, HV_P] = simplify_sym_fraction(HV_Q, HV_P)

fprintf('###HV_Q roots###\n');

solve(HV_Q)

fprintf('###HV_P - HV_Q roots###\n');

solve(HV_P - HV_Q)

W1Q = (W0P * HV_Q);

W1P = (W0Q * (HV_P - HV_Q));

fprintf('simplified fraction:\n');

[W1Q, W1P] = simplify_sym_fraction(W1Q, W1P)

W1Q_roots = solve(W1Q);

W1P_roots = solve(W1P);

CP_roots = solve(W1Q*W0Q + W1P*W0P)

d = 100;

for i=1:1:size(CP_roots)

if real(CP_roots(i)) >= 0

fprintf('UNSTABLE ROOT:');

disp(CP_roots(i));

fprintf('\n');

degree = 0;

return

end

if abs(real(d)) > abs(real(CP_roots(i)))

d = CP_roots(i);

end

end

root = d;

degree = abs(real(d));

HV = HV_Q / HV_P;

HV_MIN = subs(HV, s, -s);

W0 = W0Q / W0P;

W0_MIN = subs(W0, s, -s);

S_mm = 4/(1-s^2);

S_nn = 1;

EST1 = 1/(2*1i*pi) * int(HV*HV_MIN*(S_mm + S_nn) - (HV-HV_MIN)*S_mm + S_mm, s, -1i*inf, 1i*inf);

EST2 = 1/(2*1i*pi) * int(W0^(-1)*W0_MIN^(-1)*HV*HV_MIN*S_mm, s, -1i*inf, 1i*inf);

est1 = double(EST1);

est2 = double(EST2);

simplify_sym_fraction.m

function [ new_num, new_den ] = simplify_sym_fraction(num, den)

s = sym('s');

num_mult = get_multiplier(num)

num_roots = solve(num);

den_mult = get_multiplier(den)

den_roots = solve(den);

[new_num_roots, new_den_roots] = delete_intersected_roots(num_roots, den_roots);

new_num = sym_from_roots(new_num_roots, num_mult/den_mult);

new_den = sym_from_roots(new_den_roots, 1);

end

delete_intersected_roots.m

%not optimized, but pretty recoursed!

function [ roots1, roots2 ] = delete_intersected_roots(roots1, roots2)

i = 0; j = 0;

for i=1:1:size(roots1)

for j=1:1:size(roots2)

% this is dangerous place. normally, we can't delete near roots

% if they are from right plane.

if (roots1(i) == roots2(j)) || (abs(roots1(i)-roots2(j)) < 10e-12)

roots1(i) = [];

roots2(j) = [];

[ roots1, roots2 ] = delete_intersected_roots(roots1, roots2);

return

end

end

end

end

distribute_roots.m

function [left_roots, right_roots] = distribute_roots(roots_arr)

l_count = 0;

r_count = 0;

poles_count = 0;

for j=1:1:size(roots_arr)

if real(roots_arr(j)) < 0

l_count = l_count + 1;

left_roots(l_count) = roots_arr(j);

elseif real(roots_arr(j)) > 0

r_count = r_count + 1;

right_roots(r_count) = roots_arr(j);

else

poles_count = poles_count + 1;

poles_arr(poles_count) = roots_arr(j);

end

end

if l_count == 0

left_roots = [];

end

if r_count == 0

right_roots = [];

end

if poles_count > 0

[left_poles, right_poles] = distribute_poles(poles_arr);

left_roots = (cat(2, left_poles, left_roots));

right_roots = (cat(2, right_poles, right_roots));

end

left_roots = left_roots';

right_roots = right_roots';

end

function [counted_poles] = add_pole(counted_poles, pole)

found = 0;

for i=1:1:size(counted_poles)

if counted_poles(i,1) == pole

counted_poles(i,2) = counted_poles(i,2) + 1;

found = 1;

break

end

end

if found ~= 1

poles_count = size(counted_poles, 1) + 1;

counted_poles(poles_count,1) = pole;

counted_poles(poles_count,2) = 1;

end

end

%we distribute poles equally between both planes

function [left_poles, right_poles] = distribute_poles(poles)

counted_poles = [];

left_poles = [];

right_poles = [];

for j=1:1:size(poles, 2)

[counted_poles] = add_pole(counted_poles, poles(1,j));

end

left_poles_count = 1;

right_poles_count = 1;

for j=1:1:size(counted_poles)

for k=1:1:(counted_poles(j,2)/2)

left_poles(left_poles_count) = counted_poles(j,1);

left_poles_count = left_poles_count + 1;

right_poles(right_poles_count) = counted_poles(j,1);

right_poles_count = right_poles_count + 1;

end

end

end

get_multiplier.m

function [multiplier] = get_multiplier(sym_poly)

poly = sym2poly(sym_poly);

multiplier = poly(1);

end

sym_from_poly.m

function [sym_poly] = sym_from_poly(poly)

s = sym('s');

sym_poly = 0;

degree = size(poly, 2);

for k=1:1:degree

real_part = real(poly(k));

imag_part = imag(poly(k));

if abs(imag_part) < 10e-12

imag_part = 0;

end

if abs(real_part) < 10e-12

real_part = 0;

end

coeff = real_part + imag_part * 1i;

sym_poly = sym_poly + coeff*s^(degree-k);

end

end

sym_from_roots.m

function [ sym_poly ] = sym_from_roots(roots, multiplier)

s = sym('s');

sym_poly = 1;

for k=1:1:size(roots)

sym_poly = sym_poly * (s - roots(k));

end

sym_poly = multiplier * sym_poly;

end

Размещено на Allbest.ru