sim_single_raim.m

%% SIM_SINGLE_RAIM
% 300-step simulation of single-robot cooperative RAIM.
%
% Robot motion (all deterministic, no random walk):
%   Detector  — waypoint route, single pass over 300 steps
%   Helper 1  — large circle, left-of-centre, r=0.28*A
%   Helper 2  — large circle, right-of-centre, r=0.28*A
%   Helper 3  — lawnmower, full width, lower 60%  ← FAULTY from step 60
%   Helper 4  — lawnmower, left  60%, full height
%   Helper 5  — lawnmower, full width, upper 60%  (overlaps H3 in mid-band)
%
% Two identification methods compared:
%   (A) Clustering RAIM  — subset enumeration + cluster + union-complement
%   (B) Max-Residual     — largest normalised residual (classical baseline)

clear; clc; close all;
% rng(42);

% =========================================================================
%% PARAMETERS
% =========================================================================

arenaSize   = 30;
nWaypoints  = 8;      
wpMargin    = 0.15 * arenaSize; 
N           = 5;        % helpers
stateDim    = 2;

sigma_range = 0.2;      % UWB range noise std dev (m)
sigma_pos   = 0.15;     % helper position estimate noise std dev (m)

fault_bot   = randi(5);        % faulty helper index (1..N)
fault_time  = 60;       % step fault activates
fault_mag   = 0.5;      % bias magnitude (m)

alpha       = 1e-3;
method      = 'ahc';
criterion   = 'largest';

T           = 300;

% =========================================================================
%% PRE-COMPUTE PATHS
% =========================================================================

fprintf('Generating robot paths...\n');
wp = wpMargin + (arenaSize - 2*wpMargin) * rand(nWaypoints, 2);
startPhases = rand(6, 1);
paths = generate_paths(T, arenaSize,wp,startPhases);

% Fault direction — fixed for the run
% rng(42);
fault_dir = randn(1,2);  fault_dir = fault_dir / norm(fault_dir);
fault_vec = fault_mag * fault_dir;

covHelper = sigma_pos^2 * eye(2);

% =========================================================================
%% PREALLOCATE LOGS
% =========================================================================

log_xTrue           = zeros(T, 2);
log_xEstFull        = zeros(T, 2);
log_xEstClean_raim  = zeros(T, 2);
log_xEstClean_maxr  = zeros(T, 2);

log_errFull         = zeros(T, 1);
log_errClean_raim   = zeros(T, 1);
log_errClean_maxr   = zeros(T, 1);

log_testStat        = zeros(T, 1);
log_threshold       = zeros(T, 1);
log_detected        = false(T, 1);
log_id_raim         = false(T, 1);
log_id_maxr         = false(T, 1);
log_faultActive     = false(T, 1);
log_pred_maxr       = zeros(T, 1);   % predicted faulty index from max-residual
result_store        = cell(T, 1);    % RAIM result per detected step

% =========================================================================
%% SIMULATION LOOP
% =========================================================================

fprintf('Running %d-step simulation  (fault on helper %d from step %d, mag=%.1fm)\n', ...
        T, fault_bot, fault_time, fault_mag);

