-a0 -a1 -a2 -a3 -a4];
B = [0; 0; 0; 0; 1];
C = [b0 b1 0 0 0];
% Начальные условия
X_0 = [10; 0; 6; 4; 8];
Time = 1;
h = 0.01;
% ------------------------------------------------------------------------%
tic
% ------------------------------------------------------------------------%
% Получение max значений из файла
load Sostoyaniya X_max U_max
% ------------------------------------------------------------------------%
% Нахождение элементов матриц Q и R
r(1) = 100;
q(1) = 1/poryadok * r(1) * (U_max)^2 / (X_max(1))^2;
for i = 2 : poryadok
q(i) = q(1) * (X_max(1))^2 / (X_max(i))^2;
end
Q = diag(q);
R = diag(r);
% Для изменения коэффициентов
Q(1,1) = Q(1,1)*1e+12;
Q(2,2) = Q(2,2)*1e+8;
Q(3,3) = Q(3,3)*1e+7;
Q(4,4) = Q(4,4)*1e+0;
Q(5,5) = Q(5,5)*1e+2;
R(1,1) = R(1,1);
% P_0 = ones(poryadok, poryadok);
% P_0(1,1) = P_0(1,1)*1e12;
% P_0(2,2) = P_0(2,2)*1e8;
% P_0(3,3) = P_0(3,3)*1e7;
% P_0(4,4) = P_0(4,4)*1e0;
% P_0(5,5) = P_0(5,5)*1e2;
% ------------------------------------------------------------------------%
P_nach = zeros(poryadok, poryadok);%+P_0;
% ------------------------------------------------------------------------%
% Решение уравнения Риккати методом обратного интегрирования
P = Solve_Riccati_Method_Revers_Integr(A,B,Q,R,Time,poryadok, P_nach);
load Solve_Riccati_Method_Revers_Integr_for_slegenie Time_R P N_str
PP = P;
for k = 1 : N_str
P1 = reshape(PP(k, :), poryadok, poryadok);
for i = 1 : poryadok
for j = 1 : poryadok
P2(i,j,k) = P1(i,j);
end
end
end
size_P = size(P2);
% ------------------------------------------------------------------------%
tic
% ------------------------------------------------------------------------%
% Получение дискретных значений задающего воздействия в обратном времени
% для нахождения вспомогательной функции q(t)
Vozmyshyayushee_Vozdeistvie_Discrete_Revers(h, 0, Time);
% ------------------------------------------------------------------------%
load Vozmyshyayushee_Vozdeistvie_Discrete_Revers w_discrete_rev
% ------------------------------------------------------------------------%
size(w_discrete_rev);
% Начальное значение q(t)
q = zeros(poryadok,1);
% Интегрирование q(t) в обратном времени
for k = 1 : N_str
q(:, k+1) = q(:, k) - h * ((P2(:,:,k)*B*inv(R)*B'-A') * q(:, k) - P2(:,:,k)*w_discrete_rev(:,k));
end
q(:, k+1) = [];
size_q = size(q);
% ------------------------------------------------------------------------%
% Нахождение переменных коэффициентов регулятора
for k = 1 : N_str
K_o(k, :) = -inv(R) * B' * P2(:,:,k);
K_pr(k, :) = -inv(R) * B';
end
% Формирование вектора коэффициентов регулятора, значений задающего
% воздействия, значений вспомогательной функции в прямом порядке
size(K_o);
size(K_pr);
K_pr_p = K_pr;
i = 1;
len_K = length(K_o(:,1));
for j = len_K : -1 : 1
K_o_p(i,:) = K_o(j,:);
w_discrete(:,i) = w_discrete_rev(:,j);
q_pr(:, i) = q(:, j);
i = i + 1;
end
% ------------------------------------------------------------------------%
% Построение графика переменных коэффициентов регулятора обратной связи
% в прямом времени
toc
figure(3)
plot(Time_R,K_o(:,1),'-',Time_R,K_o(:,2),'-',Time_R,K_o(:,3),'-',...
Time_R,K_o(:,4),'-',Time_R,K_o(:,5),'-', 'LineWidth', 2);
xlabel('t')
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;
% ------------------------------------------------------------------------%
% ------------------------------------------------------------------------%
% Построение графика переменных коэффициентов регулятора прямой связи
% в прямом времени
figure(4)
plot(Time_R,K_pr(:,1),'-',Time_R,K_pr(:,2),'-',Time_R,K_pr(:,3),'-',...
Time_R,K_pr(:,4),'-',Time_R,K_pr(:,5),'-', 'LineWidth', 2);
xlabel('t')
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;
% ------------------------------------------------------------------------%
tic
% ------------------------------------------------------------------------%
for k = 1 : len_K
A_(:,:,k) = A + B * K_o_p(k,:);
end
size_A_ = size(A_);
% ------------------------------------------------------------------------%
% ------------------------------------------------------------------------%
% Нахождение фазовых координат
X(:,1) = X_0;
time_X(1) = 0;
for k = 1 : len_K
X(:, k+1) = X(:, k) + h * (A_(:,:,k) * X(:, k) + B * K_pr_p(k,:) * q_pr(:,k) + w_discrete(:,k));
time_X(k+1) = time_X(k) + h;
end
X(:, k+1) = [];
time_X(k+1) = [];
size_X = size(X);
% ------------------------------------------------------------------------%
% ------------------------------------------------------------------------%
% Нахождение управления
for k = 1 : len_K
u(k) = K_o_p(k,:) * X(:,k) + K_pr_p(k,:) * q_pr(:,k);
end
size_u = size(u);
% ------------------------------------------------------------------------%
toc
% Построение u(t) и X(t)
figure(5);
plot(time_X, u, 'r-', 'LineWidth', 2)
title ('u(t)');
xlabel('t')
hl=legend('u(t) - управление',0);
set(hl,'FontName','Courier');
grid on
figure(6);
plot(time_X, X(1,:),'r-', time_X, w_discrete(1,:), 'LineWidth', 2)
hold on
title ('x_1(t)');
xlabel('t');
hl=legend('X(t) - реальный сигнал','w(t) - возмущающее воздействие',0);
set(hl,'FontName','Courier');
grid on
figure(7);
plot(time_X, X(2,:),'r-', time_X, w_discrete(2,:), 'LineWidth', 2)
title ('x_2(t)');
xlabel('t');
hl=legend('X(t) - реальный сигнал','w(t) - возмущающее воздействие',0);
set(hl,'FontName','Courier');
grid on
figure(8);
plot(time_X, X(3,:),'r-', time_X, w_discrete(3,:), 'LineWidth', 2)
title ('x_3(t)');
xlabel('t');
hl=legend('X(t) - реальный сигнал','w(t) - возмущающее воздействие',0);
set(hl,'FontName','Courier');
grid on
figure(9);
plot(time_X, X(4,:),'r-', time_X, w_discrete(4,:), 'LineWidth', 2)
title ('x_4(t)');
xlabel('t');
hl=legend('X(t) - реальный сигнал','w(t) - возмущающее воздействие',0);
set(hl,'FontName','Courier');
grid on
figure(10);
plot(time_X, X(5,:),'r-', time_X, w_discrete(5,:), 'LineWidth', 2)
title ('x_5(t)');
xlabel('t');
hl=legend('X(t) - реальный сигнал','w(t) - возмущающее воздействие',0);
set(hl,'FontName','Courier');
grid on
figure(11);
plot(time_X, q(1,:), time_X, q(2,:), time_X, q(3,:), time_X, q(4,:), time_X, q(5,:), 'LineWidth', 2)
title ('q(t)- vector-function');
xlabel('t');
hl=legend('q_1(t)', 'q_2(t)', 'q_3(t)', 'q_4(t)', 'q_5(t)');
set(hl,'FontName','Courier');
grid on
clc
clear all
close all
poryadok = 5;
% ------------------------------------------------------------------------%
b_0 = 5;
b_1 = 9;
% Укороченная система данного объекта
a_5 = 0.1153;
a_4 = 1.78;
a_3 = 3.92;
a_2 = 14.42;
a_1 = 8.583;
a_0 = 0;
% ------------------------------------------------------------------------%
% Приведение системы
b0 = b_0/a_5;
b1 = b_1/a_5;
a5 = a_5/a_5;
a4 = a_4/a_5;
a3 = a_3/a_5;
a2 = a_2/a_5;
a1 = a_1/a_5;
a0 = a_0/a_5;
% ------------------------------------------------------------------------%
% ------------------------------------------------------------------------%
% Представление системы в пространстве состояний
A = [0 1 0 0 0;
0 0 1 0 0;
0 0 0 1 0
0 0 0 0 1;
-a0 -a1 -a2 -a3 -a4];
B = [0; 0; 0; 0; 1];
C = [b0 b1 0 0 0];
% Начальные условия
X_0 = [10; 0; 6; 4; 8;];
Time = 1;
% ------------------------------------------------------------------------%
% ------------------------------------------------------------------------%
% Получение max значений из файла
load Sostoyaniya X_max U_max
% ------------------------------------------------------------------------%
% Нахождение элементов матриц Q и R
r(1) = 100;
q(1) = 1/poryadok * r(1) * (U_max)^2 / (X_max(1))^2;
for i = 2 : poryadok
q(i) = q(1) * (X_max(1))^2 / (X_max(i))^2;
end
Q = diag(q);
R = diag(r);
% Для изменения коэффициентов
% Q(1,1) = Q(1,1)*1e+10;
% Q(2,2) = Q(2,2)*1e+8;
% Q(3,3) = Q(3,3)*1e+6;
% Q(4,4) = Q(4,4)*1e+2;
% Q(5,5) = Q(5,5)*1e+2;
Q(1,1) = Q(1,1)*1e+12;
Q(2,2) = Q(2,2)*1e+8;
Q(3,3) = Q(3,3)*1e+7;
Q(4,4) = Q(4,4)*1e+0;
Q(5,5) = Q(5,5)*1e+2;
R(1,1) = R(1,1);
% ------------------------------------------------------------------------%
% Задающее воздействие
A_o = [0 1 0 0 0;
0 0 1 0 0;
0 0 0 1 0
0 0 0 0 1;
-a0 -a1 -a2 -a3 -a4];
X_o_0 = [12; 10; 14; 8; 16];
% ------------------------------------------------------------------------%
% ------------------------------------------------------------------------%
% Расширенный вектор состояния и расширенные матрицы A,B,Q
%X_rassh = [X_0; X_o];
NULL_M1 = zeros(size(A));
A_rassh = [A NULL_M1;
NULL_M1 A_o];
NULL_M2 = zeros(length(A(:,1)), 1);
B_rassh = [B; NULL_M2];
Q_rassh = [Q -Q;
-Q Q];
X_rassh_0 = [X_0; X_o_0]
% ------------------------------------------------------------------------%
P_nach = zeros(2*poryadok, 2*poryadok);%+ones(poryadok, poryadok);
% ------------------------------------------------------------------------%
% Решение уравнения Риккати методом обратного интегрирования
P = Solve_Riccati_Method_Revers_Integr(A_rassh,B_rassh,Q_rassh,R,Time,2*poryadok, P_nach)
% ------------------------------------------------------------------------%
% Нахождение переменных коэффициентов регулятора
load Solve_Riccati_Method_Revers_Integr_for_slegenie Time_R P N_str
% ------------------------------------------------------------------------%
% % Формирование матриц P11 и P12
PP = P;
for k = 1 : N_str
P = reshape(PP(k, :), 2*poryadok, 2*poryadok);
for i = 1 : poryadok
for j = 1 : poryadok
P11(i,j,k) = P(i,j);
end
end
for i = 1 : poryadok
for j = (poryadok+1) : (2*poryadok)
P12(i,j-poryadok,k) = P(i,j);
end
end
end
P11(:,:,k)
P12(:,:,k)
% ------------------------------------------------------------------------%
for k = 1 : N_str
K_o(k, :) = -inv(R) * B' * P11(:,:,k);
K_pr(k, :) = -inv(R) * B' * P12(:,:,k);
end
% Формирование вектора коэффициентов регулятора
% в прямом порядке
size(K_o)
size(K_pr)
i = 1;
len_K = length(K_o(:,1))
for j = len_K : -1 : 1
K_o_p(i,:) = K_o(j,:)
K_pr_p(i,:) = K_pr(j,:);
i = i + 1;
end
% ------------------------------------------------------------------------%
% Построение графика переменных коэффициентов регулятора обратной связи
% в прямом времени
figure(2)
plot(Time_R,K_o(:,1),'-',Time_R,K_o(:,2),'-',Time_R,K_o(:,3),'-',...
Time_R,K_o(:,4),'-',Time_R,K_o(:,5),'-', 'LineWidth', 2);
xlabel('t')
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;
% ------------------------------------------------------------------------%
% ------------------------------------------------------------------------%
% Построение графика переменных коэффициентов регулятора прямой связи
% в прямом времени
figure(3)
plot(Time_R,K_pr(:,1),'-',Time_R,K_pr(:,2),'-',Time_R,K_pr(:,3),'-',...
Time_R,K_pr(:,4),'-',Time_R,K_pr(:,5),'-', 'LineWidth', 2);
xlabel('t')
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;
% ------------------------------------------------------------------------%
% ------------------------------------------------------------------------%
% Нахождение отслеживаемого сигнала
X_o(:,1) = X_o_0;
h = 0.01;
for k = 1 : len_K
X_o(:, k+1) = X_o(:, k) + h * A_o * X_o(:, k);
end
X_o(:, k+1) = [];
% ------------------------------------------------------------------------%
% ------------------------------------------------------------------------%
for k = 1 : len_K
A_(:,:,k) = A + B * K_o_p(k,:);
end
size(A_)
% ------------------------------------------------------------------------%
% Нахождение фазовых координат
X(:,1) = X_0;
time_X(1) = 0;
for k = 1 : len_K
X(:, k+1) = X(:, k) + h * (A_(:,:,k) * X(:, k) + B * K_pr_p(k,:) * X_o(:,k));
time_X(k+1) = time_X(k) + h;
end
X(:, k+1) = [];
time_X(k+1) = [];
% ------------------------------------------------------------------------%
% Нахождение управления
for k = 1 : len_K
u(k) = K_o_p(k,:) * X(:,k) + K_pr_p(k,:) * X_o(:,k);
end
% ------------------------------------------------------------------------%
% Построение u(t) и X(t)
figure(4);
plot(time_X, u, 'r-', 'LineWidth', 2)
title ('u(t)');
xlabel('t')
hl=legend('u(t) - управление',0);
set(hl,'FontName','Courier');
grid on
figure(5);
plot(time_X, X(1,:),'r-', time_X, X_o(1,:), 'LineWidth', 2)
hold on