vis_loo_comparison.m

% vis_loo_comparison.m
%
% Illustrates the inverted clustering pattern using LOO subsets only.
%
% Left panel  — GNSS SS-RAIM: 5 satellites at large baselines.
%   Removing any one satellite barely changes the Jacobian, so all
%   healthy LOO solutions cluster tightly. The faulty LOO solution
%   (excluding the bad satellite) is also near the cluster — it is the
%   all-in-view solution that is displaced.  SS-RAIM detects the
%   separation between the all-in-view solution and the healthy cluster.
%
% Right panel — Cooperative localization: 5 peers at meter scale.
%   Removing any one peer significantly changes the GDOP, so each
%   faulty LOO subset (still containing the bad peer) lands in a
%   different scattered location. Only the one LOO subset that excludes
%   the faulty peer produces a clean, noise-limited solution near true.
%
% Fault: 10 m step on peer/satellite 1.
% -------------------------------------------------------------------------
clear; clc; close all;
rng(7);
warning('off','MATLAB:singularMatrix');
warning('off','MATLAB:nearlySingularMatrix');

% ═════════════════════════════════════════════════════════════════════
%  PARAMETERS
% ═════════════════════════════════════════════════════════════════════
sigma_UWB  = 0.20;        % ranging noise std (m)
sigma_pos  = 0.15;        % broadcast position noise std (m)  [coop only]
fault_peer = 1;           % index of faulty satellite / peer
fault_mag  = 10.0;        % step fault magnitude (m)
fault_dir  = [1; 0];      % fault direction (unit vector, horizontal)
N_peers    = 5;
N_MC       = 800;         % Monte Carlo draws per subset
p_ego      = [0; 0];      % true ego position

% ─── GNSS geometry: satellites spread on a large-radius circle ───────
% 1000 m stand-in for "very large baseline"; only direction matters
r_sat   = 1000;
theta_s = linspace(15, 345, N_peers) * pi/180;
peers_G = r_sat * [cos(theta_s); sin(theta_s)];   % 2×5

% ─── Cooperative geometry: peers within ~20 m arena ──────────────────
peers_C = [ 12,  -8, -14,   6,   3 ;
            -5,  10,   2, -11,   8 ];              % 2×5  (meters)

% ═════════════════════════════════════════════════════════════════════
%  LOO SUBSET BANK (5 subsets — exclude one peer at a time)
% ═════════════════════════════════════════════════════════════════════
peer_idx = 1:N_peers;
LOO = cell(N_peers,1);
for k = 1:N_peers
    LOO{k} = peer_idx(peer_idx ~= k);
end

% Subset k is "healthy" only if it excludes the faulty peer
% (fault_peer not in LOO{k})
is_healthy = false(N_peers,1);
for k = 1:N_peers
    is_healthy(k) = ~ismember(fault_peer, LOO{k});
end

% ═════════════════════════════════════════════════════════════════════
%  MONTE CARLO: solve WLS for every LOO subset, both panels
% ═════════════════════════════════════════════════════════════════════
sol_G = nan(N_peers, N_MC, 2);
sol_C = nan(N_peers, N_MC, 2);
sol_G_full = nan(N_MC, 2);   % all-in-view (GNSS)
sol_C_full = nan(N_MC, 2);   % all-in-view (coop)

for mc = 1:N_MC

    % Noisy peer broadcasts (coop) — GNSS peers are at fixed known positions
    phat_G = peers_G;                              % satellites: positions known
    phat_C = peers_C + sigma_pos*randn(2,N_peers); % robots: broadcast is noisy

    % Inject fault
    phat_G(:,fault_peer) = phat_G(:,fault_peer) + fault_mag*fault_dir;
    phat_C(:,fault_peer) = phat_C(:,fault_peer) + fault_mag*fault_dir;

    % All-in-view solutions
    rG = make_ranges(p_ego, peers_G, sigma_UWB);
    rC = make_ranges(p_ego, peers_C, sigma_UWB);
    sol_G_full(mc,:) = wls_solve(rG, phat_G, sigma_UWB, 0       )';
    sol_C_full(mc,:) = wls_solve(rC, phat_C, sigma_UWB, sigma_pos)';

    % LOO subsets
    for k = 1:N_peers
        idx = LOO{k};

        rG_sub = make_ranges(p_ego, peers_G(:,idx), sigma_UWB);
        rC_sub = make_ranges(p_ego, peers_C(:,idx), sigma_UWB);

        sol_G(k,mc,:) = wls_solve(rG_sub, phat_G(:,idx), sigma_UWB, 0       );
        sol_C(k,mc,:) = wls_solve(rC_sub, phat_C(:,idx), sigma_UWB, sigma_pos);
    end
