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

codegen fixes, bug fixes, gets running on testbed environment

Browse Source
This commit is contained in:
Kevin Dee
2026-02-24 19:05:54 -08:00
parent 58d87cd16f
commit 1ada914384
38 changed files with 1732 additions and 263 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
@agent/agent.m
View File

@@ -17,8 +17,8 @@ classdef agent
fovGeometry;
% Communication
commsGeometry = spherical;
lesserNeighbors = [];
commsGeometry;
lesserNeighbors = zeros(1, 0);
% Performance
performance = 0;
@@ -34,6 +34,17 @@ classdef agent
end
methods (Access = public)
function obj = agent()
arguments (Input)
end
arguments (Output)
obj (1, 1) agent
end
obj.collisionGeometry = spherical;
obj.sensorModel = sigmoidSensor;
obj.fovGeometry = cone;
obj.commsGeometry = spherical;
end
[obj] = initialize(obj, pos, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
[obj] = run(obj, domain, partitioning, t, index, agents);
[partitioning] = partition(obj, agents, objective)

4
@agent/initialize.m
View File

@@ -23,7 +23,9 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
obj.stepDecayRate = obj.initialStepSize / maxIter;
% Initialize performance vector
obj.performance = [0, NaN(1, maxIter), 0];
if coder.target('MATLAB')
obj.performance = [0, NaN(1, maxIter), 0];
end
% Add spherical geometry based on com range
obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label));

48
@agent/partition.m
View File

@@ -7,29 +7,33 @@ function [partitioning] = partition(obj, agents, objective)
arguments (Output)
partitioning (:, :) double;
end
% Assess sensing performance of each agent at each sample point
% in the domain
agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, [objective.X(:), objective.Y(:), zeros(size(objective.X(:)))]), size(objective.X)), agents, "UniformOutput", false);
agentPerformances{end + 1} = objective.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
agentPerformances = cat(3, agentPerformances{:});
% Get highest performance value at each point
[~, idx] = max(agentPerformances, [], 3);
nAgents = size(agents, 1);
gridM = size(objective.X, 1);
gridN = size(objective.X, 2);
nPoints = gridM * gridN;
% Collect agent indices in the same way as performance
indices = 1:size(agents, 1);
agentInds = squeeze(tensorprod(indices, ones(size(objective.X))));
if size(agentInds, 1) ~= size(agents, 1)
agentInds = reshape(agentInds, [size(agents, 1), size(agentInds)]); % needed for cases with 1 agent where prior squeeze is too agressive
% Assess sensing performance of each agent at each sample point.
% agentPerf is (nPoints x nAgents+1): the extra column is the
% minimum threshold that must be exceeded for any assignment.
agentPerf = zeros(nPoints, nAgents + 1);
for aa = 1:nAgents
p = agents{aa}.sensorModel.sensorPerformance(agents{aa}.pos, ...
[objective.X(:), objective.Y(:), zeros(nPoints, 1)]);
agentPerf(:, aa) = p(:);
end
agentInds = num2cell(agentInds, 2:3);
agentInds = cellfun(@(x) squeeze(x), agentInds, "UniformOutput", false);
agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
agentInds = cat(3, agentInds{:});
agentPerf(:, nAgents + 1) = objective.sensorPerformanceMinimum;
% Use highest performing agent's index to form partitions
[m, n, ~] = size(agentInds);
[jj, kk] = ndgrid(1:m, 1:n);
partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx));
end
% Find which agent has highest performance at each point.
% If the threshold column wins (idx == nAgents+1) the point is unassigned (0).
[~, idx] = max(agentPerf, [], 2);
assignedAgent = zeros(nPoints, 1);
for pp = 1:nPoints
if idx(pp) <= nAgents
assignedAgent(pp) = idx(pp);
end
end
partitioning = reshape(assignedAgent, gridM, gridN);
end

23
@agent/run.m
View File

@@ -13,7 +13,7 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
% Collect objective function values across partition
partitionMask = partitioning == index;
if ~unique(partitionMask)
if ~any(partitionMask(:))
% This agent has no partition, maintain current state
return;
end
@@ -32,10 +32,10 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
pos = obj.pos + delta * deltaApplicator(ii, 1:3);
% Compute performance values on partition
if ii < 5
if ii < 6
% Compute sensing performance
sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
% Objective performance does not change for 0, +/- X, Y steps.
% Objective performance does not change for 0, +/- X, +/- Y steps.
% Those values are computed once before the loop and are only
% recomputed when +/- Z steps are applied
else
@@ -64,17 +64,26 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
end
% Store agent performance at current time and place
obj.performance(timestepIndex + 1) = C_delta(1);
if coder.target('MATLAB')
obj.performance(timestepIndex + 1) = C_delta(1);
end
% Compute gradient by finite central differences
gradC = [(C_delta(2)-C_delta(3))/(2*delta), (C_delta(4)-C_delta(5))/(2*delta), (C_delta(6)-C_delta(7))/(2*delta)];
% Compute scaling factor
targetRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
rateFactor = targetRate / norm(gradC);
gradNorm = norm(gradC);
% Compute unconstrained next position
pNext = obj.pos + rateFactor * gradC;
% Compute unconstrained next position.
% Guard against near-zero gradient: when sensor performance is saturated
% or near-zero across the whole partition, rateFactor -> Inf and pNext
% explodes. Stay put instead.
if gradNorm < 1e-10
pNext = obj.pos;
else
pNext = obj.pos + (targetRate / gradNorm) * gradC;
end
% Move to next position
obj.lastPos = obj.pos;

138
@miSim/constrainMotion.m
View File

