183742 (Математическая модель в пространстве состояний линейного стационарного объекта управления), страница 10

tit1 = title('Коэффициенты обратной связи в прямом времени');

set(tit1,'FontName','Courier');

hl=legend('k_1_о_с','k_2_о_с','k_3_о_с','k_4_о_с','k_5_о_с',0);

set(hl,'FontName','Courier');

grid on;

% ------------------------------------------------------------------------%

% Нахождение коэффициентов регулятора

disp('Коэффициенты регулятора:')

K = -inv(R) * B' * P1

% ------------------------------------------------------------------------%

% ------------------------------------------------------------------------%

A_ = A + B * K;

% ------------------------------------------------------------------------%

% Нахождение фазовых координат

X(:,1) = X_0;

h = 0.01;

time_X(1) = 0;

for k = 1 : N_str

X(:, k+1) = X(:, k) + h * A_ * X(:, k);

time_X(k+1) = time_X(k) + h;

end

X(:, k+1) = [];

time_X(k+1) = [];

% ------------------------------------------------------------------------%

% Нахождение управления

for k = 1 : N_str

u(k) = K * X(:,k);

end

% ------------------------------------------------------------------------%

% Нахождение коэффициентов наблюдателя

M_n = [C' A'*C' (A^2)'*C' (A^3)'*C' (A^4)'*C']

rank_M_n = rank(M_n)

A_r = A_

disp('Спектр матрицы регулятора:')

spektr_A_r = eig(A_r)

koeff = 1;

min_lyamda_A_r = min(real(spektr_A_r))

% lyamda = min_lyamda_A_r * koeff;

lyamda = -5;

disp('Спектр матрицы наблюдателя эталонный:')

lyamda_A_n = [lyamda - koeff * 4; lyamda - koeff * 3; lyamda - koeff * 2;...

lyamda - koeff; lyamda]'

syms k_n1 k_n2 k_n3 k_n4 k_n5 lyam

K_n = [k_n1; k_n2; k_n3; k_n4; k_n5];

Koeff_poly_n_etalon = poly(lyamda_A_n)

disp('Характеристический полином наблюдателя эталонный:')

poly_n_etalon = poly2sym(Koeff_poly_n_etalon, lyam)

disp('Характеристический полином наблюдателя реальный:')

poly_n_real = collect(expand(simplify(det(lyam*eye(poryadok) - (A - K_n*C)))),lyam)

raznost_poly = collect(poly_n_etalon-poly_n_real,lyam)

for i = 1 : poryadok

Koeff_raznost_poly(i) = subs(diff(raznost_poly,poryadok-i,lyam)/factorial(poryadok-i),lyam,0);

end

Koeff_raznost_poly

[Kn1 Kn2 Kn3 Kn4 Kn5]= solve(Koeff_raznost_poly(5), Koeff_raznost_poly(4),...

Koeff_raznost_poly(3), Koeff_raznost_poly(2), Koeff_raznost_poly(1), ...

k_n1, k_n2, k_n3, k_n4, k_n5)

Kn = [Kn1; Kn2; Kn3; Kn4; Kn5];

Kn = vpa(Kn,50)

% Проверка

Proverka = solve(det(lyam*eye(poryadok)-(A-Kn*C)))

% ------------------------------------------------------------------------%

% ------------------------------------------------------------------------%

% Нахождение x и x_оценочного

X_ocen_0 = [0 0 0 0 0]';

A_rash = [A B*K;

Kn*C A-Kn*C+B*K]

X_rash_0 = [X_0;X_ocen_0]

X_rash(:,1) = X_rash_0;

for k = 1 : N_str

X_rash(:,k+1) = X_rash(:,k) + h * A_rash * X_rash(:,k);

end

X_rash(:,k+1) = [];

% ------------------------------------------------------------------------%

% ------------------------------------------------------------------------%

% Разделение x и x_оценочного

for i = 1 : poryadok

X_n(i,:) = X_rash(i,:);

end

for i = poryadok + 1 : 2*poryadok

X_n_ocen(i - poryadok,:) = X_rash(i,:);

end

% ------------------------------------------------------------------------%

% ------------------------------------------------------------------------%

% Нахождение управления

for i = 1 : N_str

u_n(i) = K * X_n_ocen(:,i);

end

% Построение u(t) и X(t)

figure(3);

plot(time_X, u, 'r-', time_X, u_n, 'b-', 'LineWidth', 2)

title ('u(t)');

xlabel('t')

hl=legend('управление без наблюдателя','управление c наблюдателем');

set(hl,'FontName','Courier');

grid on

figure(4);

plot(time_X, X(1,:), time_X, X_n(1,:), time_X, X_n_ocen(1,:),'LineWidth', 2)

hold on

title ('x_1(t)');

xlabel('t')

hl=legend('x_1(t) без наблюдателя','x_1(t) c наблюдателем', 'x_о_ц_е_н_1(t)');

set(hl,'FontName','Courier');

grid on

figure(5);

plot(time_X, X(2,:), time_X, X_n(2,:), time_X, X_n_ocen(2,:),'LineWidth', 2)

title ('x_2(t)');

xlabel('t')

hl=legend('x_2(t) без наблюдателя','x_2(t) c наблюдателем', 'x_о_ц_е_н_2(t)');

set(hl,'FontName','Courier');

grid on

figure(6);

plot(time_X, X(3,:), time_X, X_n(3,:), time_X, X_n_ocen(3,:),'LineWidth', 2)

title ('x_3(t)');

xlabel('t')

hl=legend('x_3(t) без наблюдателя','x_3(t) c наблюдателем', 'x_о_ц_е_н_3(t)');

set(hl,'FontName','Courier');

grid on

figure(7);

plot(time_X, X(4,:), time_X, X_n(4,:), time_X, X_n_ocen(4,:),'LineWidth', 2)

title ('x_4(t)');

xlabel('t')

hl=legend('x_4(t) без наблюдателя','x_4(t) c наблюдателем', 'x_о_ц_е_н_4(t)');

set(hl,'FontName','Courier');

grid on

figure(8);

plot(time_X, X(5,:), time_X, X_n(5,:), time_X, X_n_ocen(5,:),'LineWidth', 2)

title ('x_5(t)');

xlabel('t')

hl=legend('x_5(t) без наблюдателя','x_5(t) c наблюдателем', 'x_о_ц_е_н_5(t)');

set(hl,'FontName','Courier');

grid on

Solve_Riccati_Method_Diag.m

% ------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Метод диагонализации для решения алгебраического уравнения Риккати

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function P = Solve_Riccati_Method_Diag(A,B,Q,R)

% Расширенная матрица системы

Z = [A B*inv(R)*B';

Q -A']

% Нахождение собственных векторов и собственных чисел матрицы Z

[V,D] = eig(Z)

% ------------------------------------------------------------------------%

% Построение матрицы S

% Индексы столбцов собственных значений Re(lyamda) > 0

Ind_Re_plus = find(sum(real(D)) > 0);

% Индексы столбцов собственных значений Re(lyamda) < 0

Ind_Re_minus = find(sum(real(D)) < 0);

% Формирование матрицы D в виде Re(lyamda) > 0 -> Re(lyamda) < 0

D1 = sum(D(:, Ind_Re_plus));

D2 = sum(D(:, Ind_Re_minus));

D = [D1 D2];

% Формирование матрицы S в виде Re(lyamda) > 0 -> Re(lyamda) < 0

S1 = V(:, Ind_Re_plus);

S2 = V(:, Ind_Re_minus);

S = [S1 S2];

% Поиск столбцов с комплексными корнями в матрице D

Ind_Complex_D = find(imag(D) ~= 0);

% Формирование конечной матрицы S

for i = 1 : 2 : length(Ind_Complex_D)

S (:, Ind_Complex_D(i) + 1) = imag(S(:, Ind_Complex_D(i)));

S (:, Ind_Complex_D(i)) = real(S(:, Ind_Complex_D(i)));

end

S = S

% ------------------------------------------------------------------------%

poryadok = length(A(1,:));

S12 = S(1 : poryadok, poryadok+1 : 2*poryadok);

S22 = S(poryadok+1 : 2*poryadok, poryadok+1 : 2*poryadok);

% ------------------------------------------------------------------------%

% Вычисление матрицы P

P = -S22 * inv(S12);

Solve_Riccati_Method_Revers_Integr.m

% ------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Решение уравнения Риккати интегрированием в обратном времени

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function P = Solve_Riccati_Method_Revers_Integr(A,B,Q,R,Time,poryadok, P1)

save For_Riccati A B Q R poryadok

% Решение дифференциального уравнения Риккати

P1 = reshape(P1, poryadok^2, 1);

[Time_R, P] = ode45(@Riccati, [Time : -0.01 : 0], P1);

[N_str, N_stolb] = size(P);

% Построение полученного решения

figure(1)

for i = 1 : poryadok^2

plot(Time_R, P(:,i),'-')

hold on

end

% plot(Time_R,P(:,1),'-',Time_R,P(:,2),'-',Time_R,P(:,3),'-',Time_R,P(:,4),'-',Time_R,P(:,5),'-',Time_R,P(:,6),'-',...

% Time_R,P(:,7),'-',Time_R,P(:,8),'-',Time_R,P(:,9),'-',Time_R,P(:,10),'-',Time_R,P(:,11),'-',Time_R,P(:,12),'-',...

% Time_R,P(:,13),'-',Time_R,P(:,14),'-',Time_R,P(:,15),'-',Time_R,P(:,16),'-',Time_R,P(:,17),'-',Time_R,P(:,18),'-',...

% Time_R,P(:,19),'-',Time_R,P(:,20),'-',Time_R,P(:,21),'-',Time_R,P(:,22),'-',Time_R,P(:,23),'-',Time_R,P(:,24),'-',...

% Time_R,P(:,25),'-', 'lineWidth', 2);

grid on;

tit1 = title('Решения уравнения Риккати');

set(tit1,'FontName','Courier');

xlabel('t');

% legend('p_1','p_2','p_3','p_4','p_5','p_6','p_7','p_8','p_9','p_1_0','p_1_1','p_1_2','p_1_3','p_1_4','p_1_5','p_1_6',...

% 'p_1_7','p_1_8','p_1_9','p_2_0','p_2_1','p_2_2','p_2_3','p_2_4','p_2_5');

save Solve_Riccati_Method_Revers_Integr Time_R P N_str

save Solve_Riccati_Method_Revers_Integr_for_slegenie Time_R P N_str

P = reshape(P(N_str,:), poryadok, poryadok);

function dP = Riccati(Time,P)

load For_Riccati A B Q R poryadok

P = reshape(P, poryadok, poryadok);

% Дифференциальное уравнение Риккати

dP = -P*A - A'*P + P*B*inv(R)*B'*P - Q;

dP = reshape(dP, poryadok^2, 1);

Vozmyshyayushee_Vozdeistvie_Discrete_Revers.m

% Получение дискретных значений возмущающего воздействия в обратном времени

% для нахождения вспомогательной функции q(t)

function Vozmyshyayushee_Vozdeistvie_Discrete_Revers(h, T_nach, T_konech)

% ------------------------------------------------------------------------%

% Возмущающее воздействие

A = 1;

w = 4*pi;

k = 1;

RETURN = 1;

while RETURN == 1

disp('Возмущающее воздействие - const: 1')

disp('Возмущающее воздействие - A*sin(w*t): 2')

reply = input('Выберете возмущающее воздействие [1 или 2]: ', 's');

switch reply

case '1'

disp('Возмущающее воздействие - const')

for t = T_konech: -h : T_nach

w_discrete_rev(:, k) = [A + 0 * t; 0; 0; 0; 0];

k = k + 1;

end

RETURN = 2;

case '2'

disp('Возмущающее воздействие - A*sin(w*t)')

for t = T_konech: -h : T_nach

w_discrete_rev(:, k) = [A * sin(w * t); 0; 0; 0; 0];

k = k + 1;

end

RETURN = 2;

otherwise

disp('Неизвестное воздействие.')

RETURN = 1;

end

end

figure(2)

t = T_konech : -h : T_nach;

plot(t, w_discrete_rev(1,:), 'r-', 'LineWidth', 2);

xlabel('t')

tit1 = title('Возмущающее воздействие');

set(tit1,'FontName','Courier');

hl=legend('Возмущающее воздействие',0);

set(hl,'FontName','Courier');

grid on;

save Vozmyshyayushee_Vozdeistvie_Discrete_Revers w_discrete_rev

% ------------------------------------------------------------------------%

Zadayushee_Vozdeistvie_Discrete_Revers_Modern.m

% Получение дискретных значений задающего воздействия в обратном времени

% для нахождения вспомогательной функции q(t)

function Zadayushee_Vozdeistvie_Discrete_Revers_Modern(h, T_nach, T_konech)

% ------------------------------------------------------------------------%

% Задающее воздействие

alfa = 0.2;

beta = 10;

H = 0.8;

k = 1;

for t = T_konech: -h : T_nach

X_o_1 = 10*exp(-1/5*t)*t+4/5;

X_o_2 = -2*exp(-1/5*t)*t+10*exp(-1/5*t);

X_o_3 = 2/5*exp(-1/5*t)*t-4*exp(-1/5*t);

X_o_4 = -2/25*exp(-1/5*t)*t+6/5*exp(-1/5*t);

X_o_5 = 2/125*exp(-1/5*t)*t-8/25*exp(-1/5*t);

X_o_discrete_rev(:, k) = [X_o_1; X_o_2; X_o_3; X_o_4; X_o_5];

k = k + 1;

end

figure(2)

t = T_konech : -h : T_nach;

plot(t, X_o_discrete_rev(1,:), 'r-', t, X_o_discrete_rev(1,:)-H, 'LineWidth', 2);

xlabel('t')

tit1 = title('Задающее воздействие');

set(tit1,'FontName','Courier');

hl=legend('Отслеживание зад. возд. на H ','Задающее воздействие',0);

set(hl,'FontName','Courier');

grid on;

save Zadayushee_Vozdeistvie_Discrete_Revers X_o_discrete_rev

% ------------------------------------------------------------------------%