Merge branch 'main' into AERPAW-experiments

.gitignore

@@ -4,6 +4,7 @@
 *.autosave
 *.slx.r*
 *.mdl.r*
+*.bak
 
 # Derived content-obscured files
 *.p

@miSim/initialize.m

@@ -72,16 +72,18 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
     obj.constraintAdjacencyMatrix = logical(eye(size(agents, 1)));
 
     % Set labels for agents and collision geometries in cases where they
-    % were not provieded at the time of their initialization
+    % were not provieded at the time of their initialization.
-    for ii = 1:size(obj.agents, 1)
+    % Guarded by coder.target: label is a fixed-size "" in codegen, and
-        % Agent
+    % sprintf returns variable-length — incompatible even in a dead branch.
-        if isempty(char(obj.agents{ii}.label))
+    % On the compiled path agents always receive explicit labels from the caller.
-            obj.agents{ii}.label = sprintf("Agent %d", int8(ii));
+    if coder.target('MATLAB')
-        end
+        for ii = 1:size(obj.agents, 1)
-
+            if isempty(char(obj.agents{ii}.label))
-        % Collision geometry
+                obj.agents{ii}.label = sprintf("Agent %d", int8(ii));
-        if isempty(char(obj.agents{ii}.collisionGeometry.label))
+            end
-            obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", int8(ii));
+            if isempty(char(obj.agents{ii}.collisionGeometry.label))
+                obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", int8(ii));
+            end
         end
     end
 

@miSim/initializeFromCsv.m

@@ -52,8 +52,19 @@ BETA_TILT_VEC        = scenario.betaTilt;           % 1×N
 
 DOMAIN_MIN                 = scenario.domainMin;                % 1×3
 DOMAIN_MAX                 = scenario.domainMax;                % 1×3
-OBJECTIVE_GROUND_POS       = scenario.objectivePos;             % 1×2
+
-OBJECTIVE_VAR              = reshape(scenario.objectiveVar, 2, 2); % 2×2 covariance matrix
+% objectivePos: 2 values per Gaussian component (1 or 2 components supported)
+nObjComponents = numel(scenario.objectivePos) / 2;
+assert(mod(numel(scenario.objectivePos), 2) == 0, ...
+    'objectivePos must have an even number of values (2 per Gaussian component)');
+assert(nObjComponents >= 1 && nObjComponents <= 2, ...
+    'At most 2 objective Gaussian components supported; got %d', nObjComponents);
+assert(numel(scenario.objectiveVar) == nObjComponents * 4, ...
+    'objectiveVar must have %d values for %d component(s); got %d', ...
+    nObjComponents * 4, nObjComponents, numel(scenario.objectiveVar));
+OBJECTIVE_GROUND_POS = reshape(scenario.objectivePos, 2, nObjComponents)';           % nObj×2
+OBJECTIVE_VAR        = permute(reshape(scenario.objectiveVar, 2, 2, nObjComponents), [3, 1, 2]); % nObj×2×2
+
 SENSOR_PERFORMANCE_MINIMUM = scenario.sensorPerformanceMinimum; % scalar
 
 % Initial UAV positions: flat vector reshaped to N×3

aerpaw/config/scenario.csv

@@ -1,2 +1,2 @@
 timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
-1, 100, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 80.0", "0.25, 0.25", "8.0, 8.0", "0.1, 0.1", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "66.6, 66.6", "55, 35, 35, 55", 0.15, "15.0, 15.0, 50.0, 40.0, 15.0, 50.0", 1, "0.0, 35.0, 0.0", "50, 40.0, 60", 1, 2.0, 1
+1, 80, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "60.0, 80.0, 45.0, 70.0", "70, 15, 15, 20, 20, 15, 15, 70", 0.15, "15.0, 15.0, 50.0, 40.0, 10.0, 45.0", 8, "0.0, 30.0, 0.0, 42.0, 30.0, 0.0, 84.0, 30.0, 0.0, 13.0, 60.0, 0.0, 55.0, 60.0, 0.0, 0.0, 90, 0.0, 42.0, 90.0, 0.0, 84.0, 90.0, 0.0", "16.0, 40.0, 100.0, 58.0, 40.0, 100.0, 100.0, 40.0, 100.0, 29.0, 70.0, 100.0, 71.0, 70.0, 100.0, 16.0, 100.0, 100.0, 58.0, 100.0, 100.0, 100.0, 100.0, 100.0", 0, 2.0, 1