@@ -6,138 +6,152 @@ function [obj] = constrainMotion(obj)
obj (1, 1) {mustBeA(obj, "miSim")};
end
if size(obj.agents, 1) < 2
nAgents = size(obj.agents, 1);
if nAgents < 2
nAAPairs = 0;
else
nAAPairs = nchoosek(size(obj.agents, 1), 2); % unique agent/agent pairs
nAAPairs = nchoosek(nAgents, 2); % unique agent/agent pairs
end
agents = [obj.agents{:}];
v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
if all(isnan(v), "all") || all(v == zeros(size(obj.agents, 1), 3), "all")
% Compute velocity matrix from unconstrained gradient-ascent step
v = zeros(nAgents, 3);
for ii = 1:nAgents
v(ii, :) = (obj.agents{ii}.pos - obj.agents{ii}.lastPos) ./ obj.timestep;
end
if all(isnan(v), "all") || all(v == zeros(nAgents, 3), "all")
% Agents are not attempting to move, so there is no motion to be
% constrained
return;
end
% Initialize QP based on number of agents and obstacles
nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
nADPairs = size(obj.agents, 1) * 5; % agents x (4 walls + 1 ceiling)
nLNAPairs = sum(obj.constraintAdjacencyMatrix, "all") - size(obj.agents, 1);
nAOPairs = nAgents * size(obj.obstacles, 1); % unique agent/obstacle pairs
nADPairs = nAgents * 6; % agents x (4 walls + 1 floor + 1 ceiling)
nLNAPairs = sum(obj.constraintAdjacencyMatrix, "all") - nAgents;
total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
kk = 1;
A = zeros(total, 3 * size(obj.agents, 1));
A = zeros(total, 3 * nAgents);
b = zeros(total, 1);
% Set up collision avoidance constraints
h = NaN(size(obj.agents, 1));
h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
for ii = 1:(size(obj.agents, 1) - 1)
for jj = (ii + 1):size(obj.agents, 1)
h(ii, jj) = norm(agents(ii).pos - agents(jj).pos)^2 - (agents(ii).collisionGeometry.radius + agents(jj).collisionGeometry.radius)^2;
h = NaN(nAgents, nAgents);
h(logical(eye(nAgents))) = 0; % self value is 0
for ii = 1:(nAgents - 1)
for jj = (ii + 1):nAgents
h(ii, jj) = norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2;
h(jj, ii) = h(ii, jj);
A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - agents(jj).pos);
A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
b(kk) = obj.barrierGain * h(ii, jj)^obj.barrierExponent;
b(kk) = obj.barrierGain * max(0, h(ii, jj))^obj.barrierExponent;
kk = kk + 1;
end
end
hObs = NaN(size(obj.agents, 1), size(obj.obstacles, 1));
hObs = NaN(nAgents, size(obj.obstacles, 1));
% Set up obstacle avoidance constraints
for ii = 1:size(obj.agents, 1)
for ii = 1:nAgents
for jj = 1:size(obj.obstacles, 1)
% find closest position to agent on/in obstacle
cPos = obj.obstacles{jj}.closestToPoint(agents(ii).pos);
cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.pos);
hObs(ii, jj) = dot(agents(ii).pos - cPos, agents(ii).pos - cPos) - agents(ii).collisionGeometry.radius^2;
hObs(ii, jj) = dot(obj.agents{ii}.pos - cPos, obj.agents{ii}.pos - cPos) - obj.agents{ii}.collisionGeometry.radius^2;
A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - cPos);
b(kk) = obj.barrierGain * max(0, hObs(ii, jj))^obj.barrierExponent;
A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - cPos);
b(kk) = obj.barrierGain * hObs(ii, jj)^obj.barrierExponent;
kk = kk + 1;
end
end
% Set up domain constraints (walls and ceiling only)
% Floor constraint is implicit with an obstacle corresponding to the
% minimum allowed altitude, but I included it anyways
for ii = 1:size(obj.agents, 1)
h_xMin = 0.0; h_xMax = 0.0; h_yMin = 0.0; h_yMax = 0.0; h_zMin = 0.0; h_zMax = 0.0;
for ii = 1:nAgents
% X minimum
h_xMin = (agents(ii).pos(1) - obj.domain.minCorner(1)) - agents(ii).collisionGeometry.radius;
h_xMin = (obj.agents{ii}.pos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0];
b(kk) = obj.barrierGain * h_xMin^obj.barrierExponent;
b(kk) = obj.barrierGain * max(0, h_xMin)^obj.barrierExponent;
kk = kk + 1;
% X maximum
h_xMax = (obj.domain.maxCorner(1) - agents(ii).pos(1)) - agents(ii).collisionGeometry.radius;
h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.pos(1)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0];
b(kk) = obj.barrierGain * h_xMax^obj.barrierExponent;
b(kk) = obj.barrierGain * max(0, h_xMax)^obj.barrierExponent;
kk = kk + 1;
% Y minimum
h_yMin = (agents(ii).pos(2) - obj.domain.minCorner(2)) - agents(ii).collisionGeometry.radius;
h_yMin = (obj.agents{ii}.pos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0];
b(kk) = obj.barrierGain * h_yMin^obj.barrierExponent;
b(kk) = obj.barrierGain * max(0, h_yMin)^obj.barrierExponent;
kk = kk + 1;
% Y maximum
h_yMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius;
h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.pos(2)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0];
b(kk) = obj.barrierGain * h_yMax^obj.barrierExponent;
b(kk) = obj.barrierGain * max(0, h_yMax)^obj.barrierExponent;
kk = kk + 1;
% Z minimum
h_zMin = (agents(ii).pos(3) - obj.domain.minCorner(3)) - agents(ii).collisionGeometry.radius;
h_zMin = (obj.agents{ii}.pos(3) - obj.domain.minCorner(3)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1];
b(kk) = obj.barrierGain * h_zMin^obj.barrierExponent;
b(kk) = obj.barrierGain * max(0, h_zMin)^obj.barrierExponent;
kk = kk + 1;
% Z maximum
h_zMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius;
h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.pos(3)) - obj.agents{ii}.collisionGeometry.radius;
A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1];
b(kk) = obj.barrierGain * h_zMax^obj.barrierExponent;
b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent;
kk = kk + 1;
end
% Save off h function values (ignoring network constraints which may evolve in time)
obj.h(:, obj.timestepIndex) = [h(triu(true(size(obj.agents, 1)), 1)); reshape(hObs, [], 1); h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax;];
if coder.target('MATLAB')
% Save off h function values (logging only — not needed in compiled mode)
obj.h(:, obj.timestepIndex) = [h(triu(true(nAgents), 1)); reshape(hObs, [], 1); h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax;];
end
% Add communication network constraints
hComms = NaN(size(obj.agents, 1));
hComms(logical(eye(size(obj.agents, 1)))) = 0;
for ii = 1:(size(obj.agents, 1) - 1)
for jj = (ii + 1):size(obj.agents, 1)
hComms = NaN(nAgents, nAgents);
hComms(logical(eye(nAgents))) = 0;
for ii = 1:(nAgents - 1)
for jj = (ii + 1):nAgents
if obj.constraintAdjacencyMatrix(ii, jj)
hComms(ii, jj) = min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])^2 - norm(agents(ii).pos - agents(jj).pos)^2;
hComms(ii, jj) = min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])^2 - norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2;
A(kk, (3 * ii - 2):(3 * ii)) = 2 * (agents(ii).pos - agents(jj).pos);
A(kk, (3 * ii - 2):(3 * ii)) = 2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
b(kk) = obj.barrierGain * hComms(ii, jj)^obj.barrierExponent;
b(kk) = obj.barrierGain * max(0, hComms(ii, jj))^obj.barrierExponent;
kk = kk + 1;
kk = kk + 1;
end
end
end
% Solve QP program generated earlier
vhat = reshape(v', 3 * size(obj.agents, 1), 1);
H = 2 * eye(3 * size(obj.agents, 1));
vhat = reshape(v', 3 * nAgents, 1);
H = 2 * eye(3 * nAgents);
f = -2 * vhat;
% Update solution based on constraints
assert(size(A,2) == size(H,1))
assert(size(A,1) == size(b,1))
assert(size(H,1) == length(f))
if coder.target('MATLAB')
assert(size(A,2) == size(H,1))
assert(size(A,1) == size(b,1))
assert(size(H,1) == length(f))
end
opt = optimoptions("quadprog", "Display", "off", "Algorithm", "active-set", "UseCodegenSolver", true);
x0 = zeros(size(H, 1), 1);
[vNew, ~, exitflag, m] = quadprog(H, double(f), A, b, [], [], [], [], x0, opt);
assert(exitflag == 1, sprintf("quadprog failure... %s%s", newline, m.message));
vNew = reshape(vNew, 3, size(obj.agents, 1))';
if coder.target('MATLAB')
assert(exitflag == 1, sprintf("quadprog failure... %s%s", newline, m.message));
end
vNew = reshape(vNew, 3, nAgents)';
if exitflag <= 0
warning("QP failed, continuing with unconstrained solution...")
if coder.target('MATLAB')
warning("QP failed, continuing with unconstrained solution...")
end
vNew = v;
end

60
@miSim/initialize.m
View File

@@ -20,14 +20,20 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
obj.makePlots = makePlots;
if ~obj.makePlots
if makeVideo
warning("makeVideo set to true, but makePlots set to false. Setting makeVideo to false.");
if coder.target('MATLAB')
warning("makeVideo set to true, but makePlots set to false. Setting makeVideo to false.");
end
makeVideo = false;
end
end
obj.makeVideo = makeVideo;
% Generate artifact(s) name
obj.artifactName = strcat(string(datetime("now"), "yyyy_MM_dd_HH_mm_ss"));
if coder.target('MATLAB')
obj.artifactName = strcat(string(datetime("now"), "yyyy_MM_dd_HH_mm_ss"));
else
obj.artifactName = ""; % Generate no artifacts from simulation in codegen
end
% Define simulation time parameters
obj.timestep = timestep;
@@ -37,14 +43,24 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
% Define domain
obj.domain = domain;
% Add geometries representing obstacles within the domain
obj.obstacles = obstacles;
% Add geometries representing obstacles within the domain, pre-allocating
% one extra slot for the minimum altitude floor obstacle if needed
numInputObs = size(obstacles, 1);
if minAlt > 0
obj.obstacles = repmat({rectangularPrism}, numInputObs + 1, 1);
else
obj.obstacles = repmat({rectangularPrism}, numInputObs, 1);
end
for kk = 1:numInputObs
obj.obstacles{kk} = obstacles{kk};
end
% Add an additional obstacle spanning the domain's footprint to
% Add an additional obstacle spanning the domain's footprint to
% represent the minimum allowable altitude
if minAlt > 0
obj.obstacles{end + 1, 1} = rectangularPrism;
obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
minAltObstacle = rectangularPrism;
minAltObstacle = minAltObstacle.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
obj.obstacles{numInputObs + 1} = minAltObstacle;
end
% Define agents
@@ -56,12 +72,12 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
for ii = 1:size(obj.agents, 1)
% Agent
if isempty(char(obj.agents{ii}.label))
obj.agents{ii}.label = sprintf("Agent %d", ii);
obj.agents{ii}.label = sprintf("Agent %d", int8(ii));
end
% Collision geometry
if isempty(char(obj.agents{ii}.collisionGeometry.label))
obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", ii);
obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", int8(ii));
end
end
@@ -76,22 +92,26 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
% Set up times to iterate over
obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
% Prepare performance data store (at t = 0, all have 0 performance)
obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
if coder.target('MATLAB')
% Prepare performance data store (at t = 0, all have 0 performance)
obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
% Prepare h function data store
obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1));
% Prepare h function data store
obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1));
end
% Create initial partitioning
obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
% Initialize variable that will store agent positions for trail plots
obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3);
obj.posHist(1:size(obj.agents, 1), 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);
if coder.target('MATLAB')
% Initialize variable that will store agent positions for trail plots
obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3);
obj.posHist(1:size(obj.agents, 1), 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);
% Set up plots showing initialized state
obj = obj.plot();
% Set up plots showing initialized state
obj = obj.plot();
% Run validations
obj.validate();
% Run validations
obj.validate();
end
end