end

% ── Mean solutions (filter extreme outliers) ──────────────────────────
BOUND = 200;   % m — anything beyond this is a diverged solve
mean_G = zeros(N_peers,2);  mean_C = zeros(N_peers,2);
for k = 1:N_peers
    pG = squeeze(sol_G(k,:,:));
    pC = squeeze(sol_C(k,:,:));
    okG = all(isfinite(pG),2) & all(abs(pG)<BOUND,2);
    okC = all(isfinite(pC),2) & all(abs(pC)<BOUND,2);
    if any(okG), mean_G(k,:) = mean(pG(okG,:),1); end
    if any(okC), mean_C(k,:) = mean(pC(okC,:),1); end
end

okGf = all(isfinite(sol_G_full),2) & all(abs(sol_G_full)<BOUND,2);
okCf = all(isfinite(sol_C_full),2) & all(abs(sol_C_full)<BOUND,2);
mean_G_full = mean(sol_G_full(okGf,:),1);
mean_C_full = mean(sol_C_full(okCf,:),1);

% ═════════════════════════════════════════════════════════════════════
%  FIGURE
% ═════════════════════════════════════════════════════════════════════
col_h   = [0.18 0.50 0.85];   % blue  — healthy LOO subsets
col_f   = [0.82 0.22 0.18];   % red   — faulty  LOO subsets
col_aiv = [0.25 0.25 0.25];   % dark gray — all-in-view solution
col_tr  = [0.15 0.15 0.15];   % true position

msz_dot  = 10;    % scatter dot size
msz_mean = 8;     % mean marker size

fig = figure('Name','LOO: GNSS vs Cooperative', ...
             'Color','white','Units','inches','Position',[1 1 12 5.6]);



% ════════════════════════════════════════════════════════════════════
%  LEFT: GNSS SS-RAIM
% ════════════════════════════════════════════════════════════════════
ax1 = subplot(1,2,1); hold on; box on; grid on;
ax1.GridAlpha = 0.13; ax1.GridColor = [.55 .55 .55];
ax1.FontSize = 9;

% Scatter clouds — LOO subsets
for k = 1:N_peers
    pts = squeeze(sol_G(k,:,:));
    ok  = all(isfinite(pts),2) & all(abs(pts)<BOUND,2);
    pts = pts(ok,:);
    if isempty(pts), continue; end
    if is_healthy(k)
        scatter(pts(:,1),pts(:,2),msz_dot,col_h,'.','MarkerEdgeAlpha',0.12);
    else
        scatter(pts(:,1),pts(:,2),msz_dot,col_f,'.','MarkerEdgeAlpha',0.10);
    end
end

% All-in-view cloud
ok = all(isfinite(sol_G_full),2) & all(abs(sol_G_full)<BOUND,2);
scatter(sol_G_full(ok,1),sol_G_full(ok,2),msz_dot,col_aiv,'.','MarkerEdgeAlpha',0.10);

% Mean markers — LOO
for k = 1:N_peers
    if is_healthy(k)
        plot(mean_G(k,1),mean_G(k,2),'s','Color',col_h,...
             'MarkerFaceColor',col_h,'MarkerSize',msz_mean,'LineWidth',0.8);
        text(mean_G(k,1)+0.3,mean_G(k,2)+0.3,...
             sprintf('LOO$_{%d}$',k),'Interpreter','latex',...
             'FontSize',8,'Color',col_h);
    else
        plot(mean_G(k,1),mean_G(k,2),'o','Color',col_f,...
             'MarkerFaceColor',col_f,'MarkerSize',msz_mean,'LineWidth',0.8);
        text(mean_G(k,1)+0.3,mean_G(k,2)-0.8,...
             sprintf('LOO$_{%d}$',k),'Interpreter','latex',...
             'FontSize',8,'Color',col_f);
    end
end

% All-in-view mean
plot(mean_G_full(1),mean_G_full(2),'d','Color',col_aiv,...
     'MarkerFaceColor',col_aiv,'MarkerSize',msz_mean+1,'LineWidth',1);
text(mean_G_full(1)+0.3,mean_G_full(2)+0.5,'all-in-view',...
     'FontSize',8,'Color',col_aiv);

