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

added 2 objective support for AERPAW

Browse Source
This commit is contained in:
Kevin Dee
2026-05-25 14:51:39 -07:00
parent 0b63cb4874
commit c73559cd69
5 changed files with 87 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
@miSim/initializeFromCsv.m
+13 -2
View File
@@ -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/controller.m
+1 -1
View File
@@ -58,7 +58,7 @@ if ~coder.target('MATLAB')
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);
aerpaw/guidance_step.m
+19 -14
View File
@@ -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);
OBJECTIVE_VAR = reshape(scenarioParams(41:44), 2, 2);
SENSOR_PERFORMANCE_MINIMUM = scenarioParams(45);
USE_DOUBLE_INTEGRATOR = logical(scenarioParams(46));
DAMPING_COEFF = scenarioParams(47);
USE_FIXED_TOPOLOGY = logical(scenarioParams(48));
NUM_OBJ_COMPONENTS = int32(scenarioParams(39));
OBJECTIVE_POS_FLAT = scenarioParams(40:43); % [x1,y1,x2,y2]; zero-padded if N=1
OBJECTIVE_VAR_FLAT = scenarioParams(44:51); % [v11,v12,v21,v22 per component]
SENSOR_PERFORMANCE_MINIMUM = scenarioParams(52);
USE_DOUBLE_INTEGRATOR = logical(scenarioParams(53));
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);
dy = gridY - OBJECTIVE_GROUND_POS(2);
% 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));
objValues = zeros(size(gridX));
for kk = 1:NUM_OBJ_COMPONENTS
pos_k = OBJECTIVE_POS_FLAT((kk-1)*2+1 : (kk-1)*2+2);
var_k = reshape(OBJECTIVE_VAR_FLAT((kk-1)*4+1 : (kk-1)*4+4), 2, 2);
dx = gridX - pos_k(1);
dy = gridY - pos_k(2);
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
+46 -19
View File
@@ -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)
// 40-43: objectiveVar (2x2 col-major: v11, v12, v21, v22)
// 44 : sensorPerformanceMinimum (CSV column 18)
// 45 : useDoubleIntegrator (CSV column 23; 0=single-integrator, 1=double-integrator)
// 46 : dampingCoeff (CSV column 24)
// 47 : useFixedTopology (CSV column 25; 0=dynamic lesser-neighbor, 1=fixed)
// 38 : numObjectiveComponents (1 or 2; inferred from objectivePos field length)
// 39-42: objectivePos flat [x1,y1,x2,y2] (4 slots; zero-padded if N=1)
// 43-50: objectiveVar flat [v11,v12,v21,v22 per component] (8 slots; zero-padded if N=1)
// 51 : sensorPerformanceMinimum (CSV column 18)
// 52 : useDoubleIntegrator (CSV column 23)
// 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) {
fprintf(stderr, "loadScenario: failed to parse objectivePos: %s\n", t);
double posVals[4] = {0, 0, 0, 0};
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) {
fprintf(stderr, "loadScenario: failed to parse objectiveVar: %s\n", t);
int nObj = (int)params[38];
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",
params[32], params[33], params[34], params[35], params[36], params[37]);
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], (int)params[38]);
return 1;
}
aerpaw/impl/controller_impl.h
+8 -7
View File
@@ -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
// 40-43 objectiveVar (2x2 col-major)
// 44 sensorPerformanceMinimum
// 45 useDoubleIntegrator (0=single-integrator, 1=double-integrator)
// 46 dampingCoeff
// 47 useFixedTopology (0=dynamic lesser-neighbor, 1=fixed)
#define NUM_SCENARIO_PARAMS 48
// 38 numObjectiveComponents (1 or 2; inferred from objectivePos field length)
// 39-42 objectivePos flat [x1,y1,x2,y2] (4 slots; zero-padded if N=1)
// 43-50 objectiveVar flat [v11,v12,v21,v22 per component] (8 slots; zero-padded if N=1)
// 51 sensorPerformanceMinimum
// 52 useDoubleIntegrator (0=single-integrator, 1=double-integrator)
// 53 dampingCoeff
// 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