70
@miSim/lesserNeighbor.m
View File

@@ -9,42 +9,48 @@ function obj = lesserNeighbor(obj)
% initialize solution with self-connections only
constraintAdjacencyMatrix = logical(eye(size(obj.agents, 1)));
for ii = 1:size(obj.agents, 1)
nAgents = size(obj.agents, 1);
for ii = 1:nAgents
% Find lesser neighbors of each agent
% Lesser neighbors of ii are jj < ii in range of ii
lesserNeighbors = [];
lnBuf = zeros(1, nAgents);
lnCount = 0;
for jj = 1:(ii - 1)
if obj.adjacency(ii, jj)
lesserNeighbors = [lesserNeighbors, jj];
lnCount = lnCount + 1;
lnBuf(lnCount) = jj;
end
end
obj.agents{ii}.lesserNeighbors = lesserNeighbors;
obj.agents{ii}.lesserNeighbors = lnBuf(1:lnCount);
% Early exit for isolated agents
if isempty(obj.agents{ii}.lesserNeighbors)
if lnCount == 0
continue
end
% Focus on subgraph defined by lesser neighbors
subgraphAdjacency = obj.adjacency(obj.agents{ii}.lesserNeighbors, obj.agents{ii}.lesserNeighbors);
% Find connected components in each agent's subgraph
% TODO: rewrite this using matlab "conncomp" function?
visited = false(size(subgraphAdjacency, 1), 1);
components = {};
for jj = 1:size(subgraphAdjacency, 1)
% Find connected components; store only the max global index per
% component (the only value needed below) to avoid a cell array
visited = false(1, lnCount);
maxInComponent = zeros(1, lnCount);
numComponents = 0;
for jj = 1:lnCount
if ~visited(jj)
reachable = bfs(subgraphAdjacency, jj);
visited(reachable) = true;
components{end+1} = obj.agents{ii}.lesserNeighbors(reachable);
numComponents = numComponents + 1;
maxInComponent(numComponents) = max(obj.agents{ii}.lesserNeighbors(reachable));
end
end
% Connect to the greatest index in each connected component in the
% lesser neighborhood of this agent
for jj = 1:size(components, 2)
constraintAdjacencyMatrix(ii, max(components{jj})) = true;
constraintAdjacencyMatrix(max(components{jj}), ii) = true;
for jj = 1:numComponents
maxIdx = maxInComponent(jj);
constraintAdjacencyMatrix(ii, maxIdx) = true;
constraintAdjacencyMatrix(maxIdx, ii) = true;
end
end
obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix | constraintAdjacencyMatrix';
@@ -53,24 +59,34 @@ end
function cComp = bfs(subgraphAdjacency, startIdx)
n = size(subgraphAdjacency, 1);
visited = false(1, n);
queue = startIdx;
cComp = startIdx;
% Pre-allocated queue and component buffer with head/tail pointers
% to avoid element deletion and dynamic array growth
queue = zeros(1, n);
cCompBuf = zeros(1, n);
qHead = 1;
qTail = 2;
queue(1) = startIdx;
cCompBuf(1) = startIdx;
cSize = 1;
visited(startIdx) = true;
while ~isempty(queue)
current = queue(1);
queue(1) = [];
while qHead < qTail
current = queue(qHead);
qHead = qHead + 1;
% Find all neighbors of current node in the subgraph
neighbors = find(subgraphAdjacency(current, :));
for neighbor = neighbors
for kk = 1:numel(neighbors)
neighbor = neighbors(kk);
if ~visited(neighbor)
visited(neighbor) = true;
cComp = [cComp, neighbor];
queue = [queue, neighbor];
cCompBuf(cSize + 1) = neighbor;
cSize = cSize + 1;
queue(qTail) = neighbor;
qTail = qTail + 1;
end
end
end
cComp = sort(cComp);
cComp = sort(cCompBuf(1:cSize));
end