aerpaw/config/scenario_columns.csv

@@ -1,2 +1,2 @@
 timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
-1, 50, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "60.0, 80.0, 45.0, 70.0", "70, 15, 15, 20, 20, 15, 15, 70", 0.15, "10.0, 10.0, 50.0, 40.0, 15.0, 45.0", 8, "0.0, 30.0, 0.0, 42.0, 30.0, 0.0, 84.0, 30.0, 0.0, 13.0, 60.0, 0.0, 55.0, 60.0, 0.0, 0.0, 90, 0.0, 42.0, 90.0, 0.0, 84.0, 90.0, 0.0", "16.0, 40.0, 100.0, 58.0, 40.0, 100.0, 100.0, 40.0, 100.0, 29.0, 70.0, 100.0, 71.0, 70.0, 100.0, 16.0, 100.0, 100.0, 58.0, 100.0, 100.0, 100.0, 100.0, 100.0", 0, 2.0, 1
+1, 80, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "60.0, 80.0, 45.0, 70.0", "70, 15, 15, 20, 20, 15, 15, 70", 0.15, "15.0, 15.0, 50.0, 40.0, 10.0, 45.0", 8, "0.0, 30.0, 0.0, 42.0, 30.0, 0.0, 84.0, 30.0, 0.0, 13.0, 60.0, 0.0, 55.0, 60.0, 0.0, 0.0, 90, 0.0, 42.0, 90.0, 0.0, 84.0, 90.0, 0.0", "16.0, 40.0, 100.0, 58.0, 40.0, 100.0, 100.0, 40.0, 100.0, 29.0, 70.0, 100.0, 71.0, 70.0, 100.0, 16.0, 100.0, 100.0, 58.0, 100.0, 100.0, 100.0, 100.0, 100.0", 0, 2.0, 1

aerpaw/config/scenario_lock.csv

@@ -1,2 +1,2 @@
 timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
-1, 65, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "30.0, 80.0", "60, 20, 20, 30", 0.15, "65.0, 15.0, 65.0, 65.0, 15.0, 45.0", 3, "0.0, 25.0, 55.0, 40.0, 10.0, 0.0, 40.0, 45.0, 60.0", "100.0, 70.0, 60.0, 45.0, 80.0, 55.0, 100.0, 50.0, 100.0", 0, 2.0, 1
+1, 125, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "30.0, 80.0", "60, 20, 20, 30", 0.15, "65.0, 15.0, 65.0, 65.0, 15.0, 45.0", 3, "0.0, 25.0, 55.0, 40.0, 10.0, 0.0, 40.0, 45.0, 60.0", "100.0, 70.0, 60.0, 45.0, 80.0, 55.0, 100.0, 50.0, 100.0", 0, 2.0, 1

aerpaw/config/scenario_walls.csv

@@ -1,2 +1,2 @@
 timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
-1, 65, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "30.0, 80.0", "60, 20, 20, 30", 0.15, "90.0, 10.0, 50.0, 65.0, 15.0, 45.0", 4, "0.0, 30.0, 0.0, 70.0, 30.0, 0.0, 0.0, 60.0, 0.0, 55.0, 60.0, 0.0", "50.0, 40.0, 100.0, 100.0, 40.0, 100.0, 35.0, 70.0, 100.0, 100.0, 70.0, 100.0", 0, 2.0, 1
+1, 125, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "30.0, 80.0", "60, 20, 20, 30", 0.15, "90.0, 10.0, 50.0, 65.0, 15.0, 45.0", 4, "0.0, 30.0, 0.0, 70.0, 30.0, 0.0, 0.0, 60.0, 0.0, 55.0, 60.0, 0.0", "50.0, 40.0, 100.0, 100.0, 40.0, 100.0, 35.0, 70.0, 100.0, 100.0, 70.0, 100.0", 0, 2.0, 1

aerpaw/controller.m

@@ -7,58 +7,47 @@ coder.extrinsic('disp', 'readScenarioCsv');
 
 % Maximum clients supported (one initial position per UAV)
 MAX_CLIENTS = 4;
