%% 1. 清空环境与参数初始化

clear; clc; close all;

%% 2. 核心参数设置（无修改，确保n_agent=4）

n_agent = 4; % 智能体数量（明确设为4）

t_sim = 10; % 仿真总时间（秒）

dt = 0.01; % 时间步长

t = 0:dt:t_sim; % 时间向量

n_step = length(t); % 总迭代步数

% 通信拓扑：无向星型邻接矩阵A（4智能体）

A = [

0 1 1 1; % 智能体1连接2、3、4

1 0 0 0; % 智能体2连接1

1 0 0 0; % 智能体3连接1（故障体）

1 0 0 0 % 智能体4连接1

];

% 控制与故障参数

k_normal = 2; % 正常控制增益

k_fault = 5; % 故障后控制增益

fault_agent = 3; % 故障智能体编号

fault_step = 300; % 故障时刻（t=3秒，第300步）

fault_ratio = 0.3; % 故障执行器效率

% 初始状态与状态矩阵（4智能体初始状态不同，确保轨迹有差异）

x0 = [10, 2, 15, 5]; % 智能体1=10，2=2，3=15，4=5（初始状态差异明显）

x = zeros(n_agent, n_step); % 行=4个智能体，列=时间步

x(:,1) = x0;

% 拉普拉斯矩阵L

D = diag(sum(A, 2)); % 度矩阵（对角元素=邻居数）

L = D - A; % 拉普拉斯矩阵

%% 3. 仿真迭代（无修改，确保4智能体状态正常更新）

for step = 2:n_step

% 切换控制增益（故障前后）

if step <= fault_step

k = k_normal;

fault_flag = 0;

else

k = k_fault;

fault_flag = 1;

end

% 计算控制输入u（4智能体分别计算）

u = zeros(n_agent, 1);

Lx = L * x(:, step-1);

for i = 1:n_agent

u(i) = -k * Lx(i); % 4智能体各有独立控制输入

% 容错处理（仅智能体3故障）

if fault_flag && (i == fault_agent)

u(i) = u(i) * fault_ratio;

end

end

% 状态更新（4智能体状态同步更新）

x(:, step) = x(:, step-1) + u * dt;

end

%% 4. 结果可视化（重点修正：绘图前开启hold on，确保4条轨迹全显示）

figure('Position', [100, 100, 1000, 700]);

colors = ['r', 'g', 'm', 'b']; % 4种颜色，对应4个智能体（红1、绿2、品3、蓝4）

% 4.1 状态轨迹（修正：循环前开启hold on）

subplot(3,1,1);

hold on; % 关键修正：在绘图循环前开启，所有轨迹叠加显示

for i = 1:n_agent % 遍历4个智能体，绘制各自轨迹

plot(t, x(i,:), colors(i), 'LineWidth', 2, ...

'DisplayName', sprintf('智能体%d', i)); % 每个智能体标签不同

end

% 绘制故障时刻线

plot([t(fault_step), t(fault_step)], [min(x(:)), max(x(:))], ...

'k--', 'LineWidth', 1.5, 'DisplayName', '故障时刻(t=3s)');

xlabel('时间 t (s)'); ylabel('智能体状态 x_i');

title('4智能体一致性轨迹（红1、绿2、品3、蓝4）');

legend('Location', 'best'); % 显示4个智能体图例

grid on;

hold off; % 结束当前图的hold on

% 4.2 一致性误差（同理：循环前开启hold on）

subplot(3,1,2);

hold on; % 修正：循环前开启hold on

x_mean = mean(x, 1); % 4智能体状态平均值

for i = 1:n_agent

error_i = abs(x(i,:) - x_mean); % 每个智能体的一致性误差

plot(t, error_i, colors(i), 'LineWidth', 1.5, ...

'DisplayName', sprintf('智能体%d误差', i));

end

% 绘制故障时刻线

plot([t(fault_step), t(fault_step)], [0, max(abs(x(:)-x_mean))], ...

'k--', 'LineWidth', 1.5, 'DisplayName', '故障时刻(t=3s)');

xlabel('时间 t (s)'); ylabel('|x_i - 均值|');

title('4智能体一致性误差');

legend('Location', 'best');

grid on;

hold off;

% 4.3 控制输入（同理：循环前开启hold on）

subplot(3,1,3);

hold on; % 修正：循环前开启hold on

u_plot = zeros(n_agent, n_step);

for step = 1:n_step-1

% 确定当前控制增益（if-else替代三目运算符）

if step <= fault_step

k = k_normal;

else

k = k_fault;

end

Lx = L * x(:, step);

for i = 1:n_agent

u_plot(i, step) = -k * Lx(i);

% 故障体输入衰减

if step > fault_step && i == fault_agent

u_plot(i, step) = u_plot(i, step) * fault_ratio;

end

end

end

u_plot(:, end) = u_plot(:, end-1); % 补全最后一步输入

% 绘制4个智能体的控制输入

for i = 1:n_agent

plot(t, u_plot(i,:), colors(i), 'LineWidth', 1.5, ...

'DisplayName', sprintf('智能体%d输入', i));

end

% 绘制故障时刻线

plot([t(fault_step), t(fault_step)], [min(u_plot(:)), max(u_plot(:))], ...

'k--', 'LineWidth', 1.5, 'DisplayName', '故障时刻(t=3s)');

xlabel('时间 t (s)'); ylabel('控制输入 u_i');

title('4智能体控制输入变化');

legend('Location', 'best');

grid on;

hold off;

%% 5. 结果输出（无修改，验证4智能体状态）

fprintf('==================== 4智能体仿真结果 ====================\n');

% 输出4个智能体的初始状态和最终状态

fprintf('初始状态：智能体1=%.1f, 智能体2=%.1f, 智能体3=%.1f, 智能体4=%.1f\n', ...

x0(1), x0(2), x0(3), x0(4));

fprintf('最终状态：智能体1=%.6f, 智能体2=%.6f, 智能体3=%.6f, 智能体4=%.6f\n', ...

x(1,end), x(2,end), x(3,end), x(4,end));

% 输出一致性误差

normal_error = max(abs(x(:, fault_step) - mean(x(:, fault_step))));

stable_step = find(t>=8, 1, 'first');

fault_error = max(abs(x(:, stable_step:end) - mean(x(:, stable_step:end), 1)));

fprintf('正常阶段最大误差：%.6f\n', normal_error);

fprintf('故障后稳定误差：%.6f\n', fault_error);

fprintf('=========================================================\n');