%----------------状态反馈鲁棒镇定------------
clear all;clc;close all;
%% 参数初始化
A = [0 1 0;0 0 1;57000 1938 -16]; B = [0;0;-14.25];
syms r s;
deltaA = [0 0 0;0 0 0;114000*r -324.9*r -3.2*r];
deltaB = [0;0;-2.85*s];
Dr = B\deltaA; Er = B\deltaB;
Q0 = eye(3);
P0 = care(A,B,Q0);
K = -B'*P0;
A2 = A+B*K;
delta = 1.6;
Q = [3200000 122005 2016.5;122005 8203.0 125.43;2016.5 125.43 1.9344] + eye(3);
P = care(A2,B,Q,-1);
F = K - (1/1.6)*B'*P;
x = [2 5 2]';
deltaA = [0 0 0;0 0 0;114000*rand -324.9*rand -3.2*rand];
deltaB = [0;0;-2.85*rand];
AA = A+deltaA;
BB = B+deltaB;
h = 0.25; % 步长
t = 0:h:20;
% u =0
AA = A;BB = B;
K = [-20 -15 -15];
for i =1:1:length(t)
u = K*x(:,i);
K1 = AA*x(:,i)+BB*u;
K2 = AA*(x(:,i)+K1*h/2)+BB*u;
K3 = AA*(x(:,i)+K2*h/2)+BB*u;
K4 = AA*(x(:,i)+K3*h)+BB*u;
x(:,i+1) = x(:,i) + h*(K1+2*K2+2*K3+K4)/6;
end
figure
plot(t,x(1,1:length(t)),'b');
hold on
plot(t,x(2,1:length(t)),'r');
hold on
plot(t,x(3,1:length(t)),'m');
hold on
% plot(t,y(1:length(t)),'k');
% plot(t,xba(1,1:length(t)),'b--');
% hold on
% plot(t,xba(2,1:length(t)),'r--');
% hold on
% plot(t,xba(3,1:length(t)),'m--');
% hold on
legend('x1','x2','x3')
xlabel('t/s');
ylabel('状态');
grid minor
% axis equal
