codegen fixes, bug fixes, gets running on testbed environment
This commit is contained in:
179
aerpaw/guidance_step.m
Normal file
179
aerpaw/guidance_step.m
Normal file
@@ -0,0 +1,179 @@
|
||||
function nextPositions = guidance_step(currentPositions, isInit)
|
||||
% guidance_step One step of the miSim sensing coverage guidance algorithm.
|
||||
%
|
||||
% Wraps the miSim gradient-ascent + CBF motion algorithm for AERPAW.
|
||||
% Holds full simulation state in a persistent variable between calls.
|
||||
%
|
||||
% Usage (from controller.m):
|
||||
% guidance_step(initPositions, true) % first call: initialise state
|
||||
% nextPos = guidance_step(gpsPos, false) % subsequent calls: run one step
|
||||
%
|
||||
% Inputs:
|
||||
% currentPositions (MAX_CLIENTS × 3) double ENU [east north up] metres
|
||||
% isInit (1,1) logical true on first call only
|
||||
%
|
||||
% Output:
|
||||
% nextPositions (MAX_CLIENTS × 3) double guidance targets, ENU metres
|
||||
%
|
||||
% Codegen notes:
|
||||
% - Persistent variable 'sim' holds the miSim object between calls.
|
||||
% - Plotting/video are disabled (makePlots=false, makeVideo=false) for
|
||||
% deployment. The coder.target('MATLAB') guards in the miSim/agent
|
||||
% class files must be in place before codegen will succeed.
|
||||
% - objectiveFunctionWrapper returns a function handle which is not
|
||||
% directly codegen-compatible; the MATLAB path uses it normally. The
|
||||
% compiled path requires an equivalent C impl (see TODO below).
|
||||
|
||||
coder.extrinsic('disp', 'objectiveFunctionWrapper');
|
||||
|
||||
% =========================================================================
|
||||
% Tunable guidance parameters — adjust here and recompile as needed.
|
||||
% =========================================================================
|
||||
MAX_CLIENTS = int32(4); % must match MAX_CLIENTS in controller.m
|
||||
|
||||
% Domain bounds in ENU metres [east, north, up]
|
||||
DOMAIN_MIN = [ 0.0, 0.0, 25.0];
|
||||
DOMAIN_MAX = [ 20.0, 20.0, 55.0];
|
||||
MIN_ALT = 25.0; % hard altitude floor (m)
|
||||
|
||||
% Sensing objective: bivariate Gaussian centred at [east, north]
|
||||
OBJECTIVE_GROUND_POS = [10.0, 10.0];
|
||||
DISCRETIZATION_STEP = 0.1; % objective grid step (m) — coarser = faster
|
||||
PROTECTED_RANGE = 1.0; % objective centre must be >this from domain edge
|
||||
|
||||
% Agent safety geometry
|
||||
COLLISION_RADIUS = 3.0; % spherical collision radius (m)
|
||||
COMMS_RANGE = 60.0; % communications range (m)
|
||||
|
||||
% Gradient-ascent parameters
|
||||
INITIAL_STEP_SIZE = 1; % step size at iteration 0 (decays to 0 at MAX_ITER)
|
||||
MAX_ITER = 100; % guidance steps (sets decay rate)
|
||||
|
||||
% Sensor model (sigmoidSensor)
|
||||
ALPHA_DIST = 60.0; % effective sensing distance (m) — set beyond max domain slant range (~53 m)
|
||||
BETA_DIST = 0.2; % distance sigmoid steepness — gentle, tilt drives the coverage gradient
|
||||
ALPHA_TILT = 10.0; % max useful tilt angle (degrees)
|
||||
BETA_TILT = 1.0; % tilt sigmoid steepness
|
||||
|
||||
% Safety filter — Control Barrier Function (CBF/QP)
|
||||
BARRIER_GAIN = 100;
|
||||
BARRIER_EXPONENT = 3;
|
||||
|
||||
% Simulation timestep (s) — should match GUIDANCE_RATE_MS / 1000 in controller.m
|
||||
TIMESTEP = 5.0;
|
||||
% =========================================================================
|
||||
|
||||
persistent sim;
|
||||
if isempty(sim)
|
||||
sim = miSim;
|
||||
end
|
||||
|
||||
% Pre-allocate output with known static size (required for codegen)
|
||||
nextPositions = zeros(MAX_CLIENTS, 3);
|
||||
|
||||
numAgents = int32(size(currentPositions, 1));
|
||||
|
||||
if isInit
|
||||
if coder.target('MATLAB')
|
||||
disp('[guidance_step] Initialising simulation...');
|
||||
end
|
||||
|
||||
% --- Build domain geometry ---
|
||||
dom = rectangularPrism;
|
||||
dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain");
|
||||
|
||||
% --- Build sensing objective ---
|
||||
dom.objective = sensingObjective;
|
||||
if coder.target('MATLAB')
|
||||
% objectiveFunctionWrapper returns a function handle — MATLAB only.
|
||||
objFcn = objectiveFunctionWrapper(OBJECTIVE_GROUND_POS, 3*eye(2));
|
||||
dom.objective = dom.objective.initialize(objFcn, dom, ...
|
||||
DISCRETIZATION_STEP, PROTECTED_RANGE);
|
||||
else
|
||||
% Evaluate bivariate Gaussian inline (codegen-compatible; no function handle).
|
||||
% Must build the same grid that initializeWithValues uses internally.
|
||||
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);
|
||||
objValues = exp(-0.5 * (dx .* dx + dy .* dy));
|
||||
dom.objective = dom.objective.initializeWithValues(objValues, dom, ...
|
||||
DISCRETIZATION_STEP, PROTECTED_RANGE);
|
||||
end
|
||||
|
||||
% --- Build shared sensor model ---
|
||||
sensor = sigmoidSensor;
|
||||
sensor = sensor.initialize(ALPHA_DIST, BETA_DIST, ALPHA_TILT, BETA_TILT);
|
||||
|
||||
% --- Initialise agents from GPS positions ---
|
||||
agentList = cell(numAgents, 1);
|
||||
for ii = 1:numAgents
|
||||
pos = currentPositions(ii, :);
|
||||
geom = spherical;
|
||||
geom = geom.initialize(pos, COLLISION_RADIUS, REGION_TYPE.COLLISION, ...
|
||||
sprintf("UAV %d Collision", ii));
|
||||
ag = agent;
|
||||
ag = ag.initialize(pos, geom, sensor, COMMS_RANGE, MAX_ITER, ...
|
||||
INITIAL_STEP_SIZE, sprintf("UAV %d", ii));
|
||||
agentList{ii} = ag;
|
||||
end
|
||||
|
||||
% --- Initialise simulation (plots and video disabled) ---
|
||||
sim = miSim;
|
||||
sim = sim.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ...
|
||||
MIN_ALT, TIMESTEP, MAX_ITER, cell(0, 1), false, false);
|
||||
|
||||
if coder.target('MATLAB')
|
||||
disp('[guidance_step] Initialisation complete.');
|
||||
end
|
||||
|
||||
% On the init call return current positions unchanged
|
||||
for ii = 1:numAgents
|
||||
nextPositions(ii, :) = currentPositions(ii, :);
|
||||
end
|
||||
|
||||
else
|
||||
% =====================================================================
|
||||
% One guidance step
|
||||
% =====================================================================
|
||||
|
||||
% 1. Inject actual GPS positions (closed-loop feedback)
|
||||
for ii = 1:size(sim.agents, 1)
|
||||
sim.agents{ii}.lastPos = sim.agents{ii}.pos;
|
||||
sim.agents{ii}.pos = currentPositions(ii, :);
|
||||
|
||||
% Re-centre collision geometry at new position
|
||||
d = currentPositions(ii, :) - sim.agents{ii}.collisionGeometry.center;
|
||||
sim.agents{ii}.collisionGeometry = sim.agents{ii}.collisionGeometry.initialize( ...
|
||||
sim.agents{ii}.collisionGeometry.center + d, ...
|
||||
sim.agents{ii}.collisionGeometry.radius, ...
|
||||
REGION_TYPE.COLLISION);
|
||||
end
|
||||
|
||||
% 2. Advance timestep counter
|
||||
sim.timestepIndex = sim.timestepIndex + 1;
|
||||
|
||||
% 3. Update communications topology (Lesser Neighbour Assignment)
|
||||
sim = sim.lesserNeighbor();
|
||||
|
||||
% 4. Compute Voronoi partitioning
|
||||
sim.partitioning = sim.agents{1}.partition(sim.agents, sim.domain.objective);
|
||||
|
||||
% 5. Unconstrained gradient-ascent step for each agent
|
||||
for ii = 1:size(sim.agents, 1)
|
||||
sim.agents{ii} = sim.agents{ii}.run(sim.domain, sim.partitioning, ...
|
||||
sim.timestepIndex, ii, sim.agents);
|
||||
end
|
||||
|
||||
% 6. Apply CBF safety filter (collision / comms / domain constraints via QP)
|
||||
sim = sim.constrainMotion();
|
||||
|
||||
% 7. Return constrained next positions
|
||||
for ii = 1:size(sim.agents, 1)
|
||||
nextPositions(ii, :) = sim.agents{ii}.pos;
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
Reference in New Issue
Block a user