-% Two waypoints per UAV: altitude-staggered transit + final position
+% Three waypoints per UAV: one axis-aligned move per dimension (taxicab flyout/flyback)
-MAX_TARGETS = MAX_CLIENTS * 2;
+MAX_TARGETS = MAX_CLIENTS * 3;
+
+% Taxicab flyout/flyback only supports exactly 2 UAVs
+if numClients ~= int32(2)
+    error('Taxicab flyout/flyback requires exactly 2 UAVs');
+end
 
 % Allocate targets array (MAX_TARGETS x 3)
 targets = zeros(MAX_TARGETS, 3);
 numWaypoints = int32(0);
 totalLoaded  = int32(0);  % pre-declare type for coder.ceval %#ok<NASGU>
 
-% Load initial UAV positions from scenario CSV
+% Experiment start positions from scenario CSV (N x 3)
+scenarioPositions = zeros(MAX_CLIENTS, 3);
+
+% Load experiment start positions from scenario CSV
 if coder.target('MATLAB')
     disp('Loading initial positions from scenario.csv (simulation)...');
     tmpSim = miSim;
     sc = tmpSim.readScenarioCsv('aerpaw/config/scenario.csv');
     flatPos = double(sc.initialPositions);  % 1×(3*N) flat vector
-    posMatrix = reshape(flatPos, 3, [])';   % N×3, same layout as initializeFromCsv
+    posMatrix = reshape(flatPos, 3, [])';   % N×3
     totalLoaded = int32(size(posMatrix, 1));
+    scenarioPositions(1:totalLoaded, :) = posMatrix;
+    % MATLAB path: send directly to scenario positions in one waypoint
     targets(1:totalLoaded, :) = posMatrix;
     numWaypoints = int32(1);
     disp(['Loaded ', num2str(double(totalLoaded)), ' initial positions']);
 else
     coder.cinclude('controller_impl.h');
     filename = ['config/scenario.csv', char(0)];
+    % Load into targets temporarily; copy to scenarioPositions below
     totalLoaded = coder.ceval('loadInitialPositions', coder.ref(filename), ...
                 coder.ref(targets), int32(MAX_TARGETS));
-    numWaypoints = totalLoaded / int32(numClients);
+    scenarioPositions(1:totalLoaded, :) = targets(1:totalLoaded, :);
-end
+    numWaypoints = int32(3);
-
-% In the compiled path, inject altitude-staggered transit waypoints so UAVs
-% are vertically separated while flying horizontally to their start positions.
-% ArduPilot reaches target altitude before horizontal movement, so UAV i is at
-% altitude (TRANSIT_ALT_BASE + (i-1)*TRANSIT_ALT_STEP) throughout its transit,
-% preventing collisions regardless of horizontal path geometry.
-% TRANSIT_ALT_STEP must exceed 2 * max(collisionRadius).
-% Waypoint 1: each UAV flies to (finalX, finalY) at its unique transit altitude.
-% Waypoint 2: each UAV adjusts to its actual target altitude.
-% These constants are also used for the altitude-staggered return before RTL.
-TRANSIT_ALT_BASE = 25.0;  % must match drone.takeoff() altitude in uav_runner.py
-TRANSIT_ALT_STEP = 25;    % vertical separation per UAV (m); must exceed 2*collisionRadius
-if ~coder.target('MATLAB')
-    for ii = double(totalLoaded):-1:1
-        transitRow = (ii - 1) * 2 + 1;
-        finalRow   = (ii - 1) * 2 + 2;
-        finalPos   = targets(ii, :);
-        transitAlt = TRANSIT_ALT_BASE + (ii - 1) * TRANSIT_ALT_STEP;
-        targets(finalRow, :)   = finalPos;
-        targets(transitRow, :) = [finalPos(1), finalPos(2), transitAlt];
-    end
-    numWaypoints = int32(2);
 end
 
 % Load guidance scenario from CSV (parameters for guidance_step)
-NUM_SCENARIO_PARAMS = 48;
+NUM_SCENARIO_PARAMS = 55;
 MAX_OBSTACLES_CTRL  = int32(8);
 scenarioParams = zeros(1, NUM_SCENARIO_PARAMS);
 obstacleMin    = zeros(MAX_OBSTACLES_CTRL, 3);
@@ -92,6 +81,46 @@ for i = 1:numClients
     end
 end
 