for t = 1:T

    % --- 1. Read true positions from pre-computed paths ---
    p_det_true  = paths.detector(t, :)';            % [2x1]
    p_help_true = squeeze(paths.helpers(t, :, :))'; % [N x 2]

    % --- 2. Noisy helper position estimates ---
    p_help_est = p_help_true + sigma_pos * randn(N, 2);

    % --- 3. Inject fault ---
    faultActive = (t >= fault_time);
    if faultActive
        p_help_est(fault_bot, :) = p_help_est(fault_bot, :) + fault_vec;
    end
    log_faultActive(t) = faultActive;

    % --- 4. UWB range measurements ---
    ranges = zeros(N, 1);
    for i = 1:N
        ranges(i) = norm(p_det_true - p_help_true(i,:)') + sigma_range * randn();
    end

    % --- 5a. Full WLS + chi2 detection ---
    [xEstFull, vRes, ~, W] = wls_position(p_help_est, ranges, covHelper, sigma_range);
    [faultDetected, testStat, threshold] = chi2_detect(vRes, W, stateDim, alpha);

    log_xTrue(t,:)    = p_det_true';
    log_xEstFull(t,:) = xEstFull';
    log_testStat(t)   = testStat;
    log_threshold(t)  = threshold;
    log_detected(t)   = faultDetected;
    log_errFull(t)    = norm(xEstFull - p_det_true);

    if ~faultDetected
        log_xEstClean_raim(t,:) = xEstFull';
        log_xEstClean_maxr(t,:) = xEstFull';
        log_errClean_raim(t)    = log_errFull(t);
        log_errClean_maxr(t)    = log_errFull(t);
        continue;
    end

    % --- 5b. Clustering RAIM ---
    result = run_raim(p_help_est, ranges, covHelper, sigma_range, ...
                      'alpha', alpha, 'method', method, ...
                      'criterion', criterion, 'stateDim', stateDim, ...
                      'verbose', false);

    log_xEstClean_raim(t,:) = result.xEst';
    log_errClean_raim(t)    = norm(result.xEst - p_det_true);
    result_store{t}         = result;   % store for per-step annotation
    if faultActive && ~isempty(result.faultyMeasIdx)
        log_id_raim(t) = (result.faultyMeasIdx == fault_bot);
    end

    % --- 5c. Max-residual ---
    [faultyIdx_maxr, ~] = identify_fault_maxresidual(p_help_est, ranges, covHelper, sigma_range);
    cleanIdx  = setdiff(1:N, faultyIdx_maxr);
    xClean_mr = wls_position(p_help_est(cleanIdx,:), ranges(cleanIdx), covHelper, sigma_range);

    log_xEstClean_maxr(t,:) = xClean_mr';
    log_errClean_maxr(t)    = norm(xClean_mr - p_det_true);
    log_pred_maxr(t)        = faultyIdx_maxr;
    if faultActive
        log_id_maxr(t) = (faultyIdx_maxr == fault_bot);
    end
end

% =========================================================================
%% SUMMARY
% =========================================================================

fault_steps     = find(log_faultActive);
healthy_steps   = find(~log_faultActive);
det_fault_steps = fault_steps(log_detected(fault_steps));

fprintf('\n============================================================\n');
fprintf('                     SIMULATION SUMMARY\n');
fprintf('============================================================\n');
fprintf('Steps before fault  : %d\n', numel(healthy_steps));
fprintf('Steps with fault    : %d\n', numel(fault_steps));
fprintf('Detection rate      : %.1f%%\n', ...
        100*sum(log_detected(fault_steps))/numel(fault_steps));
fprintf('False alarm rate    : %.2f%%  (target: %.2f%%)\n', ...
        100*sum(log_detected(healthy_steps))/numel(healthy_steps), 100*alpha);

fprintf('\n%-34s  %14s  %14s\n', '', 'Clustering RAIM', 'Max Residual');
fprintf('%s\n', repmat('-',1,66));
fprintf('%-34s  %13.4fm  %13.4fm\n', 'Mean error — healthy', ...
        mean(log_errFull(healthy_steps)), mean(log_errFull(healthy_steps)));
fprintf('%-34s  %13.4fm  %13.4fm\n', 'Mean error — full WLS (fault)', ...
        mean(log_errFull(fault_steps)), mean(log_errFull(fault_steps)));
fprintf('%-34s  %13.4fm  %13.4fm\n', 'Mean error — clean WLS (fault)', ...
        mean(log_errClean_raim(fault_steps)), mean(log_errClean_maxr(fault_steps)));
if ~isempty(det_fault_steps)
    id_raim = 100*sum(log_id_raim(det_fault_steps))/numel(det_fault_steps);
    id_maxr = 100*sum(log_id_maxr(det_fault_steps))/numel(det_fault_steps);
    fprintf('%-34s  %13.1f%%  %13.1f%%\n', 'ID accuracy (detected steps)', id_raim, id_maxr);
end
fprintf('============================================================\n');

% =========================================================================
%% FIGURE 1 — Performance metrics
% =========================================================================

figure('Name','RAIM Performance','Position',[50 50 1300 850]);
tAxis = 1:T;
fp    = @(ax,ymax) patch(ax, [fault_time T T fault_time],[0 0 ymax ymax], ...
                          [1 0.8 0.8],'FaceAlpha',0.35,'EdgeColor','none', ...
                          'HandleVisibility','off');

% Panel 1: Position error
ax1 = subplot(2,3,1); hold on; grid on;
fp(ax1, max(log_errFull)*1.5+0.01);
plot(tAxis, log_errFull,       'Color',[0.6 0.6 0.6],'LineWidth',1,  'DisplayName','Full WLS (biased)');
plot(tAxis, log_errClean_raim, 'b-','LineWidth',1.5,'DisplayName','Clustering RAIM');
plot(tAxis, log_errClean_maxr, 'r-','LineWidth',1.5,'DisplayName','Max Residual');
xline(fault_time,'k--','LineWidth',1,'HandleVisibility','off');
xlabel('Step'); ylabel('Position error (m)'); title('Position Error vs Step');
legend('Location','northwest'); ylim([0 inf]);

% Panel 2: Chi2 test statistic
ax2 = subplot(2,3,2); hold on; grid on;
fp(ax2, max(log_testStat)*1.2+0.01);
plot(tAxis, log_testStat,  'b-', 'LineWidth',1,  'DisplayName','Test statistic T');
plot(tAxis, log_threshold, 'r--','LineWidth',1.5,'DisplayName',sprintf('Threshold (\\alpha=%.0e)',alpha));
xline(fault_time,'k--','LineWidth',1,'HandleVisibility','off');
xlabel('Step'); ylabel('\chi^2'); title('Detection Test Statistic');
legend('Location','northwest'); ylim([0 inf]);

% Panel 3: Per-step ID correctness — wrong IDs annotated with predicted helper
ax3 = subplot(2,3,3); hold on; grid on;
patch(ax3,[fault_time T T fault_time],[0.2 0.2 1.5 1.5], ...
      [1 0.8 0.8],'FaceAlpha',0.35,'EdgeColor','none','HandleVisibility','off');
for s = det_fault_steps'
    % Clustering RAIM row (y=1.15)
    col_raim = id_color(log_id_raim(s));
    plot(ax3, s, 1.15, '.', 'Color', col_raim, 'MarkerSize', 10, 'HandleVisibility','off');
    if ~log_id_raim(s) && ~isempty(result_store{s}) && ~isempty(result_store{s}.faultyMeasIdx)
        text(ax3, s, 1.22, sprintf('H%d', result_store{s}.faultyMeasIdx), ...
             'FontSize', 7, 'Color', col_raim, 'HorizontalAlignment','center');
    end
    % Max residual row (y=0.65)
    col_maxr = id_color(log_id_maxr(s));
    plot(ax3, s, 0.65, '.', 'Color', col_maxr, 'MarkerSize', 10, 'HandleVisibility','off');
    if ~log_id_maxr(s) && log_pred_maxr(s) > 0
        text(ax3, s, 0.72, sprintf('H%d', log_pred_maxr(s)), ...
             'FontSize', 7, 'Color', col_maxr, 'HorizontalAlignment','center');
    end
end
plot(ax3,nan,nan,'g.','MarkerSize',14,'DisplayName','Correct ID');
plot(ax3,nan,nan,'r.','MarkerSize',14,'DisplayName','Wrong ID (Hx = predicted)');
xline(ax3,fault_time,'k--','LineWidth',1,'HandleVisibility','off');
yticks([0.65 1.15]); yticklabels({'Max Residual','Clustering RAIM'});
ylim([0.3 1.5]); xlabel('Step');
title('Per-Step ID (detected fault steps)');
% legend('Location','northeast');

% Panel 4: Cumulative ID accuracy
ax4 = subplot(2,3,4); hold on; grid on;
cum_raim = cumsum(log_id_raim(fault_steps))./(1:numel(fault_steps))';
cum_maxr = cumsum(log_id_maxr(fault_steps))./(1:numel(fault_steps))';
plot(fault_steps, 100*cum_raim,'b-','LineWidth',2,'DisplayName','Clustering RAIM');
plot(fault_steps, 100*cum_maxr,'r-','LineWidth',2,'DisplayName','Max Residual');
xline(fault_time,'k--','LineWidth',1,'HandleVisibility','off');
xlabel('Step'); ylabel('Cumulative ID accuracy (%)');
title('Cumulative Identification Accuracy');
legend('Location','southeast'); ylim([0 105]);

% Panel 5: Error boxplot
ax5 = subplot(2,3,5); hold on; grid on;
boxplot([log_errFull(fault_steps), log_errClean_raim(fault_steps), log_errClean_maxr(fault_steps)], ...
        'Labels',{'Full WLS','Clustering RAIM','Max Residual'}, ...
        'Colors',[0.5 0.5 0.5; 0 0 1; 1 0 0],'Symbol','.');
ylabel('Position error (m)'); title('Error Distribution — Fault Steps');

% Panel 6: UWB range to each helper over time
ax6 = subplot(2,3,6); hold on; grid on;
colors6 = lines(N);
for i = 1:N
    r_i = zeros(T,1);
    for t2 = 1:T
        r_i(t2) = norm(paths.detector(t2,:)' - squeeze(paths.helpers(t2,:,i))');
    end
    lw = 1 + (i == fault_bot);
    ls = '-'; if i == fault_bot; ls = '--'; end
    plot(tAxis, r_i, ls,'Color',colors6(i,:),'LineWidth',lw, ...
         'DisplayName',sprintf('H%d%s',i,ternary(i==fault_bot,' (faulty)','')));
end
xline(fault_time,'k--','LineWidth',1,'HandleVisibility','off');
xlabel('Step'); ylabel('True range (m)'); title('Detector–Helper Ranges Over Time');
legend('Location','best');

sgtitle(sprintf('RAIM Comparison  |  N=%d helpers  |  fault bot=%d  |  mag=%.1fm  |  T=%d steps', ...
        N, fault_bot, fault_mag, T),'FontSize',13,'FontWeight','bold');


%% FIGURE 2 — Robot trajectories
% =========================================================================

figure('Name','Robot Trajectories','Position',[100 100 800 750]);
hold on; grid on; axis equal;
ax = gca;
ax.XLim = [0 arenaSize]; ax.YLim = [0 arenaSize];


% Helper paths — healthy helpers in muted colours, faulty in solid red
col_h  = lines(N);
faultCol = [0.85 0.12 0.12];   % strong red reserved for faulty robot
hNames = {'H1 — circle left','H2 — circle right', ...
          'H3 — lawnmower lower','H4 — lawnmower left','H5 — lawnmower upper'};

for i = 1:N
    hp = squeeze(paths.helpers(:,:,i));
    if i == fault_bot
        % True path (what the robot actually does)
        plot(hp(:,1), hp(:,2), '-', 'Color', faultCol*0.5+0.5, 'LineWidth', 1, ...
             'HandleVisibility','off');
        % Apparent path after fault: position estimate shifted by fault_vec
        hp_faulty = hp;
        hp_faulty(fault_time:end,:) = hp(fault_time:end,:) + fault_vec;
        plot(hp_faulty(fault_time:end,1), hp_faulty(fault_time:end,2), '-', ...
             'Color', faultCol, 'LineWidth', 2.5, ...
             'DisplayName', sprintf('H%d — apparent path after fault', fault_bot));
        % Mark fault onset
        plot(hp_faulty(fault_time,1), hp_faulty(fault_time,2), 'rv', ...
             'MarkerSize', 9, 'MarkerFaceColor', faultCol, 'HandleVisibility','off');
        % Mark start
        plot(hp(1,1), hp(1,2), 'o', 'Color', faultCol, 'MarkerSize', 7, ...
             'MarkerFaceColor', faultCol*0.5+0.5, 'HandleVisibility','off');
    else
        plot(hp(:,1), hp(:,2), '-', 'Color', col_h(i,:)*0.65, 'LineWidth', 1, ...
             'DisplayName', sprintf('H%d — %s', i, hNames{i}));
        plot(hp(1,1), hp(1,2), 'o', 'Color', col_h(i,:)*0.65, 'MarkerSize', 6, ...
             'MarkerFaceColor', col_h(i,:)*0.65, 'HandleVisibility','off');
    end
end

% Detector path
dp = paths.detector;
plot(dp(:,1), dp(:,2), 'k-', 'LineWidth', 2, 'DisplayName', 'Detector (waypoint route)');
plot(dp(1,1), dp(1,2),  'ks','MarkerSize',10,'MarkerFaceColor','k','HandleVisibility','off');
plot(dp(end,1),dp(end,2),'k^','MarkerSize',10,'MarkerFaceColor','k','HandleVisibility','off');

% Mark fault onset position on detector path
plot(dp(fault_time,1), dp(fault_time,2), 'r*', 'MarkerSize', 14, 'LineWidth', 2, ...
     'DisplayName', sprintf('Fault onset (step %d)', fault_time));

% Waypoints — default perimeter route scaled to arena
m_wp = 0.05*arenaSize;
default_wp = [m_wp,m_wp; 0.5*arenaSize,m_wp; arenaSize-m_wp,m_wp;
              arenaSize-m_wp,0.5*arenaSize; arenaSize-m_wp,arenaSize-m_wp;
              0.5*arenaSize,arenaSize-m_wp; m_wp,arenaSize-m_wp; m_wp,0.5*arenaSize];
plot(default_wp(:,1), default_wp(:,2), 'kd', 'MarkerSize',9, 'MarkerFaceColor',[0.8 0.8 0.8], ...
     'DisplayName','Waypoints');
for w = 1:size(default_wp,1)
    text(default_wp(w,1)+0.3, default_wp(w,2)+0.3, sprintf('WP%d',w), 'FontSize',8);
end

xlabel('x (m)'); ylabel('y (m)');
title(sprintf('Robot Trajectories — %dx%dm arena  (T=%d steps)', arenaSize, arenaSize, T), 'FontSize',13);
legend('Location','eastoutside');

%% LOCAL HELPERS
% =========================================================================

function col = id_color(correct)
    if correct; col = [0.1 0.75 0.1]; else; col = [0.9 0.1 0.1]; end
end

function out = ternary(cond, a, b)
    if cond; out = a; else; out = b; end
end