25
@miSim/miSim.m
View File

@@ -2,17 +2,17 @@ classdef miSim
% multiagent interconnection simulation
% Simulation parameters
properties (SetAccess = private, GetAccess = public)
properties (SetAccess = public, GetAccess = public)
timestep = NaN; % delta time interval for simulation iterations
timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
maxIter = NaN; % maximum number of simulation iterations
domain = rectangularPrism;
objective = sensingObjective;
obstacles = cell(0, 1); % geometries that define obstacles within the domain
agents = cell(0, 1); % agents that move within the domain
adjacency = NaN; % Adjacency matrix representing communications network graph
constraintAdjacencyMatrix = NaN; % Adjacency matrix representing desired lesser neighbor connections
partitioning = NaN;
domain;
objective;
obstacles; % geometries that define obstacles within the domain
agents; % agents that move within the domain
adjacency = false(0, 0); % Adjacency matrix representing communications network graph
constraintAdjacencyMatrix = false(0, 0); % Adjacency matrix representing desired lesser neighbor connections
partitioning = zeros(0, 0);
perf; % sensor performance timeseries array
performance = 0; % simulation performance timeseries vector
barrierGain = NaN; % CBF gain parameter
@@ -54,6 +54,15 @@ classdef miSim
end
methods (Access = public)
function obj = miSim()
arguments (Output)
obj (1, 1) miSim
end
obj.domain = rectangularPrism;
obj.objective = sensingObjective;
obj.obstacles = {rectangularPrism};
obj.agents = {agent};
end
[obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo);
[obj] = run(obj);
[obj] = lesserNeighbor(obj);

56
@miSim/run.m
View File

@@ -6,21 +6,24 @@ function [obj] = run(obj)
obj (1, 1) {mustBeA(obj, "miSim")};
end
% Start video writer
if obj.makeVideo
v = obj.setupVideoWriter();
v.open();
if coder.target('MATLAB')
% Start video writer
if obj.makeVideo
v = obj.setupVideoWriter();
v.open();
end
end
for ii = 1:size(obj.times, 1)
% Display current sim time
obj.t = obj.times(ii);
obj.timestepIndex = ii;
fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
if coder.target('MATLAB')
fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
% Before moving
% Validate current simulation configuration
obj.validate();
% Validate current simulation configuration
obj.validate();
end
% Update partitioning before moving (this one is strictly for
% plotting purposes, the real partitioning is done by the agents)
@@ -39,28 +42,31 @@ function [obj] = run(obj)
% CBF constraints solved by QP
obj = constrainMotion(obj);
% After moving
% Update agent position history array
obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);
if coder.target('MATLAB')
% Update agent position history array
obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3);
% Update total performance
obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))];
% Update total performance
obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))];
% Update adjacency matrix
obj = obj.updateAdjacency();
% Update adjacency matrix
obj = obj.updateAdjacency();
% Update plots
obj = obj.updatePlots();
% Update plots
obj = obj.updatePlots();
% Write frame in to video
if obj.makeVideo
I = getframe(obj.f);
v.writeVideo(I);
% Write frame in to video
if obj.makeVideo
I = getframe(obj.f);
v.writeVideo(I);
end
end
end
if obj.makeVideo
% Close video file
v.close();
if coder.target('MATLAB')
if obj.makeVideo
% Close video file
v.close();
end
end
end

3
@sensingObjective/initialize.m
View File

@@ -30,8 +30,7 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
[obj.X, obj.Y] = meshgrid(xGrid, yGrid);
% Evaluate function over grid points
obj.objectiveFunction = objectiveFunction;
obj.values = reshape(obj.objectiveFunction(obj.X, obj.Y), size(obj.X));
obj.values = reshape(objectiveFunction(obj.X, obj.Y), size(obj.X));
% Normalize
obj.values = obj.values ./ max(obj.values, [], "all");

42
@sensingObjective/initializeWithValues.m Normal file
View File