+% Query takeoff-pad GPS positions before sending any waypoints.
+% These are also the flyback targets so each UAV lands where it took off.
+initialPositions = zeros(MAX_CLIENTS, 3);
+if ~coder.target('MATLAB')
+    coder.ceval('sendRequestPositions', int32(numClients));
+    coder.ceval('recvPositions', int32(numClients), coder.ref(initialPositions), int32(MAX_CLIENTS));
+else
+    % Simulation: treat scenario positions as the takeoff locations
+    initialPositions(1:totalLoaded, :) = scenarioPositions(1:totalLoaded, :);
+end
+
+% ---- Build taxicab flyout waypoints ------------------------------------------
+% Determine which UAV has the higher final altitude; it moves Z first so it
+% clears vertical separation before the lower UAV converges on the same X/Y.
+%   Higher UAV order: Z → Y → X
+%   Lower  UAV order: X → Y → Z
+if ~coder.target('MATLAB')
+    if scenarioPositions(1, 3) >= scenarioPositions(2, 3)
+        higherIdx = int32(1);
+        lowerIdx  = int32(2);
+    else
+        higherIdx = int32(2);
+        lowerIdx  = int32(1);
+    end
+
+    hBase = double(higherIdx - 1) * double(numWaypoints);
+    lBase = double(lowerIdx  - 1) * double(numWaypoints);
+
+    % Higher UAV: Z first
+    targets(hBase + 1, :) = [initialPositions(higherIdx,1), initialPositions(higherIdx,2), scenarioPositions(higherIdx,3)];
+    targets(hBase + 2, :) = [initialPositions(higherIdx,1), scenarioPositions(higherIdx,2), scenarioPositions(higherIdx,3)];
+    targets(hBase + 3, :) =  scenarioPositions(higherIdx, :);
+
+    % Lower UAV: X first
+    targets(lBase + 1, :) = [scenarioPositions(lowerIdx,1), initialPositions(lowerIdx,2), initialPositions(lowerIdx,3)];
+    targets(lBase + 2, :) = [scenarioPositions(lowerIdx,1), scenarioPositions(lowerIdx,2), initialPositions(lowerIdx,3)];
+    targets(lBase + 3, :) =  scenarioPositions(lowerIdx, :);
+end
+% ------------------------------------------------------------------------------
+
 % Waypoint loop: send each waypoint to all clients, wait for all to arrive
 for w = 1:numWaypoints
     % Send TARGET for waypoint w to each client
@@ -127,8 +156,13 @@ for w = 1:numWaypoints
 end
 
 % ---- Phase 2: miSim guidance loop ----------------------------------------
-% Guidance parameters (adjust here and recompile as needed)
+% Number of guidance steps comes from maxIter in scenario.csv (scenarioParams(2))
-MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations
+% so the controller and the agent step-decay schedule stay in sync.
+if coder.target('MATLAB')
+    MAX_GUIDANCE_STEPS = int32(sc.maxIter);
+else
+    MAX_GUIDANCE_STEPS = int32(scenarioParams(2));
+end
 
 % Enter guidance mode on all clients
 if ~coder.target('MATLAB')
@@ -141,8 +175,8 @@ if ~coder.target('MATLAB')
     coder.ceval('sendRequestPositions', int32(numClients));
     coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS));
 else
-    % Simulation: seed positions from CSV waypoints so agents don't start at origin
+    % Simulation: seed positions from scenario positions so agents don't start at origin
-    positions(1:totalLoaded, :) = targets(1:totalLoaded, :);
+    positions(1:totalLoaded, :) = scenarioPositions(1:totalLoaded, :);
 end
 guidance_step(positions(1:numClients, :), true, ...
               scenarioParams, obstacleMin, obstacleMax, numObstacles);
@@ -197,20 +231,48 @@ if ~coder.target('MATLAB')
 end
 % --------------------------------------------------------------------------
 
-% Altitude-staggered return: separate UAVs vertically before issuing RTL,
+% ---- Taxicab flyback: return each UAV to its takeoff-pad position ---------
-% mirroring the initial positioning stagger so UAVs transit laterally at
+% The UAV that ended guidance at the higher altitude moves Z last (X → Y → Z)
-% unique altitudes and cannot collide during the return flight.
+% so it stays high while the lower UAV descends first, maintaining separation
+% as both converge back on their respective home X/Y positions.
+NUM_RETURN_WP = int32(3);
+returnTargets = zeros(MAX_TARGETS, 3);
+
 if ~coder.target('MATLAB')
