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

scenario edits

Browse Source
This commit is contained in:
Kevin Dee
2026-03-02 18:18:40 -08:00
parent 813b124c47
commit 05ac8a6e97
12 changed files with 70 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
aerpaw/results/plotGpsCsvs.m
View File

@@ -2,7 +2,9 @@
f = uifigure;
gf = geoglobe(f);
hold(gf, "on");
c = ["r", "g", "b", "m", "c", "y", "k"]; % plotting colors
c = ["g", "b", "m", "c"]; % plotting colors
% coordinate system constants
seaToGroundLevel = 110; % meters, measured approximately from USGS national map viewer
lla0 = [35.72550610629396, -78.70019657805574, seaToGroundLevel]; % origin (LLA)
@@ -14,28 +16,27 @@ for ii = 1:size(gpsCsvs, 1)
% Read CSV
G{ii} = readGpsCsv(fullfile(gpsCsvs(ii).folder, gpsCsvs(ii).name));
% Find when algorithm begins/ends (using ENU altitude)
% Find when algorithm begins/ends (using ENU altitude rate change)
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');
% Automatically detect start/stop of algorithm flight (ignore takeoff, setup, return to liftoff, landing segments of flight)
startIdx = find(verticalSpeed <= prctile(verticalSpeed, 25), 1, 'first'); % 25 pct threshold may need adjusting
stopIdx = find(verticalSpeed <= prctile(verticalSpeed, 25), 1, 'last');
% % Plot whole flight, including setup/cleanup
% startIdx = 1;
% stopIdx = length(verticalSpeed);
speed = vecnorm(diff(enuTraj), 2, 2);
meanSpeed = movmean(speed, [10, 0]);
% Plot recorded trajectory
% 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);
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);
geoplot3(gf, [llaObj(1), llaObj(1)], [llaObj(2), llaObj(2)], [llaObj(3), llaObj(3) + 50], 'LineWidth', 3, "Color", 'y');
% Plot domain
altOffset = 1; % to avoid clipping into the ground when displayed
@@ -47,4 +48,9 @@ geoplot3(gf, [domain(2, 1), domain(2, 1)], [domain(1, 2), domain(1, 2)], domain(
geoplot3(gf, [domain(1, 1), domain(1, 1)], [domain(2, 2), domain(2, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
geoplot3(gf, [domain(2, 1), domain(2, 1)], [domain(2, 2), domain(2, 2)], domain(:, 3) + altOffset, 'LineWidth', 3, 'Color', 'k');
% Plot floor (minimum altitude constraint)
floorAlt = 30;
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');
% finish
hold(gf, "off");