diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index 55dc091..497768a 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -109,8 +109,8 @@ function [obj] = constrainMotion(obj) b(kk) = obj.barrierGain * max(0, h_yMax)^obj.barrierExponent; kk = kk + 1; - % Z minimum - h_zMin = (obj.agents{ii}.pos(3) - obj.domain.minCorner(3)) - obj.agents{ii}.collisionGeometry.radius; + % Z minimum — enforce z >= minAlt + radius (not just z >= domain floor + radius) + h_zMin = (obj.agents{ii}.pos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1]; b(kk) = obj.barrierGain * max(0, h_zMin)^obj.barrierExponent; kk = kk + 1; diff --git a/@miSim/initialize.m b/@miSim/initialize.m index 16e1cd9..c7e0747 100644 --- a/@miSim/initialize.m +++ b/@miSim/initialize.m @@ -84,6 +84,7 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m % Set CBF parameters obj.barrierGain = barrierGain; obj.barrierExponent = barrierExponent; + obj.minAlt = minAlt; % Compute adjacency matrix and lesser neighbors obj = obj.updateAdjacency(); diff --git a/@miSim/initializeFromCsv.m b/@miSim/initializeFromCsv.m index b296ea8..350a630 100644 --- a/@miSim/initializeFromCsv.m +++ b/@miSim/initializeFromCsv.m @@ -49,6 +49,7 @@ BETA_TILT = scenario.betaTilt; 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 SENSOR_PERFORMANCE_MINIMUM = scenario.sensorPerformanceMinimum; % scalar % Initial UAV positions: flat vector reshaped to N×3 @@ -66,7 +67,7 @@ dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Dom % ---- Build sensing objective (MATLAB path: objectiveFunctionWrapper) ----- dom.objective = sensingObjective; -objFcn = objectiveFunctionWrapper(OBJECTIVE_GROUND_POS, 3 * eye(2)); +objFcn = objectiveFunctionWrapper(OBJECTIVE_GROUND_POS, OBJECTIVE_VAR); dom.objective = dom.objective.initialize(objFcn, dom, DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM); % ---- Build shared sensor model ------------------------------------------- diff --git a/@miSim/miSim.m b/@miSim/miSim.m index dcabeff..8009121 100644 --- a/@miSim/miSim.m +++ b/@miSim/miSim.m @@ -17,6 +17,7 @@ classdef miSim performance = 0; % simulation performance timeseries vector barrierGain = NaN; % CBF gain parameter barrierExponent = NaN; % CBF exponent parameter + minAlt = 0; % minimum allowable altitude (m) artifactName = ""; fPerf; % performance plot figure end diff --git a/@miSim/validate.m b/@miSim/validate.m index 03a4fff..818a6c0 100644 --- a/@miSim/validate.m +++ b/@miSim/validate.m @@ -20,8 +20,8 @@ function validate(obj) for kk = 1:size(obj.agents, 1) P = min(max(obj.agents{kk}.pos, obj.obstacles{jj}.minCorner), obj.obstacles{jj}.maxCorner); d = obj.agents{kk}.pos - P; - if dot(d, d) <= obj.agents{kk}.collisionGeometry.radius^2 - error("%s colliding with %s", obj.agents{kk}.label, obj.obstacles{jj}.label); % this will cause quadprog to fail + if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2 + warning("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, dot(d, d) - obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail end end end diff --git a/aerpaw/config/scenario.csv b/aerpaw/config/scenario.csv index 4dd5c71..fdd0cca 100644 --- a/aerpaw/config/scenario.csv +++ b/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, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax -5, 100, 30.0, 0.1, 1.0, 2.0, 100, 3, 3.0, 30.0, 60.0, 0.2, 10.0, 1.0, "0.0, 0.0, 0.0", "50.0, 50.0, 50.0", "35.0, 35.0", 1e-6, "5.0, 10.0, 45.0, 15.0, 10.0, 35.0", 0, "", "" \ No newline at end of file +timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax +5, 100, 30.0, 0.1, 1.0, 2.0, 100, 3, 3.0, 30.0, 80.0, 0.25, 5.0, 0.1, "0.0, 0.0, 0.0", "50.0, 50.0, 80.0", "35.0, 35.0", "10, 5, 5, 10", 2e-1, "5.0, 10.0, 45.0, 15.0, 10.0, 35.0", 0, "", "" \ No newline at end of file diff --git a/aerpaw/controller.m b/aerpaw/controller.m index 3276c34..48a486b 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -34,7 +34,7 @@ else end % Load guidance scenario from CSV (parameters for guidance_step) -NUM_SCENARIO_PARAMS = 23; +NUM_SCENARIO_PARAMS = 27; MAX_OBSTACLES_CTRL = int32(8); scenarioParams = zeros(1, NUM_SCENARIO_PARAMS); obstacleMin = zeros(MAX_OBSTACLES_CTRL, 3); diff --git a/aerpaw/guidance_step.m b/aerpaw/guidance_step.m index 4ce650b..eb26834 100644 --- a/aerpaw/guidance_step.m +++ b/aerpaw/guidance_step.m @@ -27,7 +27,8 @@ function nextPositions = guidance_step(currentPositions, isInit, ... % 7 barrierGain 15-17 domainMin % 8 barrierExponent 18-20 domainMax % 21-22 objectivePos -% 23 sensorPerformanceMinimum +% 23-26 objectiveVar (2x2, col-major) +% 27 sensorPerformanceMinimum % obstacleMin (MAX_OBSTACLES × 3) double column-major obstacle corners (compiled path) % obstacleMax (MAX_OBSTACLES × 3) double % numObstacles (1,1) int32 actual obstacle count @@ -89,7 +90,8 @@ if isInit DOMAIN_MIN = scenarioParams(15:17); DOMAIN_MAX = scenarioParams(18:20); OBJECTIVE_GROUND_POS = scenarioParams(21:22); - SENSOR_PERFORMANCE_MINIMUM = scenarioParams(23); + OBJECTIVE_VAR = reshape(scenarioParams(23:26), 2, 2); + SENSOR_PERFORMANCE_MINIMUM = scenarioParams(27); % --- Build domain geometry --- dom = rectangularPrism; @@ -102,7 +104,11 @@ if isInit [gridX, gridY] = meshgrid(xGrid, yGrid); dx = gridX - OBJECTIVE_GROUND_POS(1); dy = gridY - OBJECTIVE_GROUND_POS(2); - objValues = exp(-0.5 * (dx .* dx + dy .* dy)); + % Bivariate Gaussian using objectiveVar covariance matrix (avoids inv()) + ov_a = OBJECTIVE_VAR(1,1); ov_b = OBJECTIVE_VAR(1,2); + ov_c = OBJECTIVE_VAR(2,1); ov_d = OBJECTIVE_VAR(2,2); + ov_det = ov_a * ov_d - ov_b * ov_c; + objValues = exp((-0.5 / ov_det) .* (ov_d .* dx.*dx - (ov_b + ov_c) .* dx.*dy + ov_a .* dy.*dy)); dom.objective = dom.objective.initializeWithValues(objValues, dom, ... DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM); diff --git a/aerpaw/impl/controller_impl.cpp b/aerpaw/impl/controller_impl.cpp index d6c4b75..174324f 100644 --- a/aerpaw/impl/controller_impl.cpp +++ b/aerpaw/impl/controller_impl.cpp @@ -180,6 +180,8 @@ static int readScenarioDataRow(const char* filename, char* line, int lineSize) { // 14-16: domainMin (east, north, up) // 17-19: domainMax (east, north, up) // 20-21: objectivePos (east, north) +// 22-25: objectiveVar (2x2 col-major: v11, v12, v21, v22) +// 26 : sensorPerformanceMinimum // Returns 1 on success, 0 on failure. int loadScenario(const char* filename, double* params) { char line[4096]; @@ -191,8 +193,8 @@ int loadScenario(const char* filename, double* params) { char* fields[32]; int nf = splitCSVRow(copy, fields, 32); - if (nf < 21) { - fprintf(stderr, "loadScenario: expected >=21 columns, got %d\n", nf); + if (nf < 19) { + fprintf(stderr, "loadScenario: expected >=19 columns, got %d\n", nf); return 0; } @@ -231,10 +233,20 @@ int loadScenario(const char* filename, double* params) { } } - // sensorPerformanceMinimum: column 17 + // objectiveVar: column 17, format "v11, v12, v21, v22" { - char tmp[64]; strncpy(tmp, fields[17], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; - params[22] = atof(trimField(tmp)); + char tmp[256]; strncpy(tmp, fields[17], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; + char* t = trimField(tmp); + if (sscanf(t, "%lf , %lf , %lf , %lf", ¶ms[22], ¶ms[23], ¶ms[24], ¶ms[25]) != 4) { + fprintf(stderr, "loadScenario: failed to parse objectiveVar: %s\n", t); + return 0; + } + } + + // sensorPerformanceMinimum: column 18 + { + char tmp[64]; strncpy(tmp, fields[18], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; + params[26] = atof(trimField(tmp)); } printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g]\n", @@ -242,7 +254,7 @@ int loadScenario(const char* filename, double* params) { return 1; } -// Load initial UAV positions from scenario.csv (column 17: initialPositions). +// Load initial UAV positions from scenario.csv (column 19: initialPositions). // targets is a column-major [maxClients x 3] array (same layout as loadTargets): // targets[i + 0*maxClients] = east // targets[i + 1*maxClients] = north @@ -258,13 +270,13 @@ int loadInitialPositions(const char* filename, double* targets, int maxClients) char* fields[32]; int nf = splitCSVRow(copy, fields, 32); - if (nf < 19) { - fprintf(stderr, "loadInitialPositions: expected >=19 columns, got %d\n", nf); + if (nf < 20) { + fprintf(stderr, "loadInitialPositions: expected >=20 columns, got %d\n", nf); return 0; } - // Column 18: initialPositions flat "x1,y1,z1, x2,y2,z2, ..." - char tmp[1024]; strncpy(tmp, fields[18], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; + // Column 19: initialPositions flat "x1,y1,z1, x2,y2,z2, ..." + char tmp[1024]; strncpy(tmp, fields[19], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; char* t = trimField(tmp); double vals[3 * 4]; // up to MAX_CLIENTS triples @@ -287,7 +299,7 @@ int loadInitialPositions(const char* filename, double* targets, int maxClients) } // Load obstacle bounding-box corners from scenario.csv. -// Columns used: 18 (numObstacles), 19 (obstacleMin flat), 20 (obstacleMax flat). +// Columns used: 20 (numObstacles), 21 (obstacleMin flat), 22 (obstacleMax flat). // obstacleMin and obstacleMax are column-major [maxObstacles x 3] arrays: // obstacleMin[obs + 0*maxObstacles] = east_min // obstacleMin[obs + 1*maxObstacles] = north_min @@ -304,19 +316,19 @@ int loadObstacles(const char* filename, double* obstacleMin, double* obstacleMax char* fields[32]; int nf = splitCSVRow(copy, fields, 32); - if (nf < 22) return 0; + if (nf < 23) return 0; - // Column 19: numObstacles - int n = (int)atof(trimField(fields[19])); + // Column 20: numObstacles + int n = (int)atof(trimField(fields[20])); if (n <= 0) return 0; if (n > maxObstacles) { fprintf(stderr, "loadObstacles: %d obstacles exceeds MAX_OBSTACLES=%d\n", n, maxObstacles); n = maxObstacles; } - // Column 20: obstacleMin flat "x0,y0,z0, x1,y1,z1, ..." + // Column 21: obstacleMin flat "x0,y0,z0, x1,y1,z1, ..." { - char tmp[1024]; strncpy(tmp, fields[20], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; + char tmp[1024]; strncpy(tmp, fields[21], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; char* t = trimField(tmp); double vals[3 * 8]; // up to MAX_OBSTACLES triples int parsed = 0; @@ -336,9 +348,9 @@ int loadObstacles(const char* filename, double* obstacleMin, double* obstacleMax } } - // Column 21: obstacleMax flat + // Column 22: obstacleMax flat { - char tmp[1024]; strncpy(tmp, fields[21], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; + char tmp[1024]; strncpy(tmp, fields[22], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; char* t = trimField(tmp); double vals[3 * 8]; int parsed = 0; diff --git a/aerpaw/impl/controller_impl.h b/aerpaw/impl/controller_impl.h index ed71d44..cef9764 100644 --- a/aerpaw/impl/controller_impl.h +++ b/aerpaw/impl/controller_impl.h @@ -17,8 +17,9 @@ int loadTargets(const char* filename, double* targets, int maxClients); // Scenario loading (scenario.csv) // Number of elements in the flat params array passed to guidance_step. -// Indices: 0-13 scalars, 14-16 domainMin, 17-19 domainMax, 20-21 objectivePos. -#define NUM_SCENARIO_PARAMS 23 +// Indices: 0-13 scalars, 14-16 domainMin, 17-19 domainMax, 20-21 objectivePos, +// 22-25 objectiveVar (2x2 col-major: [v11,v12,v21,v22]), 26 sensorPerformanceMinimum. +#define NUM_SCENARIO_PARAMS 27 // Maximum number of obstacles (upper bound for pre-allocated arrays). #define MAX_OBSTACLES 8 diff --git a/aerpaw/results/plotGpsCsvs.m b/aerpaw/results/plotGpsCsvs.m index 6dc37cd..745b711 100644 --- a/aerpaw/results/plotGpsCsvs.m +++ b/aerpaw/results/plotGpsCsvs.m @@ -2,7 +2,9 @@ f = uifigure; gf = geoglobe(f); hold(gf, "on"); -c = ["r", "g", "b", "m", "c", "y", "k"]; % plotting colors +c = ["g", "b", "m", "c"]; % plotting colors + +% coordinate system constants seaToGroundLevel = 110; % meters, measured approximately from USGS national map viewer lla0 = [35.72550610629396, -78.70019657805574, seaToGroundLevel]; % origin (LLA) @@ -14,28 +16,27 @@ for ii = 1:size(gpsCsvs, 1) % Read CSV G{ii} = readGpsCsv(fullfile(gpsCsvs(ii).folder, gpsCsvs(ii).name)); - % Find when algorithm begins/ends (using ENU altitude) + % Find when algorithm begins/ends (using ENU altitude rate change) enuTraj = lla2enu([G{ii}.Latitude, G{ii}.Longitude, G{ii}.Altitude], lla0, 'flat'); verticalSpeed = movmean(abs(diff(G{ii}.Altitude)), [10, 0]); - startIdx = find(verticalSpeed <= prctile(verticalSpeed, 25), 1, 'first'); - stopIdx = find(verticalSpeed <= prctile(verticalSpeed, 10), 1, 'last'); + % Automatically detect start/stop of algorithm flight (ignore takeoff, setup, return to liftoff, landing segments of flight) + startIdx = find(verticalSpeed <= prctile(verticalSpeed, 25), 1, 'first'); % 25 pct threshold may need adjusting + stopIdx = find(verticalSpeed <= prctile(verticalSpeed, 25), 1, 'last'); + + % % Plot whole flight, including setup/cleanup % startIdx = 1; % stopIdx = length(verticalSpeed); - - speed = vecnorm(diff(enuTraj), 2, 2); - meanSpeed = movmean(speed, [10, 0]); - % Plot recorded trajectory + % Plot recorded trajectory over specified range of indices geoplot3(gf, G{ii}.Latitude(startIdx:stopIdx), G{ii}.Longitude(startIdx:stopIdx), G{ii}.Altitude(startIdx:stopIdx) + seaToGroundLevel, c(mod(ii, length(c))), 'LineWidth', 2, "MarkerSize", 5); - end % Plot objective objectivePos = [35, 35, 0]; llaObj = enu2lla(objectivePos, lla0, 'flat'); -geoplot3(gf, [llaObj(1), llaObj(1)], [llaObj(2), llaObj(2)], [llaObj(3), llaObj(3) + 50], 'LineWidth', 3); +geoplot3(gf, [llaObj(1), llaObj(1)], [llaObj(2), llaObj(2)], [llaObj(3), llaObj(3) + 50], 'LineWidth', 3, "Color", 'y'); % Plot domain altOffset = 1; % to avoid clipping into the ground when displayed @@ -47,4 +48,9 @@ geoplot3(gf, [domain(2, 1), domain(2, 1)], [domain(1, 2), domain(1, 2)], domain( geoplot3(gf, [domain(1, 1), domain(1, 1)], [domain(2, 2), domain(2, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k'); geoplot3(gf, [domain(2, 1), domain(2, 1)], [domain(2, 2), domain(2, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k'); +% Plot floor (minimum altitude constraint) +floorAlt = 30; +geoplot3(gf, [domain(1, 1), domain(2, 1), domain(2, 1), domain(1, 1), domain(1, 1)], [domain(1, 2), domain(1, 2), domain(2, 2), domain(2, 2), domain(1, 2)], repmat(domain(1, 3) + altOffset + floorAlt, 1, 5), 'LineWidth', 3, 'Color', 'r'); + +% finish hold(gf, "off"); \ No newline at end of file diff --git a/test/parametricTestSuite.m b/test/parametricTestSuite.m index 1610a4f..b1f1728 100644 --- a/test/parametricTestSuite.m +++ b/test/parametricTestSuite.m @@ -34,7 +34,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase % Define scenario according to CSV specification tc.domain = tc.domain.initialize([params.domainMin; params.domainMax], REGION_TYPE.DOMAIN, "Domain"); - tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper(params.objectivePos), tc.domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum); + tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper(params.objectivePos, reshape(params.objectiveVar, [2 2])), tc.domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum); agents = cell(size(params.initialPositions, 2) / 3, 1); for ii = 1:size(agents, 1)