@@ -0,0 +1,42 @@
function obj = initializeWithValues(obj, values, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
arguments (Input)
obj (1,1) {mustBeA(obj, "sensingObjective")};
values (:,:) double;
domain (1, 1) {mustBeGeometry};
discretizationStep (1, 1) double = 1;
protectedRange (1, 1) double = 1;
sensorPerformanceMinimum (1, 1) double = 1e-6;
end
arguments (Output)
obj (1,1) {mustBeA(obj, "sensingObjective")};
end
obj.discretizationStep = discretizationStep;
obj.sensorPerformanceMinimum = sensorPerformanceMinimum;
obj.protectedRange = protectedRange;
% Extract footprint limits
xMin = min(domain.footprint(:, 1));
xMax = max(domain.footprint(:, 1));
yMin = min(domain.footprint(:, 2));
yMax = max(domain.footprint(:, 2));
xGrid = unique([xMin:obj.discretizationStep:xMax, xMax]);
yGrid = unique([yMin:obj.discretizationStep:yMax, yMax]);
% Store grid points
[obj.X, obj.Y] = meshgrid(xGrid, yGrid);
% Use pre-computed values (caller must evaluate on same grid)
obj.values = reshape(values, size(obj.X));
% Normalize
obj.values = obj.values ./ max(obj.values, [], "all");
% Store ground position (peak of objective)
idx = obj.values == 1;
obj.groundPos = [obj.X(idx), obj.Y(idx)];
obj.groundPos = obj.groundPos(1, 1:2);
assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
end

4
@sensingObjective/sensingObjective.m
View File

@@ -4,7 +4,6 @@ classdef sensingObjective
label = "";
groundPos = [NaN, NaN];
discretizationStep = NaN;
objectiveFunction = @(x, y) NaN; % define objective functions over a grid in this manner
X = [];
Y = [];
values = [];
@@ -14,7 +13,8 @@ classdef sensingObjective
methods (Access = public)
[obj] = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum);
[obj] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep, protectedRange);
[obj] = initializeWithValues(obj, values, domain, discretizationStep, protectedRange, sensorPerformanceMinimum);
[obj] = initializeRandomMvnpdf(obj, domain, discretizationStep, protectedRange);
[f ] = plot(obj, ind, f);
end
end

7
aerpaw/MESSAGE_TYPE.m
View File

@@ -3,7 +3,10 @@ classdef MESSAGE_TYPE < uint8
TARGET (1) % Server->Client: target coordinates follow (3 doubles)
ACK (2) % Client->Server: command received
READY (3) % Both: ready for next command / mission complete
RTL (4) % Server->Client: return to launch
LAND (5) % Server->Client: land now
RTL (4) % Server->Client: return to launch
LAND (5) % Server->Client: land now
GUIDANCE_TOGGLE (6) % Server->Client: toggle guidance mode on/off
REQUEST_POSITION (7) % Server->Client: respond with current ENU position
POSITION (8) % Client->Server: current ENU position (3 doubles)
end
end

77
aerpaw/client/uav_runner.py
View File

@@ -36,11 +36,14 @@ from aerpawlib.vehicle import Drone
# Message types - must match MESSAGE_TYPE.m enum
class MessageType(IntEnum):
TARGET = 1
ACK = 2
READY = 3
RTL = 4
LAND = 5
TARGET = 1
ACK = 2
READY = 3
RTL = 4
LAND = 5
GUIDANCE_TOGGLE = 6
REQUEST_POSITION = 7
POSITION = 8
AERPAW_DIR = Path(__file__).parent.parent
@@ -177,6 +180,7 @@ class UAVRunner(BasicRunner):
return
log_task = None
nav_task = None
try:
# Takeoff to above AERPAW minimum altitude
print("[UAV] Taking off...")
@@ -186,32 +190,64 @@ class UAVRunner(BasicRunner):
# Start GPS logging in background
log_task = asyncio.create_task(_gps_log_loop(drone))
# Command loop - handle TARGET, RTL, LAND, READY from controller
# Command loop - handle all messages from controller
waypoint_num = 0
in_guidance = False
while True:
msg_type = await recv_message_type(reader)
print(f"[UAV] Received: {msg_type.name}")
if msg_type == MessageType.TARGET:
if msg_type == MessageType.GUIDANCE_TOGGLE:
in_guidance = not in_guidance
print(f"[UAV] Guidance mode: {'ON' if in_guidance else 'OFF'}")
if not in_guidance:
# Exiting guidance: wait for current navigation to finish
# before resuming sequential (ACK/READY) mode
if nav_task and not nav_task.done():
print("[UAV] Waiting for current navigation to complete...")
await nav_task
nav_task = None
# Acknowledge that we are ready for sequential commands
await send_message_type(writer, MessageType.ACK)
print("[UAV] Sent ACK (guidance mode exited, ready for sequential commands)")
elif msg_type == MessageType.REQUEST_POSITION:
# Respond immediately with current ENU position relative to origin
pos = drone.position
enu = pos - self.origin # VectorNED(north, east, down)
await send_message_type(writer, MessageType.POSITION)
writer.write(struct.pack('<ddd', enu.east, enu.north, -enu.down))
await writer.drain()
print(f"[UAV] Sent POSITION: E={enu.east:.1f} N={enu.north:.1f} U={-enu.down:.1f}")
elif msg_type == MessageType.TARGET:
# Read 24 bytes of coordinates (3 little-endian doubles)
data = await recv_exactly(reader, 24)
enu_x, enu_y, enu_z = struct.unpack('<ddd', data)
waypoint_num += 1
print(f"[UAV] TARGET (waypoint {waypoint_num}): x={enu_x}, y={enu_y}, z={enu_z}")
# Convert ENU to lat/lon (ENU: x=East, y=North, z=Up)
target = self.origin + VectorNED(north=enu_y, east=enu_x, down=-enu_z)
print(f"[UAV] Target coord: {target.lat:.6f}, {target.lon:.6f}, {target.alt:.1f}")
await send_message_type(writer, MessageType.ACK)
print(f"[UAV] Sent ACK")
print(f"[UAV] Moving to waypoint {waypoint_num}...")
await drone.goto_coordinates(target)
print(f"[UAV] Arrived at waypoint {waypoint_num}")
await send_message_type(writer, MessageType.READY)
print(f"[UAV] Sent READY")
if in_guidance:
# Guidance mode: non-blocking — cancel previous nav and start new
print(f"[UAV] Guidance TARGET: E={enu_x:.1f} N={enu_y:.1f} U={enu_z:.1f}")
if nav_task and not nav_task.done():
nav_task.cancel()
await asyncio.gather(nav_task, return_exceptions=True)
nav_task = asyncio.create_task(drone.goto_coordinates(target))
# No ACK/READY in guidance mode
else:
# Sequential mode: ACK → navigate → READY
waypoint_num += 1
print(f"[UAV] TARGET (waypoint {waypoint_num}): x={enu_x:.1f}, y={enu_y:.1f}, z={enu_z:.1f}")
print(f"[UAV] Target coord: {target.lat:.6f}, {target.lon:.6f}, {target.alt:.1f}")
await send_message_type(writer, MessageType.ACK)
print("[UAV] Sent ACK")
print(f"[UAV] Moving to waypoint {waypoint_num}...")
await drone.goto_coordinates(target)
print(f"[UAV] Arrived at waypoint {waypoint_num}")
await send_message_type(writer, MessageType.READY)
print("[UAV] Sent READY")
elif msg_type == MessageType.RTL:
await send_message_type(writer, MessageType.ACK)
@@ -246,6 +282,9 @@ class UAVRunner(BasicRunner):
print(f"[UAV] Error: {e}")
finally:
if nav_task is not None and not nav_task.done():
nav_task.cancel()
await asyncio.gather(nav_task, return_exceptions=True)
if log_task is not None:
log_task.cancel()
await asyncio.gather(log_task, return_exceptions=True)

