kdee/miSim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

scenario edits

Browse Source
This commit is contained in:
Kevin Dee
2026-03-02 18:18:40 -08:00
parent 032f50774f
commit d49bf61d1d
12 changed files with 70 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
@miSim/constrainMotion.m
View File

@@ -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;

1
@miSim/initialize.m
View File

@@ -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();

3
@miSim/initializeFromCsv.m
View File

@@ -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 -------------------------------------------

1
@miSim/miSim.m
View File

@@ -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

4
@miSim/validate.m
View File

@@ -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

4
aerpaw/config/scenario.csv
View File

@@ -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, "", ""
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, "", ""
1 timestep maxIter minAlt discretizationStep protectedRange initialStepSize barrierGain barrierExponent collisionRadius comRange alphaDist betaDist alphaTilt betaTilt domainMin domainMax objectivePos objectiveVar sensorPerformanceMinimum initialPositions numObstacles obstacleMin obstacleMax
2 5 100 30.0 0.1 1.0 2.0 100 3 3.0 30.0 60.0 80.0 0.2 0.25 10.0 5.0 1.0 0.1 0.0, 0.0, 0.0 50.0, 50.0, 50.0 50.0, 50.0, 80.0 35.0, 35.0 10, 5, 5, 10 1e-6 2e-1 5.0, 10.0, 45.0, 15.0, 10.0, 35.0 0

2
aerpaw/controller.m
View File

@@ -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);

12
aerpaw/guidance_step.m
View File

@@ -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);

48
aerpaw/impl/controller_impl.cpp
View File

@@ -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", &params[22], &params[23], &params[24], &params[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;

5
aerpaw/impl/controller_impl.h
View File

@@ -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

26
aerpaw/results/plotGpsCsvs.m
View File

@@ -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");

2
test/parametricTestSuite.m
View File

@@ -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)