clc
clear all
%% 对领导者的旋转跟踪
% syms i
%% test
% A = [0,1;0,0];
% B = [0;1];
% C = [1,0];
% D = [0];
% E = [0,0;0,0.5*1];
% F = [-1,0];
% S = [0,1;-1,0];
%% 计算各自的调节器方程得到X U
S = kron(eye(3),[0,1;0,0]);
F = kron(eye(3),[1,0]);
matix(1).A = zeros(3,3);
matix(1).B = eye(3);
matix(1).C = eye(3);
for i=2:6
matix(i).A = kron(eye(3),[0,1;i-2,i-2]);
matix(i).B = kron(eye(3),[0;i-1]);
matix(i).C = kron(eye(3),[1,0]);
end
matix(7).A = kron(eye(3),[0,1,0;0,0,1;1,2,1]);
matix(7).B = kron(eye(3),[0;0;1]);
matix(7).C = kron(eye(3),[1,0,0]);
for i=1:7
D = [0];
matix(i).E = zeros(size(matix(i).A,1),size(S,2));
matix(i).result = regulator(matix(i).A,matix(i).B,matix(i).C,D,matix(i).E,F,S);
end
%% X U矩阵符号数字化
matix(1).X = [ double(matix(1).result.X1_1),double(matix(1).result.X1_2),double(matix(1).result.X1_3),double(matix(1).result.X1_4),double(matix(1).result.X1_5),double(matix(1).result.X1_6);
double(matix(1).result.X2_1),double(matix(1).result.X2_2),double(matix(1).result.X2_3),double(matix(1).result.X2_4),double(matix(1).result.X2_5),double(matix(1).result.X2_6);
double(matix(1).result.X3_1),double(matix(1).result.X3_2),double(matix(1).result.X3_3),double(matix(1).result.X3_4),double(matix(1).result.X3_5),double(matix(1).result.X3_6)];
for i = 1:7
matix(i).U = [ double(matix(i).result.U1_1),double(matix(i).result.U1_2),double(matix(i).result.U1_3),double(matix(i).result.U1_4),double(matix(i).result.U1_5),double(matix(i).result.U1_6);
double(matix(i).result.U2_1),double(matix(i).result.U2_2),double(matix(i).result.U2_3),double(matix(i).result.U2_4),double(matix(i).result.U2_5),double(matix(i).result.U2_6);
double(matix(i).result.U3_1),double(matix(i).result.U3_2),double(matix(i).result.U3_3),double(matix(i).result.U3_4),double(matix(i).result.U3_5),double(matix(i).result.U3_6)];
if i>=2&&i<=6
matix(i).X = [ double(matix(i).result.X1_1),double(matix(i).result.X1_2),double(matix(i).result.X1_3),double(matix(i).result.X1_4),double(matix(i).result.X1_5),double(matix(i).result.X1_6);
double(matix(i).result.X2_1),double(matix(i).result.X2_2),double(matix(i).result.X2_3),double(matix(i).result.X2_4),double(matix(i).result.X2_5),double(matix(i).result.X2_6);
double(matix(i).result.X3_1),double(matix(i).result.X3_2),double(matix(i).result.X3_3),double(matix(i).result.X3_4),double(matix(i).result.X3_5),double(matix(i).result.X3_6);
double(matix(i).result.X4_1),double(matix(i).result.X4_2),double(matix(i).result.X4_3),double(matix(i).result.X4_4),double(matix(i).result.X4_5),double(matix(i).result.X4_6);
double(matix(i).result.X5_1),double(matix(i).result.X5_2),double(matix(i).result.X5_3),double(matix(i).result.X5_4),double(matix(i).result.X5_5),double(matix(i).result.X5_6);
double(matix(i).result.X6_1),double(matix(i).result.X6_2),double(matix(i).result.X6_3),double(matix(i).result.X6_4),double(matix(i).result.X6_5),double(matix(i).result.X6_6)];
end
end
i = 7;
matix(i).X = [ double(matix(i).result.X1_1),double(matix(i).result.X1_2),double(matix(i).result.X1_3),double(matix(i).result.X1_4),double(matix(i).result.X1_5),double(matix(i).result.X1_6);
double(matix(i).result.X2_1),double(matix(i).result.X2_2),double(matix(i).result.X2_3),double(matix(i).result.X2_4),double(matix(i).result.X2_5),double(matix(i).result.X2_6);
double(matix(i).result.X3_1),double(matix(i).result.X3_2),double(matix(i).result.X3_3),double(matix(i).result.X3_4),double(matix(i).result.X3_5),double(matix(i).result.X3_6);
double(matix(i).result.X4_1),double(matix(i).result.X4_2),double(matix(i).result.X4_3),double(matix(i).result.X4_4),double(matix(i).result.X4_5),double(matix(i).result.X4_6);
double(matix(i).result.X5_1),double(matix(i).result.X5_2),double(matix(i).result.X5_3),double(matix(i).result.X5_4),double(matix(i).result.X5_5),double(matix(i).result.X5_6);
double(matix(i).result.X6_1),double(matix(i).result.X6_2),double(matix(i).result.X6_3),double(matix(i).result.X6_4),double(matix(i).result.X6_5),double(matix(i).result.X6_6);
double(matix(i).result.X7_1),double(matix(i).result.X7_2),double(matix(i).result.X7_3),double(matix(i).result.X7_4),double(matix(i).result.X7_5),double(matix(i).result.X7_6);
double(matix(i).result.X8_1),double(matix(i).result.X8_2),double(matix(i).result.X8_3),double(matix(i).result.X8_4),double(matix(i).result.X8_5),double(matix(i).result.X8_6);
double(matix(i).result.X9_1),double(matix(i).result.X9_2),double(matix(i).result.X9_3),double(matix(i).result.X9_4),double(matix(i).result.X9_5),double(matix(i).result.X9_6)];
save('matix.mat','matix');
load('matix.mat');
for i = 1:7
% 假设希望特征值的实部均为负数
if i==1
desiredEigenvalues = -2*ones(1,3);
elseif i==7
desiredEigenvalues = -[2 2 2 4 4 4 6 6 6];
else
desiredEigenvalues = -[2 2 2 4 4 4];
end
% 使用极小实部法计算矩阵K
% place 函数将A-BK的特征值放置在desiredEigenvalues处，且特征根的重数不超过B的秩
Kx = place(matix(i).A, -matix(i).B, desiredEigenvalues);
%% 计算得到Kx,Kv
matix(i).Kx = Kx;
[~,matix(i).Acl] = eig(matix(i).A+matix(i).B*matix(i).Kx);
matix(i).Kv = -(matix(i).U-matix(i).Kx*matix(i).X);
end
L = [ 0, 0, 0, 0, 0, 0, 0, 0;
0, 2, -1, 0, 0, -1, 0, 0;
-1, -1, 3, 0, -1, 0, 0, 0;
-1, 0, 0, 2, -1, 0, 0, 0;
0, 0, -1, -1, 3, 0, 0, -1;
0, -1, 0, 0, 0, 2, -1, 0;
0, 0, 0, 0, 0, -1, 2, -1;
0, 0, 0, 0, -1, 0, -1, 2];
A = [ 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 1, 0, 0, 1, 0, 0;
1, 1, 0, 0, 1, 0, 0, 0;
1, 0, 0, 0, 1, 0, 0, 0;
0, 0, 1, 1, 0, 0, 0, 1;
0, 1, 0, 0, 0, 0, 1, 0;
0, 0, 0, 0, 0, 1, 0, 1;
0, 0, 0, 0, 1, 0, 1, 0];
%% 初始化
for i=1:7
matix(i).x(:,1) = 3*rand(size(matix(i).A,1),1);
matix(i).u(:,1) = ones(3,1);
matix(i).yita(:,1) = ones(6,1);
end
z = zeros(6,1);
z = [0;2;2;0.5;3;1];
q = F*z;
%%
miu = 1;
dt = 0.01;
for time = 1:5000
t(time+1) = time*dt;
z(:,time+1) = z(:,time) + S*z(:,time)*dt;
q(:,time+1) = F*z(:,time+1);
for i = 1:7
matix(i).h(:,time+1) = [0;0;3*cos(t(time+1)+2*(i-1)*pi/7);-3*sin(t(time+1)+2*(i-1)*pi/7);3*sin(t(time+1)+2*(i-1)*pi/7);3*cos(t(time+1)+2*(i-1)*pi/7)];
matix(i).u(:,time) = matix(i).Kx*matix(i).x(:,time) + matix(i).Kv*(matix(i).yita(:,time)+matix(i).h(:,time+1));
matix(i).dx(:,time+1) = matix(i).A*matix(i).x(:,time) + matix(i).B*matix(i).u(:,time);
matix(i).x(:,time+1) = matix(i).x(:,time) + (matix(i).A*matix(i).x(:,time) + matix(i).B*matix(i).u(:,time))*dt;
%% 计算偏差求和
deltayita = 0;
for j = 1:7
deltayita = deltayita + A(i+1,j+1)*(matix(j).yita(:,time)-matix(i).yita(:,time));
end
matix(i).yita(:,time+1) = matix(i).yita(:,time) + ( S*matix(i).yita(:,time) + miu*( deltayita +A(i+1,1)*(z(:,time)-matix(i).yita(:,time)))) *dt;
matix(i).y(:,time+1) = matix(i).C*matix(i).x(:,time+1);
matix(i).e(:,time+1) = matix(i).y(:,time+1)-q(:,time+1);
end
time*dt
end
%%
figure(666)
mm=2000
plot3(matix(1).y(1,1:mm),matix(1).y(2,1:mm),matix(1).y(3,1:mm),'r','LineWidth',1.5);
grid on
hold on
plot3(matix(2).y(1,1:mm),matix(2).y(2,1:mm),matix(2).y(3,1:mm),'g','LineWidth',1.5);
plot3(matix(3).y(1,1:mm),matix(3).y(2,1:mm),matix(3).y(3,1:mm),'b','LineWidth',1.5);
plot3(matix(4).y(1,1:mm),matix(4).y(2,1:mm),matix(4).y(3,1:mm),'c','LineWidth',1.5);
plot3(matix(5).y(1,1:mm),matix(5).y(2,1:mm),matix(5).y(3,1:mm),'--r','LineWidth',1.5);
plot3(matix(6).y(1,1:mm),matix(6).y(2,1:mm),matix(6).y(3,1:mm),'--b','LineWidth',1.5);
plot3(matix(7).y(1,1:mm