-    for i = 1:numClients
+    if positions(1, 3) >= positions(2, 3)
-        transitAlt = TRANSIT_ALT_BASE + (double(i) - 1) * TRANSIT_ALT_STEP;
+        higherRetIdx = int32(1);
-        target = [positions(i, 1), positions(i, 2), transitAlt];
+        lowerRetIdx  = int32(2);
-        coder.ceval('sendTarget', int32(i), coder.ref(target));
+    else
+        higherRetIdx = int32(2);
+        lowerRetIdx  = int32(1);
+    end
+
+    hRetBase = double(higherRetIdx - 1) * double(NUM_RETURN_WP);
+    lRetBase = double(lowerRetIdx  - 1) * double(NUM_RETURN_WP);
+
+    % Higher post-guidance UAV: X → Y → Z (descend last)
+    returnTargets(hRetBase + 1, :) = [initialPositions(higherRetIdx,1), positions(higherRetIdx,2),        positions(higherRetIdx,3)];
+    returnTargets(hRetBase + 2, :) = [initialPositions(higherRetIdx,1), initialPositions(higherRetIdx,2), positions(higherRetIdx,3)];
+    returnTargets(hRetBase + 3, :) =  initialPositions(higherRetIdx, :);
+
+    % Lower post-guidance UAV: Z → Y → X (descend first)
+    returnTargets(lRetBase + 1, :) = [positions(lowerRetIdx,1),        positions(lowerRetIdx,2),        initialPositions(lowerRetIdx,3)];
+    returnTargets(lRetBase + 2, :) = [positions(lowerRetIdx,1),        initialPositions(lowerRetIdx,2), initialPositions(lowerRetIdx,3)];
+    returnTargets(lRetBase + 3, :) =  initialPositions(lowerRetIdx, :);
+
+    for w = 1:NUM_RETURN_WP
+        for i = 1:numClients
+            retIdx = double(i - 1) * double(NUM_RETURN_WP) + w;
+            retTarget = returnTargets(retIdx, :);
+            coder.ceval('sendTarget', int32(i), coder.ref(retTarget));
+        end
+        coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.ACK));
+        coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.READY));
     end
-    coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.ACK));
-    coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.READY));
 else
-    disp('Altitude-staggered return (simulation): UAVs commanded to transit altitudes.');
+    disp('Taxicab return (simulation): UAVs commanded back to takeoff positions.');
 end
+% --------------------------------------------------------------------------
 
 % Send RTL command to all clients
 for i = 1:numClients

aerpaw/guidance_step.m

@@ -94,29 +94,34 @@ if isInit
         BETA_TILT_VEC        = scenarioParams(29:32);
         DOMAIN_MIN                  = scenarioParams(33:35);
         DOMAIN_MAX                  = scenarioParams(36:38);
-        OBJECTIVE_GROUND_POS        = scenarioParams(39:40);
+        NUM_OBJ_COMPONENTS          = int32(scenarioParams(39));
-        OBJECTIVE_VAR               = reshape(scenarioParams(41:44), 2, 2);
+        OBJECTIVE_POS_FLAT          = scenarioParams(40:43); % [x1,y1,x2,y2]; zero-padded if N=1
-        SENSOR_PERFORMANCE_MINIMUM  = scenarioParams(45);
+        OBJECTIVE_VAR_FLAT          = scenarioParams(44:51); % [v11,v12,v21,v22 per component]
-        USE_DOUBLE_INTEGRATOR       = logical(scenarioParams(46));
+        SENSOR_PERFORMANCE_MINIMUM  = scenarioParams(52);
-        DAMPING_COEFF               = scenarioParams(47);
+        USE_DOUBLE_INTEGRATOR       = logical(scenarioParams(53));
-        USE_FIXED_TOPOLOGY          = logical(scenarioParams(48));
+        DAMPING_COEFF               = scenarioParams(54);
+        USE_FIXED_TOPOLOGY          = logical(scenarioParams(55));
 
         % --- Build domain geometry ---
         dom = rectangularPrism;
         dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
 
-        % --- Build sensing objective (inline Gaussian; codegen-compatible) ---
+        % --- Build sensing objective: sum of N bivariate Gaussians (codegen-compatible) ---
         dom.objective = sensingObjective;
         xGrid = unique([DOMAIN_MIN(1):DISCRETIZATION_STEP:DOMAIN_MAX(1), DOMAIN_MAX(1)]);
         yGrid = unique([DOMAIN_MIN(2):DISCRETIZATION_STEP:DOMAIN_MAX(2), DOMAIN_MAX(2)]);
         [gridX, gridY] = meshgrid(xGrid, yGrid);
