title ('x_3(t)');
xlabel('t');
hl=legend('X(t) - слежение','X_o(t) - эталон',0);
set(hl,'FontName','Courier');
grid on
figure(7);
plot(time_X, X(4,:),'r-', time_X, X_o_discrete(4,:), 'LineWidth', 2)
title ('x_4(t)');
xlabel('t');
hl=legend('X(t) - слежение','X_o(t) - эталон',0);
set(hl,'FontName','Courier');
grid on
figure(8);
plot(time_X, X(5,:),'r-', time_X, X_o_discrete(5,:), 'LineWidth', 2)
title ('x_5(t)');
xlabel('t');
hl=legend('X(t) - слежение','X_o(t) - эталон',0);
set(hl,'FontName','Courier');
grid on
function Solve_Interval(P_nach, N_str, h, P2, A,B,Q,R, T_nach, T_konech, X_0, poryadok, K_o, K_pr)
Zadayushee_Vozdeistvie_Discrete_Revers_Modern(h, T_nach, T_konech);
load Zadayushee_Vozdeistvie_Discrete_Revers X_o_discrete_rev
% ------------------------------------------------------------------------%
% Нахождение q(t)
for i = 1 : poryadok
qq = -P_nach(:,:,1) * X_o_discrete_rev(i,1);
q(i,1) = qq(i,1);
end
% Интегрирование q(t) в обратном времени
for k = 1 : N_str
q(:, k+1) = q(:, k) - h * ((P2(:,:,k)*B*inv(R)*B'-A') * q(:, k) + Q*X_o_discrete_rev(:,k));
end
q(:, k+1) = [];
size_q = size(q)
% ------------------------------------------------------------------------%
% Формирование вектора коэффициентов регулятора, значений задающего
% воздействия, значений вспомогательной функции в прямом порядке
K_pr_p = K_pr;
i = 1;
for j = N_str : -1 : 1
K_o_p(i,:) = K_o(j,:);
X_o_discrete(:,i) = X_o_discrete_rev(:,j);
q_pr(:, i) = q(:, j);
i = i + 1;
end
% ------------------------------------------------------------------------%
% ------------------------------------------------------------------------%
for k = 1 : N_str
A_(:,:,k) = A + B * K_o_p(k,:);
end
size_A_ = size(A_)
% ------------------------------------------------------------------------%
% ------------------------------------------------------------------------%
% Нахождение фазовых координат
X(:,1) = X_0;
time_X(1) = T_nach;
for k = 1 : N_str
X(:, k+1) = X(:, k) + h * (A_(:,:,k) * X(:, k) + B * K_pr_p(k,:) * q_pr(:,k));
time_X(k+1) = time_X(k) + h;
end
X(:, k+1) = [];
time_X(k+1) = [];
size_X = size(X)
% ------------------------------------------------------------------------%
% ------------------------------------------------------------------------%
% Нахождение управления
for k = 1 : N_str
u(k) = K_o_p(k,:) * X(:,k) + K_pr_p(k,:) * q_pr(:,k);
end
size_u = size(u)
save Solve_Interval time_X X u X_o_discrete
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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 = 10;
% ------------------------------------------------------------------------%
% ------------------------------------------------------------------------%
% Получение 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);
Q(2,2) = Q(2,2);
Q(3,3) = Q(3,3);
Q(4,4) = Q(4,4);
Q(5,5) = Q(5,5);
% 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_nach = zeros(poryadok, poryadok);%+ones(poryadok, poryadok);
% ------------------------------------------------------------------------%
% Решение уравнения Риккати методом обратного интегрирования
P1 = Solve_Riccati_Method_Revers_Integr(A,B,Q,R,Time,poryadok, P_nach)
% ------------------------------------------------------------------------%
% Построение графика коэффициентов регулятора
load Solve_Riccati_Method_Revers_Integr Time_R P N_str
PP = P;
for i = 1 : N_str
P = reshape(PP(i, :), poryadok, poryadok);
K(i, :) = -inv(R)*B'*P;
end
figure(2)
plot(Time_R,K(:,1),'-',Time_R,K(:,2),'-',Time_R,K(:,3),'-',Time_R,K(:,4),'-',Time_R,K(:,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;
% ------------------------------------------------------------------------%
% Нахождение коэффициентов регулятора
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
% ------------------------------------------------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Метод диагонализации для решения алгебраического уравнения Риккати
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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);
% ------------------------------------------------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Решение уравнения Риккати интегрированием в обратном времени
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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',...