diff --git a/@agent/run.m b/@agent/run.m index 3df8627..4cb7e76 100644 --- a/@agent/run.m +++ b/@agent/run.m @@ -79,7 +79,7 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents) % 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 + if gradNorm < 1e-100 pNext = obj.pos; else pNext = obj.pos + (targetRate / gradNorm) * gradC; diff --git a/aerpaw/config/client.yaml b/aerpaw/config/client.yaml index eb3a7b0..517dd2b 100644 --- a/aerpaw/config/client.yaml +++ b/aerpaw/config/client.yaml @@ -2,8 +2,8 @@ # ENU coordinate system origin (AERPAW Lake Wheeler Road Field) origin: - lat: 35.727436 - lon: -78.696471 + lat: 35.72550610629396 + lon: -78.70019657805574 alt: 0.0 # Alt=0 means ENU z directly becomes target altitude above home # Environment-specific settings environments: diff --git a/aerpaw/config/scenario.csv b/aerpaw/config/scenario.csv index d42583b..a345dde 100644 --- a/aerpaw/config/scenario.csv +++ b/aerpaw/config/scenario.csv @@ -1,2 +1,2 @@ timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax -5, 100, 25.0, 0.1, 1.0, 1.0, 100, 3, 3.0, 60.0, 60.0, 0.2, 10.0, 1.0, "0.0, 0.0, 0.0", "20.0, 20.0, 55.0", "10.0, 10.0", 1e-6, "5.0, 10.0, 45.0, 15.0, 10.0, 30.0", 0, "", "" \ No newline at end of file +5, 100, 25.0, 0.1, 1.0, 1.0, 100, 3, 3.0, 30.0, 60.0, 0.2, 10.0, 1.0, "0.0, 0.0, 0.0", "50.0, 50.0, 100.0", "35.0, 35.0", 1e-6, "5.0, 10.0, 45.0, 15.0, 10.0, 30.0", 0, "", "" \ No newline at end of file diff --git a/aerpaw/controller.coderprj b/aerpaw/controller.coderprj index 73eb287..20409eb 100644 --- a/aerpaw/controller.coderprj +++ b/aerpaw/controller.coderprj @@ -93,6 +93,16 @@ + + int32 + + + + + int32 + + + @@ -1060,7 +1070,7 @@ true - 2026-02-27T17:26:39 + 2026-03-02T11:41:10 diff --git a/aerpaw/results/plotGpsCsvs.m b/aerpaw/results/plotGpsCsvs.m index 11c1de7..8b2bc22 100644 --- a/aerpaw/results/plotGpsCsvs.m +++ b/aerpaw/results/plotGpsCsvs.m @@ -4,11 +4,12 @@ gf = geoglobe(f); hold(gf, "on"); c = ["r", "g", "b", "m", "c", "y", "k"]; % plotting colors seaToGroundLevel = 110; % meters, measured approximately from USGS national map viewer +lla0 = [35.72550610629396, -78.70019657805574, seaToGroundLevel]; % origin (LLA) % Paths to logs -gpsCsvs = fullfile ("sandbox", "test2", ... - ["GPS_DATA_0c8d904aa159_2026-02-27_20:52:33.csv"; ... - "GPS_DATA_8e4f52dac04d_2026-02-27_20:52:33.csv"; ... +gpsCsvs = fullfile ("sandbox", "test4", ... + ["GPS_DATA_0c8d904aa159_2026-03-02_14:42:51.csv"; ... + "GPS_DATA_8e4f52dac04d_2026-03-02_14:42:52.csv"; ... ]); G = cell(size(gpsCsvs)); @@ -16,8 +17,31 @@ for ii = 1:size(gpsCsvs, 1) % Read CSV G{ii} = readGpsCsv(gpsCsvs(ii)); + % Find when algorithm begins/ends (using ENU altitude) + enuTraj = lla2enu([G{ii}.Latitude, G{ii}.Longitude, G{ii}.Altitude], lla0, 'flat'); + + verticalSpeed = movmean(abs(diff(G{ii}.Altitude)), [10, 0]); + + startIdx = find(verticalSpeed <= prctile(verticalSpeed, 25), 1, 'first'); + stopIdx = find(verticalSpeed <= prctile(verticalSpeed, 10), 1, 'last'); + % startIdx = 1; + % stopIdx = length(verticalSpeed); + + speed = vecnorm(diff(enuTraj), 2, 2); + meanSpeed = movmean(speed, [10, 0]); + % Plot recorded trajectory - geoplot3(gf, G{ii}.Latitude, G{ii}.Longitude, G{ii}.Altitude + 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 + +% Plot objective +objectivePos = [35, 35, 0]; +llaObj = enu2lla(objectivePos, lla0, 'flat'); +geoplot3(gf, [llaObj(1), llaObj(1)], [llaObj(2), llaObj(2)], [llaObj(3), llaObj(3) + 50], 'LineWidth', 3); + +% Plot domain +domain = [lla0; enu2lla([50, 50, 100], lla0, 'flat')]; +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) + 10, 1, 5), 'LineWidth', 3); + hold(gf, "off"); \ No newline at end of file diff --git a/resources/project/6V6Vr7QAsqxEmRj-ezOm1ygcTak/bJiTrGQ5GQ9DMI6whRMInBQY2yQd.xml b/resources/project/6V6Vr7QAsqxEmRj-ezOm1ygcTak/bJiTrGQ5GQ9DMI6whRMInBQY2yQd.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/6V6Vr7QAsqxEmRj-ezOm1ygcTak/bJiTrGQ5GQ9DMI6whRMInBQY2yQd.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/6V6Vr7QAsqxEmRj-ezOm1ygcTak/bJiTrGQ5GQ9DMI6whRMInBQY2yQp.xml b/resources/project/6V6Vr7QAsqxEmRj-ezOm1ygcTak/bJiTrGQ5GQ9DMI6whRMInBQY2yQp.xml new file mode 100644 index 0000000..01cb34e --- /dev/null +++ b/resources/project/6V6Vr7QAsqxEmRj-ezOm1ygcTak/bJiTrGQ5GQ9DMI6whRMInBQY2yQp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/TTsh0h9cBRUx2c5O2kJyFwG-XEQd.xml b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/TTsh0h9cBRUx2c5O2kJyFwG-XEQd.xml new file mode 100644 index 0000000..7152233 --- /dev/null +++ b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/TTsh0h9cBRUx2c5O2kJyFwG-XEQd.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/TTsh0h9cBRUx2c5O2kJyFwG-XEQp.xml b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/TTsh0h9cBRUx2c5O2kJyFwG-XEQp.xml new file mode 100644 index 0000000..6115a45 --- /dev/null +++ b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/TTsh0h9cBRUx2c5O2kJyFwG-XEQp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/pjpApCZ_IyThRbNNVtyf0HS9lGA/OvzpqD8IaJL2rl4SaQSOPQ-QRMkd.xml b/resources/project/pjpApCZ_IyThRbNNVtyf0HS9lGA/OvzpqD8IaJL2rl4SaQSOPQ-QRMkd.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/pjpApCZ_IyThRbNNVtyf0HS9lGA/OvzpqD8IaJL2rl4SaQSOPQ-QRMkd.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/pjpApCZ_IyThRbNNVtyf0HS9lGA/OvzpqD8IaJL2rl4SaQSOPQ-QRMkp.xml b/resources/project/pjpApCZ_IyThRbNNVtyf0HS9lGA/OvzpqD8IaJL2rl4SaQSOPQ-QRMkp.xml new file mode 100644 index 0000000..01cb34e --- /dev/null +++ b/resources/project/pjpApCZ_IyThRbNNVtyf0HS9lGA/OvzpqD8IaJL2rl4SaQSOPQ-QRMkp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/6V6Vr7QAsqxEmRj-ezOm1ygcTakd.xml b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/6V6Vr7QAsqxEmRj-ezOm1ygcTakd.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/6V6Vr7QAsqxEmRj-ezOm1ygcTakd.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/6V6Vr7QAsqxEmRj-ezOm1ygcTakp.xml b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/6V6Vr7QAsqxEmRj-ezOm1ygcTakp.xml new file mode 100644 index 0000000..7d3ed5e --- /dev/null +++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/6V6Vr7QAsqxEmRj-ezOm1ygcTakp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/pjpApCZ_IyThRbNNVtyf0HS9lGAd.xml b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/pjpApCZ_IyThRbNNVtyf0HS9lGAd.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/pjpApCZ_IyThRbNNVtyf0HS9lGAd.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/pjpApCZ_IyThRbNNVtyf0HS9lGAp.xml b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/pjpApCZ_IyThRbNNVtyf0HS9lGAp.xml new file mode 100644 index 0000000..0ee6de0 --- /dev/null +++ b/resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/pjpApCZ_IyThRbNNVtyf0HS9lGAp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/test/parametricTestSuite.m b/test/parametricTestSuite.m index b396112..1610a4f 100644 --- a/test/parametricTestSuite.m +++ b/test/parametricTestSuite.m @@ -27,7 +27,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase methods (Test) % Test cases - function test_scenario(tc) + function test_AERPAW_scenario(tc) % Load scenario definition tc.csvPath = fullfile(matlab.project.rootProject().RootFolder, "aerpaw", "config", "scenario.csv"); params = tc.testClass.readScenarioCsv(tc.csvPath);