-        dx = gridX - OBJECTIVE_GROUND_POS(1);
+        objValues = zeros(size(gridX));
-        dy = gridY - OBJECTIVE_GROUND_POS(2);
+        for kk = 1:NUM_OBJ_COMPONENTS
-        % Bivariate Gaussian using objectiveVar covariance matrix (avoids inv())
+            pos_k = OBJECTIVE_POS_FLAT((kk-1)*2+1 : (kk-1)*2+2);
-        ov_a = OBJECTIVE_VAR(1,1); ov_b = OBJECTIVE_VAR(1,2);
+            var_k = reshape(OBJECTIVE_VAR_FLAT((kk-1)*4+1 : (kk-1)*4+4), 2, 2);
-        ov_c = OBJECTIVE_VAR(2,1); ov_d = OBJECTIVE_VAR(2,2);
+            dx = gridX - pos_k(1);
-        ov_det = ov_a * ov_d - ov_b * ov_c;
+            dy = gridY - pos_k(2);
-        objValues = exp((-0.5 / ov_det) .* (ov_d .* dx.*dx - (ov_b + ov_c) .* dx.*dy + ov_a .* dy.*dy));
+            ov_a = var_k(1,1); ov_b = var_k(1,2);
+            ov_c = var_k(2,1); ov_d = var_k(2,2);
+            ov_det = ov_a * ov_d - ov_b * ov_c;
+            objValues = objValues + exp((-0.5 / ov_det) .* (ov_d .* dx.*dx - (ov_b + ov_c) .* dx.*dy + ov_a .* dy.*dy));
+        end
         dom.objective = dom.objective.initializeWithValues(objValues, dom, ...
             DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM);
 

aerpaw/impl/controller_impl.cpp

