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

per-UAV parameters

Browse Source
This commit is contained in:
Kevin Dee
2026-03-03 16:18:07 -08:00
parent 252423eb29
commit fe7b1b2ed3
13 changed files with 171 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
@miSim/initializeFromCsv.m
View File

@@ -14,9 +14,12 @@ function obj = initializeFromCsv(obj, csvPath)
% %
% Expected CSV columns (see scenario.csv): % Expected CSV columns (see scenario.csv):
% timestep, maxIter, minAlt, discretizationStep, protectedRange, % timestep, maxIter, minAlt, discretizationStep, protectedRange,
% initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, % initialStepSize, barrierGain, barrierExponent,
% alphaDist, betaDist, alphaTilt, betaTilt, % collisionRadius (per-UAV), comRange (per-UAV),
% alphaDist (per-UAV), betaDist (per-UAV),
% alphaTilt (per-UAV), betaTilt (per-UAV),
% domainMin ("x,y,z"), domainMax ("x,y,z"), objectivePos ("x,y"), % domainMin ("x,y,z"), domainMax ("x,y,z"), objectivePos ("x,y"),
% objectiveVar ("v11,v12,v21,v22"), sensorPerformanceMinimum,
% initialPositions (flat "x1,y1,z1, x2,y2,z2,..."), % initialPositions (flat "x1,y1,z1, x2,y2,z2,..."),
% numObstacles, obstacleMin (flat), obstacleMax (flat) % numObstacles, obstacleMin (flat), obstacleMax (flat)
@@ -39,12 +42,13 @@ PROTECTED_RANGE = scenario.protectedRange;
INITIAL_STEP_SIZE = scenario.initialStepSize; INITIAL_STEP_SIZE = scenario.initialStepSize;
BARRIER_GAIN = scenario.barrierGain; BARRIER_GAIN = scenario.barrierGain;
BARRIER_EXPONENT = scenario.barrierExponent; BARRIER_EXPONENT = scenario.barrierExponent;
COLLISION_RADIUS = scenario.collisionRadius; % Per-UAV parameters (vectors with one element per UAV)
COMMS_RANGE = scenario.comRange; COLLISION_RADIUS_VEC = scenario.collisionRadius; % 1×N
ALPHA_DIST = scenario.alphaDist; COMMS_RANGE_VEC = scenario.comRange; % 1×N
BETA_DIST = scenario.betaDist; ALPHA_DIST_VEC = scenario.alphaDist; % 1×N
ALPHA_TILT = scenario.alphaTilt; BETA_DIST_VEC = scenario.betaDist; % 1×N
BETA_TILT = scenario.betaTilt; ALPHA_TILT_VEC = scenario.alphaTilt; % 1×N
BETA_TILT_VEC = scenario.betaTilt; % 1×N
DOMAIN_MIN = scenario.domainMin; % 1×3 DOMAIN_MIN = scenario.domainMin; % 1×3
DOMAIN_MAX = scenario.domainMax; % 1×3 DOMAIN_MAX = scenario.domainMax; % 1×3
@@ -59,6 +63,20 @@ assert(mod(numel(flatPos), 3) == 0, ...
positions = reshape(flatPos, 3, [])'; % N×3 positions = reshape(flatPos, 3, [])'; % N×3
numAgents = size(positions, 1); numAgents = size(positions, 1);
% Validate per-UAV parameter lengths match numAgents
assert(numel(COLLISION_RADIUS_VEC) == numAgents, ...
"collisionRadius has %d values but expected %d (one per UAV)", numel(COLLISION_RADIUS_VEC), numAgents);
assert(numel(COMMS_RANGE_VEC) == numAgents, ...
"comRange has %d values but expected %d (one per UAV)", numel(COMMS_RANGE_VEC), numAgents);
assert(numel(ALPHA_DIST_VEC) == numAgents, ...
"alphaDist has %d values but expected %d (one per UAV)", numel(ALPHA_DIST_VEC), numAgents);
assert(numel(BETA_DIST_VEC) == numAgents, ...
"betaDist has %d values but expected %d (one per UAV)", numel(BETA_DIST_VEC), numAgents);
assert(numel(ALPHA_TILT_VEC) == numAgents, ...
"alphaTilt has %d values but expected %d (one per UAV)", numel(ALPHA_TILT_VEC), numAgents);
assert(numel(BETA_TILT_VEC) == numAgents, ...
"betaTilt has %d values but expected %d (one per UAV)", numel(BETA_TILT_VEC), numAgents);
numObstacles = scenario.numObstacles; numObstacles = scenario.numObstacles;
% ---- Build domain -------------------------------------------------------- % ---- Build domain --------------------------------------------------------
@@ -70,19 +88,23 @@ dom.objective = sensingObjective;
objFcn = objectiveFunctionWrapper(OBJECTIVE_GROUND_POS, OBJECTIVE_VAR); objFcn = objectiveFunctionWrapper(OBJECTIVE_GROUND_POS, OBJECTIVE_VAR);
dom.objective = dom.objective.initialize(objFcn, dom, DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM); dom.objective = dom.objective.initialize(objFcn, dom, DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM);
% ---- Build shared sensor model -------------------------------------------
sensor = sigmoidSensor;
sensor = sensor.initialize(ALPHA_DIST, BETA_DIST, ALPHA_TILT, BETA_TILT);
% ---- Initialise agents from scenario positions --------------------------- % ---- Initialise agents from scenario positions ---------------------------
% Each agent gets its own sensor model and collision/comms radii from the
% per-UAV parameter vectors.
agentList = cell(numAgents, 1); agentList = cell(numAgents, 1);
for ii = 1:numAgents for ii = 1:numAgents
pos = positions(ii, :); pos = positions(ii, :);
% Per-UAV sensor model
sensor = sigmoidSensor;
sensor = sensor.initialize(ALPHA_DIST_VEC(ii), BETA_DIST_VEC(ii), ...
ALPHA_TILT_VEC(ii), BETA_TILT_VEC(ii));
geom = spherical; geom = spherical;
geom = geom.initialize(pos, COLLISION_RADIUS, REGION_TYPE.COLLISION, ... geom = geom.initialize(pos, COLLISION_RADIUS_VEC(ii), REGION_TYPE.COLLISION, ...
sprintf("UAV %d Collision", ii)); sprintf("UAV %d Collision", ii));
ag = agent; ag = agent;
ag = ag.initialize(pos, geom, sensor, COMMS_RANGE, MAX_ITER, ... ag = ag.initialize(pos, geom, sensor, COMMS_RANGE_VEC(ii), MAX_ITER, ...
INITIAL_STEP_SIZE, sprintf("UAV %d", ii)); INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
agentList{ii} = ag; agentList{ii} = ag;
end end

2
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, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax 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, "", "" 5, 100, 30.0, 0.1, 1.0, 2.0, 100, 3, "3.0, 3.0", "30.0, 30.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "50.0, 50.0, 80.0", "35.0, 35.0", "10, 5, 5, 10", 0.15, "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 3.0, 3.0 30.0 30.0, 30.0 80.0 80.0, 80.0 0.25 0.25, 0.25 5.0 5.0, 5.0 0.1 0.1, 0.1 0.0, 0.0, 0.0 50.0, 50.0, 80.0 35.0, 35.0 10, 5, 5, 10 2e-1 0.15 5.0, 10.0, 45.0, 15.0, 10.0, 35.0 0

2
aerpaw/controller.coderprj
View File

@@ -1095,7 +1095,7 @@
</Artifacts> </Artifacts>
<BuildFolder type="coderapp.internal.util.mfz.FileSpec"/> <BuildFolder type="coderapp.internal.util.mfz.FileSpec"/>
<Success>true</Success> <Success>true</Success>
<Timestamp>2026-03-03T15:32:50</Timestamp> <Timestamp>2026-03-03T15:54:20</Timestamp>
</MainBuildResult> </MainBuildResult>
</coderapp.internal.mlc.mfz.MatlabCoderProjectState> </coderapp.internal.mlc.mfz.MatlabCoderProjectState>
</MF0> </MF0>

2
aerpaw/controller.m
View File

@@ -34,7 +34,7 @@ else
end end
% Load guidance scenario from CSV (parameters for guidance_step) % Load guidance scenario from CSV (parameters for guidance_step)
NUM_SCENARIO_PARAMS = 27; NUM_SCENARIO_PARAMS = 45;
MAX_OBSTACLES_CTRL = int32(8); MAX_OBSTACLES_CTRL = int32(8);
scenarioParams = zeros(1, NUM_SCENARIO_PARAMS); scenarioParams = zeros(1, NUM_SCENARIO_PARAMS);
obstacleMin = zeros(MAX_OBSTACLES_CTRL, 3); obstacleMin = zeros(MAX_OBSTACLES_CTRL, 3);

62
aerpaw/guidance_step.m
View File

@@ -18,17 +18,17 @@ function nextPositions = guidance_step(currentPositions, isInit, ...
% On MATLAB path this is ignored; parameters are loaded % On MATLAB path this is ignored; parameters are loaded
% from scenario.csv via initializeFromCsv instead. % from scenario.csv via initializeFromCsv instead.
% Index mapping (1-based): % Index mapping (1-based):
% 1 timestep 9 collisionRadius % 1 timestep 9-12 collisionRadius[1:4]
% 2 maxIter 10 comRange % 2 maxIter 13-16 comRange[1:4]
% 3 minAlt 11 alphaDist % 3 minAlt 17-20 alphaDist[1:4]
% 4 discretizationStep 12 betaDist % 4 discretizationStep 21-24 betaDist[1:4]
% 5 protectedRange 13 alphaTilt % 5 protectedRange 25-28 alphaTilt[1:4]
% 6 initialStepSize 14 betaTilt % 6 initialStepSize 29-32 betaTilt[1:4]
% 7 barrierGain 15-17 domainMin % 7 barrierGain 33-35 domainMin
% 8 barrierExponent 18-20 domainMax % 8 barrierExponent 36-38 domainMax
% 21-22 objectivePos % 39-40 objectivePos
% 23-26 objectiveVar (2x2, col-major) % 41-44 objectiveVar (2x2, col-major)
% 27 sensorPerformanceMinimum % 45 sensorPerformanceMinimum
% obstacleMin (MAX_OBSTACLES × 3) double column-major obstacle corners (compiled path) % obstacleMin (MAX_OBSTACLES × 3) double column-major obstacle corners (compiled path)
% obstacleMax (MAX_OBSTACLES × 3) double % obstacleMax (MAX_OBSTACLES × 3) double
% numObstacles (1,1) int32 actual obstacle count % numObstacles (1,1) int32 actual obstacle count
@@ -72,6 +72,8 @@ if isInit
else else
% ================================================================ % ================================================================
% Compiled path: unpack scenarioParams array and obstacle arrays. % Compiled path: unpack scenarioParams array and obstacle arrays.
% Per-UAV parameters are stored as MAX_CLIENTS-wide blocks; only
% the first numAgents entries of each block are used.
% ================================================================ % ================================================================
TIMESTEP = scenarioParams(1); TIMESTEP = scenarioParams(1);
MAX_ITER = int32(scenarioParams(2)); MAX_ITER = int32(scenarioParams(2));
@@ -81,17 +83,17 @@ if isInit
INITIAL_STEP_SIZE = scenarioParams(6); INITIAL_STEP_SIZE = scenarioParams(6);
BARRIER_GAIN = scenarioParams(7); BARRIER_GAIN = scenarioParams(7);
BARRIER_EXPONENT = scenarioParams(8); BARRIER_EXPONENT = scenarioParams(8);
COLLISION_RADIUS = scenarioParams(9); COLLISION_RADIUS_VEC = scenarioParams(9:12); % per-UAV [1:MAX_CLIENTS]
COMMS_RANGE = scenarioParams(10); COMMS_RANGE_VEC = scenarioParams(13:16);
ALPHA_DIST = scenarioParams(11); ALPHA_DIST_VEC = scenarioParams(17:20);
BETA_DIST = scenarioParams(12); BETA_DIST_VEC = scenarioParams(21:24);
ALPHA_TILT = scenarioParams(13); ALPHA_TILT_VEC = scenarioParams(25:28);
BETA_TILT = scenarioParams(14); BETA_TILT_VEC = scenarioParams(29:32);
DOMAIN_MIN = scenarioParams(15:17); DOMAIN_MIN = scenarioParams(33:35);
DOMAIN_MAX = scenarioParams(18:20); DOMAIN_MAX = scenarioParams(36:38);
OBJECTIVE_GROUND_POS = scenarioParams(21:22); OBJECTIVE_GROUND_POS = scenarioParams(39:40);
OBJECTIVE_VAR = reshape(scenarioParams(23:26), 2, 2); OBJECTIVE_VAR = reshape(scenarioParams(41:44), 2, 2);
SENSOR_PERFORMANCE_MINIMUM = scenarioParams(27); SENSOR_PERFORMANCE_MINIMUM = scenarioParams(45);
% --- Build domain geometry --- % --- Build domain geometry ---
dom = rectangularPrism; dom = rectangularPrism;
@@ -112,19 +114,21 @@ if isInit
dom.objective = dom.objective.initializeWithValues(objValues, dom, ... dom.objective = dom.objective.initializeWithValues(objValues, dom, ...
DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM); DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM);
% --- Build shared sensor model --- % --- Initialise agents from GPS positions (per-UAV parameters) ----
sensor = sigmoidSensor;
sensor = sensor.initialize(ALPHA_DIST, BETA_DIST, ALPHA_TILT, BETA_TILT);
% --- Initialise agents from GPS positions ---
agentList = cell(numAgents, 1); agentList = cell(numAgents, 1);
for ii = 1:numAgents for ii = 1:numAgents
pos = currentPositions(ii, :); pos = currentPositions(ii, :);
% Per-UAV sensor model
sensor = sigmoidSensor;
sensor = sensor.initialize(ALPHA_DIST_VEC(ii), BETA_DIST_VEC(ii), ...
ALPHA_TILT_VEC(ii), BETA_TILT_VEC(ii));
geom = spherical; geom = spherical;
geom = geom.initialize(pos, COLLISION_RADIUS, REGION_TYPE.COLLISION, ... geom = geom.initialize(pos, COLLISION_RADIUS_VEC(ii), REGION_TYPE.COLLISION, ...
sprintf("UAV %d Collision", ii)); sprintf("UAV %d Collision", ii));
ag = agent; ag = agent;
ag = ag.initialize(pos, geom, sensor, COMMS_RANGE, MAX_ITER, ... ag = ag.initialize(pos, geom, sensor, COMMS_RANGE_VEC(ii), MAX_ITER, ...
INITIAL_STEP_SIZE, sprintf("UAV %d", ii)); INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
agentList{ii} = ag; agentList{ii} = ag;
end end

73
aerpaw/impl/controller_impl.cpp
View File

@@ -174,14 +174,19 @@ static int readScenarioDataRow(const char* filename, char* line, int lineSize) {
// Load guidance parameters from scenario.csv into flat params[NUM_SCENARIO_PARAMS]. // Load guidance parameters from scenario.csv into flat params[NUM_SCENARIO_PARAMS].
// Index mapping (0-based): // Index mapping (0-based):
// 0-13 : timestep, maxIter, minAlt, discretizationStep, protectedRange, // 0-7 : timestep, maxIter, minAlt, discretizationStep, protectedRange,
// initialStepSize, barrierGain, barrierExponent, collisionRadius, // initialStepSize, barrierGain, barrierExponent
// comRange, alphaDist, betaDist, alphaTilt, betaTilt // 8-11 : collisionRadius[1:4] (per-UAV, MAX_CLIENTS_PER_PARAM slots)
// 14-16: domainMin (east, north, up) // 12-15: comRange[1:4]
// 17-19: domainMax (east, north, up) // 16-19: alphaDist[1:4]
// 20-21: objectivePos (east, north) // 20-23: betaDist[1:4]
// 22-25: objectiveVar (2x2 col-major: v11, v12, v21, v22) // 24-27: alphaTilt[1:4]
// 26 : sensorPerformanceMinimum // 28-31: betaTilt[1:4]
// 32-34: domainMin (east, north, up)
// 35-37: domainMax (east, north, up)
// 38-39: objectivePos (east, north)
// 40-43: objectiveVar (2x2 col-major: v11, v12, v21, v22)
// 44 : sensorPerformanceMinimum
// Returns 1 on success, 0 on failure. // Returns 1 on success, 0 on failure.
int loadScenario(const char* filename, double* params) { int loadScenario(const char* filename, double* params) {
char line[4096]; char line[4096];
@@ -198,16 +203,52 @@ int loadScenario(const char* filename, double* params) {
return 0; return 0;
} }
// Scalar fields (columns 0–13) // Zero-initialise all params so unused per-UAV slots default to 0
for (int i = 0; i < 14; i++) { for (int i = 0; i < NUM_SCENARIO_PARAMS; i++) params[i] = 0.0;
// Scalar fields (columns 0–7): timestep..barrierExponent
for (int i = 0; i < 8; i++) {
params[i] = atof(trimField(fields[i])); params[i] = atof(trimField(fields[i]));
} }
// Per-UAV fields (columns 8–13): collisionRadius, comRange,
// alphaDist, betaDist, alphaTilt, betaTilt.
// Each is a quoted comma-separated list with up to MAX_CLIENTS_PER_PARAM values.
// Stored in params as consecutive blocks of MAX_CLIENTS_PER_PARAM (4) slots.
{
const int perUavCols[] = {8, 9, 10, 11, 12, 13};
const int perUavOffsets[] = {8, 12, 16, 20, 24, 28}; // 0-based param indices
const char* perUavNames[] = {"collisionRadius", "comRange",
"alphaDist", "betaDist",
"alphaTilt", "betaTilt"};
for (int p = 0; p < 6; p++) {
char tmp[256];
strncpy(tmp, fields[perUavCols[p]], sizeof(tmp) - 1);
tmp[sizeof(tmp) - 1] = '\0';
char* t = trimField(tmp);
// Parse comma-separated values
double vals[4] = {0, 0, 0, 0};
int count = 0;
char* tok = strtok(t, ",");
while (tok && count < MAX_CLIENTS_PER_PARAM) {
vals[count++] = atof(tok);
tok = strtok(nullptr, ",");
}
if (count == 0) {
fprintf(stderr, "loadScenario: failed to parse %s\n", perUavNames[p]);
return 0;
}
for (int k = 0; k < MAX_CLIENTS_PER_PARAM; k++) {
params[perUavOffsets[p] + k] = vals[k];
}
}
}
// domainMin: column 14, format "east, north, up" // domainMin: column 14, format "east, north, up"
{ {
char tmp[256]; strncpy(tmp, fields[14], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; char tmp[256]; strncpy(tmp, fields[14], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
char* t = trimField(tmp); char* t = trimField(tmp);
if (sscanf(t, "%lf , %lf , %lf", &params[14], &params[15], &params[16]) != 3) { if (sscanf(t, "%lf , %lf , %lf", &params[32], &params[33], &params[34]) != 3) {
fprintf(stderr, "loadScenario: failed to parse domainMin: %s\n", t); fprintf(stderr, "loadScenario: failed to parse domainMin: %s\n", t);
return 0; return 0;
} }
@@ -217,7 +258,7 @@ int loadScenario(const char* filename, double* params) {
{ {
char tmp[256]; strncpy(tmp, fields[15], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; char tmp[256]; strncpy(tmp, fields[15], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
char* t = trimField(tmp); char* t = trimField(tmp);
if (sscanf(t, "%lf , %lf , %lf", &params[17], &params[18], &params[19]) != 3) { if (sscanf(t, "%lf , %lf , %lf", &params[35], &params[36], &params[37]) != 3) {
fprintf(stderr, "loadScenario: failed to parse domainMax: %s\n", t); fprintf(stderr, "loadScenario: failed to parse domainMax: %s\n", t);
return 0; return 0;
} }
@@ -227,7 +268,7 @@ int loadScenario(const char* filename, double* params) {
{ {
char tmp[256]; strncpy(tmp, fields[16], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; char tmp[256]; strncpy(tmp, fields[16], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
char* t = trimField(tmp); char* t = trimField(tmp);
if (sscanf(t, "%lf , %lf", &params[20], &params[21]) != 2) { if (sscanf(t, "%lf , %lf", &params[38], &params[39]) != 2) {
fprintf(stderr, "loadScenario: failed to parse objectivePos: %s\n", t); fprintf(stderr, "loadScenario: failed to parse objectivePos: %s\n", t);
return 0; return 0;
} }
@@ -237,7 +278,7 @@ int loadScenario(const char* filename, double* params) {
{ {
char tmp[256]; strncpy(tmp, fields[17], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; char tmp[256]; strncpy(tmp, fields[17], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
char* t = trimField(tmp); char* t = trimField(tmp);
if (sscanf(t, "%lf , %lf , %lf , %lf", &params[22], &params[23], &params[24], &params[25]) != 4) { if (sscanf(t, "%lf , %lf , %lf , %lf", &params[40], &params[41], &params[42], &params[43]) != 4) {
fprintf(stderr, "loadScenario: failed to parse objectiveVar: %s\n", t); fprintf(stderr, "loadScenario: failed to parse objectiveVar: %s\n", t);
return 0; return 0;
} }
@@ -246,11 +287,11 @@ int loadScenario(const char* filename, double* params) {
// sensorPerformanceMinimum: column 18 // sensorPerformanceMinimum: column 18
{ {
char tmp[64]; strncpy(tmp, fields[18], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; char tmp[64]; strncpy(tmp, fields[18], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0';
params[26] = atof(trimField(tmp)); params[44] = 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]\n",
params[14], params[15], params[16], params[17], params[18], params[19]); params[32], params[33], params[34], params[35], params[36], params[37]);
return 1; return 1;
} }

18
aerpaw/impl/controller_impl.h
View File

@@ -17,9 +17,21 @@ int loadTargets(const char* filename, double* targets, int maxClients);
// Scenario loading (scenario.csv) // Scenario loading (scenario.csv)
// Number of elements in the flat params array passed to guidance_step. // 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, // Indices (0-based):
// 22-25 objectiveVar (2x2 col-major: [v11,v12,v21,v22]), 26 sensorPerformanceMinimum. // 0-7 scalars (timestep..barrierExponent)
#define NUM_SCENARIO_PARAMS 27 // 8-11 collisionRadius[1:4] (per-UAV, MAX_CLIENTS slots)
// 12-15 comRange[1:4]
// 16-19 alphaDist[1:4]
// 20-23 betaDist[1:4]
// 24-27 alphaTilt[1:4]
// 28-31 betaTilt[1:4]
// 32-34 domainMin
// 35-37 domainMax
// 38-39 objectivePos
// 40-43 objectiveVar (2x2 col-major)
// 44 sensorPerformanceMinimum
#define NUM_SCENARIO_PARAMS 45
#define MAX_CLIENTS_PER_PARAM 4
// Maximum number of obstacles (upper bound for pre-allocated arrays). // Maximum number of obstacles (upper bound for pre-allocated arrays).
#define MAX_OBSTACLES 8 #define MAX_OBSTACLES 8

16
aerpaw/results/plotGpsCsvs.m
View File

@@ -20,7 +20,7 @@ fclose(fID);
lla0 = [str2double(yaml((strfind(yaml, 'lat:') + 4):(strfind(yaml, 'lon:') - 1))), str2double(yaml((strfind(yaml, 'lon:') + 4):(strfind(yaml, 'alt:') - 1))), seaToGroundLevel]; lla0 = [str2double(yaml((strfind(yaml, 'lat:') + 4):(strfind(yaml, 'lon:') - 1))), str2double(yaml((strfind(yaml, 'lon:') + 4):(strfind(yaml, 'alt:') - 1))), seaToGroundLevel];
% Paths to logs % Paths to logs
gpsCsvs = dir(fullfile("sandbox", "test9", "*.csv")); gpsCsvs = dir(fullfile("sandbox", "test10", "*.csv"));
G = cell(size(gpsCsvs)); G = cell(size(gpsCsvs));
for ii = 1:size(gpsCsvs, 1) for ii = 1:size(gpsCsvs, 1)
@@ -38,18 +38,13 @@ for ii = 1:size(gpsCsvs, 1)
stopIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, 'last'); stopIdx = find(verticalSpeed <= prctile(verticalSpeed, pctThreshold), 1, 'last');
% % Plot whole flight, including setup/cleanup % % Plot whole flight, including setup/cleanup
% startIdx = 1; startIdx = 1;
% stopIdx = length(verticalSpeed); stopIdx = length(verticalSpeed);
% Plot recorded trajectory over specified range of indices % 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); 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 end
% Plot objective
objectivePos = [params.objectivePos, 0];
llaObj = enu2lla(objectivePos, lla0, 'flat');
geoplot3(gf, [llaObj(1), llaObj(1)], [llaObj(2), llaObj(2)], [llaObj(3), llaObj(3) + 50], 'LineWidth', 3, "Color", 'y');
% Plot domain % Plot domain
altOffset = 1; % to avoid clipping into the ground when displayed altOffset = 1; % to avoid clipping into the ground when displayed
domain = [lla0; enu2lla(params.domainMax, lla0, 'flat')]; domain = [lla0; enu2lla(params.domainMax, lla0, 'flat')];
@@ -64,5 +59,10 @@ geoplot3(gf, [domain(2, 1), domain(2, 1)], [domain(2, 2), domain(2, 2)], domain(
floorAlt = params.minAlt; floorAlt = params.minAlt;
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'); 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');
% Plot objective
objectivePos = [params.objectivePos, 0];
llaObj = enu2lla(objectivePos, lla0, 'flat');
geoplot3(gf, [llaObj(1), llaObj(1)], [llaObj(2), llaObj(2)], [llaObj(3), domain(2, 3)], 'LineWidth', 3, "Color", 'y');
% finish % finish
hold(gf, "off"); hold(gf, "off");

2
resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/soCFsvL91lhqvUw-h8HkHCtfsiwd.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info/>

2
resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/soCFsvL91lhqvUw-h8HkHCtfsiwp.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="test10" type="File"/>

2
resources/project/soCFsvL91lhqvUw-h8HkHCtfsiw/1fQ44ZBWSpGWUWzrR6iBNoEkvh4d.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info/>

2
resources/project/soCFsvL91lhqvUw-h8HkHCtfsiw/1fQ44ZBWSpGWUWzrR6iBNoEkvh4p.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="1" type="DIR_SIGNIFIER"/>

21
test/parametricTestSuite.m
View File

@@ -30,7 +30,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase
function test_AERPAW_scenario(tc) function test_AERPAW_scenario(tc)
% Load scenario definition % Load scenario definition
tc.csvPath = fullfile(matlab.project.rootProject().RootFolder, "aerpaw", "config", "scenario.csv"); tc.csvPath = fullfile(matlab.project.rootProject().RootFolder, "aerpaw", "config", "scenario.csv");
params = tc.testClass.readScenarioCsv(tc.csvPath); params = readScenarioCsv(tc.csvPath);
% Define scenario according to CSV specification % Define scenario according to CSV specification
tc.domain = tc.domain.initialize([params.domainMin; params.domainMax], REGION_TYPE.DOMAIN, "Domain"); tc.domain = tc.domain.initialize([params.domainMin; params.domainMax], REGION_TYPE.DOMAIN, "Domain");
@@ -41,16 +41,20 @@ classdef parametricTestSuite < matlab.unittest.TestCase
agents{ii} = agent; agents{ii} = agent;
sensorModel = sigmoidSensor; sensorModel = sigmoidSensor;
sensorModel = sensorModel.initialize(params.alphaDist, params.betaDist, params.alphaTilt, params.betaTilt); sensorModel = sensorModel.initialize(params.alphaDist(ii), params.betaDist(ii), params.alphaTilt(ii), params.betaTilt(ii));
collisionGeometry = spherical; collisionGeometry = spherical;
collisionGeometry = collisionGeometry.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), params.collisionRadius, REGION_TYPE.COLLISION, sprintf("Agent %d collision geometry", ii)); collisionGeometry = collisionGeometry.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), params.collisionRadius(ii), REGION_TYPE.COLLISION, sprintf("Agent %d collision geometry", ii));
agents{ii} = agents{ii}.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), collisionGeometry, sensorModel, params.comRange, params.maxIter, params.initialStepSize, sprintf("Agent %d", ii), tc.plotCommsGeometry); agents{ii} = agents{ii}.initialize(params.initialPositions((((ii - 1) * 3) + 1):(ii * 3)), collisionGeometry, sensorModel, params.comRange(ii), params.maxIter, params.initialStepSize, sprintf("Agent %d", ii), tc.plotCommsGeometry);
end end
% TODO % Create obstacles
obstacles = {}; obstacles = cell(params.numObstacles, 1);
for ii = 1:size(obstacles, 1)
obstacles{ii} = rectangularPrism;
obstacles{ii} = obstacles{ii}.initialize([params.obstacleMin((((ii - 1) * 3) + 1):(ii * 3)); params.obstacleMax((((ii - 1) * 3) + 1):(ii * 3))], "OBSTACLE", sprintf("Obstacle %d", ii));
end
% Set up simulation % Set up simulation
tc.testClass = tc.testClass.initialize(tc.domain, agents, params.barrierGain, params.barrierExponent, params.minAlt, params.timestep, params.maxIter, obstacles, tc.makePlots, tc.makeVideo); tc.testClass = tc.testClass.initialize(tc.domain, agents, params.barrierGain, params.barrierExponent, params.minAlt, params.timestep, params.maxIter, obstacles, tc.makePlots, tc.makeVideo);
@@ -60,13 +64,10 @@ classdef parametricTestSuite < matlab.unittest.TestCase
% Run % Run
tc.testClass = tc.testClass.run(); tc.testClass = tc.testClass.run();
% Cleanup
tc.testClass = tc.testClass.teardown();
end end
function csv_parametric_tests_random_agents(tc) function csv_parametric_tests_random_agents(tc)
% Read in parameters to iterate over % Read in parameters to iterate over
params = tc.testClass.readScenarioCsv(tc.csvPath); params = readScenarioCsv(tc.csvPath);
% Test case setup % Test case setup
l = 10; % domain size l = 10; % domain size