% True position
plot(p_ego(1),p_ego(2),'k+','MarkerSize',14,'LineWidth',2);
text(p_ego(1)+0.3,p_ego(2)-0.9,'true pos.','FontSize',8,'Color',col_tr);

% Separation arrow: all-in-view to healthy LOO cluster mean
h_center_G = mean(mean_G(is_healthy,:),1);
draw_sep_arrow(ax1, mean_G_full, h_center_G, col_aiv);

% Healthy cluster ellipse
draw_ellipse(mean_G(is_healthy,:), col_h, ax1);

% Axis limits
ap = [mean_G; mean_G_full; p_ego'];
ap = ap(all(isfinite(ap),2) & all(abs(ap)<1e4,2),:);
pad1 = max((max(ap)-min(ap))*0.35, 1.5);
xlim(ax1, [min(ap(:,1))-pad1(1), max(ap(:,1))+pad1(1)]);
ylim(ax1, [min(ap(:,2))-pad1(2), max(ap(:,2))+pad1(2)]);

title('GNSS SS-RAIM (LOO subsets)','FontSize',11,'FontWeight','bold');
subtitle('Large baselines — geometry stable across subsets','FontSize',8,...
         'Color',[0.4 0.4 0.4]);
xlabel('$x$ (m)','Interpreter','latex');
ylabel('$y$ (m)','Interpreter','latex');

% Annotation text
xl = xlim; yl = ylim;
text(xl(1)+0.03*diff(xl), yl(2)-0.04*diff(yl),...
     {'\color[rgb]{0.18,0.50,0.85}Healthy LOO solutions: tight cluster',...
      '\color[rgb]{0.82,0.22,0.18}Faulty LOO solutions: also near cluster',...
      '\color[rgb]{0.25,0.25,0.25}All-in-view: displaced outlier'},...
     'FontSize',8,'VerticalAlignment','top');

% ════════════════════════════════════════════════════════════════════
%  RIGHT: Cooperative Robot Localization
% ════════════════════════════════════════════════════════════════════
ax2 = subplot(1,2,2); hold on; box on; grid on;
ax2.GridAlpha = 0.13; ax2.GridColor = [.55 .55 .55];
ax2.FontSize = 9;

% Scatter clouds — LOO subsets
for k = 1:N_peers
    pts = squeeze(sol_C(k,:,:));
    ok  = all(isfinite(pts),2) & all(abs(pts)<BOUND,2);
    pts = pts(ok,:);
    if isempty(pts), continue; end
    if is_healthy(k)
        scatter(pts(:,1),pts(:,2),msz_dot,col_h,'.','MarkerEdgeAlpha',0.18);
    else
        scatter(pts(:,1),pts(:,2),msz_dot,col_f,'.','MarkerEdgeAlpha',0.12);
    end
end

% All-in-view cloud
ok = all(isfinite(sol_C_full),2) & all(abs(sol_C_full)<BOUND,2);
scatter(sol_C_full(ok,1),sol_C_full(ok,2),msz_dot,col_aiv,'.','MarkerEdgeAlpha',0.10);

% Peer positions
for i = 1:N_peers
    if i == fault_peer
        mk='v'; mfc=[1 0.84 0.84]; mec=col_f;
    else
        mk='^'; mfc=[0.84 0.92 1.0]; mec=[0.15 0.38 0.72];
    end
    plot(peers_C(1,i),peers_C(2,i),mk,'MarkerFaceColor',mfc,...
         'MarkerEdgeColor',mec,'MarkerSize',9,'LineWidth',1.0);
    if i == fault_peer
        lbl = sprintf('$R_%d^\\star$',i);
    else
        lbl = sprintf('$R_%d$',i);
    end
    text(peers_C(1,i)+0.5,peers_C(2,i)+0.6,lbl,...
         'Interpreter','latex','FontSize',8.5,'Color',mec);
end

% Mean markers — LOO
for k = 1:N_peers
    if is_healthy(k)
        plot(mean_C(k,1),mean_C(k,2),'s','Color',col_h,...
             'MarkerFaceColor',col_h,'MarkerSize',msz_mean,'LineWidth',0.8);
        text(mean_C(k,1)+0.4,mean_C(k,2)+0.5,...
             sprintf('LOO$_{%d}$',k),'Interpreter','latex',...
             'FontSize',8,'Color',col_h);
    else
        plot(mean_C(k,1),mean_C(k,2),'o','Color',col_f,...
             'MarkerFaceColor',col_f,'MarkerSize',msz_mean,'LineWidth',0.8);
        text(mean_C(k,1)+0.4,mean_C(k,2)-0.9,...
             sprintf('LOO$_{%d}$',k),'Interpreter','latex',...
             'FontSize',8,'Color',col_f);
    end
end

% All-in-view mean
plot(mean_C_full(1),mean_C_full(2),'d','Color',col_aiv,...
     'MarkerFaceColor',col_aiv,'MarkerSize',msz_mean+1,'LineWidth',1);
text(mean_C_full(1)+0.4,mean_C_full(2)+0.5,'all-in-view',...
     'FontSize',8,'Color',col_aiv);

% True position
plot(p_ego(1),p_ego(2),'k+','MarkerSize',14,'LineWidth',2);
text(p_ego(1)+0.3,p_ego(2)-0.9,'true pos.','FontSize',8,'Color',col_tr);

% Healthy cluster ellipse
draw_ellipse(mean_C(is_healthy,:), col_h, ax2);

% Axis limits
bp = [mean_C; mean_C_full; p_ego'; peers_C'];
bp = bp(all(isfinite(bp),2) & all(abs(bp)<1e4,2),:);
pad2 = max((max(bp)-min(bp))*0.25, 1.5);
xlim(ax2, [min(bp(:,1))-pad2(1), max(bp(:,1))+pad2(1)]);
ylim(ax2, [min(bp(:,2))-pad2(2), max(bp(:,2))+pad2(2)]);

title('Cooperative Localization (LOO subsets)','FontSize',11,'FontWeight','bold');
subtitle('Meter-scale separations — GDOP changes per subset','FontSize',8,...
         'Color',[0.4 0.4 0.4]);
xlabel('$x$ (m)','Interpreter','latex');
ylabel('$y$ (m)','Interpreter','latex');

xl = xlim; yl = ylim;
text(xl(1)+0.03*diff(xl), yl(2)-0.04*diff(yl),...
     {'\color[rgb]{0.18,0.50,0.85}LOO excluding faulty peer: near true pos.',...
      '\color[rgb]{0.82,0.22,0.18}LOO including faulty peer: scattered',...
      '\color[rgb]{0.25,0.25,0.25}All-in-view: biased by fault'},...
     'FontSize',8,'VerticalAlignment','top');

% ════════════════════════════════════════════════════════════════════
%  SHARED LEGEND  (on Panel B)
% ════════════════════════════════════════════════════════════════════
h1 = plot(nan,nan,'s','Color',col_h,'MarkerFaceColor',col_h,'MarkerSize',7);
h2 = plot(nan,nan,'o','Color',col_f,'MarkerFaceColor',col_f,'MarkerSize',7);
h3 = plot(nan,nan,'d','Color',col_aiv,'MarkerFaceColor',col_aiv,'MarkerSize',7);
h4 = plot(nan,nan,'k+','MarkerSize',9,'LineWidth',1.5);
legend([h1 h2 h3 h4],...
    {'Healthy LOO soln (mean)','Faulty LOO soln (mean)',...
     'All-in-view soln (mean)','True position'},...
    'Location','southeast','FontSize',8);

% ════════════════════════════════════════════════════════════════════
%  SUPERTITLE
% ════════════════════════════════════════════════════════════════════
sgtitle({sprintf('LOO subset solutions: ||f|| = %.0f m step fault on peer/sat 1,  N = %d', ...
                 fault_mag, N_peers), ...
         sprintf('sigma_UWB = %.2f m,  MC draws = %d per subset', sigma_UWB, N_MC)}, ...
        'FontSize',10);

% ════════════════════════════════════════════════════════════════════
%  CONSOLE SUMMARY
% ════════════════════════════════════════════════════════════════════
fprintf('\n── LOO subset mean solutions ────────────────────────────────\n');
fprintf('  %-6s  %-10s  %-12s  %-20s  %-20s\n',...
        'LOO_k','excludes','healthy?','GNSS mean (m)','Coop mean (m)');
fprintf('  %s\n',repmat('-',1,72));
for k = 1:N_peers
    hlth = 'healthy'; if ~is_healthy(k), hlth='FAULTY'; end
    fprintf('  LOO_%d   peer %d      %-12s  (%7.3f, %7.3f)     (%7.3f, %7.3f)\n',...
            k,k,hlth,...
            mean_G(k,1),mean_G(k,2),...
            mean_C(k,1),mean_C(k,2));
end
fprintf('\n  All-in-view GNSS:  (%7.3f, %7.3f)\n',mean_G_full(1),mean_G_full(2));
fprintf('  All-in-view coop:  (%7.3f, %7.3f)\n\n',mean_C_full(1),mean_C_full(2));

% ════════════════════════════════════════════════════════════════════
%  SAVE
% ════════════════════════════════════════════════════════════════════
exportgraphics(fig,'loo_comparison.pdf','ContentType','vector');
exportgraphics(fig,'loo_comparison.png','Resolution',300);
fprintf('Saved: loo_comparison.pdf / .png\n');


% =========================================================================
%  LOCAL FUNCTIONS
% =========================================================================

function r_obs = make_ranges(p_ego, p_true_anchors, sigma_UWB)
% Noisy true ranges from ego to anchor true positions.
% Fault is baked into the broadcast positions upstream.
    dist  = vecnorm(p_true_anchors - p_ego, 2, 1)';
    r_obs = dist + sigma_UWB * randn(numel(dist), 1);
end

% -------------------------------------------------------------------------

function p_est = wls_solve(r_obs, p_anchors, sigma_UWB, sigma_pos)
% Robust damped Gauss-Newton WLS trilateration (multi-start).
    n          = numel(r_obs);
    sigma2_eff = sigma_UWB^2 + sigma_pos^2 + 1e-9;
    w          = ones(n,1) / sigma2_eff;
    W          = diag(w);
    max_iter   = 60;
    tol        = 1e-9;
    step_cap   = 60;

    function c = cost(p)
        d = max(vecnorm(p - p_anchors,2,1)',1e-6);
        r = r_obs - d;
        c = r' * (w.*r);
    end

    function [p,c] = gn(p0)
        p = p0(:);
        for it = 1:max_iter
            dv   = p - p_anchors;
            dist = max(vecnorm(dv,2,1),1e-6);
            H    = -(dv./dist)';
            res  = r_obs - dist';
            HtWH = H'*W*H;
            rhs  = H'*(w.*res);
            if rcond(HtWH) > 1e-13
                step = HtWH \ rhs;
            else
                step = pinv(HtWH)*rhs;
            end
            sn = norm(step);
            if sn > step_cap, step = step*(step_cap/sn); end
            c0 = cost(p); alpha = 1;
            for ls = 1:8
                if cost(p + alpha*step) < c0, break; end
                alpha = alpha*0.5;
            end
            p = p + alpha*step;
            if norm(alpha*step) < tol, break; end
        end
        c = cost(p);
    end

    seeds = [mean(p_anchors,2), p_anchors];
    best_p = seeds(:,1); best_c = inf;
    for s = 1:size(seeds,2)
        [pc,cc] = gn(seeds(:,s));
        if isfinite(cc) && cc < best_c
            best_c = cc; best_p = pc;
        end
    end
    p_est = best_p;
end

% -------------------------------------------------------------------------

function draw_ellipse(pts, color, ax)
% 2-sigma covariance ellipse around a set of 2D mean-solution points.
    if size(pts,1) < 2, return; end
    mu = mean(pts,1);
    C  = cov(pts);
    if ~all(isfinite(C(:))), return; end
    [V,D] = eig(C);
    t  = linspace(0,2*pi,120);
    xy = 2*V*sqrt(max(D,0))*[cos(t);sin(t)];
    plot(ax, mu(1)+xy(1,:), mu(2)+xy(2,:),'--',...
         'Color',[color,0.55],'LineWidth',1.2);
end
% -------------------------------------------------------------------------

function draw_sep_arrow(ax, from_pt, to_pt, color)
% Dashed arrow with delta_i label indicating solution separation.
    axes(ax);
    d  = to_pt - from_pt;
    dn = norm(d);
    if dn < 1e-3, return; end
    quiver(from_pt(1),from_pt(2),d(1)*0.82,d(2)*0.82,0,...
           'Color',color,'LineWidth',0.9,'LineStyle','--',...
           'MaxHeadSize',0.4,'AutoScale','off');
    mid = from_pt + 0.45*d;
    ang = atan2d(d(2),d(1));
    text(mid(1),mid(2),'$\delta_i$','Interpreter','latex',...
         'FontSize',9,'Color',color,'Rotation',ang,...
         'HorizontalAlignment','center','VerticalAlignment','bottom');
end