@@ -222,12 +222,13 @@ static int readScenarioDataRow(const char* filename, char* line, int lineSize) {
 //   28-31: betaTilt[1:4]
 //   32-34: domainMin  (east, north, up)
 //   35-37: domainMax  (east, north, up)
-//   38-39: objectivePos (east, north)
+//   38   : numObjectiveComponents (1 or 2; inferred from objectivePos field length)
-//   40-43: objectiveVar (2x2 col-major: v11, v12, v21, v22)
+//   39-42: objectivePos flat [x1,y1,x2,y2] (4 slots; zero-padded if N=1)
-//   44   : sensorPerformanceMinimum  (CSV column 18)
+//   43-50: objectiveVar flat [v11,v12,v21,v22 per component] (8 slots; zero-padded if N=1)
-//   45   : useDoubleIntegrator       (CSV column 23; 0=single-integrator, 1=double-integrator)
+//   51   : sensorPerformanceMinimum  (CSV column 18)
-//   46   : dampingCoeff              (CSV column 24)
+//   52   : useDoubleIntegrator       (CSV column 23)
-//   47   : useFixedTopology          (CSV column 25; 0=dynamic lesser-neighbor, 1=fixed)
+//   53   : dampingCoeff              (CSV column 24)
+//   54   : useFixedTopology          (CSV column 25)
 // Returns 1 on success, 0 on failure.
 int loadScenario(const char* filename, double* params) {
     char line[4096];
@@ -305,52 +306,78 @@ int loadScenario(const char* filename, double* params) {
         }
     }
 
-    // objectivePos: column 16
+    // objectivePos: column 16 — 2 values per component (up to 2 components).
+    // Infer numObjectiveComponents from the number of values parsed.
     {
         char tmp[256]; strncpy(tmp, fields[16], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
         char* t = trimField(tmp);
-        if (sscanf(t, "%lf , %lf", &params[38], &params[39]) != 2) {
+        double posVals[4] = {0, 0, 0, 0};
-            fprintf(stderr, "loadScenario: failed to parse objectivePos: %s\n", t);
+        int posCount = 0;
+        char* tok = strtok(t, ",");
+        while (tok && posCount < 4) {
+            posVals[posCount++] = atof(tok);
+            tok = strtok(nullptr, ",");
+        }
+        // Check for a 5th token — would mean > 2 components
+        if (tok) {
+            fprintf(stderr, "loadScenario: at most 2 objective Gaussian components supported (objectivePos has >4 values)\n");
             return 0;
         }
+        if (posCount == 0 || posCount % 2 != 0) {
+            fprintf(stderr, "loadScenario: objectivePos must have 2 or 4 values, got %d\n", posCount);
+            return 0;
+        }
+        int nObj = posCount / 2;
+        params[38] = (double)nObj;
+        for (int k = 0; k < 4; k++) params[39 + k] = posVals[k];  // zero-padded for nObj=1
     }
 
-    // objectiveVar: column 17, format "v11, v12, v21, v22"
+    // objectiveVar: column 17 — 4 values per component (v11,v12,v21,v22).
     {
-        char tmp[256]; strncpy(tmp, fields[17], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
+        char tmp[512]; strncpy(tmp, fields[17], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
         char* t = trimField(tmp);
-        if (sscanf(t, "%lf , %lf , %lf , %lf", &params[40], &params[41], &params[42], &params[43]) != 4) {
+        int nObj = (int)params[38];
-            fprintf(stderr, "loadScenario: failed to parse objectiveVar: %s\n", t);
+        double varVals[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+        int varCount = 0;
+        char* tok = strtok(t, ",");
+        while (tok && varCount < 8) {
+            varVals[varCount++] = atof(tok);
+            tok = strtok(nullptr, ",");
+        }
+        if (varCount != nObj * 4) {
+            fprintf(stderr, "loadScenario: objectiveVar has %d values but expected %d (4 per component)\n",
+                    varCount, nObj * 4);
             return 0;
         }
+        for (int k = 0; k < 8; k++) params[43 + k] = varVals[k];  // zero-padded for nObj=1
     }
 
     // sensorPerformanceMinimum: column 18
     {
         char tmp[64]; strncpy(tmp, fields[18], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[44] = atof(trimField(tmp));
+        params[51] = atof(trimField(tmp));
     }
 
     // useDoubleIntegrator: column 23
     {
         char tmp[64]; strncpy(tmp, fields[23], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[45] = atof(trimField(tmp));
+        params[52] = atof(trimField(tmp));
     }
 
     // dampingCoeff: column 24
     {
         char tmp[64]; strncpy(tmp, fields[24], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[46] = atof(trimField(tmp));
+        params[53] = atof(trimField(tmp));
     }
 
     // useFixedTopology: column 25
     {
         char tmp[64]; strncpy(tmp, fields[25], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
-        params[47] = atof(trimField(tmp));
+        params[54] = atof(trimField(tmp));
     }
 
-    printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g]\n",
+    printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g], %d objective component(s)\n",
-           params[32], params[33], params[34], params[35], params[36], params[37]);
+           params[32], params[33], params[34], params[35], params[36], params[37], (int)params[38]);
     return 1;
 }
 

aerpaw/impl/controller_impl.h

@@ -27,13 +27,14 @@ int loadTargets(const char* filename, double* targets, int maxClients);
 //   28-31 betaTilt[1:4]
 //   32-34 domainMin
 //   35-37 domainMax
-//   38-39 objectivePos
+//   38    numObjectiveComponents (1 or 2; inferred from objectivePos field length)
-//   40-43 objectiveVar (2x2 col-major)
+//   39-42 objectivePos flat [x1,y1,x2,y2] (4 slots; zero-padded if N=1)
-//   44    sensorPerformanceMinimum
+//   43-50 objectiveVar flat [v11,v12,v21,v22 per component] (8 slots; zero-padded if N=1)
-//   45    useDoubleIntegrator  (0=single-integrator, 1=double-integrator)
+//   51    sensorPerformanceMinimum
-//   46    dampingCoeff
+//   52    useDoubleIntegrator  (0=single-integrator, 1=double-integrator)
-//   47    useFixedTopology     (0=dynamic lesser-neighbor, 1=fixed)
+//   53    dampingCoeff
-#define NUM_SCENARIO_PARAMS 48
+//   54    useFixedTopology     (0=dynamic lesser-neighbor, 1=fixed)
+#define NUM_SCENARIO_PARAMS 55
 #define MAX_CLIENTS_PER_PARAM 4
 // Maximum number of obstacles (upper bound for pre-allocated arrays).
 #define MAX_OBSTACLES 8

util/objectiveFunctionWrapper.m

@@ -13,6 +13,7 @@ function f = objectiveFunctionWrapper(center, sigma)
     if size(sigma, 1) == 1 && size(center, 1) > 1
         sigma = repmat(sigma, size(center, 1), 1, 1);
     end
+
     assert(size(center, 1) == size(sigma, 1));
     f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), squeeze(sigma(i, :, :))), 1:size(center,1), "UniformOutput", false)), 2);
 end