Initial ARSS. Need to improve and add functions.

2026-03-02 14:48:21 +09:00
commit 775668afdb
20 changed files with 1610 additions and 0 deletions
@@ -0,0 +1,140 @@
+function [det_mask, threshold_map, detections] = detect_targets_cfar(rd_map, RadarParams)
+% CFAR target detection for range-doppler map
+% - method: 'CA' or 'OS'
+% - dimension: '1D' or '2D'
+% - axis (for 1D): 'range' or 'doppler'
+%
+% Input:
+%   rd_map      : [Ndoppler x Nrange] complex or real map
+%   RadarParams : struct containing SP.CFAR options
+%
+% Output:
+%   det_mask      : logical detection mask [Ndoppler x Nrange]
+%   threshold_map : threshold map [Ndoppler x Nrange]
+%   detections    : [Ndet x 2] = [doppler_bin, range_bin]
+
+    cfg = RadarParams.SP.CFAR;
+
+    method = upper(string(cfg.method));
+    dim_mode = upper(string(cfg.dimension));
+    axis_mode = lower(string(cfg.axis));
+
+    pfa = cfg.pfa;
+    train = cfg.train;
+    guard = cfg.guard;
+
+    if numel(train) == 1
+        train = [train, train];
+    end
+    if numel(guard) == 1
+        guard = [guard, guard];
+    end
+
+    os_rank_ratio = cfg.rank;
+    os_scale = cfg.os_scale;
+
+    rd_power = abs(rd_map).^2;
+    [n_dop, n_rng] = size(rd_power);
+
+    det_mask = false(n_dop, n_rng);
+    threshold_map = nan(n_dop, n_rng);
+
+    switch dim_mode
+        case "2D"
+            td = train(1); tr = train(2);
+            gd = guard(1); gr = guard(2);
+
+            for d = (td+gd+1):(n_dop-(td+gd))
+                for r = (tr+gr+1):(n_rng-(tr+gr))
+                    d_idx = (d-(td+gd)):(d+(td+gd));
+                    r_idx = (r-(tr+gr)):(r+(tr+gr));
+
+                    win = rd_power(d_idx, r_idx);
+                    cut_d = td+gd+1;
+                    cut_r = tr+gr+1;
+
+                    guard_mask = false(size(win));
+                    guard_mask((cut_d-gd):(cut_d+gd), (cut_r-gr):(cut_r+gr)) = true;
+
+                    train_cells = win(~guard_mask);
+                    th = local_cfar_threshold(train_cells, method, pfa, os_rank_ratio, os_scale);
+
+                    threshold_map(d, r) = th;
+                    det_mask(d, r) = rd_power(d, r) > th;
+                end
+            end
+
+        case "1D"
+            switch axis_mode
+                case "range"
+                    tr = train(2); gr = guard(2);
+                    for d = 1:n_dop
+                        [det_row, th_row] = cfar_1d_line(rd_power(d, :), tr, gr, method, pfa, os_rank_ratio, os_scale);
+                        det_mask(d, :) = det_row;
+                        threshold_map(d, :) = th_row;
+                    end
+
+                case "doppler"
+                    td = train(1); gd = guard(1);
+                    for r = 1:n_rng
+                        [det_col, th_col] = cfar_1d_line(rd_power(:, r).', td, gd, method, pfa, os_rank_ratio, os_scale);
+                        det_mask(:, r) = det_col.';
+                        threshold_map(:, r) = th_col.';
+                    end
+
+                otherwise
+                    error('CFAR axis must be ''range'' or ''doppler'' when dimension is 1D.');
+            end
+
+        otherwise
+            error('CFAR dimension must be ''1D'' or ''2D''.');
+    end
+
+    [d_idx, r_idx] = find(det_mask);
+    detections = [d_idx, r_idx];
+end
+
+function [det_line, th_line] = cfar_1d_line(x, t, g, method, pfa, rank_ratio, os_scale)
+    n = numel(x);
+    det_line = false(1, n);
+    th_line = nan(1, n);
+
+    left = t + g;
+    right = t + g;
+
+    for i = (left+1):(n-right)
+        l_train = x((i-g-t):(i-g-1));
+        r_train = x((i+g+1):(i+g+t));
+        train_cells = [l_train, r_train];
+
+        th = local_cfar_threshold(train_cells, method, pfa, rank_ratio, os_scale);
+        th_line(i) = th;
+        det_line(i) = x(i) > th;
+    end
+end
+
+function th = local_cfar_threshold(train_cells, method, pfa, rank_ratio, os_scale)
+    train_cells = train_cells(:);
+    n_train = numel(train_cells);
+
+    if n_train == 0
+        th = inf;
+        return;
+    end
+
+    switch method
+        case "CA"
+            noise_hat = mean(train_cells);
+            alpha = n_train * (pfa^(-1/n_train) - 1);
+            th = alpha * noise_hat;
+
+        case "OS"
+            sorted_cells = sort(train_cells, 'ascend');
+            k = max(1, min(n_train, round(rank_ratio * n_train)));
+            noise_hat = sorted_cells(k);
+            th = os_scale * noise_hat;
+
+        otherwise
+            error('CFAR method must be ''CA'' or ''OS''.');
+    end
+end