3
aerpaw/compile.sh
View File

@@ -8,6 +8,8 @@ BUILD="$AERPAW_DIR/build"
mkdir -p "$BUILD"
echo "Compiling controller executable..."
# Compile all codegen sources (handles any new generated files)
g++ -I/home/kdee/matlab/R2025a/extern/include \
-I"$CODEGEN" \
@@ -16,6 +18,7 @@ g++ -I/home/kdee/matlab/R2025a/extern/include \
"$IMPL/controller_impl.cpp" \
"$CODEGEN"/*.cpp \
-o "$BUILD/controller_app" \
-fopenmp \
-lpthread
echo "Built: $BUILD/controller_app"

16
aerpaw/config/server.yaml
View File

@@ -17,15 +17,7 @@
# Max distance from origin: ~22m (all waypoints within geofence)
#
targets:
# UAV 1: straight line along x-axis at y=5, alt=45m
- [0.0, 5.0, 45.0]
- [4.0, 5.0, 45.0]
- [8.0, 5.0, 45.0]
- [12.0, 5.0, 45.0]
- [16.0, 5.0, 45.0]
# UAV 2: triangular path diverging/converging, alt=30m
- [0.0, -5.0, 30.0]
- [4.0, -15.0, 30.0]
- [8.0, -20.0, 30.0]
- [12.0, -15.0, 30.0]
- [16.0, -5.0, 30.0]
# UAV 1 Initial Position — west side of guidance domain
- [5.0, 10.0, 45.0]
# UAV 2 Initial Position — east side of guidance domain
- [15.0, 10.0, 30.0]

938
aerpaw/controller.coderprj
View File

File diff suppressed because it is too large Load Diff

64
aerpaw/controller.m
View File

@@ -83,7 +83,69 @@ for w = 1:numWaypoints
end
end
% Wait for user input before closing experiment
% ---- Phase 2: miSim guidance loop ----------------------------------------
% Guidance parameters (adjust here and recompile as needed)
MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations
GUIDANCE_RATE_MS = int32(5000); % ms between iterations (0.2 Hz default)
% Wait for user input to start guidance loop
if coder.target('MATLAB')
input('Press Enter to start guidance loop: ', 's');
else
coder.ceval('waitForUserInput');
end
% Enter guidance mode on all clients
if ~coder.target('MATLAB')
coder.ceval('sendGuidanceToggle', int32(numClients));
end
% Request initial GPS positions and initialise guidance algorithm
positions = zeros(MAX_CLIENTS, 3);
if ~coder.target('MATLAB')
coder.ceval('sendRequestPositions', int32(numClients));
coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS));
end
guidance_step(positions(1:numClients, :), true);
% Main guidance loop
for step = 1:MAX_GUIDANCE_STEPS
% Query current GPS positions from all clients
if ~coder.target('MATLAB')
coder.ceval('sendRequestPositions', int32(numClients));
coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS));
end
% Run one guidance step: feed GPS positions in, get targets out
nextPositions = guidance_step(positions(1:numClients, :), false);
% Send target to each client (no ACK/READY expected in guidance mode)
for i = 1:numClients
target = nextPositions(i, :);
if ~coder.target('MATLAB')
coder.ceval('sendTarget', int32(i), coder.ref(target));
else
disp(['[guidance] target UAV ', num2str(i), ': ', num2str(target)]);
end
end
% Wait for the guidance rate interval before the next iteration
if ~coder.target('MATLAB')
coder.ceval('sleepMs', int32(GUIDANCE_RATE_MS));
end
end
% Exit guidance mode on all clients (second toggle)
if ~coder.target('MATLAB')
coder.ceval('sendGuidanceToggle', int32(numClients));
% Wait for ACK from all clients: confirms each client has finished its
% last guidance navigation and is back in sequential (ACK/READY) mode.
coder.ceval('waitForAllMessageType', int32(numClients), ...
int32(MESSAGE_TYPE.ACK));
end
% --------------------------------------------------------------------------
% Wait for user input before closing experiment (RTL + LAND)
if coder.target('MATLAB')
input('Press Enter to close experiment (RTL + LAND): ', 's');
else

179
aerpaw/guidance_step.m Normal file
View 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

78
aerpaw/impl/controller_impl.cpp
View File

@@ -9,6 +9,7 @@
#include <sys/select.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <time.h>
#define SERVER_PORT 5000
#define SERVER_IP "127.0.0.1"
@@ -120,6 +121,9 @@ static const char* messageTypeName(uint8_t msgType) {
case 3: return "READY";
case 4: return "RTL";
case 5: return "LAND";
case 6: return "GUIDANCE_TOGGLE";
case 7: return "REQUEST_POSITION";
case 8: return "POSITION";
default: return "UNKNOWN";
}
}
@@ -190,7 +194,17 @@ int waitForAllMessageType(int numClients, int expectedType) {
if (!completed[i] && FD_ISSET(clientSockets[i], &readfds)) {
uint8_t msgType;
int len = recv(clientSockets[i], &msgType, 1, 0);
if (len != 1) return 0;
if (len == 0) {
std::cerr << "waitForAllMessageType: client " << (i + 1)
<< " disconnected while waiting for "
<< messageTypeName(expected) << "\n";
return 0;
}
if (len < 0) {
std::cerr << "waitForAllMessageType: recv error for client " << (i + 1)
<< " while waiting for " << messageTypeName(expected) << "\n";
return 0;
}
std::cout << "Received " << messageTypeName(msgType) << " from client " << (i + 1) << "\n";
@@ -207,6 +221,66 @@ int waitForAllMessageType(int numClients, int expectedType) {
// Wait for user to press Enter
void waitForUserInput() {
std::cout << "Press Enter to close experiment (RTL + LAND)...\n";
std::cout << "Press Enter to continue...\n";
std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
}
// Broadcast GUIDANCE_TOGGLE to all clients
void sendGuidanceToggle(int numClients) {
for (int i = 1; i <= numClients; i++) {
sendMessageType(i, 6); // GUIDANCE_TOGGLE = 6
}
}
// Send REQUEST_POSITION to all clients
int sendRequestPositions(int numClients) {
for (int i = 1; i <= numClients; i++) {
if (!sendMessageType(i, 7)) return 0; // REQUEST_POSITION = 7
}
return 1;
}
// Receive POSITION response (1 byte type + 24 bytes ENU) from all clients.
// Stores results in column-major order: positions[client + 0*maxClients] = x (east),
// positions[client + 1*maxClients] = y (north),
// positions[client + 2*maxClients] = z (up)
int recvPositions(int numClients, double* positions, int maxClients) {
if (numClients <= 0 || numClients > (int)clientSockets.size()) return 0;
for (int i = 0; i < numClients; i++) {
// Expect: POSITION byte (1) + 3 doubles (24)
uint8_t msgType;
if (recv(clientSockets[i], &msgType, 1, MSG_WAITALL) != 1) {
std::cerr << "recvPositions: failed to read msg type from client " << (i + 1) << "\n";
return 0;
}
if (msgType != 8) { // POSITION = 8
std::cerr << "recvPositions: expected POSITION(8), got " << (int)msgType
<< " from client " << (i + 1) << "\n";
return 0;
}
double coords[3];
if (recv(clientSockets[i], coords, sizeof(coords), MSG_WAITALL) != (ssize_t)sizeof(coords)) {
std::cerr << "recvPositions: failed to read coords from client " << (i + 1) << "\n";
return 0;
}
// Store column-major (MATLAB layout): col 0 = east, col 1 = north, col 2 = up
positions[i + 0 * maxClients] = coords[0]; // east (x)
positions[i + 1 * maxClients] = coords[1]; // north (y)
positions[i + 2 * maxClients] = coords[2]; // up (z)
std::cout << "Position from client " << (i + 1) << ": "
<< coords[0] << "," << coords[1] << "," << coords[2] << "\n";
}
return 1;
}
// Sleep for the given number of milliseconds
void sleepMs(int ms) {
struct timespec ts;
ts.tv_sec = ms / 1000;
ts.tv_nsec = (ms % 1000) * 1000000L;
nanosleep(&ts, nullptr);
}

6
aerpaw/impl/controller_impl.h
View File

@@ -23,6 +23,12 @@ int sendMessageType(int clientId, int msgType);
int sendTarget(int clientId, const double* coords);
int waitForAllMessageType(int numClients, int expectedType);
// Guidance loop operations
void sendGuidanceToggle(int numClients);
int sendRequestPositions(int numClients);
int recvPositions(int numClients, double* positions, int maxClients); // column-major maxClients x 3
void sleepMs(int ms);
#ifdef __cplusplus
}
#endif

9
aerpaw/results/plotGpsCsvs.m
View File

@@ -6,9 +6,10 @@ c = ["r", "g", "b", "m", "c", "y", "k"]; % plotting colors
seaToGroundLevel = 110; % meters, measured approximately from USGS national map viewer
% Paths to logs
gpsCsvs = ["GPS_DATA_0c8d904aa159_2026-02-16_13:26:33.csv"; ...
"GPS_DATA_8e4f52dac04d_2026-02-16_13:26:33.csv"; ...
];
gpsCsvs = fullfile ("sandbox", "test1", ...
["GPS_DATA_0c8d904aa159_2026-02-24_21:33:25.csv"; ...
"GPS_DATA_8e4f52dac04d_2026-02-24_21:33:25.csv"; ...
]);
G = cell(size(gpsCsvs));
for ii = 1:size(gpsCsvs, 1)
@@ -16,7 +17,7 @@ for ii = 1:size(gpsCsvs, 1)
G{ii} = readGpsCsv(gpsCsvs(ii));
% Plot recorded trajectory
geoplot3(gf, G{ii}.Latitude, G{ii}.Longitude, G{ii}.Altitude + seaToGroundLevel, c(mod(ii, length(c))), 'LineWidth', 3, "MarkerSize", 5);
geoplot3(gf, G{ii}.Latitude, G{ii}.Longitude, G{ii}.Altitude + seaToGroundLevel, c(mod(ii, length(c))), 'LineWidth', 2, "MarkerSize", 5);
end
hold(gf, "off");

2
geometries/@cone/plot.m
View File

@@ -31,7 +31,7 @@ function [obj, f] = plot(obj, ind, f, maxAlt)
o = surf(f.CurrentAxes, X, Y, Z);
else
hold(f.Children(1).Children(ind(1)), "on");
o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(obj.tag.color, 1, 1, 3), "FaceAlpha", 0.25, "EdgeColor", "none");
o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(regionTypeColor(obj.tag), 1, 1, 3), "FaceAlpha", 0.25, "EdgeColor", "none");
hold(f.Children(1).Children(ind(1)), "off");
end

4
geometries/@rectangularPrism/plotWireframe.m
View File

@@ -19,10 +19,10 @@ function [obj, f] = plotWireframe(obj, ind, f)
% Plot the boundaries of the geometry into 3D view
if isnan(ind)
o = plot3(f.CurrentAxes, X, Y, Z, "-", "Color", obj.tag.color, "LineWidth", 2);
o = plot3(f.CurrentAxes, X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 2);
else
hold(f.Children(1).Children(ind(1)), "on");
o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, "-", "Color", obj.tag.color, "LineWidth", 2);
o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 2);
hold(f.Children(1).Children(ind(1)), "off");
end

6
geometries/@rectangularPrism/rectangularPrism.m
View File

@@ -28,6 +28,12 @@ classdef rectangularPrism
end
methods (Access = public)
function obj = rectangularPrism()
arguments (Output)
obj (1, 1) rectangularPrism
end
obj.objective = sensingObjective;
end
[obj ] = initialize(obj, bounds, tag, label, objectiveFunction, discretizationStep);
[obj ] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain);
[r ] = random(obj);

4
geometries/@spherical/plotWireframe.m
View File

@@ -25,10 +25,10 @@ function [obj, f] = plotWireframe(obj, ind, f)
% Plot the boundaries of the geometry into 3D view
if isnan(ind)
o = plot3(f.CurrentAxes, X, Y, Z, "-", "Color", obj.tag.color, "LineWidth", 2);
o = plot3(f.CurrentAxes, X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 2);
else
hold(f.Children(1).Children(ind(1)), "on");
o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, "-", "Color", obj.tag.color, "LineWidth", 1);
o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 1);
hold(f.Children(1).Children(ind(1)), "off");
end

12
geometries/@spherical/spherical.m
View File

@@ -2,12 +2,12 @@ classdef spherical
% Rectangular prism geometry
properties (SetAccess = private, GetAccess = public)
% Spatial
center = NaN;
center = NaN(1, 3);
radius = NaN;
diameter = NaN;
vertices; % fake vertices
edges; % fake edges
vertices = NaN(6, 3); % fake vertices
edges = NaN(8, 2); % fake edges
% Plotting
lines;
@@ -22,6 +22,12 @@ classdef spherical
end
methods (Access = public)
function obj = spherical()
arguments (Output)
obj (1, 1) spherical
end
obj.objective = sensingObjective;
end
[obj ] = initialize(obj, center, radius, tag, label);
[r ] = random(obj);
[c ] = contains(obj, pos);

24
geometries/REGION_TYPE.m
View File

@@ -1,20 +1,10 @@
classdef REGION_TYPE < uint8
properties
id
color
end
enumeration
INVALID (0, [255, 127, 255]); % default value
DOMAIN (1, [0, 0, 0]); % domain region
OBSTACLE (2, [255, 127, 127]); % obstacle region
COLLISION (3, [255, 255, 128]); % collision avoidance region
FOV (4, [255, 165, 0]); % field of view region
COMMS (5, [0, 255, 0]); % comunications region
INVALID (0) % default value
DOMAIN (1) % domain region
OBSTACLE (2) % obstacle region
COLLISION (3) % collision avoidance region
FOV (4) % field of view region
COMMS (5) % comunications region
end
methods
function obj = REGION_TYPE(id, color)
obj.id = id;
obj.color = color./ 255;
end
end
end
end

24
geometries/regionTypeColor.m Normal file
View File

@@ -0,0 +1,24 @@
function color = regionTypeColor(tag)
% regionTypeColor Return the RGB color (0-1 range) for a REGION_TYPE value.
arguments (Input)
tag (1, 1) REGION_TYPE
end
arguments (Output)
color (1, 3) double
end
switch tag
case REGION_TYPE.DOMAIN
color = [0, 0, 0] / 255;
case REGION_TYPE.OBSTACLE
color = [255, 127, 127] / 255;
case REGION_TYPE.COLLISION
color = [255, 255, 128] / 255;
case REGION_TYPE.FOV
color = [255, 165, 0] / 255;
case REGION_TYPE.COMMS
color = [0, 255, 0] / 255;
otherwise % INVALID
color = [255, 127, 255] / 255;
end
end

6
resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/LiklXLmEZberq-e28LLA86kEkNMd.xml Normal file
View File

@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info>
<Category UUID="FileClassCategory">
<Label UUID="design"/>
</Category>
</Info>

2
resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/LiklXLmEZberq-e28LLA86kEkNMp.xml Normal file
View File

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

6
resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/M5zOD2wqz2AjI0NiSW1HbszatFMd.xml Normal file
View File

@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info>
<Category UUID="FileClassCategory">
<Label UUID="design"/>
</Category>
</Info>

2
resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/M5zOD2wqz2AjI0NiSW1HbszatFMp.xml Normal file
View File

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

6
resources/project/qFRcMsci5bTUnTBevgtb2pITyQU/l6p0lRpsuYLVyH-ULL9p4PIRTu4d.xml Normal file
View File

@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info>
<Category UUID="FileClassCategory">
<Label UUID="design"/>
</Category>
</Info>

2
resources/project/qFRcMsci5bTUnTBevgtb2pITyQU/l6p0lRpsuYLVyH-ULL9p4PIRTu4p.xml Normal file
View File

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

12
test/test_miSim.m
View File

@@ -207,6 +207,12 @@ classdef test_miSim < matlab.unittest.TestCase
break;
end
end
% Make sure candidate clears domain floor
if newAgent.pos(3) - newAgent.collisionGeometry.radius <= tc.minAlt
violation = true;
end
if violation
continue;
end
@@ -340,6 +346,12 @@ classdef test_miSim < matlab.unittest.TestCase
break;
end
end
% Make sure candidate clears domain floor
if newAgent.pos(3) - newAgent.collisionGeometry.radius <= tc.minAlt
violation = true;
end
if violation
continue;
end

9
util/validators/arguments/mustBeGeometry.m
View File

@@ -1,10 +1,11 @@
function mustBeGeometry(geometry)
validGeometries = ["rectangularPrism"; "spherical"];
if isa(geometry, "cell")
if isa(geometry, 'cell')
for ii = 1:size(geometry, 1)
assert(any(arrayfun(@(x) isa(geometry{ii}, x), validGeometries)), "Geometry in index %d is not a valid geometry class", ii);
assert(isa(geometry{ii}, 'rectangularPrism') || isa(geometry{ii}, 'spherical'), ...
'Geometry in index %d is not a valid geometry class', ii);
end
else
assert(any(arrayfun(@(x) isa(geometry, x), validGeometries)), "Geometry is not a valid geometry class");
assert(isa(geometry, 'rectangularPrism') || isa(geometry, 'spherical'), ...
'Geometry is not a valid geometry class');
end
end

9
util/validators/arguments/mustBeSensor.m
View File

@@ -1,10 +1,11 @@
function mustBeSensor(sensorModel)
validSensorModels = ["fixedCardinalSensor"; "sigmoidSensor";];
if isa(sensorModel, "cell")
if isa(sensorModel, 'cell')
for ii = 1:size(sensorModel, 1)
assert(any(arrayfun(@(x) isa(sensorModel{ii}, x), validSensorModels)), "Sensor in index %d is not a valid sensor class", ii);
assert(isa(sensorModel{ii}, 'sigmoidSensor'), ...
'Sensor in index %d is not a valid sensor class', ii);
end
else
assert(any(arrayfun(@(x) isa(sensorModel, x), validSensorModels)), "Sensor is not a valid sensor class");
assert(isa(sensorModel, 'sigmoidSensor'), ...
'Sensor is not a valid sensor class');
end
end