diff --git a/.gitignore b/.gitignore index be5838d..2e18f34 100644 --- a/.gitignore +++ b/.gitignore @@ -40,3 +40,4 @@ octave-workspace *.dat *.zip *.TMP +*.fig diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..c16f524 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,80 @@ +# Changelog + +All notable changes to this project will be documented in this file. + +The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), +and this project should adhere to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). + +## [Unreleased] + +### Added + +### Changed + +### Fixed + +### Removed + +## [2.0.0] - 2021-10-01 + +### Added + +- New default RUN script of `RUN_2_geospatial_uncor` +- `CreateTradespacePairingGeo` is a helper function crete a tradespace of different combinations of input variables to `createEncounters_2` +- `trackTimetable2NEU` converts tracks in timetable format to an array corresponding to time, north, east, and up coordinates +- `adjustTrack` will adjust tracks based on output of `samplespeedalt` +- `estimateSpeedFromGeodetic` is a helper function to estimate speed based on latitude and longitude +- `preallocAnchors`, `createISO31662Grid`, and `parseGeoTrajDirectory` helper functions for step one +- Persistent system environment variable of `AEM_DIR_GEOPAIR` established + +### Changed + +- Inputs to `findPairs_1` changed to take advantage of calling `preallocAnchors` within `findPairs_1`. The step one RUN script was updated accordingly. +- `createEncounters_2` no longer reads in pre-generated files of tracks generated from the Bayesian encounter models; instead encounter model based tracks are generated dynamically using the new `UncorEncounterModel` class added to `em-model-manned-bayes` +- `createEncounters_2` will try multiple times to create an encounter with aircraft 1 and will now resample an encounter model to generate new Bayes tracks if needed. The previous version gave up too easily. +- `createEncounters_2` no longer organized loops by clusters and all clustering has been removed +- `createEncounters_2` loads FAA digital obstacle file using `gridDOF` from `em-core` +- `createEncounters_2` stores tracks as a `timetable`, a type of table that associates a time with each row. Helper functions like `findconflict`, `samplespeedalt`, and `findCropIdx` have been updated accordingly +- `createEncounters_2` outputs a table containing encounter metadata +- `createEncounters_2` and `loadTrack` will enforce the altitude range (altRange1_ft_agl, altRange2_ft_agl) when sampling the Bayes tracks. For Bayes tracks, altitude still cannot resampled using `samplespeedalt` and `adjustTrack`. +- `findconflict` now considers HMD, VMD, and time criteria. It previously only assessed HMD. +- `findCropIdx` also now checks for initial horizontal and vertical separation criteria +- Renamed `neu_to_wp_struct` to to `neu2Waypoints` +- Renamed `startup` to `startup_pairing_geo` +- DEM handling updated based on improvements to `msl2agl` from `em-core` +- Better random seed control +- Improved plotting +- Improved organization through the use of the code directory + +### Fixed + +- `samplespeedalt` handles rare case where the altitude span is less than 25 resulting in `adjustSpan_ft_agl` being empty +- Fixed bug in `findConflict` when calculating `hmd_ft` and `vmd_ft` but tracks are not near each other +- Various functions renamed using camelCase + +### Removed + +- `croptracks`, `calcLegsTime`, `neu2wpstruct`, `updatewaypointstruct` removed due to the addition of `trackTimetable2NEU` and updates to `findconflict` and `findCropIdx` +- `interpTime` removed because time based interpolation is handled using the built-in functionality of the `timetable` type +- `RUN_2_sUAS_uncor` and `RUN_2_sUAS_unconv` removed because they were deprecated by the more generalized `RUN_2_geospatial_uncor` script +- `RUN_2_sUAS_HAA` removed due to change of sampling Bayesian networks at runtime. The Bayesian helicopter air ambulance model hasn't been tested yet with the `EncounterModel` objects in em-model-manned-bayes. This functionality maybe reintroduced in a future release. + +## [1.1.0] - 2020-08-10 + +### Added + +- SPDX headers + +### Changed + +- Improving handling of Bayes tracks from em-model-manned-bayes + +### Fixed + +- Addressed bug `placeTrack` where tracks were not correctly rotated + +## [1.0.0] - 2020-06-24 + +### Added + +- Initial public release diff --git a/LICENSE b/LICENSE index d08a606..f5fcc64 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ BSD 2-Clause License -Copyright (c) 2019,2020 Massachusetts Institute of Technology +Copyright (c) 2019-2021 Massachusetts Institute of Technology All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/PLOT_1.m b/PLOT_1.m deleted file mode 100644 index 7de8ab7..0000000 --- a/PLOT_1.m +++ /dev/null @@ -1,70 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -%% Inputs -% Parameters from RUN_1.m -iso_3166_2 = {'US-CA','US-KS','US-MA','US-MS','US-NC','US-NV','US-NY','US-TX','US-VA'}; -usecase = 'all-noshield'; -anchorRange_nm = 1.25; % Distance pairs need to be away from anchor point 30*(150 / 3600) = 1.25...closing speed of 150 knots for 30 seconds - -% Edges to discretize anchors.num_geospatial -edges = [0,1,2,4,6,inf]; - -% Color order -colorOrder = [204 121 167;... % Reddish purple - 230 159 0;... % Orange - 0 114 178;... % Blue - 86 180 233;... % Sky Blue - 0 158 155;... % Bluish green - 213 94 0;.... % Vermillion - 240 228 66;... % Yellow - ] / 255; - -% Map bounds buffer -buff_deg = 0.05; - -%% Iterate over locations -for j=1:1:numel(iso_3166_2) - % Output from RUN_1.m - jhash = [usecase '_' iso_3166_2{j} '-anchorRange' num2str(anchorRange_nm)]; - inFile = ['output' filesep 'pairs-' jhash '.mat']; - - % Load - load(inFile,'anchors'); - - % Discretize - Y = discretize(anchors.num_geospatial,edges); - - % Create GeographicAxes and Base Map - figure(j); set(gcf,'name',inFile); - gx = geoaxes('Basemap','streets-dark',... - 'FontSize',16,... - 'FontWeight','bold',... - 'Position',[0 0 1 1]); - bmap = geobasemap(gx); - - % Calculate bounds - %minLat_deg = min(anchors.lat_deg) - buff_deg; - %maxLat_deg = max(anchors.lat_deg) + buff_deg; - %minLon_deg = min(anchors.lon_deg) - buff_deg; - %maxLon_deg = max(anchors.lon_deg) + buff_deg; - %geolimits(gx,[minLat_deg maxLat_deg],[minLon_deg maxLon_deg]); - - % Iterate through discretized anchors.num_geospatial - hold on - for i=1:1:max(Y) - l = Y == i; - geoscatter(anchors.lat_deg(l), anchors.lon_deg(l),25,colorOrder(i,:),'o','filled'); - - %geoshow(anchors.lat_deg(l), anchors.lon_deg(l)); - labels{i} = sprintf('[%i, %i)',edges(i),edges(i+1)); - end - hold off - legend(labels); - - % Adjust map size - set(gcf,'Units','inches','Position',[1 1 11 8]) - - % Save figure - %saveas(gcf,['output' filesep 'plot_1_' jhash '.png'],'png'); - print(['output' filesep 'plot_1_' jhash '.png'],'-dpng','-r300'); -end \ No newline at end of file diff --git a/README.md b/README.md index bc2182e..b832ea7 100644 --- a/README.md +++ b/README.md @@ -9,14 +9,16 @@ This repository is a collection of MATLAB code to generate uncorrelated encounte - [Contribution](#contribution) - [Method](#method) - [Run Order](#run-order) - - [Confirm Persistent System Environment Variable](#confirm-persistent-system-environment-variable) + - [Persistent System Environment Variable](#persistent-system-environment-variable) + - [Confirm Other Persistent System Environment Variable](#confirm-other-persistent-system-environment-variable) - [em-core](#em-core) - - [Generate trajectories](#generate-trajectories) - - [Identify Pairs (RUN_1*)](#identify-pairs-run1) - - [Parameters: findPairs_1](#parameters-findpairs1) + - [Generate geospatial trajectories](#generate-geospatial-trajectories) + - [Identify Pairs (RUN_1*)](#identify-pairs-run_1) + - [Parameters: findPairs_1](#parameters-findpairs_1) + - [Performance](#performance) - [Plotting](#plotting) - - [Generate Encounters (RUN_2*)](#generate-encounters-run2) - - [Parameters: createEncounters_2](#parameters-createencounters2) + - [Generate Encounters (RUN_2*)](#generate-encounters-run_2) + - [Parameters: createEncounters_2](#parameters-createencounters_2) - [Example Encounters](#example-encounters) - [Encounter Dynamics](#encounter-dynamics) - [Crossing, US-TN](#crossing-us-tn) @@ -77,7 +79,19 @@ Then for each grid coordinate and pairs of tracks (multi-threat encounters are n Code developed and tested in Windows for Matlab R2018a and R2019b. The dev machine had a CPU of Intel Xeon Gold 6130 at 2.10GHz and 64 GB of RAM. The built-in Matlab function [`parfor`](https://www.mathworks.com/help/matlab/ref/parfor.html) is used routinely throughout the code. Majority of the computation intensive work is related to working with digital elevation models or loading files. -### Confirm Persistent System Environment Variable +### Persistent System Environment Variable + +Immediately after cloning this repository, [create a persistent system environment](https://superuser.com/q/284342/44051) variable titled `AEM_DIR_GEOPAIR` with a value of the full path to this repository root directory. + +In unix there are many ways to do this, here is an example using [`/etc/profile.d`](https://unix.stackexchange.com/a/117473). Create a new file `aem-env.sh` using `sudo vi /etc/profile.d/aem-env.sh` and add the command to set the variable: + +```bash +export AEM_DIR_GEOPAIR=PATH TO /em-pairing-geospatial +``` + +You can confirm `AEM_DIR_BAYES` was set in unix by inspecting the output of `env`. + +### Confirm Other Persistent System Environment Variable Persistent system variables are used to reference different repositories within the Airspace Encounter Models organization. Confirm that they have been set. @@ -85,19 +99,21 @@ Persistent system variables are used to reference different repositories within | :------------- | :------------- | `AEM_DIR_BAYES` | [`em-model-manned-bayes`](https://github.com/Airspace-Encounter-Models/em-model-manned-bayes) `AEM_DIR_CORE` | [`em-core`](https://github.com/Airspace-Encounter-Models/em-core) -`AEM_DIR_GEOSPATIAL` | `em-model-geospatial` +`AEM_DIR_GEOSPATIAL` | [`em-model-geospatial`](https://github.com/Airspace-Encounter-Models/em-model-geospatial) ### em-core If you have not already, complete the initial setup associated with the [`em-core`](https://github.com/Airspace-Encounter-Models/em-core) repository. -### Generate trajectories +### Generate geospatial trajectories -Using a model specific repository, such as [`em-model-manned-bayes`](https://github.com/Airspace-Encounter-Models/em-model-manned-bayes) or `em-model-geospatial`, generate uncorrelated independent trajectories. +Using a model specific repository, such as `em-model-geospatial`, generate uncorrelated independent trajectories. ### Identify Pairs (RUN_1*) -The [`RUN_1.m`](RUN_1.m) script calls [`findPairs_1`](findPairs_1.m) and determines which trajectories generated by `em-model-geospatial` are within the distance defined by `anchorRange_nm` of the coordinates specified by `gridLat_deg` and `gridLon_deg`. These coordinates are referred to as the "anchor point." As noted in the [Output Directory](#output-directory) section, the output of [`RUN_1.m`](RUN_1.m) uses the "pairs" prefix. +The [`RUN_1.m`](RUN_1.m) script calls [`findPairs_1`](findPairs_1.m) and determines which trajectories generated by `em-model-geospatial` are within the distance defined by `anchorRange_nm` of the coordinates specified by `gridLat_deg` and `gridLon_deg`. + + These coordinates are referred to as the "anchor point." As noted in the [Output Directory](#output-directory) section, the output of [`RUN_1.m`](RUN_1.m) uses the "pairs" prefix. The high-level workflow for identifying feasible pairs and anchor points is: @@ -107,20 +123,36 @@ The high-level workflow for identifying feasible pairs and anchor points is: #### Parameters: findPairs_1 -The script is based on the [`findPairs_1`](findPairs_1.m) function, which has the following parameters: +The script is based on the [`findPairs_1`](findPairs_1.m) function. A notable helper functions is `preallocAnchors` and [`findPairs_1`](findPairs_1.m) has the following parameters: | Parameter | Example | Description | | :------------- | :-- | :------------- | +| iso_3166_2 | `US-MA` | [ISO 3166-2](https://en.wikipedia.org/wiki/ISO_3166-2) subdivision code for administrative boundary | inDir | `dir([getenv('AEM_DIR_GEOSPATIAL') filesep 'output' filesep 'trajectories' filesep 'US-MA' '*' filesep '*'])` | Cell array of directories from output of `em-model-geospatial/RUN_OSM_2_**.m` | outHash | `all_US-MA` | Text to append to output filename -| gridLat_deg | N X 1 double column array | List of latitude coordinates in decimal degrees -| gridLon_deg | N X 1 double column array | List of longitude coordinates in decimal degrees -| anchorRange_nm | `1.25` | Maximum allowable distance.=, in nautical miles, aircraft can be away from a anchor point -| airspace | `[getenv('AEM_DIR_CORE') filesep 'output' filesep 'airspace-B-C-D-24-Oct-2019.mat']` | Output from `em-core/matlab/**/RUN_Airspace_1.m` | +| anchorRange_nm | `1.00` | Maximum allowable distance, in nautical miles, aircraft can be away from a anchor point | classInclude | `{'B','C','D','O'}` | Airspace classes to include +| fileAirspace | `[getenv('AEM_DIR_CORE') filesep 'output' filesep 'airspace.mat']` | Filename of the output from `em-core/matlab/**/RUN_Airspace_1.m` | | spheroid | [`wgs84Ellipsoid('nm')`](https://www.mathworks.com/help/map/ref/wgs84ellipsoid.html) | [Reference ellipsoid](https://www.mathworks.com/help/map/reference-spheroids.html) +| seed | `1` | If not empty, the random seed to use | isSave | `true` | If true, save to file +#### Performance + +The following table are reference performance benchmarks when `anchorRange_nm = 1.00`. + +| ISO 3166-2 | # Anchor Points | # Geospatial Files | # Clusters | LLSC Run Time (s) | Windows Local Run Time (s) | +| :------------- | --: | --: | --: | --: | --: | +| US-CO | 101,170 | 8,098 | 17 | 188 | 160 +| US-HI | 5,254 | 195 | 10 | 90 | 67 +| US-KS | 79,587 | 15,724 | 14 | 272 | 206 +| US-MA | 8,305 | 4,344 | 4 | 136 | 76 +| US-MS | 42,957 |4,738 | 11 | 171 | 137 +| US-NC | 45,716 | 12,247 | 15 | 208 | 150 +| US-ND | 78,447 | 3,487 | 14 | 183 | 152 +| US-NV | 108,163 | 2,514 | 26 | 147 | 107 +| US-NY | 54,465 | 11,700| 22 | 219 | 146 + #### Plotting The output of [`RUN_1.m`](RUN_1.m) can be visualized using [`PLOT_1.m`](PLOT_1.m). This script uses MATALB's [`geobasemap`](https://www.mathworks.com/help/matlab/creating_plots/access-basemaps-in-matlab.html) capabilities to color pairs of `gridLat_deg` and `gridLon_deg` coordinates by the number of geospatial trajectories within the `anchorRange_nm` distance. The [Discussion](#discussion) section includes example outputs of [`PLOT_1.m`](PLOT_1.m). @@ -137,20 +169,24 @@ These scripts are based on the [`createEncounters_2`](createEncounters_2.m) func | Parameter | Example | Description | | :------------- | :--- | :------------- | -| inFile | `['output' filesep pairs-all_US-MA-anchorRange1.25.mat]` | Output from RUN_1 +| inAnchors | `['output' filesep pairs-all_US-MA-anchorRange1.25.mat]` | Either a .mat containing a the table output from RUN_1 or the table itself +| outDir | `[getenv('AEM_DIR_GEOPAIR') filesep 'output' filesep '2_encounters']` | Directory where encounters are saved | encTime_s | `60` | Encounter duration in seconds | thresHorz_ft | `500` |Conflict range threshold in feet | thresVert_ft | `100` |Conflict vertical separation threshold in feet | initHorz_ft | `[6076 15190]` | Initial horizontal range in feet | initVert_ft | `[0 500]` | Initial vertical separation magnitude in feet -| maxEncPerPair | `10` | Maximum encounters to generate for each combination of anchor coordinates and aircraft tracks -| timeStep_s | `1` |Update rate in seconds +| maxEncPerPair | `1` | Maximum encounters to generate for each combination of anchor coordinates and aircraft tracks +| timeStep| `1` | Update rate | isSampleAlt1 | `true` | If true, sample aircraft 1 AGL altitude from [`minAlt1_ft_agl` `maxAlt1_ft_agl`] | minAlt1_ft_agl | `100` | Minimum sampled AGL altitude for aircraft 1 | maxAlt1_ft_agl | `400` | Maximum sampled AGL altitude for aircraft 1 -| vrange1_kts | `[5 87]` | Minimum and maximum airspeed in knots of aircraft 1 (only if aircraft 1 isn't from a Bayes model) -| acmodel2 | `bayes` | Denote if aircraft 2 was generated either from the geospatial or bayes model -| demBayes | `dted1` | Digital elevation model to use when translating Bayes tracks from local (x,y,z) to (latitude, longitude, MSL altitude) +| rangeV1_ft_s | `[9 168]` | Minimum and maximum airspeed in feet per second of aircraft 1 (only if aircraft 1 isn't from a Bayes model) +| rangeV2_ft_s | `[9 168]` | Minimum and maximum airspeed in feet per second of aircraft 2 (only if aircraft 2 isn't from a Bayes model) +| acmodel1 | `mdlType1` | Denote if aircraft 1 should be pre-generated trajectory from `em-model-geospatial` or sampled from `em-model-manned-bayes` +| bayesFile1 | `[getenv('AEM_DIR_BAYES') filesep 'model' filesep 'uncor_1200only_fwse_v1p2.txt']` | Which Bayesian encounter model ASCII parameter file to use if `mdlType1 = 'bayes'` +| dem | `dted1` | Digital elevation model to use when translating Bayes tracks from local (x,y,z) to (latitude, longitude, MSL altitude) +| demBackup | `globe` | Digital elevation model to use when translating Bayes tracks from local (x,y,z) to (latitude, longitude, MSL altitude) | z_agl_tol_ft| `250` | Altitude tolerance when translating to MSL altitude from a Bayes track sampled in AGL | dofFile| `[getenv('AEM_DIR_CORE') filesep 'output' filesep 'dof-' '25-Feb-2020' '.mat']` | Output from `em-core/matlab/**/RUN_readfaadof.m` | dofObs| `{'silo','tower','windmill'}` | Cell array of obstacles to include @@ -158,7 +194,7 @@ These scripts are based on the [`createEncounters_2`](createEncounters_2.m) func | dofMaxRange_ft | `500` | Horizontal separation in feet between Bayes aircraft and center of obstacles | dofMaxVert_ft | `50` | Vertical separation in feet between Bayes aircraft and the top of obstacles | classInclude | `{'B','C','D','O'}` | Airspace classes to include -| spheroid | [`wgs84Ellipsoid('nm')`](https://www.mathworks.com/help/map/ref/wgs84ellipsoid.html) | [Reference ellipsoid](https://www.mathworks.com/help/map/reference-spheroids.html) +| spheroid | [`wgs84Ellipsoid('ft')`](https://www.mathworks.com/help/map/ref/wgs84ellipsoid.html) | [Reference ellipsoid](https://www.mathworks.com/help/map/reference-spheroids.html) | isPlot | `true` | If true, plot trajectories and anchor points | isZip | `true` | If true, will archive directory containing encounters into a single .zip file @@ -316,10 +352,6 @@ Default output directory for RUN scripts and functions. Files and subdirectories ## Citation -Please use this DOI number reference when citing the software: - -[![DOI](https://zenodo.org/badge/DOI/10.5281/zenodo.3978267.svg)](https://doi.org/10.5281/zenodo.3978267) - Please use this publication when citing the high level concepts of the encounter generation method:
A. Weinert, M. Edwards, L. Alvarez, and S. Katz, “Representative Small UAS Trajectories for Encounter Modeling,” in AIAA Scitech 2020 Forum, 2020, pp. 1–10. @@ -345,7 +377,7 @@ DISTRIBUTION STATEMENT A. Approved for public release. Distribution is unlimited This material is based upon work supported by the Federal Aviation Administration under Air Force Contract No. FA8702-15-D-0001. Any opinions, findings, conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Federal Aviation Administration. -© 2019, 2020 Massachusetts Institute of Technology. +© 2019-2021 Massachusetts Institute of Technology. Delivered to the U.S. Government with Unlimited Rights, as defined in DFARS Part 252.227-7013 or 7014 (Feb 2014). Notwithstanding any copyright notice, U.S. Government rights in this work are defined by DFARS 252.227-7013 or DFARS 252.227-7014 as detailed above. Use of this work other than as specifically authorized by the U.S. Government may violate any copyrights that exist in this work. diff --git a/RUN_1.m b/RUN_1.m index 5cacdc0..8663d12 100644 --- a/RUN_1.m +++ b/RUN_1.m @@ -1,141 +1,52 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory +% Copyright 2019 - 2021, MIT Lincoln Laboratory % SPDX-License-Identifier: BSD-2-Clause %% INPUTS % Adminstrative Boundaries and Trajectory Parameters -%iso_3166_2 = {'US-CA','US-FL','US-KS','US-MA','US-MS','US-NC','US-ND','US-NH','US-NY','US-NV','US-OK','US-PR','US-RI','US-TN','US-TX','US-VA'}; % dev cases -iso_3166_2 = {'US-KS','US-MA','US-MS','US-NC','US-ND','US-NV','US-TN','US-VA'}; -%iso_3166_2 = {'US-RI'}; % dev cases - -file_airspace = [getenv('AEM_DIR_CORE') filesep 'output' filesep 'airspace-B-C-D-03-Aug-2020.mat']; - -usecase = 'shield'; +iso_3166_2 = {'US-CO';'US-HI';'US-KS';'US-MA';'US-MS';'US-NC';'US-ND';'US-NV';'US-NY'}; % Pairs parameters % 30*(150 / 3600) = 1.25...closing speed of 150 knots for 30 seconds % 2000 ft = well clear -anchorRange_nm = 1.00; % Distance pairs need to be away from anchor point +anchorRange_nm = 1.00; % Distance pairs need to be away from anchor point + +%% Inputs hardcode +usecase = 'all'; + +% Root directory of geospatial trajectories +inDirRoot = ['~', filesep 'CASSATT_shared' filesep 'geospatial_trajectories']; %% Create status table to record performance if ~iscolumn(iso_3166_2) iso_3166_2 = iso_3166_2'; end -status = table(iso_3166_2,zeros(size(iso_3166_2)),zeros(size(iso_3166_2)),zeros(size(iso_3166_2)),zeros(size(iso_3166_2)),'VariableNames',{'iso_3166_2','runtime_s','n_points','n_files','n_clusters'}); +status = table(iso_3166_2,zeros(size(iso_3166_2)),zeros(size(iso_3166_2)),zeros(size(iso_3166_2)),zeros(size(iso_3166_2)),'VariableNames',{'iso_3166_2','runtime_s','nPoints','nGeoFiles','nClusters'}); %% Iterate through adminstrative boundaries -for i=1:1:numel(iso_3166_2) +for ii=1:1:numel(iso_3166_2) tic - % Load Natural Earth Adminstrative Boundaries - ne_admin = shaperead([getenv('AEM_DIR_CORE') filesep 'data' filesep 'NE-Adminstrative' filesep 'ne_10m_admin_1_states_provinces.shp']); - - % Create bounding box - BoundingBox_wgs84 = ne_admin(strcmp({ne_admin.iso_3166_2},iso_3166_2{i})).BoundingBox; - - % Load airspace - load(file_airspace,'airspace'); - - % Filter to include airspace only in bounding box - [~, ~, inAirspace] = filterboundingbox(airspace.LAT_deg,airspace.LON_deg,BoundingBox_wgs84); - airspace = airspace(inAirspace,:); - - neLat_deg = ne_admin(strcmp({ne_admin.iso_3166_2},iso_3166_2{i})).Y; - neLon_deg = ne_admin(strcmp({ne_admin.iso_3166_2},iso_3166_2{i})).X; - - % Create grid - [gridLat_deg, gridLon_deg] = meshgrid(min(neLat_deg):nm2deg(anchorRange_nm):max(neLat_deg),min(neLon_deg):nm2deg(anchorRange_nm):max(neLon_deg)); - % Filter grid for those only in the location - isGrid = InPolygon(gridLat_deg,gridLon_deg,neLat_deg,neLon_deg); - - gridLat_deg = gridLat_deg(isGrid); - gridLon_deg = gridLon_deg(isGrid); - - % Uncomment to plot - % figure(i); set(gcf,'name',iso_3166_2{i}); worldmap(BoundingBox_wgs84(:,2),BoundingBox_wgs84(:,1)); geoshow(gridLat_deg,gridLon_deg,'DisplayType','point'); grid on; - - % Input directories for ownship files - % The following code assumes you're using em-model-geospatial. - % If not, replace this section with code to identify the filenames of your ownship trajectory files - listing = dir([getenv('AEM_DIR_GEOSPATIAL') filesep 'output' filesep 'trajectories' filesep iso_3166_2{i} '*' filesep '*']); - listing(ismember( {listing.name}, {'.', '..'})) = []; %remove . and .. - isShield = contains({listing.folder},'shield'); - inDir = cellfun(@(f,n)([f filesep n]),{listing.folder},{listing.name},'UniformOutput',false)'; - switch usecase - case 'all' - % No filtering needed - case 'all-noshield' - inDir(isShield) = []; - case 'unconv-noshield' - % Pairs to be used with the unconventional bayes model - %(i.e. paragliders, gliders,) - isCostal = contains({listing.folder},{'landuse_beach','landuse_cliff','landuse_volcano','gshhg_gshhs_resf_lvl1'}); - inDir(~isCostal) = []; - case 'shield' - inDir(~isShield) = []; - case 'longlinearinfrastructure' - islli = contains({listing.folder},{'pipeline','roads','waterway','railway','electrictransmission'}); - inDir(~islli) = []; - case 'railway' - irrail = contains({listing.folder},'railway'); - inDir(~irrail) = []; - case 'agriculture' - isAg = contains({listing.folder},{'farm','orchard','vineyard'}); - inDir(~isAg) = []; - case 'heliport5-all-noshield' - inDir(isShield) = []; - % Just filtering for grid coordinates - % Load airport data and filter to heliports - S_airports = shaperead([getenv('AEM_DIR_CORE') filesep 'data' filesep 'FAA-Airports' filesep 'Airports.shp'],'BoundingBox',BoundingBox_wgs84,'UseGeoCoords',true); - S_hp = S_airports(strcmpi({S_airports.TYPE_CODE},'HP')); - - % Determine which points are within 5 nm of airport - inHP = sparse(numel(gridLat_deg),size(S_hp,1)); - for j=1:1:size(S_hp,1) - [latc,lonc] = scircle1(S_hp(j).Lat,S_hp(j).Lon,5,[],wgs84Ellipsoid('nm')); - inHP(:,j) = InPolygon(gridLat_deg,gridLon_deg,latc,lonc); - end - % Aggregate across rows - inHP = any(inHP,2); - - % Filter - gridLat_deg = gridLat_deg(find(inHP)); - gridLon_deg = gridLon_deg(find(inHP)); - - % Plot - figure(i); set(gcf,'name',iso_3166_2{i}); - gx = geoaxes('Basemap','streets-dark',... - 'FontSize',16,... - 'FontWeight','bold',... - 'Position',[0 0 1 1]); - bmap = geobasemap(gx); hold on; - geoscatter(gridLat_deg,gridLon_deg,'o','MarkerEdgeColor',[86 180 233]/255); - geoscatter([S_hp.Lat],[S_hp.Lon],'s','filled','MarkerFaceColor',[240 228 66]/255); - geolimits( [BoundingBox_wgs84(1,2) - nm2deg(6), BoundingBox_wgs84(2,2) + nm2deg(6)], [BoundingBox_wgs84(1,1) - nm2deg(6), BoundingBox_wgs84(2,1) + nm2deg(6)]); hold off; - %legend('Potential Anchor Points','Heliports'); - set(gcf,'Units','inches','Position',[1 1 11.94 5.28]); % Adjust map size - print(['output' filesep 'plot_grid_' usecase '_' iso_3166_2{i} '.png'],'-dpng','-r300'); - otherwise - error('usecase:unknown','Unknown use case of %s\n',usecase); - end + % Parse directories of the geospatial trajectories + [inDir,~] = parseGeoTrajDirectory([inDirRoot filesep iso_3166_2{ii}], usecase); if isempty(inDir) - fprintf('No input directories for usecase = %s, iso_3166_2 = %s...skipping via CONTINUE\n',usecase,iso_3166_2{i}); - continue; + fprintf('No input directories for iso_3166_2 = %s\n',iso_3166_2{ii}); + else + % Execute + [anchors, inFiles, numClust] = findPairs_1(iso_3166_2{ii},inDir,sprintf('%s_%s',usecase,iso_3166_2{ii}),'anchorRange_nm',anchorRange_nm,'seed',ii); + + % Record stats + status.runtime_s(ii) = round(toc); + status.nPoints(ii) = size(anchors,1); + status.nGeoFiles(ii) = numel(inFiles); + status.nClusters(ii) = numClust; + + % These are saved and we only outputted for performance, don't need + % them hanging around for the next loop + clear anchors inFiles end - - % Execute - [anchors, inFiles, numClust] = findPairs_1(inDir,sprintf('%s_%s',usecase,iso_3166_2{i}),gridLat_deg,gridLon_deg,airspace,'anchorRange_nm',anchorRange_nm); - - % Record stats - status.runtime_s(i) = round(toc); - status.n_points(i) = numel(gridLat_deg); - status.n_files(i) = numel(inFiles); - status.n_clusters(i) = numClust; - - % These are saved and we only outputted for performance, don't need - % them hanging around for the next loop - clear anchors inFiles end %% Save Display status to screen -save(['output' filesep '1_performanace_' usecase '_' 'anchorRange' num2str(anchorRange_nm) '_' date '.mat'],'status','inDir','file_airspace','anchorRange_nm','usecase'); +save(['output' filesep '1_performanace_' usecase '_' 'anchorRange' num2str(anchorRange_nm) '_' date '.mat'],'status','anchorRange_nm','usecase'); disp('Done!'); + diff --git a/RUN_2_geospatial_uncor.m b/RUN_2_geospatial_uncor.m new file mode 100644 index 0000000..4568e71 --- /dev/null +++ b/RUN_2_geospatial_uncor.m @@ -0,0 +1,93 @@ +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +%% Inputs +% Tradespace +tradespace = CreateTradespacePairingGeo('mdlType1','geospatial','mdlType2','bayes',... + 'thresHorz_ft',4500,'thresVert_ft',450,... + 'dofMaxRange_ft',[500],'dofMaxVert_ft',[500],... + 'conflictTime_s',[60:30:210, 60:30:180]','encTime_s',[(60:30:210)+30, (60:30:180)+60]'); +tradespace = sortrows(tradespace,{'dofMaxRange_ft','encTime_s'}); + +% Parent output directory +outDirRoot = ['output' filesep '2_encounters' filesep 'geospatial_uncor' filesep datestr(date,'yyyy-mm-dd')]; + +% RUN_1 parameters +anchorRange_nm = 1.00; +usecase = 'all'; + +seedGen = 1; + +% Other parameter not defined in tradespace table +dem = 'globe'; + +%% Suppress warning +warning('off','map:io:UnableToDetermineCoordinateSystemType') + +%% Create tasks +[tasks, anchors,initialSeeds] = CreateTasksPairingGeo(tradespace,anchorRange_nm,usecase,seedGen); + +%% Save tradespace +[~, ~, ~] = mkdir(outDirRoot); +save([outDirRoot filesep 'tradespace.mat'],'tradespace','tasks'); + +%% Create status table to record performance +status = table( tradespace.configId,zeros(size(tradespace,1),1),zeros(size(tradespace,1),1),'VariableNames',{'tradespaceId','workTime_s','nEncounters'}); + +%% Iterate over tasks +for ii=1:1:size(tasks,1) + % Start timer + tic + + % Display status + fprintf('Starting task global id %i\n',ii); + + % Filter anchors + iiAnchors = anchors(tasks.sidx(ii):tasks.eidx(ii),:); + + iiGlobal = tasks.globalId(ii); + iiConfig = tasks.configId(ii); + + % Tradespace idx + idxTS = find( tradespace.configId == iiConfig); + + % Create output directory + outHash = sprintf('%04.f_%07.f',iiConfig,iiGlobal); + outDir = [outDirRoot filesep sprintf('%04.f',iiConfig)]; + [~, ~, ~] = mkdir(outDir); + + % Create encounters + metadata = createEncounters_2(iiAnchors,outDir,... + tradespace.encTime_s(idxTS,:),tradespace.thresHorz_ft(idxTS,:),tradespace.thresVert_ft(idxTS,:),... + 'bayesFile1',tradespace.bayesFile1{idxTS},... + 'bayesFile2',tradespace.bayesFile2{idxTS},... + 'conflictTime_s',tradespace.conflictTime_s(idxTS,:),... + 'dofMaxRange_ft',tradespace.dofMaxRange_ft(idxTS,:),... + 'dofMaxVert_ft',tradespace.dofMaxVert_ft(idxTS,:),... + 'initHorz_ft',tradespace.initHorz_ft(idxTS,:),... + 'initVert_ft',tradespace.initVert_ft(idxTS,:),... + 'mdlType1',tradespace.mdlType1(idxTS,:),... + 'mdlType2',tradespace.mdlType2(idxTS,:),... + 'dem',dem,... + 'initialSeed',initialSeeds(ii),... + 'outHash',outHash,... + 'isGeoPointOverride',true,... + 'isSampleAlt1',true,... + 'isStartEndNonZero1',true,... + 'isPlot',true,... + 'isZip',false); + + % Record performance + workTime_s = toc; + nEncounters = size(metadata,1); + + % Save to status table + status.workTime_s(ii) = workTime_s; + status.nEncounters(ii) = nEncounters; + + save([outDir filesep 'metadata_' outHash '.mat'],'metadata','workTime_s','nEncounters','ii','iiAnchors'); +end + +%% Save Display status to screen +save([outDirRoot filesep 'tradespace.mat'],'status','-append'); +warning('on','all') +disp('Done!'); diff --git a/RUN_2_sUAS_HAA.m b/RUN_2_sUAS_HAA.m deleted file mode 100644 index 31126b1..0000000 --- a/RUN_2_sUAS_HAA.m +++ /dev/null @@ -1,90 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -%% Inputs -iso_3166_2 = {'US-CA','US-FL','US-KS','US-MA','US-MS','US-NC','US-ND','US-NH','US-NV','US-NY','US-OK','US-PR','US-RI','US-TN','US-TX','US-VA'}; -%iso_3166_2 = {'US-MA','US-MS','US-NH','US-NY','US-PR','US-TN'}; % Smaller states -%iso_3166_2 = {'US-VA','US-CA','US-FL','US-KS','US-ND','US-NV','US-OK','US-TX','US-NC'}; % Larger states -%iso_3166_2 = {'US-RI'}; % Smallest state - -% RUN_1 parameters -anchorRange_nm = 1.25; -usecase = 'shield'; - -% Encounters -encTime_s = 60; % Total encounter time, where CPA occurs at encTime_s /2 -initHorz_ft = [6076 round(unitsratio('ft','nm') * anchorRange_nm * 2)]; % [1 2.5] nautical miles -initVert_ft = [0 500]; -thresHorz_ft = 500; % CPA <= minHorz_ft % Conflict threshold criteria -thresVert_ft = 100; - -% Anchors -anchorPercent = 1; -maxEncPerPair = 20; - -% Bayes -dofMaxRange_ft = 200; -z_agl_tol_ft = 200; -demBayes = 'dted1'; - -% Altitude Sampling -if strcmp(usecase,'shield') - isSampleAlt1 = false; - maxAlt1_ft_agl = []; -else - isSampleAlt1 = true; - maxAlt1_ft_agl = 1200; -end -endisSampleAlt2 = false; - -% Airspeed Sampling -vrange1_kts = [1 10]; - -%% HAA trajectories -inDir2 = arrayfun(@(x)([getenv('AEM_DIR_HAA') filesep 'output' filesep 'v1-2020-01-14-HAA' filesep num2str(x) 'ft']),0:1e2:500,'uni',false)'; - -%% Create status table to record performance -if ~iscolumn(iso_3166_2) - iso_3166_2 = iso_3166_2'; -end -status = table(iso_3166_2,zeros(size(iso_3166_2)),zeros(size(iso_3166_2)),zeros(size(iso_3166_2)),'VariableNames',{'iso_3166_2','runtime_s','n_encounters','n_pairs'}); -outHash = ['_' usecase '_haa' '_anchorPercent' num2str(anchorPercent * 100) '_maxEncPerPair' num2str(maxEncPerPair) '_encTime' num2str(encTime_s) '_thresHorz_ft' num2str(thresHorz_ft) '_thresVert_ft' num2str(thresVert_ft) '_inithorzmin' num2str(initHorz_ft(1)) '_inithorzmax' num2str(initHorz_ft(2)) '_initvertmin' num2str(initVert_ft(1)) '_initvertmax' num2str(initVert_ft(2))]; - -%% Iterate through adminstrative boundaries -for i=1:1:numel(iso_3166_2) - tic - % Create input filename - inFile = ['output' filesep sprintf('pairs-%s-anchorRange%0.2f.mat',sprintf('%s_%s',usecase,iso_3166_2{i}),anchorRange_nm)]; - - % Create output directory - outDir = ['output' filesep 'NMAC' filesep iso_3166_2{i} outHash]; - - % Create encounters - [status.n_encounters(i), anchors, files1, files2, isEncounter] = createEncounters_2(inFile,encTime_s,thresHorz_ft,thresVert_ft,outDir,... - 'acmodel1','geospatial',... - 'acmodel2','bayes',... - 'inDir2',inDir2,... - 'maxEncPerPair',10,... - 'initHorz_ft',initHorz_ft,... - 'initVert_ft',initVert_ft,... - 'isSampleAlt1',isSampleAlt1,... - 'isSampleAlt2',isSampleAlt2,... - 'vrange1_kts',vrange1_kts,... - 'maxAlt1_ft_agl',maxAlt1_ft_agl,... - 'demBayes',demBayes,... - 'dofMaxRange_ft',dofMaxRange_ft,... - 'z_agl_tol_ft',z_agl_tol_ft,... - 'rowstart2',1,... - 'anchorPercent',anchorPercent,... - 'maxEncPerPair',maxEncPerPair,... - 'labelZ','barometicAlt_ft_msl',... - 'isPlot',true); - - status.runtime_s(i) = round(toc); - status.n_pairs(i) = sum(isEncounter); - - save(['output' filesep 'NMAC' filesep '2_encounters_' iso_3166_2{i} outHash],'anchors','files1','files2','isEncounter','thresHorz_ft','encTime_s','anchorPercent','maxEncPerPair'); -end - -%% Save Display status to screen -save(['output' filesep 'NMAC' filesep '2_performanace' outHash],'status','anchorRange_nm','usecase','encTime_s','initHorz_ft','initVert_ft','thresHorz_ft','isSampleAlt1','isSampleAlt2','anchorPercent','maxEncPerPair'); -disp('Done!'); \ No newline at end of file diff --git a/RUN_2_sUAS_sUAS.m b/RUN_2_sUAS_sUAS.m index 56f2309..2d9b2c7 100644 --- a/RUN_2_sUAS_sUAS.m +++ b/RUN_2_sUAS_sUAS.m @@ -1,40 +1,59 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory +% Copyright 2019 - 2021, MIT Lincoln Laboratory % SPDX-License-Identifier: BSD-2-Clause %% Inputs -iso_3166_2 = {'US-CA','US-FL','US-KS','US-MA','US-MS','US-NC','US-ND','US-NH','US-NV','US-NY','US-OK','US-PR','US-RI','US-TN','US-TX','US-VA'}; % dev cases -%iso_3166_2 = {'US-RI'}; % dev cases -anchorRange_nm = 0.17; % Distance pairs need to be away from anchor point (0.17 = unitsratio('nm','ft') * 1000) -pairsNum = 10000; -usecase = 'longlinearinfrastructure'; +tradespace = CreateTradespacePairingGeo('mdlType1','geospatial','mdlType2','geospatial',... + 'initHorz_ft',[4405 8810],'initVert_ft',[0 300],... + 'thresHorz_ft',0.6 * unitsratio('ft','nm'),'thresVert_ft',100); %https://arxiv.org/abs/1911.00110 -% Encounters -encTime_s = 60; % Total encounter time, where CPA occurs at encTime_s /2 -initHorz_ft = [4405 8810]; -initVert_ft = [0 300]; -timeStep_s = 1; +% Parent output directory +outDirRoot = [getenv('AEM_DIR_GEOPAIR') filesep 'output' filesep '2_encounters' filesep 'geospatial_uncor' filesep datestr(date,'yyyy-mm-dd')]; -% CPA threshold criteria -thresHorz_ft = 0.6 * unitsratio('ft','nm'); %https://arxiv.org/abs/1911.00110 +% RUN_1 parameters +anchorRange_nm = 1.00; +usecase = 'all'; -%% Iterate through adminstrative boundaries -encCount = zeros(size(iso_3166_2)); -for i=1:1:numel(iso_3166_2) - % Create input filename - inFile = ['output' filesep sprintf('pairs-%s-%0.2f-%i.mat',sprintf('%s-%s',usecase,iso_3166_2{i}),anchorRange_nm,pairsNum)]; +%% Suppress warning +warning('off','map:io:UnableToDetermineCoordinateSystemType') + +%% Save tradespace +[~, ~, ~] = mkdir(outDirRoot); +save([outDirRoot filesep 'tradespace.mat'],'tradespace'); + +%% Create status table to record performance +status = table( tradespace.configId,zeros(size(tradespace,1),1),zeros(size(tradespace,1),1),'VariableNames',{'tradespaceId','runTime_s','nEncounters'}); + +%% Iterate over tradespace +for ii=1:1:size(tradespace,1) + tic + % Create input filename containing the output of RUN_1 + inFile = ['output' filesep sprintf('pairs-%s-anchorRange%0.2f.mat',sprintf('%s_%s',usecase,tradespace.subdivision1{ii}),anchorRange_nm)]; % Create output directory - outDir = ['output' filesep iso_3166_2{i} '-' usecase '-' usecase '-cpa' num2str(thresHorz_ft) '-encTime' num2str(encTime_s) '-timestep' num2str(timeStep_s) '-inithorzmin' num2str(initHorz_ft(1)) '-inithorzmax' num2str(initHorz_ft(2)) '-initvertmin' num2str(initVert_ft(1)) '-initvertmax' num2str(initVert_ft(2))]; + outDir = [outDirRoot filesep num2str(ii)]; + [~, ~, ~] = mkdir(outDir); % Create encounters - encCount(i) = createEncounters_2(inFile,encTime_s,thresHorz_ft,outDir,... - 'maxEncPerPair',10,... - 'acmodel1','geospatial',... - 'acmodel2','geospatial',... - 'initHorz_ft',initHorz_ft,... - 'initVert_ft',initVert_ft,... - 'timeStep_s', timeStep_s,... - 'isPlot',false); + metadata = createEncounters_2(inFile,outDir,... + tradespace.encTime_s(ii,:),tradespace.thresHorz_ft(ii,:),tradespace.thresVert_ft(ii,:),... + 'bayesFile1',tradespace.bayesFile1{ii},... + 'bayesFile2',tradespace.bayesFile2{ii},... + 'conflictTime_s',tradespace.conflictTime_s(ii,:),... + 'dofMaxRange_ft',tradespace.dofMaxRange_ft(ii,:),... + 'dofMaxVert_ft',tradespace.dofMaxVert_ft(ii,:),... + 'initHorz_ft',tradespace.initHorz_ft(ii,:),... + 'initVert_ft',tradespace.initVert_ft(ii,:),... + 'mdlType1',tradespace.mdlType1(ii,:),... + 'mdlType2',tradespace.mdlType2(ii,:),... + 'isStartEndNonZero1',true,... + 'isStartEndNonZero2',true,... + 'isPlot',true); + + status.runTime_s(ii) = toc; + status.nEncounters(ii) = size(metadata,1); + save([outDirRoot filesep sprintf('metadata_%05.f',ii)],'metadata','status','inFile','ii','tradespace'); end -%% Display status to screen -disp('Done!'); \ No newline at end of file +%% Save Display status to screen +save([outDirRoot filesep 'tradespace.mat'],'status','-append'); +warning('on','all') +disp('Done!'); diff --git a/RUN_2_sUAS_unconv.m b/RUN_2_sUAS_unconv.m deleted file mode 100644 index 4e68883..0000000 --- a/RUN_2_sUAS_unconv.m +++ /dev/null @@ -1,116 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -%% Inputs -iso_3166_2 = {'US-CA','US-FL','US-KS','US-MA','US-MS','US-NC','US-ND','US-NH','US-NY','US-NV','US-OK','US-PR','US-RI','US-TN','US-TX','US-VA'}; % dev cases -%iso_3166_2 = {'US-MA','US-MS','US-NH','US-NY','US-PR','US-TN'}; % Smaller states -%iso_3166_2 = {'US-VA','US-CA','US-FL','US-KS','US-ND','US-NV','US-OK','US-TX','US-NC'}; % Larger states -%iso_3166_2 = {'US-RI'}; % Smallest state - -% RUN_1 parameters -anchorRange_nm = 1.25; -usecase = 'all-noshield'; - -% Encounters -encTime_s = 60; % Total encounter time, where CPA occurs at encTime_s /2 -initVert_ft = [0 500]; -thresHorz_ft = 500; % CPA <= minHorz_ft % Conflict threshold criteria -thresVert_ft = 100; - -% Anchors -anchorPercent = 1; -maxEncPerPair = 20; - -% Bayes -z_agl_tol_ft = 200; -demBayes = 'dted1'; -models = {'glider_v1p2_08-Mar-2020','paraglider_v1p2_08-Mar-2020'}; -model = models{1}; % Manually change - -switch model - case 'glider_v1p2_08-Mar-2020' - dofMaxRange_ft = 500; - initHorz_ft = [6076 round(unitsratio('ft','nm') * anchorRange_nm * 2)]; % [1 2.5] nautical miles - maxHeightBayes_ft = 1700; - aclass = {'O'}; - case 'paraglider_v1p2_08-Mar-2020' - dofMaxRange_ft = 200; - initHorz_ft = [1000 round(unitsratio('ft','nm') * anchorRange_nm * 2)]; % [1 2.5] nautical miles - maxHeightBayes_ft = 1500; - aclass = {'B','C','D','O'}; -end - -% Altitude and Airspeed Sampling -if strcmp(usecase,'shield') - isSampleAlt1 = false; - maxAlt1_ft_agl = []; - vrange1_kts = [1 10]; -else - isSampleAlt1 = true; - maxAlt1_ft_agl = 1200; - vrange1_kts = [10 60]; -end -isSampleAlt2 = false; - -%% Manned trajectories -dirBayes = [getenv('AEM_DIR_BAYES') filesep 'output' filesep 'tracks' filesep model]; -listing = dir(dirBayes); -listing(ismember( {listing.name}, {'.', '..'})) = []; %remove . and .. - -%% Create status table to record performance -if ~iscolumn(iso_3166_2) - iso_3166_2 = iso_3166_2'; -end -status = table(iso_3166_2,zeros(size(iso_3166_2)),zeros(size(iso_3166_2)),zeros(size(iso_3166_2)),'VariableNames',{'iso_3166_2','runtime_s','n_encounters','n_pairs'}); -outHash = ['_' usecase '_unconv_' model '_anchorPercent' num2str(anchorPercent * 100) '_maxEncPerPair' num2str(maxEncPerPair) '_encTime' num2str(encTime_s) '_thresHorz_ft' num2str(thresHorz_ft) '_thresVert_ft' num2str(thresVert_ft) '_inithorzmin' num2str(initHorz_ft(1)) '_inithorzmax' num2str(initHorz_ft(2)) '_initvertmin' num2str(initVert_ft(1)) '_initvertmax' num2str(initVert_ft(2))]; - -%% Iterate through adminstrative boundaries -for i=1:1:numel(iso_3166_2) - tic - - % Set random seed - rng(i); - - % Create input filename - inFile = ['output' filesep sprintf('pairs-%s-anchorRange%0.2f.mat',sprintf('%s_%s',usecase,iso_3166_2{i}),anchorRange_nm)]; - - % Create output directory - outDir = ['output' filesep 'NMAC' filesep 'AC1speed' num2str(vrange1_kts(1)) '-' num2str(vrange1_kts(2)) filesep iso_3166_2{i} outHash]; - - % Bayes - inDirBayes = arrayfun(@(x)([dirBayes filesep num2str(x)]),randperm(numel(listing),10),'uni',false)'; - inDir2 = {}; - for k=1:1:numel(inDirBayes) - inDirK = arrayfun(@(x)([inDirBayes{k} filesep num2str(x) 'ft']),0:1e2:maxHeightBayes_ft,'uni',false)'; - inDir2 = [inDir2; inDirK]; - end - - % Create encounters - [status.n_encounters(i), anchors, files1, files2, isEncounter] = createEncounters_2(inFile,encTime_s,thresHorz_ft,thresVert_ft,outDir,... - 'acmodel1','geospatial',... - 'acmodel2','bayes',... - 'inDir2',inDir2,... - 'maxEncPerPair',10,... - 'initHorz_ft',initHorz_ft,... - 'initVert_ft',initVert_ft,... - 'isSampleAlt1',isSampleAlt1,... - 'isSampleAlt2',isSampleAlt2,... - 'maxAlt1_ft_agl',maxAlt1_ft_agl,... - 'demBayes',demBayes,... - 'dofMaxRange_ft',dofMaxRange_ft,... - 'z_agl_tol_ft',z_agl_tol_ft,... - 'rowstart2',1,... - 'vrange1_kts',vrange1_kts,... - 'anchorPercent',anchorPercent,... - 'maxEncPerPair',maxEncPerPair,... - 'classInclude',aclass,... - 'isPlot',false); - - status.runtime_s(i) = round(toc); - status.n_pairs(i) = sum(isEncounter); - - save(['output' filesep 'NMAC' filesep '2_encounters_' iso_3166_2{i} outHash],'anchors','files1','files2','isEncounter','thresHorz_ft','encTime_s','anchorPercent','maxEncPerPair'); -end - -%% Save Display status to screen -save(['output' filesep 'NMAC' filesep '2_performanace' outHash],'status','anchorRange_nm','usecase','encTime_s','initHorz_ft','initVert_ft','thresHorz_ft','isSampleAlt1','isSampleAlt2','anchorPercent','maxEncPerPair'); -disp('Done!'); \ No newline at end of file diff --git a/RUN_2_sUAS_uncor.m b/RUN_2_sUAS_uncor.m deleted file mode 100644 index 8b088fa..0000000 --- a/RUN_2_sUAS_uncor.m +++ /dev/null @@ -1,140 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -%% Inputs -%iso_3166_2 = {'US-MA'};%,'US-CA','US-TX'};%'US-FL','US-KS','US-MA','US-MS','US-NC','US-ND','US-NH','US-NV','US-NY','US-OK','US-PR','US-RI','US-TN','US-TX','US-VA'}; -iso_3166_2 = {'US-KS','US-MA','US-MS','US-NC','US-ND','US-NV','US-TN','US-VA'};%,'US-CA','US-TX'};%'US-FL','US-KS','US-MA','US-MS','US-NC','US-ND','US-NH','US-NV','US-NY','US-OK','US-PR','US-RI','US-TN','US-TX','US-VA'}; -%iso_3166_2 = {'US-MA','US-MS','US-NH','US-NY','US-PR','US-TN'}; % Smaller states -%iso_3166_2 = {'US-VA','US-CA','US-FL','US-KS','US-ND','US-NV','US-OK','US-TX','US-NC'}; % Larger states -%iso_3166_2 = {'US-RI'}; % Smallest state - -% RUN_1 parameters -anchorRange_nm = 1.00; -usecase = 'all-noshield'; - -% Encounters -encTime_s = 60; % Total encounter time, where CPA occurs at encTime_s /2 -initHorz_ft = [6076 round(unitsratio('ft','nm') * anchorRange_nm * 2)]; % [1 2.5] nautical miles -initVert_ft = [0 500]; -thresHorz_ft = 500; % CPA <= minHorz_ft % Conflict threshold criteria -thresVert_ft = 100; - -% Anchors -anchorPercent = 1; -maxEncPerPair = 2; - -% Altitude and Airspeed Sampling -if strcmp(usecase,'shield') - isSampleAlt1 = false; - maxAlt1_ft_agl = []; % Not used because isSampleAlt1 = false - vrange1_kts = [1 10]; -else - isSampleAlt1 = true; - maxAlt1_ft_agl = 700; - vrange1_kts = [10 87]; -end -minAlt1_ft_agl = 100; -isSampleAlt2 = false; - -% Bayes -bayesModel = 'uncor_allcode_rotorcraft_v1'; % Model name -bayesDate = '31-Jul-2020'; % Date created -minAlt2_ft_agl = minAlt1_ft_agl; -maxAlt2_ft_agl = round(700 + max(initVert_ft),-2); % The ASTM sUAS TOR has a ceiling of 1200 feet AGL -num_files2 = 40; % Number of files dependent upon em-model-manned-bayes/RUN_1_emsample -dofMaxRange_ft = 250; % Maximum "size" of obstacles, a circle around the obstacle will be generated with radius = dofMaxRange_ft -z_agl_tol_ft = 200; -demBayes = 'dted1'; - -% Airspace class char -% 1 = B = Class B -% 2 = C = Class C -% 3 = D = Class D -% 4 = O = Other (Class A, E, G) -classInclude = {'O'}; - -%% Create status table to record performance -if ~iscolumn(iso_3166_2) - iso_3166_2 = iso_3166_2'; -end -status = table(iso_3166_2,zeros(size(iso_3166_2)),zeros(size(iso_3166_2)),zeros(size(iso_3166_2)),'VariableNames',{'iso_3166_2','runtime_s','n_encounters','n_pairs'}); - -%% Iterate through adminstrative boundaries -for i=1:1:numel(iso_3166_2) - tic - % Create input filename - inFile = ['output' filesep sprintf('pairs-%s-anchorRange%0.2f.mat',sprintf('%s_%s',usecase,iso_3166_2{i}),anchorRange_nm)]; - - % Set random seed - rng(i); - - % Iterate over airspace classes - for j=classInclude - % Geographic string - % Puerto Rico and Hawaii are islands - if any(strcmp(iso_3166_2{i},{'US-PR','US-HI'})) - G = 'G2'; % Islands - else - G = 'G1'; % CONUS + AK - end - - % Airspace Class String - switch j{1} - case 'B' - A = ['A' '1']; - case 'C' - A = ['A' '2']; - case 'D' - A = ['A' '3']; - case 'O' - A = ['A' '4']; - otherwise - error('classInclude not recognized\n'); - end - - % Uncorrelated trajectories - % Subdirectories are organized by geographic variable (G), airspace class (A), and altitude layer (L) - inDirG = arrayfun(@(x)([getenv('AEM_DIR_BAYES') filesep 'output' filesep 'tracks' filesep bayesModel '_' bayesDate filesep num2str(x)]),randperm(num_files2,round(num_files2/2)),'uni',false); % randperm(num_files2,20) is used to reduce the number of files searched for in createEncounters_2 - inDirGA = cellfun(@(x)([x filesep G filesep A]),inDirG,'uni',false); - inDir2 = {}; - for k=1:1:numel(inDirGA) - inDirK = arrayfun(@(x)([inDirGA{k} filesep num2str(x) 'ft']),minAlt2_ft_agl:1e2:maxAlt2_ft_agl,'uni',false)'; % The altitude range corresponds to the directory structure from em-model-manned-bayes/RUN_2_sample2track - inDir2 = [inDir2; inDirK]; - end - - % Create output hash - outHash = ['_' usecase '_' strrep(bayesModel,'_','-') '_' G '_' A '_anchorPercent' num2str(anchorPercent * 100) '_maxEncPerPair' num2str(maxEncPerPair) '_encTime' num2str(encTime_s) '_thresHorz_ft' num2str(thresHorz_ft) '_thresVert_ft' num2str(thresVert_ft) '_inithorzmin' num2str(initHorz_ft(1)) '_inithorzmax' num2str(initHorz_ft(2)) '_initvertmin' num2str(initVert_ft(1)) '_initvertmax' num2str(initVert_ft(2))]; - - % Create output directory - outDir = ['output' filesep 'NMAC' filesep 'AC1speed' num2str(vrange1_kts(1)) '-' num2str(vrange1_kts(2)) filesep iso_3166_2{i} outHash]; - - % Create encounters - [status.n_encounters(i), anchors, files1, files2, isEncounter] = createEncounters_2(inFile,encTime_s,thresHorz_ft,thresVert_ft,outDir,... - 'acmodel1','geospatial',... - 'acmodel2','bayes',... - 'inDir2',inDir2,... - 'maxEncPerPair',10,... - 'initHorz_ft',initHorz_ft,... - 'initVert_ft',initVert_ft,... - 'isSampleAlt1',isSampleAlt1,... - 'isSampleAlt2',isSampleAlt2,... - 'vrange1_kts',vrange1_kts,... - 'minAlt1_ft_agl',minAlt1_ft_agl,... - 'maxAlt1_ft_agl',maxAlt1_ft_agl,... - 'demBayes',demBayes,... - 'dofMaxRange_ft',dofMaxRange_ft,... - 'z_agl_tol_ft',z_agl_tol_ft,... - 'rowstart2',1,... - 'anchorPercent',anchorPercent,... - 'maxEncPerPair',maxEncPerPair,... - 'classInclude',j,... - 'isPlot',false); - - save(['output' filesep 'NMAC' filesep '2_encounters_' iso_3166_2{i} outHash],'anchors','files1','files2','isEncounter','thresHorz_ft','encTime_s','anchorPercent','maxEncPerPair'); - end - status.runtime_s(i) = round(toc); - status.n_pairs(i) = sum(isEncounter); -end - -%% Save Display status to screen -save(['output' filesep 'NMAC' filesep '2_performanace' outHash],'status','anchorRange_nm','usecase','encTime_s','initHorz_ft','initVert_ft','thresHorz_ft','isSampleAlt1','isSampleAlt2','anchorPercent','maxEncPerPair','bayesDate','bayesModel'); -disp('Done!'); \ No newline at end of file diff --git a/RUN_2_uncor_uncor.m b/RUN_2_uncor_uncor.m new file mode 100644 index 0000000..d251c62 --- /dev/null +++ b/RUN_2_uncor_uncor.m @@ -0,0 +1,99 @@ +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +%% Inputs +% Tradespace +tradespace = CreateTradespacePairingGeo('mdlType1','bayes','mdlType2','bayes',... + 'initHorz_ft',[12152 97217],'initVert_ft',[0 1500],'thresHorz_ft',[4000],'thresVert_ft',450,... + 'conflictTime_s',[60; 120; 180],'encTime_s',[90; 150; 210],... + 'dofMaxRange_ft',500,'dofMaxVert_ft',500); + +% Parent output directory +outDirRoot = ['output' filesep '2_encounters' filesep 'uncor_uncor' filesep datestr(date,'yyyy-mm-dd')]; + +% Mock RUN_1_ parameters +anchorRange_nm = 1.00; + +% Other parameter not defined in tradespace table +rangeAlt1_ft_agl = [50 1200]; +rangeAlt2_ft_agl = [50 2000]; +dem = 'globe'; +maxTries = 12; +seedGen = 1; + +%% Suppress warning +warning('off','map:io:UnableToDetermineCoordinateSystemType') + +%% Create tasks +[tasks, anchors,initialSeeds] = CreateTasksPairingGeo(tradespace,anchorRange_nm,'uncor_uncor',seedGen); + +%% Save tradespace +[~, ~, ~] = mkdir(outDirRoot); +save([outDirRoot filesep 'tradespace.mat'],'tradespace'); + +%% Create status table to record performance +status = table( tradespace.configId,zeros(size(tradespace,1),1),zeros(size(tradespace,1),1),'VariableNames',{'tradespaceId','runTime_s','nEncounters'}); + +%% Iterate over tasks +for ii=1:1:size(tasks,1) + % Start timer + tic + + % Display status + fprintf('Starting task global id %i\n',ii); + + % Filter anchors + iiAnchors = anchors(tasks.sidx(ii):tasks.eidx(ii),:); + + iiGlobal = tasks.globalId(ii); + iiConfig = tasks.configId(ii); + + % Tradespace idx + idxTS = find( tradespace.configId == iiConfig); + + % Create output directory + outHash = sprintf('%04.f_%07.f',iiConfig,iiGlobal); + outDir = [outDirRoot filesep sprintf('%04.f',iiConfig)]; + [~, ~, ~] = mkdir(outDir); + + % Create encounters + metadata = createEncounters_2(iiAnchors,outDir,... + tradespace.encTime_s(idxTS,:),tradespace.thresHorz_ft(idxTS,:),tradespace.thresVert_ft(idxTS,:),... + 'bayesFile1',tradespace.bayesFile1{idxTS},... + 'bayesFile2',tradespace.bayesFile2{idxTS},... + 'conflictTime_s',tradespace.conflictTime_s(idxTS,:),... + 'dofMaxRange_ft',tradespace.dofMaxRange_ft(idxTS,:),... + 'dofMaxVert_ft',tradespace.dofMaxVert_ft(idxTS,:),... + 'initHorz_ft',tradespace.initHorz_ft(idxTS,:),... + 'initVert_ft',tradespace.initVert_ft(idxTS,:),... + 'mdlType1',tradespace.mdlType1(idxTS,:),... + 'mdlType2',tradespace.mdlType2(idxTS,:),... + 'dem',dem,... + 'initialSeed',initialSeeds(ii),... + 'outHash',outHash,... + 'maxTries',maxTries,... + 'rangeAlt1_ft_agl',rangeAlt1_ft_agl,... + 'rangeAlt2_ft_agl',rangeAlt2_ft_agl,... + 'isSampleAlt1',false,... + 'isSampleAlt2',false,... + 'isSampleV1',false,... + 'isSampleV2',false,... + 'isStartEndNonZero1',false,... + 'isStartEndNonZero2',false,... + 'isPlot',false,... + 'isZip',false); + + % Record performance + workTime_s = toc; + nEncounters = size(metadata,1); + + % Save to status table + status.workTime_s(ii) = workTime_s; + status.nEncounters(ii) = nEncounters; + + save([outDir filesep 'metadata_' outHash '.mat'],'metadata','workTime_s','nEncounters','ii','iiAnchors'); +end + +%% Save Display status to screen +save([outDirRoot filesep 'tradespace.mat'],'status','-append'); +warning('on','all') +disp('Done!'); diff --git a/RUN_3.m b/RUN_3.m new file mode 100644 index 0000000..cef7693 --- /dev/null +++ b/RUN_3.m @@ -0,0 +1,49 @@ +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +%% Inputs +% Directory where encounters are stored +outDirRoot = [getenv('AEM_DIR_GEOPAIR') filesep 'output' filesep '2_encounters'] + +% Desired number of encounters +nEncWant = 1e6; + +% Random seed +seed = 1; + +%% Update directory where encounters are stored +if ispc + outDirRoot = ['Z:' outDirRoot(2:end)]; +end + +%% Set random seed +rng(seed,'twister'); + +%% Preallocate +if isempty(bayesFile1) + bfs = strings(numel(bayesFile2),2); + bfs(:,1) = bayesFile1; + bfs(:,2) = bayesFile2; +else + bfs = allcomb(bayesFile1,bayesFile2); +end + +tradespaceFilter = cell(size(bfs,1),1); +tasksFilter = cell(size(bfs,1),1); +listing = cell(size(bfs,1),1); +metadata = cell(size(bfs,1),1); +files = cell(size(bfs,1),1); + +%% Load tradespace and tasks +load([outDirRoot filesep 'tradespace.mat'],'tradespace','tasks','anchors','initialSeeds','anchorRange_nm','usecase'); + +%% Iterate and identify +for ii=1:1:size(bfs,1) + [tradespaceFilter{ii}, tasksFilter{ii}, listing{ii},metadata{ii}] = IdentifyEncounterFiles(tradespace,tasks,outDirRoot,... + 'bayesFile1',bfs{ii,1},'bayesFile2',bfs{ii,2},'nEncWant',nEncWant); + files{ii} = string(compose('%s/enc_%s.dat',string(listing{ii}.folder(listing{ii}.isUse)),string(listing{ii}.nameHash(listing{ii}.isUse)))); + fprintf('%i/%i\n',ii,size(bfs,1)); +end + +%% Save +save([outDirRoot filesep 'listing.mat'],'tradespaceFilter','tasksFilter','listing','files','nEncWant','seed','bayesFile1','bayesFile2'); +save([outDirRoot filesep 'metadata.mat'],'metadata','tasksFilter','listing','files','nEncWant','seed','bayesFile1','bayesFile2','-v7.3'); diff --git a/calcLegsTime.m b/calcLegsTime.m deleted file mode 100644 index a811a60..0000000 --- a/calcLegsTime.m +++ /dev/null @@ -1,26 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -function [course, d_nm,t_legs_s] = calcLegsTime(lat,lon,v_kts) -% Calculate distance between legs -[course,d_nm] = legs(lat,lon,'rh'); - -d_nm = abs(d_nm); - -% Add functionality to account for v=0 and require altitude - -% Calculate time for each leg -if numel(v_kts) == 1 - % Constant airspeed - t_legs_hr = d_nm / abs(v_kts); -else - t_legs_hr = d_nm ./ abs(v_kts(1:end-1)); -end - -% Account for when v_kts = 0 -t_legs_hr(isinf(t_legs_hr)) = 0; - -t_legs_s = t_legs_hr * 3600; - -% Ensure positive time -% We need to do this because helicopters can fly backwards -t_legs_s = abs(t_legs_s); diff --git a/code/README.md b/code/README.md new file mode 100644 index 0000000..a802fac --- /dev/null +++ b/code/README.md @@ -0,0 +1,25 @@ +# Code + +Software developed by the aerospace encounter models team. + +| Directory | Description | +| :-------------| :-- | +anchor | Functions primarily related to generating geodetic anchors +encounter | Functions primarily related to identifying conflicts and encounters +helper | Miscellaneous helper functions +plot | Plotting functions +trajectory | Functions related to parse or manipulating track trajectories + +## Distribution Statement + +DISTRIBUTION STATEMENT A. Approved for public release. Distribution is unlimited. + +This material is based upon work supported by the Federal Aviation Administration under Air Force Contract No. FA8702-15-D-0001. Any opinions, findings, conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Federal Aviation Administration. + +© 2019-2021 Massachusetts Institute of Technology. + +Delivered to the U.S. Government with Unlimited Rights, as defined in DFARS Part 252.227-7013 or 7014 (Feb 2014). Notwithstanding any copyright notice, U.S. Government rights in this work are defined by DFARS 252.227-7013 or DFARS 252.227-7014 as detailed above. Use of this work other than as specifically authorized by the U.S. Government may violate any copyrights that exist in this work. + +Any opinions, findings, conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Federal Aviation Administration. + +This document is derived from work done for the FAA (and possibly others), it is not the direct product of work done for the FAA. The information provided herein may include content supplied by third parties. Although the data and information contained herein has been produced or processed from sources believed to be reliable, the Federal Aviation Administration makes no warranty, expressed or implied, regarding the accuracy, adequacy, completeness, legality, reliability, or usefulness of any information, conclusions or recommendations provided herein. Distribution of the information contained herein does not constitute an endorsement or warranty of the data or information provided herein by the Federal Aviation Administration or the U.S. Department of Transportation. Neither the Federal Aviation Administration nor the U.S. Department of Transportation shall be held liable for any improper or incorrect use of the information contained herein and assumes no responsibility for anyone’s use of the information. The Federal Aviation Administration and U.S. Department of Transportation shall not be liable for any claim for any loss, harm, or other damages arising from access to or use of data information, including without limitation any direct, indirect, incidental, exemplary, special or consequential damages, even if advised of the possibility of such damages. The Federal Aviation Administration shall not be liable for any decision made or action taken, in reliance on the information contained herein. diff --git a/waypoint_format/README.md b/code/anchor/README.md similarity index 93% rename from waypoint_format/README.md rename to code/anchor/README.md index 3b0fd62..12febcd 100644 --- a/waypoint_format/README.md +++ b/code/anchor/README.md @@ -1,6 +1,6 @@ -# Waypoint Format +# Anchors -Files dedicated solely to the waypoint format described in the root [README](../README.md). +Functions primarily related to generating geodetic anchors ## Distribution Statement @@ -8,11 +8,10 @@ DISTRIBUTION STATEMENT A. Approved for public release. Distribution is unlimited This material is based upon work supported by the Federal Aviation Administration under Air Force Contract No. FA8702-15-D-0001. Any opinions, findings, conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Federal Aviation Administration. -© 2019, 2020 Massachusetts Institute of Technology. +© 2019-2021 Massachusetts Institute of Technology. Delivered to the U.S. Government with Unlimited Rights, as defined in DFARS Part 252.227-7013 or 7014 (Feb 2014). Notwithstanding any copyright notice, U.S. Government rights in this work are defined by DFARS 252.227-7013 or DFARS 252.227-7014 as detailed above. Use of this work other than as specifically authorized by the U.S. Government may violate any copyrights that exist in this work. Any opinions, findings, conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Federal Aviation Administration. This document is derived from work done for the FAA (and possibly others), it is not the direct product of work done for the FAA. The information provided herein may include content supplied by third parties. Although the data and information contained herein has been produced or processed from sources believed to be reliable, the Federal Aviation Administration makes no warranty, expressed or implied, regarding the accuracy, adequacy, completeness, legality, reliability, or usefulness of any information, conclusions or recommendations provided herein. Distribution of the information contained herein does not constitute an endorsement or warranty of the data or information provided herein by the Federal Aviation Administration or the U.S. Department of Transportation. Neither the Federal Aviation Administration nor the U.S. Department of Transportation shall be held liable for any improper or incorrect use of the information contained herein and assumes no responsibility for anyone’s use of the information. The Federal Aviation Administration and U.S. Department of Transportation shall not be liable for any claim for any loss, harm, or other damages arising from access to or use of data information, including without limitation any direct, indirect, incidental, exemplary, special or consequential damages, even if advised of the possibility of such damages. The Federal Aviation Administration shall not be liable for any decision made or action taken, in reliance on the information contained herein. - diff --git a/findPairs_1.m b/code/anchor/findPairs_1.m similarity index 81% rename from findPairs_1.m rename to code/anchor/findPairs_1.m index 7531d0b..e04b00c 100644 --- a/findPairs_1.m +++ b/code/anchor/findPairs_1.m @@ -1,34 +1,39 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory +function [anchors, listing, numClust] = findPairs_1(iso_3166_2,inDir,outHash,varargin) +% Copyright 2019 - 2021, MIT Lincoln Laboratory % SPDX-License-Identifier: BSD-2-Clause -function [anchors, listing, numClust] = findPairs_1(inDir,outHash,gridLat_deg,gridLon_deg,airspace,varargin) - -%% Things that should be input parser +% +% SEE ALSO preallocAnchors %% Input parser p = inputParser; % Required +addRequired(p,'iso_3166_2',@ischar); addRequired(p,'inDir',@iscell); -addRequired(p,'outHash'); -addRequired(p,'gridLat_deg',@isnumeric); -addRequired(p,'gridLon_deg',@isnumeric); -addRequired(p,'airspace'); +addRequired(p,'outHash',@ischar); % Optional -addOptional(p,'anchorRange_nm',1.25,@isnumeric); -addOptional(p,'classInclude',{'B','C','D','O'},@iscell); - -addOptional(p,'spheroid',wgs84Ellipsoid('nm')); -addOptional(p,'isSave',true,@islogical); +addParameter(p,'anchorRange_nm',1.00,@isnumeric); +addParameter(p,'classInclude',{'B','C','D','O'},@iscell); +addParameter(p,'fileAirspace',[getenv('AEM_DIR_CORE') filesep 'output' filesep 'airspace.mat'],@ischar); +addParameter(p,'spheroid',wgs84Ellipsoid('nm')); +addParameter(p,'seed',double.empty(0,0),@isnumeric); +addParameter(p,'isSave',true,@islogical); % Parse -parse(p,inDir,outHash,gridLat_deg,gridLon_deg,airspace,varargin{:}); +parse(p,iso_3166_2,inDir,outHash,varargin{:}); anchorRange_nm = p.Results.anchorRange_nm; -%% Create outputs -% table -anchors = table(gridLat_deg,gridLon_deg,repmat('O',size(gridLon_deg)),cell(size(gridLon_deg)),zeros(size(gridLon_deg)),'VariableNames',{'lat_deg','lon_deg','class','files','num_geospatial'}); +%% Set random seed +if ~isempty(p.Results.seed) + oldSeed = rng; + rng(p.Results.seed,'twister'); +end +%% Preallocate, calculate grid, and load airspace +[anchors, gridLat_deg, gridLon_deg, airspace] = preallocAnchors(iso_3166_2, anchorRange_nm, p.Results.fileAirspace, false); + +%% More preallocation % filename outName = ['output' filesep sprintf('pairs-%s-anchorRange%0.2f.mat',outHash,anchorRange_nm)]; @@ -37,19 +42,16 @@ %% Parse filenames and metadata % Get list of filenames -listing = dir([inDir{1} filesep '*.csv']); +listing = dir([inDir{1} filesep '**' filesep '*.csv']); % Concat listings if there is more than one input directory if numel(inDir) > 1 for i=2:1:numel(inDir) - listing = [listing; dir([inDir{i} filesep '*.csv'])]; + listing = [listing; dir([inDir{i} filesep '**' filesep '*.csv'])]; end end -if isempty(listing); warning('RUN_1:listing','No files found inDir, exiting via RETURN \n'); return; end - -% Set consistent random seed -rng(size(listing,1)); +if isempty(listing); warning('RUN_1:listing','No files found inDir, exiting via RETURN'); return; end % Filter trajectories based on isAir = false(size(listing,1),1); @@ -149,7 +151,7 @@ candAir(isD) = 'D'; candAir(isC) = 'C'; candAir(isB) = 'B'; - + % Get coordinates for each trajectory % Preallocate time_s = cell(size(listk)); @@ -161,7 +163,12 @@ % Iterate over files parfor i=1:1:numel(listk) % Load trajectory - [time_s{i}, lat_deg{i}, lon_deg{i}, ~] = fastloadtraj([listk(i).folder filesep listk(i).name]) + TTout = textscantraj([listk(i).folder filesep listk(i).name]); + + % Parse + time_s{i} = seconds(TTout.Time); + lat_deg{i} = TTout.lat_deg; + lon_deg{i} = TTout.lon_deg; % Calculate bounding box lonmin = min(lon_deg{i}); @@ -213,7 +220,11 @@ idxTraj = idxTraj(isClose); % Create full filenames for trajectories which are close to anchor point - files = compose('%s/%s',string({listk(idxTraj).folder}'), string({listk(idxTraj).name}')); + if ispc + files = compose('%s\\%s',string({listk(idxTraj).folder}'), string({listk(idxTraj).name}')); + else + files = compose('%s/%s',string({listk(idxTraj).folder}'), string({listk(idxTraj).name}')); + end % Assign candFiles{i} = [candFiles{i};files]; @@ -226,7 +237,7 @@ % Assign to output anchors.class(isCluster) = candAir; - anchors.files(isCluster) = candFiles; + anchors.files(isCluster) = candFiles; end % End k loop %% Clean and Finish Up @@ -243,3 +254,8 @@ if p.Results.isSave save(outName,'anchors','anchorRange_nm'); end + +%% Reset seed +if ~isempty(p.Results.seed) + rng(oldSeed); +end diff --git a/code/anchor/preallocAnchors.m b/code/anchor/preallocAnchors.m new file mode 100644 index 0000000..4c1b28f --- /dev/null +++ b/code/anchor/preallocAnchors.m @@ -0,0 +1,53 @@ +function [anchors, gridLat_deg, gridLon_deg, airspace] = preallocAnchors(iso_3166_2, anchorRange_nm, fileAirspace, isPlot) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +% +% SEE ALSO createISO31662Grid IdentifyGeographicVariable + +% Input handling +if nargin < 3 || isempty(fileAirspace) + fileAirspace = [getenv('AEM_DIR_CORE') filesep 'output' filesep 'airspace.mat']; +end +if nargin < 4; isPlot = false; end + +% Create grid +[gridLat_deg, gridLon_deg, BoundingBox_wgs84] = createISO31662Grid(iso_3166_2, anchorRange_nm, false); + +% Calculate geographic domain +G = IdentifyGeographicVariable(gridLat_deg,gridLon_deg); + +% Load airspace +load(fileAirspace,'airspace'); + +% Filter to include airspace only in bounding box +[~, ~, inAir] = filterboundingbox(airspace.LAT_deg,airspace.LON_deg,BoundingBox_wgs84); + +% Filter airspace based on altitude surface +isAir = inAir & cellfun(@min,airspace.LOWALT_ft_agl) <= 0; +airspace = airspace(isAir,:); + +% Iterate over airspace +class = repmat('O',size(gridLon_deg)); +for ii=1:1:size(airspace,1) + isAirspace = InPolygon(gridLat_deg,gridLon_deg,airspace.LAT_deg{ii},airspace.LON_deg{ii}); + class(isAirspace) = airspace.CLASS(ii); +end + +% Plot +if isPlot + figure; set(gcf,'name',['preallocAnchors: ' iso_3166_2]); + isB = class == 'B'; + isC = class == 'C'; + isD = class == 'D'; + isO = class == 'O'; + + geoscatter(gridLat_deg(isB),gridLon_deg(isB),'r.','DisplayName','Class B'); hold on; + geoscatter(gridLat_deg(isC),gridLon_deg(isC),'m.','DisplayName','Class C'); + geoscatter(gridLat_deg(isD),gridLon_deg(isD),'b.','DisplayName','Class D'); + geoscatter(gridLat_deg(isO),gridLon_deg(isO),'w.','DisplayName','Other'); hold off; + set(gca,'Basemap','streets-dark','TickLabelFormat','dd','FontSize',12,'FontName','Arial','FontWeight','bold'); + legend('Location','best','TextColor','w','FontSize',10,'FontName','Arial','FontWeight','bold'); legend('boxoff'); +end + +% Assign output +anchors = table(gridLat_deg,gridLon_deg,class,cell(size(gridLon_deg)),zeros(size(gridLon_deg)),G,'VariableNames',{'lat_deg','lon_deg','class','files','num_geospatial','G'}); diff --git a/code/encounter/README.md b/code/encounter/README.md new file mode 100644 index 0000000..cb70e9d --- /dev/null +++ b/code/encounter/README.md @@ -0,0 +1,17 @@ +# Encounter + +Functions primarily related to identifying conflicts and encounters + +## Distribution Statement + +DISTRIBUTION STATEMENT A. Approved for public release. Distribution is unlimited. + +This material is based upon work supported by the Federal Aviation Administration under Air Force Contract No. FA8702-15-D-0001. Any opinions, findings, conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Federal Aviation Administration. + +© 2019-2021 Massachusetts Institute of Technology. + +Delivered to the U.S. Government with Unlimited Rights, as defined in DFARS Part 252.227-7013 or 7014 (Feb 2014). Notwithstanding any copyright notice, U.S. Government rights in this work are defined by DFARS 252.227-7013 or DFARS 252.227-7014 as detailed above. Use of this work other than as specifically authorized by the U.S. Government may violate any copyrights that exist in this work. + +Any opinions, findings, conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Federal Aviation Administration. + +This document is derived from work done for the FAA (and possibly others), it is not the direct product of work done for the FAA. The information provided herein may include content supplied by third parties. Although the data and information contained herein has been produced or processed from sources believed to be reliable, the Federal Aviation Administration makes no warranty, expressed or implied, regarding the accuracy, adequacy, completeness, legality, reliability, or usefulness of any information, conclusions or recommendations provided herein. Distribution of the information contained herein does not constitute an endorsement or warranty of the data or information provided herein by the Federal Aviation Administration or the U.S. Department of Transportation. Neither the Federal Aviation Administration nor the U.S. Department of Transportation shall be held liable for any improper or incorrect use of the information contained herein and assumes no responsibility for anyone’s use of the information. The Federal Aviation Administration and U.S. Department of Transportation shall not be liable for any claim for any loss, harm, or other damages arising from access to or use of data information, including without limitation any direct, indirect, incidental, exemplary, special or consequential damages, even if advised of the possibility of such damages. The Federal Aviation Administration shall not be liable for any decision made or action taken, in reliance on the information contained herein. diff --git a/code/encounter/createEncounters_2.m b/code/encounter/createEncounters_2.m new file mode 100644 index 0000000..7993b86 --- /dev/null +++ b/code/encounter/createEncounters_2.m @@ -0,0 +1,546 @@ +function [metadata] = createEncounters_2(inAnchors,outDir,encTime_s,thresHorz_ft,thresVert_ft,varargin) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +% CREATEENCOUNTERS_2 OUTPUTS ENCOUNTERS IN WAYPOINT FORMAT +% +% SEE ALSO loadTrack sampleSpeedAlt adjustTrack findConflict findCropIdx trackTimetable2NEU plotEncounter + +%% Input parser +p = inputParser; + +% Required +addRequired(p,'inAnchors'); % Output of RUN_1 +addRequired(p,'outDir'); % Output directory +addRequired(p,'encTime_s',@isnumeric); % Duration of encounter +addRequired(p,'thresHorz_ft',@isnumeric); +addRequired(p,'thresVert_ft',@isnumeric); + +% Optional - Minimum encounter initial conditions +addParameter(p,'conflictTime_s',ceil(encTime_s/2),@isnumeric); % Time at which conflict critieria are satisfied +addParameter(p,'initHorz_ft',[6076 97217],@(x) isnumeric(x) && numel(x) == 2); %97217 ~ 16 nm ~ 421 feet (250 knots) * 240 seconds (uncor MVL) +addParameter(p,'initVert_ft',[0 1500],@(x) isnumeric(x) && numel(x) == 2); +addParameter(p,'spheroid_ft',wgs84Ellipsoid('ft')); + +% Optional - Anchor +addParameter(p,'classInclude',{'B','C','D','O'},@iscell); + +% Optional - Encounter Variations +addParameter(p,'maxTries',6,@(x) isnumeric(x) && numel(x) == 1); +addParameter(p,'maxEncPerPair',1,@isnumeric); % Maximum waypoint pairs for each unique pair +addParameter(p,'timeStep',seconds(1),@isduration); % Output timestep, CSIM requires at least >= 1 +addParameter(p,'anchorPercent',1.0,@isnumeric); % Maximum waypoint combinations to consider for each pair + +% Optional - AC permutations +addParameter(p,'mdlType1','geospatial',@(x) ischar(x) && any(strcmpi(x,{'bayes','geospatial'}))); +addParameter(p,'mdlType2','bayes',@(x) ischar(x) && any(strcmpi(x,{'bayes','geospatial'}))); +addParameter(p,'inDir1',{},@iscell); +addParameter(p,'inDir2',{},@iscell); + +% Optional - Airspeed Sampling (Geospatial only) +addParameter(p,'isSampleV1',true,@islogical); +addParameter(p,'isSampleV2',false,@islogical); +addParameter(p,'rangeV1_ft_s',[9 168],@(x) isnumeric(x) && numel(x) == 2); % ~ [5,100] knots +addParameter(p,'rangeV2_ft_s',[9 422],@(x) isnumeric(x) && numel(x) == 2); % ~ [5,100] knots + +% Optional - Altitude Sampling (Geospatial only) +addParameter(p,'isSampleAlt1',true,@islogical); +addParameter(p,'isSampleAlt2',false,@islogical); +addParameter(p,'rangeAlt1_ft_agl',[50 1200],@(x) isnumeric(x) && numel(x) == 2); +addParameter(p,'rangeAlt2_ft_agl',[50 2000],@(x) isnumeric(x) && numel(x) == 2); +addParameter(p,'isGeoPointOverride',true,@islogical); + +% Optional - Bayes Tracks +addParameter(p,'bayesFile1',[getenv('AEM_DIR_BAYES') filesep 'model' filesep 'uncor_1200only_fwse_v1p2.txt']); +addParameter(p,'bayesFile2',[getenv('AEM_DIR_BAYES') filesep 'model' filesep 'uncor_1200only_fwse_v1p2.txt']); +addParameter(p,'z_agl_tol_ft',200,@(x) isnumeric(x) && numel(x) == 1); % Used by placeTrack() +addParameter(p,'dem','dted1',@(x) isstr(x) && any(strcmpi(x,{'dted1','dted2','globe','gtopo30','srtm1','srtm3','srtm30'}))); +addParameter(p,'demDir',char.empty(0,0),@ischar); +addParameter(p,'demBackup','globe',@(x) isstr(x) && any(strcmpi(x,{'dted1','dted2','globe','gtopo30','srtm1','srtm3','srtm30'}))); +addParameter(p,'demBackupDir',char.empty(0,0),@ischar); +addParameter(p,'labelTime','time_s'); +addParameter(p,'labelX','x_ft'); +addParameter(p,'labelY','y_ft'); +addParameter(p,'labelZ','z_ft'); + +% Optional - FAA DOF +addParameter(p,'dofFile',[getenv('AEM_DIR_CORE') filesep 'output' filesep 'dof.mat'],@ischar); % Location of output of RUN_readfaadof from em-core +addParameter(p,'dofTypes',{'ag equip','antenna','lgthouse','met','monument','silo','spire','stack','t-l twr','tank','tower','utility pole','windmill'},@iscell); % Obstacles to keep +addParameter(p,'dofMinHeight_ft',50,@isnumeric); % Minimum height of obstacles +addParameter(p,'dofMaxRange_ft',500,@isnumeric); +addParameter(p,'dofMaxVert_ft',50,@isnumeric); + +% Optional - Random seed +addParameter(p,'initialSeed',0,@isnumeric); + +% Optional - Output +addParameter(p,'outHash',char.empty(0,0),@ischar); +addParameter(p,'isPlot',false,@islogical); +addParameter(p,'isZip',false,@islogical); + +% Row offsets +% These are needed because the geospatial trajectory generator will add +% vertical take off and landing segements, which you may not want +addParameter(p,'isStartEndNonZero1',true,@islogical); % +addParameter(p,'isStartEndNonZero2',true,@islogical); % + +% Parse +parse(p,inAnchors,outDir,encTime_s,thresHorz_ft,thresVert_ft,varargin{:}); + +initHorz_ft = p.Results.initHorz_ft; +initVert_ft = p.Results.initVert_ft; +conflictTime_s = p.Results.conflictTime_s; + +dofMaxRange_ft = p.Results.dofMaxRange_ft; +dofMaxVert_ft = p.Results.dofMaxVert_ft; + +maxTries = p.Results.maxTries; +maxEncPerPair = p.Results.maxEncPerPair; +timeStep = p.Results.timeStep; +spheroid_ft = p.Results.spheroid_ft; +z_agl_tol_ft = p.Results.z_agl_tol_ft; +mdlType1 = p.Results.mdlType1; +mdlType2 = p.Results.mdlType2; +isPlot = p.Results.isPlot; + +isStartEndNonZero1 = p.Results.isStartEndNonZero1; +isStartEndNonZero2 = p.Results.isStartEndNonZero2; + +isSampleAlt1 = p.Results.isSampleAlt1; +isSampleV1 = p.Results.isSampleV1; +isSampleAlt2 = p.Results.isSampleAlt2; +isSampleV2 = p.Results.isSampleV2; +isGeoPointOverride = p.Results.isGeoPointOverride; + +rangeAlt1_ft_agl = p.Results.rangeAlt1_ft_agl; +rangeV1_ft_s = p.Results.rangeV1_ft_s; +rangeAlt2_ft_agl = p.Results.rangeAlt2_ft_agl; +rangeV2_ft_s = p.Results.rangeV2_ft_s; + +%% Assertions and More Input Handling +if strcmpi(mdlType1,'bayes') & (isSampleAlt1 | isSampleV1) + isSampleAlt1 = false; + isSampleV1 = false; + warning('Cannot adjust altitude and velocity for Bayes tracks, setting isSampleAlt1 and isSampleV1 to false'); +end + +if strcmpi(mdlType2,'bayes') & (isSampleAlt2 | isSampleV2) + isSampleAlt2 = false; + isSampleV2 = false; + warning('Cannot adjust altitude and velocity for Bayes tracks, setting isSampleAlt2 and isSampleV2 to false'); +end + +assert(all(encTime_s >= conflictTime_s),'Failed to satisfy encTime_s >= conflictTime_s'); +assert(~all(isSampleAlt1 && strcmp(mdlType1,'bayes')),'createEncounters_2:isSampleAlt1-acmodel1','isSampleAlt1 cannot be true when using bayes model'); +assert(~all(isSampleAlt2 && strcmp(mdlType2,'bayes')),'createEncounters_2:isSampleAlt2-acmodel2','isSampleAlt2 cannot be true when using bayes model'); +assert(strcmp(spheroid_ft.LengthUnit,'foot'),'createEncounters_2:LengthUnit','spheroid LengthUnit must be ''foot'''); + +% Look ahead and back time relative to conflict time +durBeforeConflict_s = conflictTime_s; +durAfterConflict_s = encTime_s - conflictTime_s; + +%% Inputs hardcode +bayesDuration_s = 240; +maxSeed = 2^32; +encPerFile = 100000; +metaVarNames = {'encId','anchorRow','anchorFile','lat0_deg','lon0_deg','el0_ft_msl','randSeed',... + 'hmd_conflict_ft','vmd_conflict_ft','hmd_cpa_ft','vmd_cpa_ft','hmd_initial_ft','vmd_initial_ft','hmd_final_ft','vmd_final_ft',... + 'speed1_conflict_ft_s','speed2_conflict_ft_s','speed1_cpa_ft_s','speed2_cpa_ft_s','speed1_initial_ft_s','speed2_initial_ft_s','speed1_final_ft_s','speed2_final_ft_s'... + 'time_enc_s','time_conflict_s','time_cpa_s',... + 'G','A'}; + +%% Initialize encounter structs and encounter counter +encCount = 0; +randSeed = p.Results.initialSeed; + +if isempty(p.Results.outHash) + outName = 'enc_'; +else + outName = ['enc_' p.Results.outHash '_']; +end + +rng(randSeed,'twister'); + +%% Load output from RUN_1 + +if ischar(inAnchors) && isfile(inAnchors) + + % Load anchors + load(inAnchors,'anchors','anchorRange_nm'); + + % Remove anchors without geospaital trajectories + anchors(anchors.num_geospatial == 0,:) = []; + + % Filter by airspace class + isAirspace = false(size(anchors,1),1); + for i=1:1:numel(p.Results.classInclude) + isAirspace = isAirspace | anchors.class == p.Results.classInclude{i}; + end + anchors = anchors(isAirspace,:); + + % If both aircraft are geospatial, filter to anchors with multiple files + if strcmpi(mdlType1,'geospatial') && strcmpi(mdlType2,'geospatial') + anchors = anchors(anchors.num_geospatial > 1,:); + end + + % Filter by anchor percentage + if p.Results.anchorPercent ~= 1.0 + pidx = randperm(size(anchors,1), floor(size(anchors,1) * p.Results.anchorPercent)); + anchors = anchors(pidx',:); + end + +else + if istable(inAnchors) + anchors = inAnchors; + anchorRange_nm = inAnchors.anchorRange_nm(1); + else + metadata = table.empty(0,numel(metaVarNames)); + warning('Input anchors not found...RETURNING'); + return; + end +end + +% Number of anchors +numAnchors = size(anchors,1); + +% Convert anchorRange_nm from RUN_1 to feet to match spheroid +% An assert above ensures p.Results.spheroid.LengthUnit = 'foot' +anchorRange_ft = anchorRange_nm * unitsratio(spheroid_ft.LengthUnit,'nm'); + +%% Update anchors with geographic domain and airspace class +% This information is used to set the UncorEncounterModel start property +if strcmpi(mdlType1,'bayes') | strcmpi(mdlType2,'bayes') + % Geographic domain + if ~any(strcmpi(anchors.Properties.VariableNames,'G')) + warning('anchors table does not have variable, G; calculating now'); + %anchors.G = ones(size(anchors,1),1); + anchors.G = IdentifyGeographicVariable(anchors.lat_deg,anchors.lon_deg); + end + + % Airspace class + anchors.A = repmat(4,size(anchors,1),1); + anchors.A(anchors.class == 'B') = 1; + anchors.A(anchors.class == 'C') = 2; + anchors.A(anchors.class == 'D') = 3; + anchors.A(anchors.class == 'O') = 4; +end + +%% Load FAA DOF +BoundingBox_wgs84 = [min(anchors.lon_deg), min(anchors.lat_deg); max(anchors.lon_deg), max(anchors.lat_deg)]; + +[~, Tdof] = gridDOF('inFile',p.Results.dofFile,... + 'BoundingBox_wgs84',BoundingBox_wgs84,... + 'minHeight_ft',p.Results.dofMinHeight_ft,... + 'isVerified',true,... + 'obsTypes',p.Results.dofTypes); + +%% Load Bayes models +if strcmpi(mdlType1,'bayes'); mdl1 = UncorEncounterModel('parameters_filename',p.Results.bayesFile1); end +if strcmpi(mdlType2,'bayes'); mdl2 = UncorEncounterModel('parameters_filename',p.Results.bayesFile2); end + +%% Preallocate metadata +nEnc = sum(anchors.num_geospatial) * maxEncPerPair; +pad = nan(nEnc,1); %pad = PreAllocated Double +metadata = table((1:1:nEnc)',pad,pad,pad,pad,pad,pad,pad,pad,pad,pad,pad,pad,pad,pad,pad,pad,pad,pad,pad,pad,pad,pad,repmat(encTime_s,nEnc,1),repmat(conflictTime_s,nEnc,1),pad,pad,pad,... + 'VariableNames',metaVarNames); + +%% Iterate over anchors +cEnc = 0; % Encounter counter +cFiles = 0; % Files written counter +waypointsBuffer = []; + +for ii=1:1:numAnchors + % Parse iith anchor + iiAnchor = anchors(ii,:); + lat0_deg = iiAnchor.lat_deg; + lon0_deg = iiAnchor.lon_deg; + iiFiles = iiAnchor.files{1}; + + % Load DEM near anchor + % 1 knot = 1.68781 feet per second + rad_ft = encTime_s * 600 * 1.68781; + [latc, lonc] = scircle1(lat0_deg,lon0_deg,rad_ft,[],spheroid_ft); + dem = p.Results.dem; + demDir = p.Results.demDir; + [el0_ft_msl,~,Z_m,refvec,R] = msl2agl([lat0_deg; min(latc); max(latc)], [lon0_deg; min(lonc); max(lonc)],dem,'demDir',demDir,'isCheckOcean',true,'isFillAverage',true); + + % If primary DEM didn't work, try backup + % This is often due to missing DEM tiles + if isempty(el0_ft_msl) + dem = p.Results.demBackup; + demDir = p.Results.demBackupDir; + [el0_ft_msl,~,Z_m,refvec,R] = msl2agl([lat0_deg; min(latc); max(latc)], [lon0_deg; min(lonc); max(lonc)],dem,'demDir',demDir,'isCheckOcean',true,'isFillAverage',true); + end + + % Get MSL elevation of (lat0_deg,lon0_deg) + el0_ft_msl = el0_ft_msl(1); + + % Filter obstacles within range of anchor + bbox = [min(lonc), min(latc); max(lonc), max(latc)]; + [~, ~, isbox] = filterboundingbox(Tdof.lat_deg,Tdof.lon_deg,bbox); + iiDof = Tdof(isbox,:); + + % if isPlot + % fig = plotAnchor(iiAnchor,iiDof); + % end + + % Start distribution for sampling uncorrelated encounter model + if strcmpi(mdlType1,'bayes') | strcmpi(mdlType2,'bayes') + bayesStart = {iiAnchor.G,iiAnchor.A,[],[],[],[],[]}; + end + + % Altitude start distribution is random and set further in the code + if strcmpi(mdlType1,'bayes'); + idxL1 = find(strcmp(mdl1.labels_initial,'"L"')); + binMaxL1 = find(mdl1.boundaries{idxL1} < rangeAlt1_ft_agl(2),1,'last'); + end + + if strcmpi(mdlType2,'bayes'); + idxL2 = find(strcmp(mdl2.labels_initial,'"L"')); + binMaxL2 = find(mdl2.boundaries{idxL2} < rangeAlt2_ft_agl(2),1,'last'); + end + + % Create pairs + if strcmpi(mdlType1,'geospatial') && strcmpi(mdlType2,'geospatial') + pairs = nchoosek(1:iiAnchor.num_geospatial,2); + else + % both bayes + if strcmpi(mdlType1,'bayes') && strcmpi(mdlType2,'bayes') + pairs = [nan, nan]; + else + % mdlType1 = 'geospatial' and mdlType2 = 'bayes' + if strcmpi(mdlType1,'geospatial') + pairs = [(1:iiAnchor.num_geospatial)', nan(iiAnchor.num_geospatial,1)]; + else + % mdlType1 = 'bayes' and mdlType2 = 'geospatial' + pairs = [nan(iiAnchor.num_geospatial,1), (1:iiAnchor.num_geospatial)']; + end + end + end + + % Iterate over pairs + for jj=1:1:size(pairs,1) + + % Load Aircraft 1 + switch mdlType1 + case 'geospatial' + track1 = loadTrack(mdlType1, iiFiles(pairs(jj,1)), isStartEndNonZero1,rangeAlt1_ft_agl, rangeV1_ft_s, spheroid_ft, lat0_deg,lon0_deg,el0_ft_msl); + case 'bayes' + % Update Bayesian start distribution + bayesStart{idxL1} = randi(binMaxL1,1); + mdl1.start = bayesStart; + track1 = loadTrack(mdlType1, mdl1, isStartEndNonZero1, rangeAlt1_ft_agl, rangeV1_ft_s, spheroid_ft, lat0_deg,lon0_deg,el0_ft_msl,Z_m,refvec,R,iiDof,dofMaxRange_ft,dofMaxVert_ft,bayesDuration_s,z_agl_tol_ft); + end + + % Initialize counters + c = 1; + jjNumEncs = 0; + conflicts = double.empty(0,10); + + % Iterate until we get the desired number of encounters or we reach maxTries + while c <= maxTries + + % Reload Aircraft 1 if Bayes + switch mdlType1 + case 'bayes' + % Update Bayesian start distribution + bayesStart{idxL1} = randi(binMaxL1,1); + mdl1.start = bayesStart; + track1 = loadTrack(mdlType1, mdl1, isStartEndNonZero1, rangeAlt1_ft_agl, rangeV1_ft_s, spheroid_ft, lat0_deg,lon0_deg,el0_ft_msl,Z_m,refvec,R,iiDof,dofMaxRange_ft,dofMaxVert_ft,bayesDuration_s,z_agl_tol_ft); + end + + % Load Aircraft 2 + switch mdlType2 + case 'geospatial' + track2 = loadTrack(mdlType2, iiFiles(pairs(jj,2)), isStartEndNonZero2, rangeAlt2_ft_agl, rangeV2_ft_s, spheroid_ft, lat0_deg,lon0_deg,el0_ft_msl); + case 'bayes' + % Update Bayesian start distribution + bayesStart{idxL2} = randi(binMaxL2,1); + mdl2.start = bayesStart; + track2 = loadTrack(mdlType2, mdl2, isStartEndNonZero2, rangeAlt2_ft_agl, rangeV2_ft_s, spheroid_ft, lat0_deg,lon0_deg,el0_ft_msl,Z_m,refvec,R,iiDof,dofMaxRange_ft,dofMaxVert_ft,bayesDuration_s,z_agl_tol_ft); + end + + % Determine if tracks are short (this is bad) + isShort1 = size(track1,1) < encTime_s & ~isSampleV1; + isShort2 = size(track2,1) < encTime_s & ~isSampleV2; + + % Do something if not empty, not missing, and not short + if ~isempty(track1) && ~isempty(track2) && ~any(ismissing(track1),'all') && ~any(ismissing(track2),'all') && ~isShort1 && ~isShort2 + + % Sample airspeed and altitude adjustments + % If desired, override geospatial tracks assumed to be + % point inspections. Since sampleSpeedAlt() checks model + % type, Bayes tracks will still return no adjustements + pointPatterns = {'uswtdb','dof'}; + if isGeoPointOverride && strcmpi(mdlType1,'geospatial') && contains(iiFiles(pairs(jj,1)),pointPatterns,'IgnoreCase',true) + [altAdjust1_ft, vAdjust1_ft_s] = sampleSpeedAlt(mdlType1,track1, rangeAlt1_ft_agl, [2 16], false, true, 1); + else + [altAdjust1_ft, vAdjust1_ft_s] = sampleSpeedAlt(mdlType1,track1, rangeAlt1_ft_agl, rangeV1_ft_s, isSampleAlt1, isSampleV1, 1); + end + + if isGeoPointOverride && strcmpi(mdlType2,'geospatial') && contains(iiFiles(pairs(jj,2)),pointPatterns,'IgnoreCase',true) + [altAdjust2_ft, vAdjust2_ft_s] = sampleSpeedAlt(mdlType2,track2, rangeAlt2_ft_agl, [2 16], false, true, 1); + else + [altAdjust2_ft, vAdjust2_ft_s] = sampleSpeedAlt(mdlType2,track2, rangeAlt2_ft_agl, rangeV2_ft_s, isSampleAlt2, isSampleV2, 1); + end + + % Adjust altitude and speed + ctrack1 = adjustTrack(track1, altAdjust1_ft, vAdjust1_ft_s); + ctrack2 = adjustTrack(track2, altAdjust2_ft, vAdjust2_ft_s); + + % Identify which combinaton of waypoints satisfy + % HMD, VMD, and time criteria + conflicts = findConflict(mdlType1,mdlType2,ctrack1,ctrack2,lat0_deg,lon0_deg,anchorRange_ft,thresHorz_ft,thresVert_ft,durBeforeConflict_s,durAfterConflict_s,spheroid_ft); + + % Do something if there is any conflicts + if ~isempty(conflicts) + + % Find combination of start and end indicies + [conflicts,sidx1,sidx2,eidx1,eidx2] = findCropIdx(conflicts,mdlType1,mdlType2,ctrack1,ctrack2,initHorz_ft,initVert_ft,durBeforeConflict_s,durAfterConflict_s); + + % Do something if conflicts are not empty + if ~isempty(conflicts) + % Select one + idx = randi(size(conflicts,1),1,1); + conflicts = conflicts(idx,:); + sidx1 = sidx1(idx); + sidx2 = sidx2(idx); + eidx1 = eidx1(idx); + eidx2 = eidx2(idx); + + % Convert timetables to arrays in a specific column order + [ac1NEU, ac1speed_ft_s] = trackTimetable2NEU(ctrack1,sidx1,eidx1,timeStep); + [ac2NEU, ac2speed_ft_s] = trackTimetable2NEU(ctrack2,sidx2,eidx2,timeStep); + + % Calculate horizontal and vertical seperation + r_ft = hypot(ac1NEU(:,2)-ac2NEU(:,2),ac1NEU(:,3)-ac2NEU(:,3)); + z_ft = abs(ac1NEU(:,4)-ac2NEU(:,4)); + d_ft = sqrt((ac1NEU(:,2)-ac2NEU(:,2)).^2 + (ac1NEU(:,3)-ac2NEU(:,3)).^2 + (ac1NEU(:,4)-ac2NEU(:,4)).^2); + + % Confirm that initial conditions are satisifed + initHMDGood = r_ft(1) >= initHorz_ft(1) & r_ft(1) <= initHorz_ft(2); + initVMDGood = z_ft(1) >= initVert_ft(1) & z_ft(1) <= initVert_ft(2); + if initHMDGood & initVMDGood + + % Identify when CPA occurs + [~,idxCPA] = min(d_ft); + + % Create waypoints struct + cwaypoints = neu2Waypoints(ac1NEU,ac2NEU); + + % Append waypoints buffer + waypointsBuffer = [waypointsBuffer cwaypoints]; + + % Update counters + cEnc = cEnc + 1; + jjNumEncs = jjNumEncs + 1; + + % Update metadata + metadata.anchorRow(cEnc) = ii; + metadata.anchorFile(cEnc) = jj; + metadata.lat0_deg(cEnc) = lat0_deg; + metadata.lon0_deg(cEnc) = lon0_deg; + metadata.el0_ft_msl(cEnc) = el0_ft_msl; + metadata.randSeed(cEnc) = randSeed; + + % Update metadata: HMD / VMD + metadata.hmd_conflict_ft(cEnc) = conflicts(:,9); + metadata.vmd_conflict_ft(cEnc) = conflicts(:,10); + metadata.hmd_cpa_ft(cEnc) = r_ft(idxCPA); + metadata.vmd_cpa_ft(cEnc) = z_ft(idxCPA); + metadata.hmd_initial_ft(cEnc) = r_ft(1); + metadata.vmd_initial_ft(cEnc) = z_ft(1); + metadata.hmd_final_ft(cEnc) = r_ft(end); + metadata.vmd_final_ft(cEnc) = z_ft(end); + + % Update metadata: speed + metadata.speed1_conflict_ft_s(cEnc) = ac1speed_ft_s(conflictTime_s); + metadata.speed2_conflict_ft_s(cEnc) = ac2speed_ft_s(conflictTime_s); + metadata.speed1_cpa_ft_s(cEnc) = ac1speed_ft_s(idxCPA); + metadata.speed2_cpa_ft_s(cEnc) = ac2speed_ft_s(idxCPA); + metadata.speed1_initial_ft_s(cEnc) = ac1speed_ft_s(1); + metadata.speed2_initial_ft_s(cEnc) = ac2speed_ft_s(1); + metadata.speed1_final_ft_s(cEnc) = ac1speed_ft_s(end); + metadata.speed2_final_ft_s(cEnc) = ac2speed_ft_s(end); + + % Update metadata: cpa time + metadata.time_cpa_s(cEnc) = idxCPA; + + % Update metadata: G, A + metadata.G(cEnc) = iiAnchor.G; + metadata.A(cEnc) = iiAnchor.A; + + % Write to file + if cEnc > 0 && mod(cEnc,encPerFile) == 0 + % Create filename and write to file + outFile = [outDir filesep outName sprintf('%05.f',cFiles) '.dat']; + save_waypoints(outFile,waypointsBuffer); + + % Display status to screen + fprintf('%i encounters written to file: %s\n',cEnc,outFile); + + % Update counter and reset buffer + cFiles = cFiles + 1; + waypointsBuffer = []; + end + + % Plot + if isPlot + fig = plotEncounter(conflicts,mdlType1,mdlType2,ctrack1,ctrack2,sidx1,eidx1,sidx2,eidx2,lat0_deg,lon0_deg,randSeed); + end + end + end + end + end + + % Advance counter + % If sufficient encounters generated, set counter to Inf to break loop + if jjNumEncs >= maxEncPerPair + nTries = c; + c = inf; + else + nTries = c; + c = c + 1; + end + + % Update random seed + randSeed = randSeed + 1; + if randSeed < maxSeed + rng(randSeed,'twister'); + else + warning('Maximum random seed exceeded, seed not updated'); + end + + end + + % Display status + fprintf('%i encounters generated in %i attempts when lat0_deg=%0.4f, lon0_deg=%0.4f, ii=%i, jj=%i\n',jjNumEncs,nTries,lat0_deg,lon0_deg,ii,jj); + end + + % Display status + %fprintf('%i / %i\n',ii,numAnchors); +end % End ii + + +% Write last file of remaining encounters in waypointsBuffer +if cEnc > 0 && ~isempty(waypointsBuffer) + % Create filename and write to file + outFile = [outDir filesep outName sprintf('%05.f',cFiles) '.dat']; + save_waypoints(outFile,waypointsBuffer); + + % Display status to screen + fprintf('%i encounters written to file: %s\n',cEnc,outFile); +end + +% Remove unused rows in metadata table +if cEnc > 0 + metadata = metadata(1:cEnc,:); +else + metadata = table.empty(0,13); +end + +%% Zip waypoints for easy sharing +if cEnc > 0 + if p.Results.isZip; zip([outDir '.zip'],[outDir filesep '*.dat']); end +else + fprintf('Nothing to zip, no encounter generated for %s\n',inAnchors); +end diff --git a/code/encounter/findConflict.m b/code/encounter/findConflict.m new file mode 100644 index 0000000..783b5c1 --- /dev/null +++ b/code/encounter/findConflict.m @@ -0,0 +1,159 @@ +function [conflicts] = findConflict(mdlType1,mdlType2,track1,track2,lat0_deg,lon0_deg,anchorRange_ft,thresHorz_ft,thresVert_ft,durBeforeConflict_s,durAfterConflict_s,spheroid_ft) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +% +% SEE ALSO createEncounters_2 + +%% Preallocate output +conflicts = double.empty(0,10); + +%% Parse +% aircraft 1 +ac1Lat_deg = track1.lat_deg; +ac1Lon_deg = track1.lon_deg; +ac1Alt_ft_msl = track1.alt_ft_msl; + +% aircraft 2 +ac2Lat_deg = track2.lat_deg; +ac2Lon_deg = track2.lon_deg; +ac2Alt_ft_msl = track2.alt_ft_msl; + +%% Determine which waypoints are close to anchor point +% Circle a circle around the anchor point +% Find which coordinates are within circle around anchor point +[lat0c,lon0c] = scircle1(lat0_deg,lon0_deg,anchorRange_ft,[],spheroid_ft,'degrees',100); + +idxAnchor1 = find(InPolygon(ac1Lat_deg,ac1Lon_deg,lat0c,lon0c)); +idxAnchor2 = find(InPolygon(ac2Lat_deg,ac2Lon_deg,lat0c,lon0c)); + +% When using placeTrack() for positioning Bayes tracks, it is possible to +% have the track originally fly through the anchor point but during +% filtering due to terrain or obstacles, have it removed. This would result +% in the InPolygon() check above returning empty +switch mdlType1 + case 'bayes' + if isempty(idxAnchor1) + idxAnchor1 = (1:1:numel(ac1Lat_deg))'; + end +end +switch mdlType2 + case 'bayes' + if isempty(idxAnchor2) + idxAnchor2 = (1:1:numel(ac2Lat_deg))'; + end +end + +%% Check HMD, VMD, and time criteria +% Make sure both aircraft satisfy the InPolygon check +if ~isempty(idxAnchor1) && ~isempty(idxAnchor2) + + % Calculate distance between waypoint combinations + % Determine which aircraft has more potential points + if numel(idxAnchor1) >= numel(idxAnchor2) + % More AC1, so we generate circles for AC2 + idxcol = idxAnchor2; + latcol = ac2Lat_deg(idxAnchor2); + loncol = ac2Lon_deg(idxAnchor2); + + idxrow = idxAnchor1; + latrow = ac1Lat_deg(idxAnchor1); + lonrow = ac1Lon_deg(idxAnchor1); + else + % More AC2, so we generate circles for AC1 + idxrow = idxAnchor2; + latrow = ac2Lat_deg(idxAnchor2); + lonrow = ac2Lon_deg(idxAnchor2); + + idxcol = idxAnchor1; + latcol = ac1Lat_deg(idxAnchor1); + loncol = ac1Lon_deg(idxAnchor1); + end + + % Generate scircles for columns + [latc,lonc] = scircle1(latcol,loncol,repmat(thresHorz_ft,size(loncol)),[],spheroid_ft,'degrees',100); + + % Preallocate logical matrix + isClose = sparse(numel(latrow),numel(latcol)); + + % Iterate over circles + for i=1:1:numel(latcol) + isClose(:,i) = InPolygon(latrow,lonrow,latc(:,i),lonc(:,i)); + end + + % Convert from linear indices to subscripts + [row,col,~] = find(isClose); + %[row,col] = ind2sub(size(isClose),find(isClose)); + + % Regenerate c with those that satisfy thresHorz_ft + % AC1 should be column 1, AC2 should be column 2 + if numel(idxAnchor1) >= numel(idxAnchor2) + c = [idxrow(row),idxcol(col)]; + else + c = [idxcol(col),idxrow(row)]; + end + + % Calculate HMD, VMD, and filter + if ~isempty(c) + % Calculate distance aka HMD + %hmd_ft = distance(ac1Lat_deg(c(:,1)),ac1Lon_deg(c(:,1)),ac2Lat_deg(c(:,2)),ac2Lon_deg(c(:,2)),spheroid_ft); + hmd_ft = hypot(track1.north_ft(c(:,1)) - track2.north_ft(c(:,2)),track1.east_ft(c(:,1)) - track2.east_ft(c(:,2))); + + % Calculate vertical miss distance + vmd_ft = abs(ac1Alt_ft_msl(c(:,1)) - ac2Alt_ft_msl(c(:,2))); + + % Identify those in VMD and HMD conflict + lhmd = hmd_ft <= thresHorz_ft; + lvmd = vmd_ft <= thresVert_ft; + idxConflicts = find(lhmd & lvmd); + + % Filter to those in VMD and HMD conflict + c = c(idxConflicts,:); + hmd_ft = hmd_ft(idxConflicts); + vmd_ft = vmd_ft(idxConflicts); + end + + % Filter time criteria and assign function output + if ~isempty(c) + % Total duration of tracks + len1 = size(track1,1); + len2 = size(track2,1); + + % Identify conflicts that have enough time before + % and after threshold satisfied + % This is forward propagation and always valid + isValid1 = ((c(:,1) + durAfterConflict_s) <= len1) & ((c(:,1) - durBeforeConflict_s) >= 1); + isValid2 = ((c(:,2) + durAfterConflict_s) <= len2) & ((c(:,2) - durBeforeConflict_s) >= 1); + + % If geospatial, also need to account fo backward propagation + % This could disqualify some valid forward / forward propagation cases + % additional flexibility can be addressed in a future release + switch mdlType1 + case 'geospatial' + isValid1 = isValid1 & ((c(:,1) + durBeforeConflict_s) <= len1) & ((c(:,1) - durAfterConflict_s) >= 1); + end + switch mdlType2 + case 'geospatial' + isValid2 = isValid2 & ((c(:,2) + durBeforeConflict_s) <= len2) & ((c(:,2) - durAfterConflict_s) >= 1); + end + + % Filter + isValidAll = isValid1 & isValid2; + c = c(isValidAll,:); + vmd_ft = vmd_ft(isValidAll); + hmd_ft = hmd_ft(isValidAll); + + % Assign output + conflicts = zeros(size(c,1),1); + conflicts(:,1) = ac1Lat_deg(c(:,1)); % AC1 Latitude + conflicts(:,2) = ac1Lon_deg(c(:,1)); % AC1 Longitude + conflicts(:,3) = ac2Lat_deg(c(:,2)); % AC2 Latitude + conflicts(:,4) = ac2Lon_deg(c(:,2)); % AC2 Longitude + conflicts(:,5) = ac1Alt_ft_msl(c(:,1)); % AC1 Altitude + conflicts(:,6) = ac2Alt_ft_msl(c(:,2)); % AC2 Altitude + conflicts(:,7) = c(:,1); + conflicts(:,8) = c(:,2); + conflicts(:,9) = hmd_ft; + conflicts(:,10) = vmd_ft; + end +end + diff --git a/code/encounter/findCropIdx.m b/code/encounter/findCropIdx.m new file mode 100644 index 0000000..54e7a01 --- /dev/null +++ b/code/encounter/findCropIdx.m @@ -0,0 +1,123 @@ +function [conflictsOut,sidx1,sidx2,eidx1,eidx2] = findCropIdx(conflicts,mdlType1,mdlType2,track1,track2,initHorz_ft,initVert_ft,durBeforeConflict_s,durAfterConflict_s) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +% +% There are four direction combinations we can arrive at the specific +% waypoint combination. Think of it was foward / backward propagation for +% each aircraft, so 2 X 2 = 4 propagation combinations. This results in +% comprehensive ways to get arrive at this CPA at a given time +% +% SEE ALSO createEncounters_2 + +%% Input parser +p = inputParser; + +% Required +addRequired(p,'conflicts',@isnumeric); % +addRequired(p,'mdlType1',@ischar); % +addRequired(p,'mdlType2',@ischar); % +addRequired(p,'track1',@istimetable); % +addRequired(p,'track2',@istimetable); % +addRequired(p,'initHorz_ft',@isvector); % +addRequired(p,'initVert_ft',@isvector); % +addRequired(p,'durBeforeConflict_s',@isnumeric); % +addRequired(p,'durAfterConflict_s',@isnumeric); % + +% Parse +parse(p,conflicts,mdlType1,mdlType2,track1,track2,initHorz_ft,initVert_ft,durBeforeConflict_s,durAfterConflict_s); + +%% Preallocate output +sidx1 = double.empty(0,1); +sidx2 = double.empty(0,1); +eidx1 = double.empty(0,1); +eidx2 = double.empty(0,1); + +conflictsOut = double.empty(0,10); + +%% Forward / Forward Propagation +% Start / end indicies - Forward Propagation - AC1 +sidxFwd1 = conflicts(:,7) - durBeforeConflict_s; +eidxFwd1 = conflicts(:,7) + durAfterConflict_s; + +% Start / end indicies - Forward Propagation - AC2 +sidxFwd2 = conflicts(:,8) - durBeforeConflict_s; +eidxFwd2 = conflicts(:,8) + durAfterConflict_s; + +% Potential initial separation - Forward Propagation +r_ft = hypot(track1.east_ft(sidxFwd1)-track2.east_ft(sidxFwd2),track1.north_ft(sidxFwd1)-track2.north_ft(sidxFwd2)); +z_ft = abs(track1.alt_ft_msl(sidxFwd1) - track2.alt_ft_msl(sidxFwd2)); + +% Filter to conflicts that satsify the initial +% separation conditions with forward +% propagation, as forward propogation will +% always be valid regardless of track type +idx = (r_ft >= initHorz_ft(1)) & (r_ft <= initHorz_ft(2)) & (z_ft >= initVert_ft(1)) & (z_ft <= initVert_ft(2)); +idxKeep = find(idx == true); + +% Filter +conflictsOut = conflicts(idx,:); +sidxFwd1 = sidxFwd1(idx); +sidxFwd2 = sidxFwd2(idx); +eidxFwd1 = eidxFwd1(idx); +eidxFwd2 = eidxFwd2(idx); + +% Assign output +sidx1 = sidxFwd1; +sidx2 = sidxFwd2; +eidx1 = eidxFwd1; +eidx2 = eidxFwd2; + +%% Now calculate potential backward propagation +% Start / end indicies - Backward Propagation - AC1 +switch mdlType1 + case 'geospatial' + sidxBck1 = conflictsOut(:,7) + durBeforeConflict_s; + eidxBck1 = conflictsOut(:,7) - durAfterConflict_s; + case 'bayes' + sidxBck1 = double.empty(1,0); + eidxBck1 = double.empty(1,0); +end + +% Start / end indicies - Backward Propagation - AC2 +switch mdlType2 + case 'geospatial' + sidxBck2 = conflictsOut(:,8) + durBeforeConflict_s; + eidxBck2 = conflictsOut(:,8) - durAfterConflict_s; + case 'bayes' + sidxBck2 = double.empty(1,0); + eidxBck2 = double.empty(1,0); +end + +%% Iterate over other propagation combinations +for ii=2:1:4 + % Create indicies pairs + switch ii + case 2 % Forward / Backward + s1 = sidxFwd1; e1 = eidxFwd1; + s2 = sidxBck2; e2 = eidxBck2; + case 3 % Backward / Forward + s1 = sidxBck1; e1 = eidxBck1; + s2 = sidxFwd2; e2 = eidxFwd2; + case 4 % Backward / Backward + s1 = sidxBck1; e1 = eidxBck1; + s2 = sidxBck2; e2 = eidxBck2; + end + + % Do something if indices are permited + if ~isempty(s1) && ~isempty(s2) + % Potential initial separation - Forward Propagation + r_ft = hypot(track1.east_ft(s1)-track2.east_ft(s2),track1.north_ft(s1)-track2.north_ft(s2)); + z_ft = abs(track1.alt_ft_msl(s1) - track2.alt_ft_msl(s2)); + + % Find indices that satify criteria + idx = (r_ft >= initHorz_ft(1)) & (r_ft <= initHorz_ft(2)) & (z_ft >= initVert_ft(1)) & (z_ft <= initVert_ft(2)); + + % Assign / append output + conflictsOut = [conflictsOut;conflictsOut(idx,:)]; + sidx1 = [sidx1; s1(idx)]; + sidx2 = [sidx2; s2(idx)]; + eidx1 = [eidx1; e1(idx)]; + eidx2 = [eidx2; e2(idx)]; + end +end + diff --git a/code/helper/CreateTasksPairingGeo.m b/code/helper/CreateTasksPairingGeo.m new file mode 100644 index 0000000..e36afca --- /dev/null +++ b/code/helper/CreateTasksPairingGeo.m @@ -0,0 +1,145 @@ +function [tasks, anchors, seeds] = CreateTasksPairingGeo(tradespace,anchorRange_nm,usecase,seedGen) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause + +%% Input handling +if nargin < 2; anchorRange_nm = 1.00; end +if nargin < 3; usecase = 'all'; end +if nargin < 4; seedGen = 0; end + +%% Inputs hardcode +chunkSize = 5000; +maxSeed = 2^30; % + +%% Set random seed +if ~isnan(seedGen) && ~isempty(seedGen) + oldSeed = rng; + rng(seedGen,'twister'); +end + +%% Prealocate +anchors = table.empty(0,6); + +%% Load all anchors + +switch usecase + case 'uncor_uncor' + + inFile = [getenv('AEM_DIR_GEOPAIR') filesep 'output' filesep sprintf('pairs-%s-anchorRange%0.2f.mat','uncor_uncor',anchorRange_nm)]; + + if exist(inFile) == 2 + data = load(inFile,'anchors','anchorRange_nm'); + anchors = data.anchors; + else + % Generate if it doesn't exist + iso_3166_2 = {'US-CO';'US-HI';'US-KS';'US-MA';'US-MS';'US-NC';'US-ND';'US-NV';'US-NY'}; + for ii=iso_3166_2' + data = preallocAnchors( ii{1}, anchorRange_nm, '', false); + data.num_geospatial = ones(size(data,1),1); + data.iso_3166_2 = repmat(ii,size(data,1),1); + + if isempty(anchors); + anchors = data; + else + anchors = [anchors;data]; + end + end + save(inFile,'anchors','iso_3166_2','anchorRange_nm'); + end + + otherwise + uIso = string(unique(tradespace.subdivision1,'stable')'); + for ii=uIso + % Create input filename containing the output of RUN_1 + inFile = [getenv('AEM_DIR_GEOPAIR') filesep 'output' filesep sprintf('pairs-%s-anchorRange%0.2f.mat',sprintf('%s_%s',usecase,ii),anchorRange_nm)]; + + % Load anchors + data = load(inFile,'anchors','anchorRange_nm'); + data = data.anchors; + data.iso_3166_2 = repmat(ii,size(data,1),1); + + if isempty(anchors); + anchors = data(data.num_geospatial > 0,:); + else + anchors = [anchors; data(data.num_geospatial > 0,:)]; + end + end +end + +% Sort by iso_3166_2 first (this is important for the next steps) +anchors = sortrows(anchors,{'iso_3166_2','num_geospatial'},{'ascend','descend'}); +anchors.anchorRange_nm = repmat(anchorRange_nm,size(anchors,1),1); + +%% Calculate start and end indicies for tasks w.r.t to anchors +nAnchors = size(anchors,1); +[uIso,~,ic] = unique(anchors.iso_3166_2,'stable'); + +agg.iso_3166_2 = strings(0,0); +agg.sidx = []; +agg.eidx = []; +for ii=1:1:numel(uIso) + % Filter + l = ic==ii; + nii = nnz(l); + B = cumsum(anchors.num_geospatial(l)); + + % Calculate indicies + idx = find(diff(mod(B,chunkSize)) <= 0); + if isempty(idx); idx = 1; end + if idx(1) ~= 1; idx = [1; idx]; end + if idx(end) ~= nii; idx = [idx; nii]; end + sidx = idx(1:end-1); + eidx = idx(2:end)-1; eidx(end) = nii; + + % Assign + agg.iso_3166_2 = [agg.iso_3166_2;repmat(uIso(ii),size(sidx))]; + if ii > 1 + agg.sidx = [agg.sidx; sidx + agg.eidx(end)]; + agg.eidx = [agg.eidx; eidx + agg.eidx(end)]; + else + agg.sidx = sidx; + agg.eidx = eidx; + end +end + +iso_3166_2 = agg.iso_3166_2; +%sidx = agg.sidx; +%eidx = agg.eidx; +clear tasks; + +%% Concat start and end incides for tasks w.r.t to tradespace +configId = []; +sidx = []; +eidx = []; +for ii=1:1:size(tradespace,1) + iiId = tradespace.configId(ii); + + switch usecase + case 'uncor_uncor' + isIso = true(size(agg.iso_3166_2)); + otherwise + isIso = strcmpi(tradespace.subdivision1{ii},agg.iso_3166_2); + end + nz = nnz(isIso); + + if ii > 1 + configId = [configId ; repmat(iiId,nz,1)]; + sidx = [sidx; agg.sidx(isIso)]; + eidx = [eidx; agg.eidx(isIso)]; + else + configId = repmat(iiId,nz,1); + sidx = agg.sidx(isIso); + eidx = agg.eidx(isIso); + end +end +globalId = (1:1:numel(configId))'; + +% Create task table and seed array +tasks = table(globalId,configId,sidx,eidx); +seeds = randi(maxSeed,size(tasks,1),1); + +%% Change back to original seed +if ~isnan(seedGen) && ~isempty(seedGen) + rng(oldSeed); +end + diff --git a/code/helper/CreateTradespacePairingGeo.m b/code/helper/CreateTradespacePairingGeo.m new file mode 100644 index 0000000..d6a7351 --- /dev/null +++ b/code/helper/CreateTradespacePairingGeo.m @@ -0,0 +1,113 @@ +function tradespace = CreateTradespacePairingGeo(varargin) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +% +% SEE ALSO createEncounters_2 + +%% Input parser +if nargin < 1; varargin = cell.empty(0,1); end +p = inputParser; + +% Initial Seperation +addParameter(p,'initHorz_ft',[6076 97217],@isnumeric); %97217 ~ 16 nm ~ 421 feet (250 knots) * 240 seconds (uncor MVL) +addParameter(p,'initVert_ft',[0 1500],@isnumeric); + +% Conflict Threshold +addParameter(p,'thresHorz_ft',[500; 2000; 4500],@iscolumn); +addParameter(p,'thresVert_ft',[100; 250; 450],@iscolumn); + +% Time +addParameter(p,'encTime_s',[90; 150; 210],@iscolumn); +addParameter(p,'conflictTime_s',[60; 120; 180],@iscolumn); + +% Obstacles +addParameter(p,'dofMaxRange_ft',[500],@isnumeric); +addParameter(p,'dofMaxVert_ft',[500],@isnumeric); % Minimum height of obstacles + +% Aircraft types +addParameter(p,'mdlType1','geospatial',@(x) ischar(x) && any(strcmpi(x,{'bayes','geospatial'}))); +addParameter(p,'mdlType2','bayes',@(x) ischar(x) && any(strcmpi(x,{'bayes','geospatial'}))); +defaultBayes = compose('%s/%s',[getenv('AEM_DIR_BAYES') filesep 'model'],["uncor_1200exclude_rotorcraft_v1p2.txt"; "uncor_1200only_rotorcraft_v1p2.txt";"uncor_1200exclude_fwse_v1p2.txt"; "uncor_1200only_fwse_v1p2.txt";"uncor_1200exclude_fwme_v1p2.txt"; "uncor_1200only_fwme_v1p2.txt"]); +addParameter(p,'bayesFile1',defaultBayes); +addParameter(p,'bayesFile2',defaultBayes); +addParameter(p,'iso_3166_2',{'US-CO';'US-HI';'US-KS';'US-MA';'US-MS';'US-NC';'US-ND';'US-NV';'US-NY'}); + +% Parse +parse(p,varargin{:}); +parms = p.Results; +fields = fieldnames(parms); + +%% Input Handling +assert(all(p.Results.encTime_s >= p.Results.conflictTime_s),'Failed to satisfy encTime_s >= conflictTime_s'); + +%% Initial Seperation, Conflict, and Time Thresholds +% Number of elements for each variable / group +nInit = size(parms.initHorz_ft,1); % Initial seperation +nThres = size(parms.thresHorz_ft,1); % Conflict seperation threshold +nTime = size(parms.encTime_s,1); % Encounter and conflict time s +nObs = size(parms.dofMaxRange_ft,1); % Obstacles + +% Combination of variables +A = allcomb(1:1:nInit,1:1:nThres,1:1:nTime,1:1:nObs,'matlab'); + +% Create tradespace with initial and threshold combinations +tradespace = table(parms.initHorz_ft(A(:,1),:),parms.initVert_ft(A(:,1),:),... + parms.thresHorz_ft(A(:,2),:), parms.thresVert_ft(A(:,2),:),... + parms.encTime_s(A(:,3),:), parms.conflictTime_s(A(:,3),:),... + parms.dofMaxRange_ft(A(:,4),:),parms.dofMaxVert_ft(A(:,4),:),... + 'VariableNames',{'initHorz_ft','initVert_ft','thresHorz_ft','thresVert_ft','encTime_s','conflictTime_s','dofMaxRange_ft','dofMaxVert_ft'}); + +%% Aircraft Types +switch parms.mdlType1 + case 'bayes' + bayes1 = parms.bayesFile1; + geo1 = char(32); % Unicode whitespace + case 'geospatial' + geo1 = parms.iso_3166_2; + bayes1 = string; +end + +switch parms.mdlType2 + case 'bayes' + bayes2 = parms.bayesFile2; + geo2 = char(32); % Unicode whitespace + case 'geospatial' + geo2 = parms.iso_3166_2; + bayes2 = string; +end + +nb1 = size(bayes1,1); nb2 = size(bayes2,1); +ng1 = size(geo1,1); ng2 = size(geo2,1); + +% Combination of variables +% bayes1, bayes2, geo1, geo2 +A = allcomb(1:1:nb1,1:1:nb2,1:1:ng1,1:1:ng2,'matlab'); + +% Iterate over combinations +oldTradespace = tradespace; +for ii=1:1:size(A,1) + % Get unappended table + iiT = oldTradespace; + nt = size(iiT,1); + + % Append table + iiT.mdlType1 = repmat(parms.mdlType1,nt,1); + iiT.mdlType2 = repmat(parms.mdlType2,nt,1); + + iiT.bayesFile1 = repmat(bayes1(A(ii,1),:),nt,1); + iiT.bayesFile2 = repmat(bayes2(A(ii,2),:),nt,1); + iiT.subdivision1 = repmat(geo1(A(ii,3),:),nt,1); + iiT.subdivision2 = repmat(geo2(A(ii,4),:),nt,1); + + % Concat + if ii == 1; + tradespace = iiT; + else + tradespace = [tradespace ; iiT]; + end +end + +%% Numerical id +tradespace.configId = (1:1:size(tradespace,1))'; +tradespace = movevars(tradespace,'configId','Before',1); + diff --git a/code/helper/IdentifyEncounterFiles.m b/code/helper/IdentifyEncounterFiles.m new file mode 100644 index 0000000..a695c52 --- /dev/null +++ b/code/helper/IdentifyEncounterFiles.m @@ -0,0 +1,147 @@ +function [tsOut, tasksOut, listing, metadata] = IdentifyEncounterFiles(tsIn,tasksIn,inDir,varargin) + +%% Input parser +if nargin < 1; varargin = cell.empty(0,1); end +p = inputParser; + +% Required +addRequired(p,'tsIn',@istable); +addRequired(p,'tasksIn',@istable); +addRequired(p,'inDir',@ischar); + +% Initial Seperation +%addParameter(p,'initHorz_ft',[6076 97217],@isnumeric); %97217 ~ 16 nm ~ 421 feet (250 knots) * 240 seconds (uncor MVL) +%addParameter(p,'initVert_ft',[0 1000],@isnumeric); + +% Conflict Threshold +addParameter(p,'thresHorz_ft',[],@isnumeric); +addParameter(p,'thresVert_ft',[],@isnumeric); + +% Time +addParameter(p,'encTime_s',[],@isnumeric); +addParameter(p,'conflictTime_s',[],@isnumeric); + +% Obstacles +addParameter(p,'dofMaxRange_ft',[],@isnumeric); +addParameter(p,'dofMaxVert_ft',[],@isnumeric); % Minimum height of obstacles + +% Aircraft types +addParameter(p,'bayesFile1',char.empty(0,0)); +addParameter(p,'bayesFile2','uncor_1200only_fwse_v1p2'); + +% Filtering +addParameter(p,'nEncWant',1e6,@isnumeric); + +% Parse +parse(p,tsIn,tasksIn,inDir,varargin{:}); +parms = p.Results; + +%% +tsOut = tsIn; +[~,tsOut.bayesFile1,~] = fileparts(tsIn.bayesFile1); +[~,tsOut.bayesFile2,~] = fileparts(tsIn.bayesFile2); + +%% Logical filtering of tradespace + +parNames = fields(parms); +varNames = tsOut.Properties.VariableNames; + +tf = true(size(tsOut,1),numel(parNames)); + +for ii=1:1:numel(parNames) + x = parms.(parNames{ii}); + if any(strcmp(parNames{ii},varNames)) && ~isempty(x) + switch class(x) + case 'char' + tf(:,ii) = string(tsOut.(parNames{ii})) == string(x); + otherwise + tf(:,ii) = tsOut.(parNames{ii}) == x; + end + end +end + +tf = all(tf,2); + +tsOut = tsOut(tf,:); +tasksOut = tasksIn(ismember(tasksIn.configId,tsOut.configId),:); + +%% +configId = []; +for ii=1:1:size(tsOut,1) + iiId = tsOut.configId(ii); + iiDir = sprintf('%s/%04.f',inDir,iiId); + + iiList = dir([iiDir filesep 'metadata*']); + n = numel(iiList); + + if ii == 1 + listing = iiList; + configId = repmat(iiId,n,1); + else + configId = [configId ; repmat(iiId,n,1)]; + listing = [listing; iiList]; + end +end + +% Convert to table +listing = struct2table(listing); + +% Parse name +listing.nameHash = strrep(strrep(listing.name,'metadata_',''),'.mat',''); +listing.configId = configId; +C = cellfun(@(x)(strsplit(x,'_')),listing.nameHash,'UniformOutput',false); +listing.globalId = cellfun(@(x)(str2double(x{2})),C,'UniformOutput',true); + +% Filter tasks +lg = ismember(tasksOut.globalId,listing.globalId); +lc = ismember(tasksOut.configId,listing.configId); +tasksOut = tasksOut(lg & lc,:); + +%% Number of encounters and workTime_s +nEncounters = zeros(size(listing,1),1); +workTime_s = zeros(size(listing,1),1); +metadata = table; + +for ii=1:1:size(nEncounters,1) + % Load + x = load([listing.folder{ii} filesep listing.name{ii}],'nEncounters','workTime_s','metadata'); + + % Assign + nEncounters(ii) = x.nEncounters; + workTime_s(ii) = x.workTime_s; + + if ii==1; + metadata = x.metadata; + else + metadata = [metadata; x.metadata]; + end + +end +listing.nEncounters = nEncounters; +listing.workTime_s = workTime_s; + +% tradespace +tsOut.nEncounters = nan(size(tsOut,1),1); +tsOut.workTime_s = nan(size(tsOut,1),1); + +[uCId,~,~] = unique(listing.configId); +tsOut.nEncounters(ismember(tsOut.configId,listing.configId)) = arrayfun(@(x)(sum(listing.nEncounters(listing.configId == x))),uCId); +tsOut.workTime_s(ismember(tsOut.configId,listing.configId)) = arrayfun(@(x)(sum(listing.workTime_s(listing.configId == x))),uCId); + +%% Filtering to desired number of encounters +% Random permutation of indicies +idx = randperm(size(listing,1)); + +% Preallocate +isUse = false(size(listing,1),1); +cIdx = 1; +cEnc = 0; + +while cEnc < parms.nEncWant && cIdx < numel(idx) + isUse(idx(cIdx)) = true; + cEnc = cEnc + listing.nEncounters(idx(cIdx)); + cIdx = cIdx + 1; +end + +listing.isUse = isUse; + diff --git a/code/helper/README.md b/code/helper/README.md new file mode 100644 index 0000000..75196e1 --- /dev/null +++ b/code/helper/README.md @@ -0,0 +1,17 @@ +# Helper + +Miscellaneous helper functions + +## Distribution Statement + +DISTRIBUTION STATEMENT A. Approved for public release. Distribution is unlimited. + +This material is based upon work supported by the Federal Aviation Administration under Air Force Contract No. FA8702-15-D-0001. Any opinions, findings, conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Federal Aviation Administration. + +© 2019-2021 Massachusetts Institute of Technology. + +Delivered to the U.S. Government with Unlimited Rights, as defined in DFARS Part 252.227-7013 or 7014 (Feb 2014). Notwithstanding any copyright notice, U.S. Government rights in this work are defined by DFARS 252.227-7013 or DFARS 252.227-7014 as detailed above. Use of this work other than as specifically authorized by the U.S. Government may violate any copyrights that exist in this work. + +Any opinions, findings, conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Federal Aviation Administration. + +This document is derived from work done for the FAA (and possibly others), it is not the direct product of work done for the FAA. The information provided herein may include content supplied by third parties. Although the data and information contained herein has been produced or processed from sources believed to be reliable, the Federal Aviation Administration makes no warranty, expressed or implied, regarding the accuracy, adequacy, completeness, legality, reliability, or usefulness of any information, conclusions or recommendations provided herein. Distribution of the information contained herein does not constitute an endorsement or warranty of the data or information provided herein by the Federal Aviation Administration or the U.S. Department of Transportation. Neither the Federal Aviation Administration nor the U.S. Department of Transportation shall be held liable for any improper or incorrect use of the information contained herein and assumes no responsibility for anyone’s use of the information. The Federal Aviation Administration and U.S. Department of Transportation shall not be liable for any claim for any loss, harm, or other damages arising from access to or use of data information, including without limitation any direct, indirect, incidental, exemplary, special or consequential damages, even if advised of the possibility of such damages. The Federal Aviation Administration shall not be liable for any decision made or action taken, in reliance on the information contained herein. diff --git a/code/helper/createISO31662Grid.m b/code/helper/createISO31662Grid.m new file mode 100644 index 0000000..6f5981c --- /dev/null +++ b/code/helper/createISO31662Grid.m @@ -0,0 +1,33 @@ +function [gridLat_deg, gridLon_deg, BoundingBox_wgs84] = createISO31662Grid(iso_3166_2, anchorRange_nm, isPlot) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +% +% SEE ALSO preallocAnchors + +% Input Handling +if nargin < 3; isPlot = false; end + +% Load Natural Earth Adminstrative Boundaries +ne_admin = shaperead([getenv('AEM_DIR_CORE') filesep 'data' filesep 'NE-Adminstrative' filesep 'ne_10m_admin_1_states_provinces.shp'],'UseGeoCoords',true); + +% Create bounding box +BoundingBox_wgs84 = ne_admin(strcmp({ne_admin.iso_3166_2},iso_3166_2)).BoundingBox; + +neLat_deg = ne_admin(strcmp({ne_admin.iso_3166_2},iso_3166_2)).Lat; +neLon_deg = ne_admin(strcmp({ne_admin.iso_3166_2},iso_3166_2)).Lon; + +% Create grid +[gridLat_deg, gridLon_deg] = meshgrid(min(neLat_deg):nm2deg(anchorRange_nm):max(neLat_deg),min(neLon_deg):nm2deg(anchorRange_nm):max(neLon_deg)); + +% Filter grid for those only in the location +isGrid = InPolygon(gridLat_deg,gridLon_deg,neLat_deg,neLon_deg); + +gridLat_deg = gridLat_deg(isGrid); +gridLon_deg = gridLon_deg(isGrid); + +if isPlot + figure; set(gcf,'name',['createISO31662Grid: ' iso_3166_2]); + + geoscatter(gridLat_deg,gridLon_deg,[],categorical(cellstr(class)),'.'); + set(gca,'Basemap','streets-dark','TickLabelFormat','dd'); +end \ No newline at end of file diff --git a/code/helper/estimateSpeedFromGeodetic.m b/code/helper/estimateSpeedFromGeodetic.m new file mode 100644 index 0000000..edd01c8 --- /dev/null +++ b/code/helper/estimateSpeedFromGeodetic.m @@ -0,0 +1,18 @@ +function [v_ft_s, d_ft] = estimateSpeedFromGeodetic(track) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause + +% Parse timestep +timeStep_s = seconds(track.Properties.TimeStep); + +% Calculate distance between legs +[~,d_nm] = legs(track.lat_deg,track.lon_deg,'rh'); + +% Convert from nautical miles to feet +d_ft = d_nm * 6076.12; +d_ft = [d_ft(1); d_ft]; +d_ft = abs(d_ft); + +% Estimate speed +v_ft_s = d_ft./ timeStep_s; + diff --git a/code/helper/parseGeoTrajDirectory.m b/code/helper/parseGeoTrajDirectory.m new file mode 100644 index 0000000..b608002 --- /dev/null +++ b/code/helper/parseGeoTrajDirectory.m @@ -0,0 +1,38 @@ +function [output,listing] = parseGeoTrajDirectory(inDir, usecase) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause + +% Input handling +if nargin < 1; inDir = [getenv('AEM_DIR_GEOSPATIAL') filesep 'output' filesep 'trajectories']; end +if nargin < 2; usecase = 'all'; end + +% Input directories of geospatial trajectories +listing = dir([inDir '*' filesep '*']); +listing(ismember( {listing.name}, {'.', '..'})) = []; %remove . and .. +isShield = contains({listing.folder},'shield'); +output = cellfun(@(f,n)([f filesep n]),{listing.folder},{listing.name},'UniformOutput',false)'; + +switch usecase + case 'all' + % No filtering needed + case 'all-noshield' + output(isShield) = []; + case 'unconv-noshield' + % Pairs to be used with the unconventional bayes model + %(i.e. paragliders, gliders,) + isCostal = contains({listing.folder},{'landuse_beach','landuse_cliff','landuse_volcano','gshhg_gshhs_resf_lvl1'}); + output(~isCostal) = []; + case 'shield' + output(~isShield) = []; + case 'longlinearinfrastructure' + islli = contains({listing.folder},{'pipeline','roads','waterway','railway','electrictransmission'}); + output(~islli) = []; + case 'railway' + irrail = contains({listing.folder},'railway'); + output(~irrail) = []; + case 'agriculture' + isAg = contains({listing.folder},{'farm','orchard','vineyard'}); + output(~isAg) = []; + otherwise + error('usecase:unknown','Unknown use case of %s\n',usecase); +end \ No newline at end of file diff --git a/code/plot/README.md b/code/plot/README.md new file mode 100644 index 0000000..e9aeada --- /dev/null +++ b/code/plot/README.md @@ -0,0 +1,17 @@ +# Plot + +Plotting functions + +## Distribution Statement + +DISTRIBUTION STATEMENT A. Approved for public release. Distribution is unlimited. + +This material is based upon work supported by the Federal Aviation Administration under Air Force Contract No. FA8702-15-D-0001. Any opinions, findings, conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Federal Aviation Administration. + +© 2019-2021 Massachusetts Institute of Technology. + +Delivered to the U.S. Government with Unlimited Rights, as defined in DFARS Part 252.227-7013 or 7014 (Feb 2014). Notwithstanding any copyright notice, U.S. Government rights in this work are defined by DFARS 252.227-7013 or DFARS 252.227-7014 as detailed above. Use of this work other than as specifically authorized by the U.S. Government may violate any copyrights that exist in this work. + +Any opinions, findings, conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Federal Aviation Administration. + +This document is derived from work done for the FAA (and possibly others), it is not the direct product of work done for the FAA. The information provided herein may include content supplied by third parties. Although the data and information contained herein has been produced or processed from sources believed to be reliable, the Federal Aviation Administration makes no warranty, expressed or implied, regarding the accuracy, adequacy, completeness, legality, reliability, or usefulness of any information, conclusions or recommendations provided herein. Distribution of the information contained herein does not constitute an endorsement or warranty of the data or information provided herein by the Federal Aviation Administration or the U.S. Department of Transportation. Neither the Federal Aviation Administration nor the U.S. Department of Transportation shall be held liable for any improper or incorrect use of the information contained herein and assumes no responsibility for anyone’s use of the information. The Federal Aviation Administration and U.S. Department of Transportation shall not be liable for any claim for any loss, harm, or other damages arising from access to or use of data information, including without limitation any direct, indirect, incidental, exemplary, special or consequential damages, even if advised of the possibility of such damages. The Federal Aviation Administration shall not be liable for any decision made or action taken, in reliance on the information contained herein. diff --git a/code/plot/plotAnchor.m b/code/plot/plotAnchor.m new file mode 100644 index 0000000..a91f93a --- /dev/null +++ b/code/plot/plotAnchor.m @@ -0,0 +1,17 @@ +function fig = plotAnchor(anchor,Tdof) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +% +% SEE ALSO createEncounters_2 + +fig = figure; +%geoplot(anchor.lat_deg,anchor.lon_deg,'k*','DisplayName',sprintf('(%0.3f,%0.3f)',anchor.lat_deg,anchor.lon_deg)); hold on; + +[uObs,~,ib] = unique(Tdof.obs_type); +for ii=1:1:numel(uObs) + l = ib == ii; + [lat,lon] = polyjoin(Tdof.lat_acc_deg(l),Tdof.lon_acc_deg(l)); + geoplot(lat,lon,'DisplayName',uObs{ii});hold on; +end +set(gca,'Basemap','satellite','TickLabelFormat','dd'); +%legend \ No newline at end of file diff --git a/code/plot/plotEncounter.m b/code/plot/plotEncounter.m new file mode 100644 index 0000000..91c5073 --- /dev/null +++ b/code/plot/plotEncounter.m @@ -0,0 +1,116 @@ +function fig = plotEncounter(conflicts,mdlType1,mdlType2,track1,track2,sidx1,eidx1,sidx2,eidx2,lat0_deg,lon0_deg,randSeed) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +% +% SEE ALSO createEncounters_2 + +%% Filter based on time +% Filter track timetables +if sidx1 <= eidx1 + track1 = track1(sidx1:1:eidx1,:); +else + track1 = track1(sidx1:-1:eidx1,:); +end +track1.Properties.StartTime = seconds(0); +track1.Properties.TimeStep = seconds(1); + +% Filter track timetables +if sidx2 <= eidx2 + track2 = track2(sidx2:1:eidx2,:); +else + track2 = track2(sidx2:-1:eidx2,:); +end +track2.Properties.StartTime = seconds(0); +track2.Properties.TimeStep = seconds(1); + +%% Parse and calculate +hmd_ft = conflicts(:,9); +vmd_ft = conflicts(:,10); +r_ft = hypot(track1.east_ft-track2.east_ft,track1.north_ft-track2.north_ft); +z_ft = abs(track1.alt_ft_msl - track2.alt_ft_msl); +d_ft = sqrt((track1.north_ft - track2.north_ft).^2 + (track1.east_ft - track2.east_ft).^2 + (track1.alt_ft_msl - track2.alt_ft_msl).^2); + +[~,idxCPA] = min(d_ft); + +%% Plot +% Create figure +fig = figure('name',sprintf('Encounter seed = %i',randSeed),'Units','inches','Position',[0.5 0.5 16 8]); +tl = tiledlayout(2,5,'Padding','compact'); + +% Altitude and speed +nexttile(1,[2 1]); +TTdyn = timetable(track1.alt_ft_agl,track1.alt_ft_msl,track2.alt_ft_agl,track2.alt_ft_msl,track1.speed_ft_s,track2.speed_ft_s,track1.speed_ft_s*0.592484,track2.speed_ft_s*0.592484,'RowTimes',track1.Time,... + 'VariableNames',{'AC 1 (ft AGL)','AC 1 (ft MSL)','AC 2 (ft AGL)','AC 2 (ft MSL)','AC 1 (fps)','AC 2 (fps)','AC 1 (kts)','AC 2 (kts)'}); +newYlabels = {'Altitude (ft AGL)','Altitude (ft MSL)','Speed (fps)','Speed (knots)'}; +stackedplot(TTdyn, {{'AC 1 (ft AGL)','AC 2 (ft AGL)'},{'AC 1 (ft MSL)','AC 2 (ft MSL)'},{'AC 1 (fps)','AC 2 (fps)'},{'AC 1 (kts)','AC 2 (kts)'}},'DisplayLabels',newYlabels) +grid on; + +% Distance +nexttile(2,[2 1]); +TTd = timetable(r_ft,z_ft,d_ft,'RowTimes',track1.Time); +stackedplot(TTd,{{'d_ft'},{'r_ft'},{'z_ft'}},'DisplayLabels',{'Distance (ft)','HMD (ft)','VMD (ft)'}); +grid on; + +% Big plot +nexttile(3,[2 2]); +geoplot(track1.lat_deg,track1.lon_deg,'b-',track2.lat_deg,track2.lon_deg,'r--',... + track1.lat_deg(1),track1.lon_deg(1),'bs',track2.lat_deg(1),track2.lon_deg(1),'rs'); +set(gca,'Basemap','topographic','TickLabelFormat','dd'); +hold on; + +%Threshold Points +for w=1:1:size(conflicts,1) + if w==1; hv = 'on'; else hv = 'off'; end; % Only want to plot conflict points once on legend + geoscatter(conflicts(w,1),conflicts(w,2),60,'b','h','filled','HandleVisibility',hv) + geoscatter(conflicts(w,3),conflicts(w,4),60,'r','p','filled','HandleVisibility',hv) + + % Alternate horizontal alignment for odd / even values of w + % o1, o2 = offsets for text + if mod(w,2) == 0 + ha1 = 'left'; + ha2 = 'right'; + o1 = nm2deg(0.04); + o2 = -nm2deg(0.04); + else + ha1 = 'right'; + ha2 = 'left'; + o1 = -nm2deg(0.04); + o2 = nm2deg(0.04); + end + + text(conflicts(w,1),conflicts(w,2)+o1,num2str(w),'Color','b','FontSize',8,'FontWeight','bold','HorizontalAlignment',ha1); + text(conflicts(w,3),conflicts(w,4)+o2,num2str(w),'Color','r','FontSize',8,'FontWeight','bold','HorizontalAlignment',ha2); +end +geoplot(lat0_deg,lon0_deg,'k*'); +hold off; +lg = legend(sprintf('Aircraft 1 (%s)',mdlType1), sprintf('Aircraft 2 (%s)',mdlType2),'AC1 @ initial','AC2 @ initial','AC1 @ conflict','AC2 @ conflict',sprintf('(%0.3f,%0.3f)',lat0_deg,lon0_deg),... + 'NumColumns',2,'Location','best'); + +% Local Cartessian (north / east) plot +nexttile(5,[1 1]); +plot(track1.east_ft,track1.north_ft,'b-',track2.east_ft,track2.north_ft,'r--',0,0,'k*'); +grid on; axis square; axis equal; xlabel('East (ft)'); ylabel('North (ft)'); +%legend(sprintf('Aircraft 1 (%s)',mdlType1), sprintf('Aircraft 2 (%s)',mdlType2),'(0,0)'); + +% nexttile(8,[1 1]); +% geoplot(track1.lat_deg,track1.lon_deg,'b-',track2.lat_deg,track2.lon_deg,'r--',lat0_deg,lon0_deg,'k*'); +% set(gca,'TickLabelFormat','dd'); +%legend(sprintf('Aircraft 1 (%s)',mdlType1), sprintf('Aircraft 2 (%s)',mdlType2),sprintf('(%0.3f,%0.3f)',lat0_deg,lon0_deg)); + +% HMD / VMD +nexttile(10,[1 1]); +scatter(r_ft(1),z_ft(1),36*2, 'g','s','filled','DisplayName','Initial'); hold on; +scatter(r_ft(idxCPA),z_ft(idxCPA),36*2, 'r','o','filled','DisplayName','CPA'); +scatter(r_ft(end),z_ft(end),36*2, 'b','d','filled','DisplayName','Final'); +scatter(hmd_ft,vmd_ft,36*2, 'k','p','filled','DisplayName','Conflict');hold off; + +legend('Location','best'); +axis([0 round(max(r_ft),-2)+100 0 round(max(z_ft),-2)+100]); +grid on; xlabel('HMD (ft)'); ylabel('VMD (ft)'); + +% Legend of big plot +%lg.Layout.Tile = 'south'; % <-- place legend south of tiles +%lg.NumColumns = 4; + +% Adjust map size, legend, hold off +% print(['output' filesep 'plot_encounter_examples' filesep 'example_2' '_lk' num2str(lk(i)) '_k' num2str(k) '_i' num2str(i) '_lat' num2str(lat0(lk(i))) '_lon' num2str(lon0(lk(i))) '_thresHorz' num2str(thresHorz_ft) '_thresVert' num2str(thresVert_ft) '_ac1' mdlType1 '_ac2' mdlType2 '.png'],'-dpng','-r300'); diff --git a/code/trajectory/README.md b/code/trajectory/README.md new file mode 100644 index 0000000..d5c8f2a --- /dev/null +++ b/code/trajectory/README.md @@ -0,0 +1,17 @@ +# Trajectory + +Functions related to parse or manipulating track trajectories + +## Distribution Statement + +DISTRIBUTION STATEMENT A. Approved for public release. Distribution is unlimited. + +This material is based upon work supported by the Federal Aviation Administration under Air Force Contract No. FA8702-15-D-0001. Any opinions, findings, conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Federal Aviation Administration. + +© 2019-2021 Massachusetts Institute of Technology. + +Delivered to the U.S. Government with Unlimited Rights, as defined in DFARS Part 252.227-7013 or 7014 (Feb 2014). Notwithstanding any copyright notice, U.S. Government rights in this work are defined by DFARS 252.227-7013 or DFARS 252.227-7014 as detailed above. Use of this work other than as specifically authorized by the U.S. Government may violate any copyrights that exist in this work. + +Any opinions, findings, conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Federal Aviation Administration. + +This document is derived from work done for the FAA (and possibly others), it is not the direct product of work done for the FAA. The information provided herein may include content supplied by third parties. Although the data and information contained herein has been produced or processed from sources believed to be reliable, the Federal Aviation Administration makes no warranty, expressed or implied, regarding the accuracy, adequacy, completeness, legality, reliability, or usefulness of any information, conclusions or recommendations provided herein. Distribution of the information contained herein does not constitute an endorsement or warranty of the data or information provided herein by the Federal Aviation Administration or the U.S. Department of Transportation. Neither the Federal Aviation Administration nor the U.S. Department of Transportation shall be held liable for any improper or incorrect use of the information contained herein and assumes no responsibility for anyone’s use of the information. The Federal Aviation Administration and U.S. Department of Transportation shall not be liable for any claim for any loss, harm, or other damages arising from access to or use of data information, including without limitation any direct, indirect, incidental, exemplary, special or consequential damages, even if advised of the possibility of such damages. The Federal Aviation Administration shall not be liable for any decision made or action taken, in reliance on the information contained herein. diff --git a/code/trajectory/adjustTrack.m b/code/trajectory/adjustTrack.m new file mode 100644 index 0000000..7aa0790 --- /dev/null +++ b/code/trajectory/adjustTrack.m @@ -0,0 +1,97 @@ +function trackOut = adjustTrack(trackIn, altAdjust_ft, vAdjust_ft_s) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +% +% SEE ALSO timetable createEncounters_2 sampleSpeedAlt + +%% Input handling +assert(isregular(trackIn),'Input timetable expected to be regular with respect to time, it was not'); +assert(trackIn.Properties.TimeStep == seconds(1),'Input timetable timestep expected to be 1 sec, it was not'); + +%% Preallocate output +trackOut = trackIn; + +% Potential variables related to altitude +% Can be multiple because it easier to adjust with the same units (ft) +varsAlt = ["alt_ft_msl", "alt_ft_agl", "up_ft", "down_ft"]; + +% Variable related to speed +% Only allow one +varSpeed = "speed_ft_s"; + +% Parse timestep +timeStep_s = seconds(trackIn.Properties.TimeStep); + +%% Adjust altitude +if altAdjust_ft ~=0 + % Iterate over potential altitude variables + for ii = varsAlt + % Determine if altitude variable ii exists in trackIn + isField = strcmpi(trackIn.Properties.VariableNames,ii); + + % Adjust if variable exists + if any(isField) + trackOut.(ii) = trackIn.(ii) + altAdjust_ft; + end + end +end + +%% Adjust velocity +if vAdjust_ft_s ~=0 + [v_ft_s, d_ft] = estimateSpeedFromGeodetic(trackIn); + + % Adjust speed + trackOut.(varSpeed) = v_ft_s + vAdjust_ft_s; + + % Calculate new time required for each leg + t_legs_s = d_ft ./ abs(trackOut.(varSpeed)); + + % Account for when v_kts = 0 + t_legs_s(isinf(t_legs_s)) = 0; + + % Assume if maneuvering solely vertical, that d_ft = 0, + % resulting in t_legs_s = NaN + isVert = isnan(t_legs_s); + + % Do something if assumed vertical only maneuver happen + if any(isVert) + % Because the timetable is regular and we didn't change vertical rate, + % we can use the existing timestep + t_legs_s(isVert) = timeStep_s; + end + + % Update + trackOut.Time = seconds(cumsum(t_legs_s)); + + % Remove any duplicate times + % https://www.mathworks.com/help/matlab/matlab_prog/clean-timetable-with-missing-duplicate-or-irregular-times.html + trackOut = sortrows(trackOut); + trackOut = unique(trackOut); + dupTimes = sort(trackOut.Time); + isDup = (diff(dupTimes) == 0); + if any(isDup) + trackOut(isDup,:) = []; + end + + % Retime track to have the same regular one second timesteps + % Also check for potential interpolation errors with retime + if ~isregular(trackOut) + trackOut = retime(trackOut,'secondly','pchip'); + isBad = trackOut.lat_deg > 90 | trackOut.alt_ft_msl > 60000 | trackOut.alt_ft_agl > 18000; + if any(isBad) + idxLastGood = find(~isBad,1,'last'); + if find(isBad) == size(trackOut,1) + trackOut{end,:}= trackIn{end,:}; + trackOut.speed_ft_s(end) = trackOut.speed_ft_s(idxLastGood); + else + trackOut = trackOut(1:idxLastGood,:); + end + end + end + + % Have track start at t = 0 + if trackOut.Properties.StartTime ~= seconds(0) + trackOut.Properties.StartTime = seconds(0); + end + +end \ No newline at end of file diff --git a/code/trajectory/loadTrack.m b/code/trajectory/loadTrack.m new file mode 100644 index 0000000..0724c29 --- /dev/null +++ b/code/trajectory/loadTrack.m @@ -0,0 +1,95 @@ +function track = loadTrack(mdlType, arg2, isStartEndNonZero, rangeAlt_ft_agl, rangeV_ft_s, spheroid_ft, lat0_deg,lon0_deg,el0_ft_msl,Z_m,refvec,R,Tdof,dofMaxRange_ft,dofMaxVert_ft,bayesDuration_s,z_agl_tol_ft) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +% +% SEE ALSO timetable textscantraj UncorEncounterModel/track + +% Inputs hardcode +% Variable name associated with speed +varSpeed = 'speed_ft_s'; + +% Behavior depends on model type +switch mdlType + case 'geospatial' + % Input handling, expected to be a filename + assert(isfile(arg2)); + + % Load track + track = textscantraj(arg2); + track.Properties.StartTime = seconds(0); + + % em-model-geospatial currently does not calculate east, north, up + % So calculate east, north, up here + [track.east_ft,track.north_ft,~] = geodetic2enu(track.lat_deg,track.lon_deg,track.alt_ft_agl,lat0_deg,lon0_deg,el0_ft_msl,spheroid_ft); + isAltGood = true; + + case 'bayes' + % Input handling, expected to be a class object + assert(isa(arg2,'UncorEncounterModel')); + + isAltGood = false; + c = 1; + while ~isAltGood && c < 10 + + track = arg2.track(1,bayesDuration_s,'lat0_deg',lat0_deg,'lon0_deg',lon0_deg,'coordSys','geodetic',... + 'Tdof',Tdof,'dofMaxRange_ft',dofMaxRange_ft,'dofMaxVert_ft',dofMaxVert_ft,'Z_m',Z_m,'refvec',refvec,'R',R,'z_agl_tol_ft',z_agl_tol_ft); + track = track{1}; + + % Check that altitude range is satsified for Bayes + if ~isempty(track) && (min(track.alt_ft_agl) >= rangeAlt_ft_agl(1) && max(track.alt_ft_agl) <= rangeAlt_ft_agl(2)) + isAltGood = true; + else + c = c + 1; + end + end +end + +if ~isAltGood + track = timetable.empty(0,12); +else + + % Remove points with zero altitude at start and end + % This often due to a vertical climb / descend at the start or end + if isStartEndNonZero + isZero = floor(track.('alt_ft_agl')) == 0; + sidx = find(~isZero,1,'first'); + eidx = find(~isZero,1,'last'); + track = track(sidx:eidx,:); + end + + % Remove any duplicate times + % https://www.mathworks.com/help/matlab/matlab_prog/clean-timetable-with-missing-duplicate-or-irregular-times.html + track = sortrows(track); + track = unique(track); + dupTimes = sort(track.Time); + isDup = (diff(dupTimes) == 0); + if any(isDup) + track(isDup,:) = []; + end + + % Retime tracks to have the same regular one second timesteps + track = retime(track,'secondly','pchip'); + + % Calculate speed + switch mdlType + case 'geospatial' + % If not available, estimate it from position + if ~any(strcmpi(varSpeed,track.Properties.VariableNames)) + track.(varSpeed) = estimateSpeedFromGeodetic(track); + end + end + + % Remove points with zero speed at start and end + % This often due to a vertical climb / descend at the start or end + if isStartEndNonZero + isZero = floor(track.(varSpeed)) == 0; + sidx = find(~isZero,1,'first'); + eidx = find(~isZero,1,'last'); + track = track(sidx:eidx,:); + end + + % Have track start at t = 0 + if track.Properties.StartTime ~= seconds(0) + track.Properties.StartTime = seconds(0); + end +end \ No newline at end of file diff --git a/code/trajectory/neu2Waypoints.m b/code/trajectory/neu2Waypoints.m new file mode 100644 index 0000000..c5147b9 --- /dev/null +++ b/code/trajectory/neu2Waypoints.m @@ -0,0 +1,47 @@ +function wp = neu2Waypoints(ac1,ac2) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +% WAYPOINTS STRUCTURE: +% The waypoints structure is an m x n structure matrix, where m is the +% number of aircraft and n is the number of encounters. This structure +% matrix has two fields: initial and update. The initial field is a 3 +% element array specifying the north, east, and altitude. The update +% field is a 4 x n matrix, where n is the number of updates. The rows +% correspond to the time, north, east, and altitude position of the +% waypoints. + +% Modify time so it starts from 0 +if ac1(1,1) ~= 0; ac1(:,1) = ac1(:,1) - ac1(1,1); end +if ac2(1,1) ~= 0; ac2(:,1) = ac2(:,1) - ac2(1,1); end + +% Fill in wp struct for ac1 +for idx = 1:size(ac1,1) + if idx == 1 + wp(1,1).initial = [ac1(idx,2);... + ac1(idx,3);... + ac1(idx,4);... + ]; + else + wp(1,1).update(:,idx-1) = [ac1(idx,1);... + ac1(idx,2);... + ac1(idx,3);... + ac1(idx,4);... + ]; + end +end + +% Fill in wp struct for ac2 +for idx = 1:size(ac2,1) + if idx == 1 + wp(2,1).initial = [ac2(idx,2);... + ac2(idx,3);... + ac2(idx,4);... + ]; + else + wp(2,1).update(:,idx-1) = [ac2(idx,1);... + ac2(idx,2);... + ac2(idx,3);... + ac2(idx,4);... + ]; + end +end diff --git a/code/trajectory/sampleSpeedAlt.m b/code/trajectory/sampleSpeedAlt.m new file mode 100644 index 0000000..fbb522b --- /dev/null +++ b/code/trajectory/sampleSpeedAlt.m @@ -0,0 +1,62 @@ +function [altAdjust_ft, vAdjust_ft_s] = sampleSpeedAlt(mdlType,track,rangeAlt_ft_agl,rangeV_ft_s, isSampleAlt, isSampleV,n) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause + +%% Input Handling +% Never assume we want adjustment, user must be explicit +if nargin < 5; isSampleAlt = false; end +if nargin < 6; isSampleV = false; end + +%% Sample aircraft speed for n times +% Behavior depends on model type +switch mdlType + case 'geospatial' + if isSampleV + imin = min([0 rangeV_ft_s(1) - min(track.speed_ft_s)]); + imax = max([0 rangeV_ft_s(2) - max(track.speed_ft_s)]); + + imin = ceil(imin); + imax = floor(imax); + + if imax < imin; imax = imin; end + vAdjust_ft_s = randi([imin imax],n,1); + else + vAdjust_ft_s = zeros(n,1); + end + case 'bayes' + vAdjust_ft_s = zeros(n,1); +end + +%% Sample altitude adjustment n times (geospatial only) + +% Behavior depends on model type +switch mdlType + case 'geospatial' + % Get AGL altitude + % Calculate the interval of potential altitude adjustments + % Sample the interval and set altitude adjustments for each combination + if isSampleAlt + below = rangeAlt_ft_agl(1)-min(track.alt_ft_agl); + above = rangeAlt_ft_agl(2)-max(track.alt_ft_agl); + if below <= above + adjustSpan_ft_agl = below:25:above; + else + adjustSpan_ft_agl = above:-25:(below + above); % Need to bring aircraft down + end + + if ~isempty(adjustSpan_ft_agl) + altAdjust_ft = adjustSpan_ft_agl(randi(numel(adjustSpan_ft_agl),n,1))'; + else + altAdjust_ft = zeros(n,1); + end + else + altAdjust_ft = zeros(n,1); + end + + case 'bayes' + altAdjust_ft = zeros(n,1); +end + +%% Make sure outputs are columns +if ~iscolumn(altAdjust_ft); altAdjust_ft = altAdjust_ft'; end +if ~iscolumn(vAdjust_ft_s); vAdjust_ft_s = vAdjust_ft_s'; end diff --git a/fastloadtraj.m b/code/trajectory/textscantraj.m similarity index 63% rename from fastloadtraj.m rename to code/trajectory/textscantraj.m index 68bd3b5..44a4a88 100644 --- a/fastloadtraj.m +++ b/code/trajectory/textscantraj.m @@ -1,6 +1,6 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory +function TTout = textscantraj(inFile) +% Copyright 2019 - 2021, MIT Lincoln Laboratory % SPDX-License-Identifier: BSD-2-Clause -function [time_s, lat_deg, lon_deg, alt_ft_msl, alt_ft_agl] = textscantraj(inFile) % Open, read, and close file % We ignore the last column with the airspace name because of a bug @@ -13,8 +13,12 @@ % Parse % Header order assumed to be: % time_s,heading_deg,groundspeed_kt,el_ft_msl,alt_ft_msl,alt_ft_agl,altRate_fps,lat_deg,lon_deg,AirspaceClass,AirspaceName -time_s = textRaw{1}; +rowTimes = seconds(textRaw{1}); lat_deg = textRaw{8}; lon_deg = textRaw{9}; alt_ft_msl = textRaw{5}; -alt_ft_agl = textRaw{6}; \ No newline at end of file +alt_ft_agl = textRaw{6}; + +% Assign to output timetable +VariableNames = {'lat_deg','lon_deg','alt_ft_msl','alt_ft_agl'}; +TTout = timetable(lat_deg,lon_deg,alt_ft_msl,alt_ft_agl,'VariableNames',VariableNames,'RowTimes',rowTimes); diff --git a/code/trajectory/trackTimetable2NEU.m b/code/trajectory/trackTimetable2NEU.m new file mode 100644 index 0000000..b2c36e1 --- /dev/null +++ b/code/trajectory/trackTimetable2NEU.m @@ -0,0 +1,45 @@ +function [arrayOut, speed_ft_s] = trackTimetable2NEU(trackIn,sidx,eidx,dt) +% Copyright 2019 - 2021, MIT Lincoln Laboratory +% SPDX-License-Identifier: BSD-2-Clause +% +% Converts timetables to arrays in a specific column order +% +% SEE ALSO createEncounters_2 findCropIdx neu2Waypoints timetable + +% Input Handling +if nargin < 2; sidx = 1; end; +if nargin < 3; eidx = size(trackIn,1); end +if nargin < 4; dt = seconds(1); end + +% Find colummn indicies for NEU variables +switch nargout + case 1 + idxVar = find(contains(trackIn.Properties.VariableNames,{'north_ft','east_ft','alt_ft_msl'})); + case 2 + idxVar = find(contains(trackIn.Properties.VariableNames,{'north_ft','east_ft','alt_ft_msl','speed_ft_s'})); + otherwise + idxVar = find(contains(trackIn.Properties.VariableNames,{'north_ft','east_ft','alt_ft_msl','speed_ft_s'})); +end + +% Filter track timetables +if sidx <= eidx + ttNEU = trackIn(sidx:1:eidx,idxVar); +else + ttNEU = trackIn(sidx:-1:eidx,idxVar); +end + +% Retime to desired timestep +if ttNEU.Properties.TimeStep ~= dt + ttNEU = retime(ttNEU,'regular','pchip','TimeStep',dt); +end + +% Have filtered NEU track to start at t=0 with positive timestep +ttNEU.Properties.StartTime = seconds(0); + +% Convert timetables to arrays in a specific column order +arrayOut = [seconds(ttNEU.Time), ttNEU.north_ft, ttNEU.east_ft, ttNEU.alt_ft_msl]; + +% Output speed as optional output +if nargout > 1 + speed_ft_s = ttNEU.speed_ft_s; +end diff --git a/createEncounters_2.m b/createEncounters_2.m deleted file mode 100644 index bf7992e..0000000 --- a/createEncounters_2.m +++ /dev/null @@ -1,489 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -function [encCount, anchors, files1, files2, isEncounter] = createEncounters_2(inFile,encTime_s,thresHorz_ft,thresVert_ft,outDir,varargin) -% CREATEENCOUNTERS_2 OUTPUTS ENCOUNTERS IN CSIM WAYPOINT FORMAT - -%% Input parser -p = inputParser; - -% Required -addRequired(p,'inFile',@ischar); % Output of RUN_1 -addRequired(p,'encTime_s',@isnumeric); % Duration of encounter -addRequired(p,'thresHorz_ft',@isnumeric); -addRequired(p,'thresVert_ft',@isnumeric); -addRequired(p,'outDir'); % Output directory - -% Optional - Minimum encounter initial conditions -addOptional(p,'initHorz_ft',[6076 121522],@(x) isnumeric(x) && numel(x) == 2); %121522 ~ 20 nm -addOptional(p,'initVert_ft',[0 300],@(x) isnumeric(x) && numel(x) == 2); -addOptional(p,'spheroid',wgs84Ellipsoid('ft')); - -% Optional - Anchor -addOptional(p,'classInclude',{'B','C','D','O'},@iscell); - -% Optional - Encounter Variations -addOptional(p,'maxEncPerPair',10,@isnumeric); % Maximum waypoint pairs for each unique pair -addOptional(p,'maxCombo',1e7,@isnumeric); % Maximum waypoint combinations to consider for each pair -addOptional(p,'timeStep_s',1,@(x) isnumeric(x) && (x >=1)); % Output timestep, CSIM requires at least >= 1 -addOptional(p,'anchorPercent',1.0,@isnumeric); % Maximum waypoint combinations to consider for each pair - -% Optional - AC permutations -addOptional(p,'acmodel1','geospatial',@(x) ischar(x) && any(strcmpi(x,{'geospatial'}))); -addOptional(p,'acmodel2','geospatial',@(x) ischar(x) && any(strcmpi(x,{'bayes','geospatial'}))); -addOptional(p,'inDir1',{},@iscell); -addOptional(p,'inDir2',{},@iscell); - -% Optional - Airspeed Sampling (Geospatial only) -addOptional(p,'vrange1_kts',[5 87],@(x) isnumeric(x) && numel(x) == 2); -addOptional(p,'vrange2_kts',[5 87],@(x) isnumeric(x) && numel(x) == 2); - -% Optional - Altitude Sampling (Geospatial only) -addOptional(p,'isSampleAlt1',false,@islogical); -addOptional(p,'isSampleAlt2',false,@islogical); -addOptional(p,'minAlt1_ft_agl',100,@isnumeric); % Minimum AGL altitude -addOptional(p,'minAlt2_ft_agl',100,@isnumeric); % Minimum AGL altitude -addOptional(p,'maxAlt1_ft_agl',400,@isnumeric); % Maximum AGL altitude -addOptional(p,'maxAlt2_ft_agl',400,@isnumeric); % Maximum AGL altitude - -% Optional - Bayes Tracks -addOptional(p,'z_agl_tol_ft',200,@(x) isnumeric(x) && numel(x) == 1); % Used by placeTrack() -addOptional(p,'demBayes','strm3',@(x) isstr(x) && any(strcmpi(x,{'dted1','dted2','globe','gtopo30','srtm1','srtm3','srtm30'}))); -addOptional(p,'demBayesBackup','globe',@(x) isstr(x) && any(strcmpi(x,{'dted1','dted2','globe','gtopo30','srtm1','srtm3','srtm30'}))); -addOptional(p,'labelTime','time_s'); -addOptional(p,'labelX','x_ft'); -addOptional(p,'labelY','y_ft'); -addOptional(p,'labelZ','z_ft'); - -% Optional - FAA DOF -addOptional(p,'dofFile',[getenv('AEM_DIR_CORE') filesep 'output' filesep 'dof-' '25-Feb-2020' '.mat'],@ischar); % Location of output of RUN_readfaadof from em-core -addOptional(p,'dofObs',{'grain elevator','lgthouse','pole','silo','tower','utility pole','windmill'},@iscell); % Obstacles to keep -addOptional(p,'dofMinHeight_ft',100,@isnumeric); % Minimum height of obstacles -addOptional(p,'dofMaxRange_ft',500,@isnumeric); -addOptional(p,'dofMaxVert_ft',50,@isnumeric); - -% Optional - Output -addOptional(p,'isPlot',false,@islogical); -addOptional(p,'isZip',true,@islogical); - -% Row offsets -% These are needed because the geospatial trajectory generator will add -% vertical take off and landing segements, which you may not want -addOptional(p,'rowstart1',2,@isnumeric); % -addOptional(p,'rowend1',1,@isnumeric); % -addOptional(p,'rowstart2',2,@isnumeric); % -addOptional(p,'rowend2',1,@isnumeric); % - -% Parse -parse(p,inFile,encTime_s,thresHorz_ft,thresVert_ft,outDir,varargin{:}); - -%% Assertions -assert(mod(p.Results.encTime_s, p.Results.timeStep_s)==0,'createEncounteres_2:timeMultiples','encTime_s is not a multiple of timeStep_s'); -assert(~all(p.Results.isSampleAlt1 && strcmp(p.Results.acmodel1,'bayes')),'createEncounters_2:isSampleAlt1-acmodel1','isSampleAlt1 cannot be true when using bayes model'); -assert(~all(p.Results.isSampleAlt2 && strcmp(p.Results.acmodel2,'bayes')),'createEncounters_2:isSampleAlt2-acmodel2','isSampleAlt2 cannot be true when using bayes model'); -assert(~all(p.Results.isSampleAlt2 && strcmp(p.Results.acmodel2,'bayes')),'createEncounters_2:isSampleAlt2-acmodel2','isSampleAlt2 cannot be true when using bayes model'); -assert(strcmp(p.Results.spheroid.LengthUnit,'foot'),'createEncounters_2:LengthUnit','spheroid LengthUnit must be ''foot'''); - -%% Initialize encounter structs and encounter counter -waypoints_struct = []; -encCount = 0; - -%% Load output from RUN_1 -if ~isfile(inFile) - anchors = table; - isEncounter = false(0,1); - files1 = strings(0,1); - files2 = strings(0,1); - warning('File not found, inFile = %s...RETURNING',inFile); - return; -end - -load(inFile,'anchors','anchorRange_nm'); - -% Remove anchors without geospaital trajectories -anchors(anchors.num_geospatial == 0,:) = []; - -% Filter by airspace -isAirspace = false(size(anchors,1),1); -for i=1:1:numel(p.Results.classInclude) - isAirspace = isAirspace | anchors.class == p.Results.classInclude{i}; -end -anchors = anchors(isAirspace,:); - -if p.Results.anchorPercent ~= 1.0 - pidx = randperm(size(anchors,1), floor(size(anchors,1) * p.Results.anchorPercent)); - anchors = anchors(pidx',:); -end - -% Number of anchors -numAnchors = size(anchors,1); - -% Tracker if an encounter was generated for anchor -isEncounter = false(numAnchors,1); - -% Convert anchorRange_nm from RUN_1 to feet to match spheroid -% An assert above ensures p.Results.spheroid.LengthUnit = 'foot' -anchorRange_ft = anchorRange_nm * unitsratio(p.Results.spheroid.LengthUnit,'nm'); - -% Set random seed -rng(numAnchors); - -%% Return if no data -if numAnchors == 0 - files1 = strings(0,1); - files2 = strings(0,1); - fprintf('No anchors in %s, RETURNING\n',inFile); - return; -end - -%% Create clusters -% Clusters currently only used when loading Bayes tracks because -% placeTrack() uses msl2agl(). Loading the DEM for each call of -% placeTrack() is horribly inefficient. To keep DEM size reasonable, we -% break the anchors up into clusters -area_deg = abs(max(anchors.lon_deg) - min(anchors.lon_deg)) * abs(max(anchors.lat_deg) - min(anchors.lat_deg)); -area_nm = deg2nm(area_deg); -numClust = ceil(area_nm / 25); -if numel(anchors.lon_deg) <= numClust | numClust == 0 - numClust = ceil(numel(anchors.lon_deg) / 5); -end -[anchors.cluster,~,~] = kmeans([anchors.lon_deg, anchors.lat_deg],numClust); - -anchors = sortrows(anchors,{'cluster','num_geospatial','class','lat_deg'}); - -% Display to screen -fprintf('Starting with %i anchor points across %i clusters\n',numAnchors,numClust); - -%% Load FAA DOF -% Load FAA DOF processing in em-core -load(p.Results.dofFile,'Tdof'); - -% Filter DOF based on obstacle -l = contains(Tdof.obs_type,p.Results.dofObs) & Tdof.alt_ft_agl >= p.Results.dofMinHeight_ft & strcmpi(Tdof.verification_status,'verified'); -Tdof = Tdof(l,[4,5,6]); % lat, lon, alt agl; - -% Filter DOF based on bounding box of anchor points -bbox = [min(anchors.lon_deg), min(anchors.lat_deg); max(anchors.lon_deg), max(anchors.lat_deg)]; -[~, ~, isbox] = filterboundingbox(Tdof.lat_deg,Tdof.lon_deg,bbox); -Tdof = Tdof(isbox,:); - -% Create obstacles polygons -[latObstacle,lonObstacle] = scircle1(Tdof.lat_deg,Tdof.lon_deg,repmat(p.Results.dofMaxRange_ft,size(Tdof,1),1),[],p.Results.spheroid,'degrees',20); -altObstacle_ft_agl = Tdof.alt_ft_agl + p.Results.dofMaxVert_ft; - -%% Load AC1 -switch p.Results.acmodel1 - case 'bayes' - % Get list of filenames - listing1 = dir([p.Results.inDir1{1} filesep '*.csv']); - - % Concat listings if there is more than one input directory - if numel(p.Results.inDir2) > 1 - for i=2:1:numel(p.Results.inDir2) - listing1 = [listing1; dir([p.Results.inDir2{i} filesep '*.csv'])]; %#ok - end - end - assert(~isempty(listing1),'createEncounters_2:listing1','No files found p.Results.inDir2\n'); - % Create full filenames for trajectories which are close to anchor point - files1 = string(compose('%s/%s',string({listing1.folder}'), string({listing1.name}'))); - case 'geospatial' - % There could be multiple files for each anchor point - files1 = strings(sum(anchors.num_geospatial),1); - s = 1; - for i=1:1:numAnchors - e = anchors.num_geospatial(i) + s - 1; - files1(s:e) = anchors.files{i}; - s = e + 1;; - end -end - -%% Load AC2 -switch p.Results.acmodel2 - case 'bayes' - % Get list of filenames - listing2 = dir([p.Results.inDir2{1} filesep '*.csv']); - - % Concat listings if there is more than one input directory - if numel(p.Results.inDir2) > 1 - for i=2:1:numel(p.Results.inDir2) - listing2 = [listing2; dir([p.Results.inDir2{i} filesep '*.csv'])]; %#ok - end - end - assert(~isempty(listing2),'createEncounters_2:listing2','No files found p.Results.inDir2\n'); - % Create full filenames for trajectories which are close to anchor point - files2 = string(compose('%s/%s',string({listing2.folder}'), string({listing2.name}'))); - case 'geospatial' - % There could be multiple files for each anchor point - files2 = strings(sum(anchors.num_geospatial),1); - s = 1; - for i=1:1:numAnchors - e = anchors.num_geospatial(i) + s - 1; - files2(s:e) = anchors.files{i}; - s = e + 1;; - end -end - -%% Randomly sample a Bayes trajectory for each anchor point -% AC1 must be geospatial -if numel(files1) >= numel(files2) - % More files in AC1 - files2 = files2(randi(numel(files2),numel(files1),1)); -else - % More files in AC2 - files2 = files2(randperm(numel(files2),numel(files1))); -end - -%% Parse out anchor points for each files1/files2 combination -% AC1 must be geospatial -lat0 = zeros(numel(files1),1); -lon0 = zeros(numel(files1),1); -clust0 = zeros(numel(files1),1); -s = 1; -for i=1:1:numAnchors - e = anchors.num_geospatial(i) + s - 1; - lat0(s:e) = anchors.lat_deg(i); - lon0(s:e) = anchors.lon_deg(i); - clust0(s:e) = anchors.cluster(i); - s = e + 1; -end - -% Update -numAnchors = numel(lat0); - -%% Iterate over clusters -for k=1:1:numClust - % Logical index of cluster - lk = find(clust0 == k); - nk = numel(lk); - - %% Load Bayes trajectories that are in relative feet - % Preallocate - time1_s = cell(size(lk)); - lat1_deg = cell(size(lk)); - lon1_deg = cell(size(lk)); - alt1_ft_msl = cell(size(lk)); - alt1_ft_agl = cell(size(lk)); - isObstacle1 = false(size(lk)); - isShort1 = false(nk,1); - - time2_s = cell(size(lk)); - lat2_deg = cell(size(lk)); - lon2_deg = cell(size(lk)); - alt2_ft_msl = cell(size(lk)); - alt2_ft_agl = cell(size(lk)); - isObstacle2 = false(size(lk)); - isShort2 = false(nk,1); - - % Aicraft 1 - switch p.Results.acmodel1 - case 'geospatial' - parfor i=1:1:nk - % Load trajectory - [time1_s{i}, lat1_deg{i}, lon1_deg{i}, alt1_ft_msl{i}, alt1_ft_agl{i}] = fastloadtraj(files1(lk(i))); - end - end - - % Aircraft 2 - switch p.Results.acmodel2 - case 'bayes' - % Load DEM - [~,~,Z_m,refvec] = msl2agl(lat0(lk), lon0(lk), p.Results.demBayes,'buff_deg',nm2deg(10)); - if isempty(Z_m) - [~,~,Z_m,refvec] = msl2agl(lat0(lk), lon0(lk), p.Results.demBayesBackup,'buff_deg',nm2deg(10)); - end - - % Iterate over files - parfor i=1:1:nk - % Set random seed - rng(i); - [time2_s{i},lat2_deg{i},lon2_deg{i},alt2_ft_msl{i},alt2_ft_agl{i},~,~,~,isObstacle2(i)] = placeTrack(files2{lk(i)},lat0(lk(i)), lon0(lk(i)),'z_agl_tol_ft',p.Results.z_agl_tol_ft,... - 'maxTries',10,... - 'latObstacle',latObstacle,'lonObstacle',lonObstacle,'altObstacle_ft_agl',altObstacle_ft_agl,... - 'labelX',p.Results.labelX,'labelY',p.Results.labelY,'labelZ',p.Results.labelZ,... - 'spheroid',p.Results.spheroid,'dem',p.Results.demBayes,'Z_m',Z_m,'refvec',refvec,'isPlot',false); - end - - isShort2 = cellfun(@numel,time2_s) < p.Results.encTime_s; - case 'geospatial' - parfor i=1:1:nk - % Load trajectory - [time2_s{i}, lat2_deg{i}, lon2_deg{i}, alt2_ft_msl{i}, alt2_ft_agl{i}] = fastloadtraj(files2(lk(i))); - end - end - %fprintf('Loaded %i pairs for %i / %i clusters\n',nk,k,numClust); - - %% Filter if we couldn't avoid obstacles - isRemove = isObstacle1 | isObstacle2 | isShort1 | isShort2; - %fprintf('Removing %i pairs due to obstacles or track length\n',sum(isRemove)); - - time1_s(isRemove,:) = []; - time2_s(isRemove,:) = []; - lat1_deg(isRemove,:) = []; - lat2_deg(isRemove,:) = []; - lon1_deg(isRemove,:) = []; - lon2_deg(isRemove,:) = []; - alt1_ft_msl(isRemove,:) = []; - alt2_ft_msl(isRemove,:) = []; - alt1_ft_agl(isRemove,:) = []; - alt2_ft_agl(isRemove,:) = []; - - % Update - lk(isRemove) = []; - nk = numel(lk); - - % Display status - fprintf('Valid %i pairs for %i / %i clusters\n',nk,k,numClust); - - %% Iterate over unique AC1 / AC2 pairs - for i=1:1:nk - % Set random seed - rng(lk(i)); - - % Identify which combinaton of waypoints satisfy thresHorz_ft - [wypts,ac1Time_s,ac1Lat_deg,ac1Lon_deg,ac1Alt_ft_msl,ac2Time_s,ac2Lat_deg,ac2Lon_deg,ac2Alt_ft_msl] = findconflict(p,time1_s{i},lat1_deg{i},lon1_deg{i},alt1_ft_msl{i},time2_s{i},lat2_deg{i},lon2_deg{i},alt2_ft_msl{i},lat0(lk(i)), lon0(lk(i)),anchorRange_ft); - - % Check for valid combinations and skip via continue if there are none - if isempty(wypts) - % fprintf('i=%i...No encounter combinations satisfy thresHorz_ft <= %0.1f...skipping via CONTINUE\n',i,thresHorz_ft) - continue; - end - - % h0_ft origin - % h0_ft used by geodetic2ned() but we don't use the NED height - h0_ft = min([ac1Alt_ft_msl;ac2Alt_ft_msl]); - - % Sample airspeed and altitude - ac1Alt_ft_agl = alt1_ft_agl{i}(p.Results.rowstart1:end-p.Results.rowend1); %#ok - ac2Alt_ft_agl = alt2_ft_agl{i}(p.Results.rowstart2:end-p.Results.rowend2); %#ok - [v1_kts,v2_kts,altAdjust1_ft,altAdjust2_ft] = samplespeedalt(p,size(wypts,1),ac1Alt_ft_agl,ac2Alt_ft_agl); - - % Update waypoints with altitude adjust - wypts(:,5) = wypts(:,5) + altAdjust1_ft; % AC1 Altitude - wypts(:,6) = wypts(:,6) + altAdjust2_ft; % AC2 Altitude - - % Calculate vertical miss distance for each waypoint - % Remove waypoints if they violate VMD condition - vmd = abs(wypts(:,5) - wypts(:,6)); - lvmd = vmd <= p.Results.thresVert_ft; - wypts = wypts(lvmd,:); - vmd = vmd(lvmd); - altAdjust1_ft = altAdjust1_ft(lvmd); - altAdjust2_ft = altAdjust2_ft(lvmd); - - % Check for valid combinations and skip via continue if there are none - if isempty(wypts) - continue; - end - - % Convert each aircraft to north / east coordinates in feet - % The CSIM waypoint uses feet but code mostly calculates using lat / lon - % We're currently ignoring the altitude adjustment because this - % function is being used for low altitudes...calling this once is - % faster than putting it in the j for loop below - [ac1North_ft,ac1East_ft,~] = geodetic2ned(ac1Lat_deg,ac1Lon_deg,ac1Alt_ft_msl,lat0(lk(i)),lon0(lk(i)),h0_ft,p.Results.spheroid); - [ac2North_ft,ac2East_ft,~] = geodetic2ned(ac2Lat_deg,ac2Lon_deg,ac2Alt_ft_msl,lat0(lk(i)),lon0(lk(i)),h0_ft,p.Results.spheroid); - - % Check to make sure coordinates are not NaN - if any(isnan([ac1North_ft; ac1North_ft; ac2North_ft; ac2East_ft])) - warning('createEncounters_2:geodetic2ned','geodetic2ned() outputs NaN coordinates\nTrajectory files:\nAC1 = %s\nAC2 = %s\nSkipping via CONTINUE\n',files1(lk(i)),files2(lk(i))); - continue; - end - - % Iterate over unique waypoint pairs - encAdd = false; - jadd = false(size(wypts,1),1); - for j=1:1:size(wypts,1) - % Crop tracks - [ac1NEU_foward,ac2NEU_foward,ac1NEU_backward,ac2NEU_backward] = croptracks(p,wypts(j,:),encTime_s,ac1Time_s,ac2Time_s,ac1Lat_deg,ac2Lat_deg,ac1Lon_deg,ac2Lon_deg,ac1North_ft,ac2North_ft,ac1East_ft,ac2East_ft,ac1Alt_ft_msl,ac2Alt_ft_msl,altAdjust1_ft(j),altAdjust2_ft(j),v1_kts(j),v2_kts(j)); - - % Update waypoints_csim struct - [waypoints_struct,encCount,encAdd] = updatewaypointstruct(p,waypoints_struct,encCount,ac1NEU_foward,ac2NEU_foward,ac1NEU_backward,ac2NEU_backward); - jadd(j) = encAdd; - end % End j loop - - % Record if an encounter was made for anchor point - if encAdd - isEncounter(lk(i)) = true; - wypts = wypts(jadd,:); - end - - if p.Results.isPlot && size(wypts,1) > 0 - if ~encAdd - %close(lk(i)); - else - % Create figure and basemap - figure(lk(i)); - gx = geoaxes('Basemap','topographic',... - 'FontSize',16,... - 'FontWeight','bold',... - 'Position',[0 0 1 1]); - bmap = geobasemap(gx); - geolimits(lat0(lk(i)) + nm2deg([-anchorRange_nm anchorRange_nm]*1.05), lon0(lk(i)) + nm2deg([-anchorRange_nm anchorRange_nm]*1.05)); - hold on; - - % Anchor Radius - [lat0c,lon0c] = scircle1(lat0(lk(i)),lon0(lk(i)),anchorRange_ft,[],p.Results.spheroid,'degrees',100); - geoplot(lat0c,lon0c,'k--'); - - % Paths - geoplot(ac1Lat_deg,ac1Lon_deg,'b-'); - geoplot(ac2Lat_deg,ac2Lon_deg,'r-'); - - % Anchor Point - geoscatter(lat0(lk(i)),lon0(lk(i)),50,'k','d','filled'); - - % Threshold Points - for w=1:1:size(wypts,1) - if w==1; hv = 'on'; else hv = 'off'; end; % Only want to plot conflict points once on legend - geoscatter(wypts(w,1),wypts(w,2),60,'b','h','filled','HandleVisibility',hv) - geoscatter(wypts(w,3),wypts(w,4),60,'r','p','filled','HandleVisibility',hv) - - % Alternate horizontal alignment for odd / even values of w - % o1, o2 = offsets for text - if mod(w,2) == 0 - ha1 = 'left'; - ha2 = 'right'; - o1 = nm2deg(0.04); - o2 = -nm2deg(0.04); - else - ha1 = 'right'; - ha2 = 'left'; - o1 = -nm2deg(0.04); - o2 = nm2deg(0.04); - end - - text(wypts(w,1),wypts(w,2)+o1,num2str(w),'Color','b','FontSize',8,'FontWeight','bold','HorizontalAlignment',ha1); - text(wypts(w,3),wypts(w,4)+o2,num2str(w),'Color','r','FontSize',8,'FontWeight','bold','HorizontalAlignment',ha2); - end - - % Adjust map size, legend, hold off - set(gcf,'Units','inches','Position',[1 1 11.94 5.28]) - legend(sprintf('Radius = %0.2fnm',anchorRange_nm),sprintf('AC1 = %s',p.Results.acmodel1), sprintf('AC2 = %s',p.Results.acmodel2),sprintf('(%0.3f,%0.3f)',lat0(lk(i)),lon0(lk(i))),'AC1 @ conflict','AC2 @ conflict'); - hold off; - - % Print - print(['output' filesep 'plot_encounter_examples' filesep 'example_2' '_lk' num2str(lk(i)) '_k' num2str(k) '_i' num2str(i) '_lat' num2str(lat0(lk(i))) '_lon' num2str(lon0(lk(i))) '_thresHorz' num2str(thresHorz_ft) '_thresVert' num2str(thresVert_ft) '_ac1' p.Results.acmodel1 '_ac2' p.Results.acmodel2 '.png'],'-dpng','-r300'); - end - end - end % End i loop - fprintf('%i encounters after %i / %i clusters\n',encCount,k,numClust); -end % End k loop - -%% Write waypoints to file -% Make output directory -mkdir(outDir); - -% Write individual encounters to file -parfor files_idx = 1:1:size(waypoints_struct,2) - output_enc_filename = [outDir filesep 'enc_' num2str(files_idx) '.dat']; - save_waypoints(output_enc_filename,waypoints_struct(:,files_idx)); -end - -% Display status to screen -fprintf('%i encounters written to file\n',size(waypoints_struct,2)); - -%% Zip waypoints for easy sharing -if encCount > 0 - if p.Results.isZip; zip([outDir '.zip'],[outDir filesep '*.dat']); end -else - fprintf('Nothing to zip, no encounter generated for %s\n',inFile); -end diff --git a/croptracks.m b/croptracks.m deleted file mode 100644 index 0109d84..0000000 --- a/croptracks.m +++ /dev/null @@ -1,39 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -function [ac1NEU_foward,ac2NEU_foward,ac1NEU_backward,ac2NEU_backward] = croptracks(p,wypts,encTime_s,ac1Time_s,ac2Time_s,ac1Lat_deg,ac2Lat_deg,ac1Lon_deg,ac2Lon_deg,ac1North_ft,ac2North_ft,ac1East_ft,ac2East_ft,ac1Alt_ft_msl,ac2Alt_ft_msl,altAdjust1_ft,altAdjust2_ft,v1_kts,v2_kts) - -% Find CPA waypoint indices -%cidx1 = find(wypts(1) == ac1Lat_deg & wypts(2) == ac1Lon_deg); -%cidx2 = find(wypts(3) == ac2Lat_deg & wypts(4) == ac2Lon_deg,1,'first'); -cidx1 = wypts(7); -cidx2 = wypts(8); - -% There are four direction combinations we can arrive at the specific -% waypoint combination. Think of it was foward / backward propagation for -% each aircraft, so 2 X 2 = 4 propagation combinations. This results in -% comprehensive ways to get arrive at this CPA at a given time -switch p.Results.acmodel1 - case 'bayes' - [sidxFoward1, sidxBackward1,eidxFoward1, eidxBackward1,timesteps1] = findCropIdx(cidx1,encTime_s/2,'time',... - 'time_s',ac1Time_s); - case 'geospatial' - [sidxFoward1, sidxBackward1,eidxFoward1, eidxBackward1,timesteps1] = findCropIdx(cidx1,encTime_s/2,'position',... - 'lat',ac1Lat_deg,'lon',ac1Lon_deg,'v_kts',v1_kts); -end - -switch p.Results.acmodel2 - case 'bayes' - [sidxFoward2, sidxBackward2,eidxFoward2, eidxBackward2,timesteps2] = findCropIdx(cidx2,encTime_s/2,'time',... - 'time_s',ac2Time_s); - case 'geospatial' - [sidxFoward2, sidxBackward2,eidxFoward2, eidxBackward2,timesteps2] = findCropIdx(cidx2,encTime_s/2,'position',... - 'lat',ac2Lat_deg,'lon',ac2Lon_deg,'v_kts',v2_kts); -end - -% AC1 -ac1NEU_foward = [timesteps1(sidxFoward1:1:eidxFoward1),ac1North_ft(sidxFoward1:1:eidxFoward1),ac1East_ft(sidxFoward1:1:eidxFoward1),ac1Alt_ft_msl(sidxFoward1:1:eidxFoward1)+altAdjust1_ft]; -ac1NEU_backward = [timesteps1(sidxBackward1:-1:eidxBackward1),ac1North_ft(sidxBackward1:-1:eidxBackward1),ac1East_ft(sidxBackward1:-1:eidxBackward1),ac1Alt_ft_msl(sidxBackward1:-1:eidxBackward1)+altAdjust1_ft]; - -% AC2 -ac2NEU_foward = [timesteps2(sidxFoward2:1:eidxFoward2),ac2North_ft(sidxFoward2:1:eidxFoward2),ac2East_ft(sidxFoward2:1:eidxFoward2),ac2Alt_ft_msl(sidxFoward2:1:eidxFoward2)+altAdjust2_ft]; -ac2NEU_backward = [timesteps2(sidxBackward2:-1:eidxBackward2),ac2North_ft(sidxBackward2:-1:eidxBackward2),ac2East_ft(sidxBackward2:-1:eidxBackward2),ac2Alt_ft_msl(sidxBackward2:-1:eidxBackward2)+altAdjust2_ft]; \ No newline at end of file diff --git a/doc/README.md b/doc/README.md index 476588d..7331f4e 100644 --- a/doc/README.md +++ b/doc/README.md @@ -6,7 +6,7 @@ Default directory for images and supporting content for documentation. DISTRIBUTION STATEMENT A. Approved for public release. Distribution is unlimited. -© 2019, 2020 Massachusetts Institute of Technology. +© 2019-2021 Massachusetts Institute of Technology. This material is based upon work supported by the Federal Aviation Administration under Air Force Contract No. FA8702-15-D-0001. diff --git a/doc/example_2_lk181_k1_i171_lat35.7331_lon-86.8775_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png b/doc/example_2_lk181_k1_i171_lat35.7331_lon-86.8775_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png index 53058dd..2ba98f7 100644 Binary files a/doc/example_2_lk181_k1_i171_lat35.7331_lon-86.8775_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png and b/doc/example_2_lk181_k1_i171_lat35.7331_lon-86.8775_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png differ diff --git a/doc/example_2_lk303_k11_i2_lat37.9159_lon-97.1379_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png b/doc/example_2_lk303_k11_i2_lat37.9159_lon-97.1379_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png index f40780d..ee7cd62 100644 Binary files a/doc/example_2_lk303_k11_i2_lat37.9159_lon-97.1379_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png and b/doc/example_2_lk303_k11_i2_lat37.9159_lon-97.1379_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png differ diff --git a/doc/example_2_lk322_k1_i318_lat42.3219_lon-71.0506_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png b/doc/example_2_lk322_k1_i318_lat42.3219_lon-71.0506_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png index a7b78f3..08cdbed 100644 Binary files a/doc/example_2_lk322_k1_i318_lat42.3219_lon-71.0506_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png and b/doc/example_2_lk322_k1_i318_lat42.3219_lon-71.0506_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png differ diff --git a/doc/example_2_lk364_k1_i364_lat37.3725_lon-77.1302_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png b/doc/example_2_lk364_k1_i364_lat37.3725_lon-77.1302_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png index e8a248b..58af939 100644 Binary files a/doc/example_2_lk364_k1_i364_lat37.3725_lon-77.1302_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png and b/doc/example_2_lk364_k1_i364_lat37.3725_lon-77.1302_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png differ diff --git a/doc/example_2_lk373_k1_i358_lat36.0871_lon-87.2522_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png b/doc/example_2_lk373_k1_i358_lat36.0871_lon-87.2522_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png index 428dec8..7e284e0 100644 Binary files a/doc/example_2_lk373_k1_i358_lat36.0871_lon-87.2522_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png and b/doc/example_2_lk373_k1_i358_lat36.0871_lon-87.2522_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png differ diff --git a/doc/example_2_lk450_k1_i432_lat36.2328_lon-87.044_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png b/doc/example_2_lk450_k1_i432_lat36.2328_lon-87.044_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png index cb6f365..95227ed 100644 Binary files a/doc/example_2_lk450_k1_i432_lat36.2328_lon-87.044_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png and b/doc/example_2_lk450_k1_i432_lat36.2328_lon-87.044_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png differ diff --git a/doc/example_2_lk481_k9_i82_lat37.9347_lon-76.3183_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png b/doc/example_2_lk481_k9_i82_lat37.9347_lon-76.3183_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png index 90f7bc5..39e7e2b 100644 Binary files a/doc/example_2_lk481_k9_i82_lat37.9347_lon-76.3183_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png and b/doc/example_2_lk481_k9_i82_lat37.9347_lon-76.3183_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png differ diff --git a/doc/example_2_lk514_k5_i18_lat30.0494_lon-95.2532_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png b/doc/example_2_lk514_k5_i18_lat30.0494_lon-95.2532_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png index 3d57daf..98c9bc4 100644 Binary files a/doc/example_2_lk514_k5_i18_lat30.0494_lon-95.2532_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png and b/doc/example_2_lk514_k5_i18_lat30.0494_lon-95.2532_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png differ diff --git a/doc/example_2_lk516_k5_i20_lat30.0494_lon-95.2532_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png b/doc/example_2_lk516_k5_i20_lat30.0494_lon-95.2532_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png index 51b87a4..41d5dd6 100644 Binary files a/doc/example_2_lk516_k5_i20_lat30.0494_lon-95.2532_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png and b/doc/example_2_lk516_k5_i20_lat30.0494_lon-95.2532_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png differ diff --git a/doc/example_2_lk84_k1_i84_lat32.8392_lon-96.8979_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png b/doc/example_2_lk84_k1_i84_lat32.8392_lon-96.8979_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png index 2a59758..95a0efc 100644 Binary files a/doc/example_2_lk84_k1_i84_lat32.8392_lon-96.8979_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png and b/doc/example_2_lk84_k1_i84_lat32.8392_lon-96.8979_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png differ diff --git a/doc/example_2_lk900_k1_i854_lat36.0038_lon-86.7734_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png b/doc/example_2_lk900_k1_i854_lat36.0038_lon-86.7734_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png index bef4d9b..1a04249 100644 Binary files a/doc/example_2_lk900_k1_i854_lat36.0038_lon-86.7734_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png and b/doc/example_2_lk900_k1_i854_lat36.0038_lon-86.7734_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png differ diff --git a/doc/example_2_lk901_k1_i855_lat36.0038_lon-86.7734_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png b/doc/example_2_lk901_k1_i855_lat36.0038_lon-86.7734_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png index 9b5245f..f793190 100644 Binary files a/doc/example_2_lk901_k1_i855_lat36.0038_lon-86.7734_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png and b/doc/example_2_lk901_k1_i855_lat36.0038_lon-86.7734_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png differ diff --git a/doc/example_2_lk910_k8_i37_lat32.7767_lon-96.8355_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png b/doc/example_2_lk910_k8_i37_lat32.7767_lon-96.8355_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png index e46dd76..ba28055 100644 Binary files a/doc/example_2_lk910_k8_i37_lat32.7767_lon-96.8355_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png and b/doc/example_2_lk910_k8_i37_lat32.7767_lon-96.8355_thresHorz2000_thresVert250_ac1geospatial_ac2bayes.png differ diff --git a/doc/example_2_lk910_k8_i37_lat32.7767_lon-96.8355_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png b/doc/example_2_lk910_k8_i37_lat32.7767_lon-96.8355_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png index 94c9aa8..d1020bd 100644 Binary files a/doc/example_2_lk910_k8_i37_lat32.7767_lon-96.8355_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png and b/doc/example_2_lk910_k8_i37_lat32.7767_lon-96.8355_thresHorz500_thresVert100_ac1geospatial_ac2bayes.png differ diff --git a/doc/montecarloframework.png b/doc/montecarloframework.png index 04fb648..1692a5a 100644 Binary files a/doc/montecarloframework.png and b/doc/montecarloframework.png differ diff --git a/doc/plot_1_agriculture_US-KS-anchorRange1.25.png b/doc/plot_1_agriculture_US-KS-anchorRange1.25.png index f3e4632..f364204 100644 Binary files a/doc/plot_1_agriculture_US-KS-anchorRange1.25.png and b/doc/plot_1_agriculture_US-KS-anchorRange1.25.png differ diff --git a/doc/plot_1_all-noshield_US-KS-anchorRange1.25.png b/doc/plot_1_all-noshield_US-KS-anchorRange1.25.png index 92f5a03..13ab046 100644 Binary files a/doc/plot_1_all-noshield_US-KS-anchorRange1.25.png and b/doc/plot_1_all-noshield_US-KS-anchorRange1.25.png differ diff --git a/doc/plot_1_all-noshield_US-NC-anchorRange1.25.png b/doc/plot_1_all-noshield_US-NC-anchorRange1.25.png index a7a22d1..fd8bf94 100644 Binary files a/doc/plot_1_all-noshield_US-NC-anchorRange1.25.png and b/doc/plot_1_all-noshield_US-NC-anchorRange1.25.png differ diff --git a/doc/plot_1_all-noshield_US-NV-anchorRange1.25.png b/doc/plot_1_all-noshield_US-NV-anchorRange1.25.png index 204ffd1..123b35f 100644 Binary files a/doc/plot_1_all-noshield_US-NV-anchorRange1.25.png and b/doc/plot_1_all-noshield_US-NV-anchorRange1.25.png differ diff --git a/doc/plot_1_longlinearinfrastructure_US-KS-anchorRange1.25.png b/doc/plot_1_longlinearinfrastructure_US-KS-anchorRange1.25.png index ef14664..16c7b8f 100644 Binary files a/doc/plot_1_longlinearinfrastructure_US-KS-anchorRange1.25.png and b/doc/plot_1_longlinearinfrastructure_US-KS-anchorRange1.25.png differ diff --git a/doc/plot_1_railway_US-KS-anchorRange1.25.png b/doc/plot_1_railway_US-KS-anchorRange1.25.png index b35be19..3745c8e 100644 Binary files a/doc/plot_1_railway_US-KS-anchorRange1.25.png and b/doc/plot_1_railway_US-KS-anchorRange1.25.png differ diff --git a/findCropIdx.m b/findCropIdx.m deleted file mode 100644 index 9d167bc..0000000 --- a/findCropIdx.m +++ /dev/null @@ -1,68 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -function [sidxFoward, sidxBackward, eidxFoward, eidxBackward, time_s] = findCropIdx(cidx,timeThres_s,method,varargin) - -% Todo -% The encounter length CPA check happens outside of this function, it maybe -% worth returning a flag that the time criteria wasn't met - -%% Input parser -p = inputParser; - -% Required -addRequired(p,'cidx',@(x) isnumeric(x) && x>0); % Index that meets CPA criteria -addRequired(p,'timeThres_s',@(x) isnumeric(x) && x>=0); % How much time to propogate -addRequired(p,'method',@(x) ischar(x) && any(strcmp(x,{'position','time'}))); % Method to use when cropping - -% Optional -addOptional(p,'lat',[],@isnumeric); % latitude -addOptional(p,'lon',[],@isnumeric); % longitude -addOptional(p,'v_kts',[],@isnumeric); % velocity -addOptional(p,'time_s',[],@isnumeric); % time - -% Parse -parse(p,cidx,timeThres_s,method,varargin{:}); - -%% Number of input points -switch p.Results.method - case 'position' - np = numel(p.Results.lat); - case 'time' - np = numel(p.Results.time_s); -end - -%% Recalculate time if using velocity or just use input -switch p.Results.method - case 'position' - [~, ~,t_legs_s] = calcLegsTime(p.Results.lat,p.Results.lon,p.Results.v_kts); - time_s = [0; cumsum(t_legs_s)]; - case 'time' - time_s = p.Results.time_s; -end - -%% Determine threshold times relative to time_s(cidx) -sidxTime = time_s(cidx) - timeThres_s; -eidxTime = time_s(cidx) + timeThres_s; - -%% Start index -if sidxTime >= 0 - [~,sidxFoward] = min(abs(time_s-sidxTime)); -else - sidxFoward = 1; -end - -%% End index -if eidxTime <= max(time_s) - [~,eidxFoward] = min(abs(time_s-eidxTime)); -else - eidxFoward = np; -end - -%% Constant airspeed with fixed spacing which means things are symetric -sidxBackward = eidxFoward; -eidxBackward = sidxFoward; - -%% Error checking -assert(sidxFoward <= cidx & cidx <= eidxFoward ,'findCropIdx indexing error') -assert(eidxFoward >= cidx & cidx >= eidxBackward ,'findCropIdx indexing error') - diff --git a/findconflict.m b/findconflict.m deleted file mode 100644 index 96fbe4f..0000000 --- a/findconflict.m +++ /dev/null @@ -1,135 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -function [wypts,ac1Time_s,ac1Lat_deg,ac1Lon_deg,ac1Alt_ft_msl,ac2Time_s,ac2Lat_deg,ac2Lon_deg,ac2Alt_ft_msl] = findconflict(p,time1_s,lat1_deg,lon1_deg,alt1_ft_msl,time2_s,lat2_deg,lon2_deg,alt2_ft_msl,lat0,lon0,anchorRange_ft) - -%% Get waypoints -% aircraft 1 -ac1Time_s = time1_s(p.Results.rowstart1:end-p.Results.rowend1); -ac1Lat_deg = lat1_deg(p.Results.rowstart1:end-p.Results.rowend1); -ac1Lon_deg = lon1_deg(p.Results.rowstart1:end-p.Results.rowend1); %#ok -ac1Alt_ft_msl = alt1_ft_msl(p.Results.rowstart1:end-p.Results.rowend1); %#ok - -% aircraft 2 -ac2Time_s = time2_s(p.Results.rowstart2:end-p.Results.rowend2); %#ok -ac2Lat_deg = lat2_deg(p.Results.rowstart2:end-p.Results.rowend2); %#ok -ac2Lon_deg = lon2_deg(p.Results.rowstart2:end-p.Results.rowend2); %#ok -ac2Alt_ft_msl = alt2_ft_msl(p.Results.rowstart2:end-p.Results.rowend2); %#ok - -%% Determine which waypoints are close to anchor point -% Circle a circle around the anchor point -% Find which coordinates are within circle around anchor point -[lat0c,lon0c] = scircle1(lat0,lon0,anchorRange_ft,[],p.Results.spheroid,'degrees',100); - -idxAnchor1 = find(InPolygon(ac1Lat_deg,ac1Lon_deg,lat0c,lon0c)); -idxAnchor2 = find(InPolygon(ac2Lat_deg,ac2Lon_deg,lat0c,lon0c)); - -% When using placeTrack() for positioning Bayes tracks, it is possible to -% have the track originally fly through the anchor point but during -% filtering due to terrain or obstacles, have it removed. This would result -% in the InPolygon() check above returning empty -switch p.Results.acmodel1 - case 'bayes' - if isempty(idxAnchor1) - idxAnchor1 = (1:1:numel(ac1Lat_deg))'; - end -end -switch p.Results.acmodel2 - case 'bayes' - if isempty(idxAnchor2) - idxAnchor2 = (1:1:numel(ac2Lat_deg))'; - end -end - -% Deprecated using distance -% This way is (negligible more) accurate because it takes into account the spheriod but is also much slower -%idxAnchor1 = find(distance(ac1Lat_deg,ac1Lon_deg,lat0,lon0,p.Results.spheroid) <= anchorRange_ft); -%idxAnchor2 = find(distance(ac2Lat_deg,ac2Lon_deg,lat0,lon0,p.Results.spheroid) <= anchorRange_ft); - -% DEPRECATED - REMOVE IN FUTURE UPDATES -% Calculate combinations of waypoints -% Filter combinations to those only near the anchor point -% c = combvec(idxAnchor1',idxAnchor2')'; -% -% % Randomly order combinations -% % Downsample combination of waypoints if needed -% if size(c,1) > p.Results.maxCombo -% pidx = randperm(size(c,1),p.Results.maxCombo); -% fprintf('Too many waypoints combinations for pair %i, sampling %i combinations\n',i,p.Results.maxCombo); -% else -% pidx = randperm(size(c,1),size(c,1)); -% end -% c = c(pidx',:); - -%% Return if not both aircraft satisfy the InPolygon check -if isempty(idxAnchor1) | isempty(idxAnchor2) - c = zeros(0,2); - wypts = zeros(0,8); - return; -end - -%% Calculate distance between waypoint combinations -% Determine which aircraft has more potential points -if numel(idxAnchor1) >= numel(idxAnchor2) - % More AC1, so we generate circles for AC2 - idxcol = idxAnchor2; - latcol = ac2Lat_deg(idxAnchor2); - loncol = ac2Lon_deg(idxAnchor2); - - idxrow = idxAnchor1; - latrow = ac1Lat_deg(idxAnchor1); - lonrow = ac1Lon_deg(idxAnchor1); -else - % More AC2, so we generate circles for AC1 - idxrow = idxAnchor2; - latrow = ac2Lat_deg(idxAnchor2); - lonrow = ac2Lon_deg(idxAnchor2); - - idxcol = idxAnchor1; - latcol = ac1Lat_deg(idxAnchor1); - loncol = ac1Lon_deg(idxAnchor1); -end - -% Generate scircles for columns -[latc,lonc] = scircle1(latcol,loncol,repmat(p.Results.thresHorz_ft,size(loncol)),[],p.Results.spheroid,'degrees',100); - -% Preallocate logical matrix -isClose = sparse(numel(latrow),numel(latcol)); - -% Iterate over circles -for i=1:1:numel(latcol) - isClose(:,i) = InPolygon(latrow,lonrow,latc(:,i),lonc(:,i)); -end - -% Convert from linear indices to subscripts -[row,col,~] = find(isClose); -%[row,col] = ind2sub(size(isClose),find(isClose)); - -% Regenerate c with those that satisfy thresHorz_ft -% AC1 should be column 1, AC2 should be column 2 -if numel(idxAnchor1) >= numel(idxAnchor2) - c = [idxrow(row),idxcol(col)]; -else - c = [idxcol(col),idxrow(row)]; -end - -% Filter to desired number of encounters -if size(c,1) > p.Results.maxEncPerPair - c = c(randperm(size(c,1),p.Results.maxEncPerPair),:); -end - -% Calculate distance (uncomment for debugging) -% d_ft = distance(ac1Lat_deg(c(:,1)),ac1Lon_deg(c(:,1)),ac2Lat_deg(c(:,2)),ac2Lon_deg(c(:,2)),p.Results.spheroid); - -%% Organize waypoints -wypts = zeros(size(c,1),8); -if ~isempty(wypts) - wypts(:,1) = ac1Lat_deg(c(:,1)); % AC1 Latitude - wypts(:,2) = ac1Lon_deg(c(:,1)); % AC1 Longitude - wypts(:,3) = ac2Lat_deg(c(:,2)); % AC2 Latitude - wypts(:,4) = ac2Lon_deg(c(:,2)); % AC2 Longitude - wypts(:,5) = ac1Alt_ft_msl(c(:,1)); % AC1 Altitude - wypts(:,6) = ac2Alt_ft_msl(c(:,2)); % AC2 Altitude - wypts(:,7) = c(:,1); - wypts(:,8) = c(:,2); - % wypts(:,9) = d_ft; -end \ No newline at end of file diff --git a/interpTime.m b/interpTime.m deleted file mode 100644 index 76f66cf..0000000 --- a/interpTime.m +++ /dev/null @@ -1,46 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -function [outTime, outX, outY, outZ] = interpTime(step_s,time_s,x,y,z,varargin) -% INTERPTIME interpolates X,Y,Z position coordinates with a new timestep -% SEE ALSO: interp1 - -%% Input parser -p = inputParser; - -% Required -addRequired(p,'step_s',@(x) isnumeric(x) && (x >=1)); % Interpolated timestep, CSIM requires at least >= 1 -addRequired(p,'time_s',@isnumeric); % time -addRequired(p,'x',@isnumeric); % x position -addRequired(p,'y',@isnumeric); % y position -addRequired(p,'z',@isnumeric); % z_position - -% Optional -addOptional(p,'isRoundX',false,@islogical); % If true, round outX -addOptional(p,'isRoundY',false,@islogical); % If true, round outY -addOptional(p,'isRoundZ',false,@islogical); % If true, round outZ - -% Parse -parse(p,step_s,time_s,x,y,z,varargin{:}); - -%% Check for unique rows -[~,ia,~] = unique(time_s); - -%% Interpolate -% Generate new timesteps -tq_s = (min(time_s(ia)):step_s:max(time_s(ia)))'; -outTime = tq_s; - -% Find unique rows, because interp1 requires unique vectors -% Due to helicopters, it is potential that position doesn't update -%[~,ia,~] = unique([x,y,z],'rows','stable'); - -% Interpolate positions -outX = interp1(time_s(ia),x(ia),tq_s); -outY = interp1(time_s(ia),y(ia),tq_s); -outZ = interp1(time_s(ia),z(ia),tq_s); - -%% Round -if p.Results.isRoundX; outX = round(outX); end -if p.Results.isRoundY; outY = round(outY); end -if p.Results.isRoundZ; outZ = round(outZ); end - diff --git a/neu2wpstruct.m b/neu2wpstruct.m deleted file mode 100644 index 68b91f2..0000000 --- a/neu2wpstruct.m +++ /dev/null @@ -1,24 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -function waypoints_csim = neu2wpstruct(ac1NEU,ac2NEU,encTime_s,initHorz_ft,initVert_ft) - -waypoints_csim = []; - -% First check encounter duration -if ac1NEU(end,1) >= encTime_s && ac2NEU(end,1) >= encTime_s - - % Calculate - range_ft = sqrt((ac1NEU(1,2)-ac2NEU(1,2))^2 + (ac1NEU(1,3)-ac2NEU(1,3))^2); - z_ft = abs(ac1NEU(1,4) - ac2NEU(1,4)); - - % Evaluate range and vertical sep. - if initHorz_ft(1) <= range_ft&&... - initHorz_ft(2) >= range_ft&&... - initVert_ft(1) <= z_ft &&... - initVert_ft(2) >= z_ft - - waypoints_csim = neu_to_wp_struct(ac1NEU,ac2NEU); - else - %fprintf('Skipping because initial separation conditions not met when i=%i, j=%i\n',i,j); - end -end \ No newline at end of file diff --git a/neu_to_wp_struct.m b/neu_to_wp_struct.m deleted file mode 100644 index e6828fb..0000000 --- a/neu_to_wp_struct.m +++ /dev/null @@ -1,38 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -function wp = neu_to_wp_struct(ac1,ac2) - % Modify time so it starts from 0 - ac1(:,1) = ac1(:,1) - ac1(1,1); - ac2(:,1) = ac2(:,1) - ac2(1,1); - - % Fill in wp struct for ac1 - for idx = 1:size(ac1,1) - if idx == 1 - wp(1,1).initial = [ac1(idx,2);... - ac1(idx,3);... - ac1(idx,4);... - ]; - else - wp(1,1).update(:,idx-1) = [ac1(idx,1);... - ac1(idx,2);... - ac1(idx,3);... - ac1(idx,4);... - ]; - end - end - % Fill in wp struct for ac2 - for idx = 1:size(ac2,1) - if idx == 1 - wp(2,1).initial = [ac2(idx,2);... - ac2(idx,3);... - ac2(idx,4);... - ]; - else - wp(2,1).update(:,idx-1) = [ac2(idx,1);... - ac2(idx,2);... - ac2(idx,3);... - ac2(idx,4);... - ]; - end - end -end \ No newline at end of file diff --git a/output/README.md b/output/README.md index cb347b5..1a83954 100644 --- a/output/README.md +++ b/output/README.md @@ -8,7 +8,7 @@ DISTRIBUTION STATEMENT A. Approved for public release. Distribution is unlimited This material is based upon work supported by the Federal Aviation Administration under Air Force Contract No. FA8702-15-D-0001. Any opinions, findings, conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Federal Aviation Administration. -© 2019, 2020 Massachusetts Institute of Technology. +© 2019-2021 Massachusetts Institute of Technology. Delivered to the U.S. Government with Unlimited Rights, as defined in DFARS Part 252.227-7013 or 7014 (Feb 2014). Notwithstanding any copyright notice, U.S. Government rights in this work are defined by DFARS 252.227-7013 or DFARS 252.227-7014 as detailed above. Use of this work other than as specifically authorized by the U.S. Government may violate any copyrights that exist in this work. diff --git a/placeTrack.m b/placeTrack.m deleted file mode 100644 index cb61eb2..0000000 --- a/placeTrack.m +++ /dev/null @@ -1,174 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -function [time_s,lat_deg,lon_deg,alt_ft_msl,alt_ft_agl,xNorth_ft,yEast_ft,zDown_ft,isObstacle] = placeTrack(inFile,lat0,lon0,varargin) - -%% Set up input parser -p = inputParser; - -% Required -addRequired(p,'inFile'); -addRequired(p,'lat0',@(x) isnumeric(x) && numel(x) == 1); -addRequired(p,'lon0',@(x) isnumeric(x) && numel(x) == 1); - -% Optional -addOptional(p,'spheroid',wgs84Ellipsoid('ft')); -addOptional(p,'Delimiter',','); -addOptional(p,'z_units','agl',@(x) isstr(x) && any(strcmpi(x,{'agl','msl'}))); -addOptional(p,'z_agl_tol_ft',250,@(x) isnumeric(x) && numel(x) == 1); -addOptional(p,'maxTries',10,@(x) isnumeric(x) && numel(x) == 1); - -% Optional - DEM -addOptional(p,'dem','globe',@ischar); -addOptional(p,'Z_m',{},@isnumeric); -addOptional(p,'refvec',{},@isnumeric); - -% Optional - Obstacles -addOptional(p,'latObstacle',[],@isnumeric); -addOptional(p,'lonObstacle',[],@isnumeric); -addOptional(p,'altObstacle_ft_agl',[],@isnumeric); - -% Optional - inFile parsing -addOptional(p,'labelTime','time_s'); -addOptional(p,'labelX','x_ft'); -addOptional(p,'labelY','y_ft'); -addOptional(p,'labelZ','z_ft'); - -% Optional Plot -addOptional(p,'isPlot',false,@islogical); - -% Parse -parse(p,inFile,lat0,lon0,varargin{:}); - -%% Load trajectory -T = readtable(inFile,'Delimiter',p.Results.Delimiter); - -% Interpolate -[tq_s, x_ft, y_ft, z_ft] = interpTime(1,T.(p.Results.labelTime),T.(p.Results.labelX),T.(p.Results.labelY),T.(p.Results.labelZ)); -time_s = tq_s; -zDown_ft = z_ft; - -% Preallocate, although this will be overwritten -alt_ft_agl = z_ft; - -% Velocity -%v2_kts = interp1(T.time_s,T.groundspeed_kt,tq_s); -%if any(v2_kts == 0); v2_kts(v2_kts == 0) = 1e-3; end - -%% Filter and parse out obstacles -l = p.Results.altObstacle_ft_agl >= min(zDown_ft); -latObstacle = p.Results.latObstacle(:,l); -lonObstacle = p.Results.lonObstacle(:,l); -numObstacle = size(latObstacle,2); - -%% Translate and rotate trajectory so it doesn't hit an obstacle -c = 1; -isObstacle = true; % Set true to enter while loop -while c <= p.Results.maxTries & isObstacle - % Select which index will be (0,0) - % Whatever is (0,0) will pass directly through the anchorpoint - i0 = randi(numel(time_s),1,1); - x_ft = x_ft - x_ft(i0); - y_ft = y_ft - y_ft(i0); - - % Randomly rotate and assign - % https://en.wikipedia.org/wiki/Rotation_matrix - rDeg = randi(360,1,1); - yEast_ft = x_ft * cosd(rDeg) - y_ft * sind(rDeg); - xNorth_ft = x_ft * sind(rDeg) + y_ft * cosd(rDeg); - - % Convert to lat / lon - % h(i0) approx el_ft_msl - z_ft(i0); - h0 = 0; % placeholder, not really used - [lat_deg,lon_deg,~] = ned2geodetic(xNorth_ft,yEast_ft,z_ft,lat0,lon0,h0,p.Results.spheroid); - - % Check to make sure not hitting obstacle - isObstacle = false(numObstacle,1); - for i=1:1:numObstacle - isObstacle(i) = any(InPolygon(lat_deg,lon_deg,latObstacle(:,i),lonObstacle(:,i))); - end - isObstacle = any(isObstacle); - - % There is a horizontal conflict, so we need to check vertical - if isObstacle - isObstacle = any(any(p.Results.altObstacle_ft_agl(isObstacle)' >= zDown_ft)); - end - - % Advance counter - c = c + 1; -end - -if isObstacle - warning('placeTracks:obstacle','After %i tries, Bayes track still hitting obstacles\nFILE = %s\n',p.Results.maxTries,inFile); -end - -%% Convert to MSL if needed and assign -alt_ft_msl = nan(size(lat_deg)); -switch p.Results.z_units - case 'agl' - % Get elevation lat / lon - if ~any(strcmpi(p.UsingDefaults,'Z_m')) - [el_ft_msl,~,~,~] = msl2agl(lat_deg, lon_deg,p.Results.dem,'Z_m',p.Results.Z_m,'refvec',p.Results.refvec); - else - [el_ft_msl,~,~,~] = msl2agl(lat_deg, lon_deg, p.Results.dem); - end - - % msl2agl() may return an empty array if there are too many NaN in - % the dem, if this happens, throw everything out - if ~isempty(el_ft_msl) - % Want to persve the the correct AGL at the anchor point - %[~,I] = min(z_ft); - I = i0; - - % Convert from AGL to MSL - % Bayes uncorrelated & nonconventional model use AGL units in the lowest - % altitude bins, so this calculation perseves the AGL shape - % However Bayes HAA uses MSL, so we're forcing a MSL = AGL assummption for HAA - alt_ft_msl = z_ft + el_ft_msl(I); - - % Determine which points are underground and bad - alt_ft_agl = alt_ft_msl - el_ft_msl; - isGood = alt_ft_agl >=0 & abs(alt_ft_agl-z_ft) <= p.Results.z_agl_tol_ft; - else - isGood = false(size(time_s)); - end - - % Find longest sequence of consecutive non-zero values - % https://www.mathworks.com/matlabcentral/answers/404502-find-the-longest-sequence-of-consecutive-non-zero-values-in-a-vector#answer_323627 - zpos = find(~[0 isGood' 0]); - [~, grpidx] = max(diff(zpos)); - idx = zpos(grpidx):zpos(grpidx+1)-2; - - % Filter using idx - % This produces the longest track above ground - time_s = time_s(idx); - lat_deg = lat_deg(idx); - lon_deg = lon_deg(idx); - alt_ft_msl = alt_ft_msl(idx); - alt_ft_agl = alt_ft_agl(idx); - xNorth_ft = xNorth_ft(idx); - yEast_ft = yEast_ft(idx); - zDown_ft = zDown_ft(idx); - - case 'msl' - % z_ft is already in MSL - alt_ft_agl = z_ft; - alt_ft_msl = z_ft; - isGood = true(size(z_ft)); -end - -%% Plot -if p.Results.isPlot - figure; set(gcf,'name',inFile); - subplot(1,2,1); - geoplot(lat_deg,lon_deg); grid on; - - subplot(1,2,2); - plot(tq_s,z_ft,tq_s(idx),alt_ft_agl,tq_s,el_ft_msl,time_s,alt_ft_msl); grid on; - xlabel('time (s)'); - legend('z_ft','alt_ft_agl','el_ft_msl','alt_ft_msl','Interpreter','none','Location','best'); -end - -%% Make sure time starts at 1 -if any(isGood) && time_s(1) ~=1 - time_s = (time_s - time_s(1))+1; -end diff --git a/samplespeedalt.m b/samplespeedalt.m deleted file mode 100644 index bcdd1f4..0000000 --- a/samplespeedalt.m +++ /dev/null @@ -1,54 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -function [v1_kts,v2_kts,altAdjust1_ft,altAdjust2_ft] = samplespeedalt(p,n,ac1Alt_ft_agl,ac2Alt_ft_agl) - -%% Sample aircraft speed for each cpa combination -% This should eventually be for each timestep, not a fixed value -switch p.Results.acmodel1 - case 'bayes' - v1_kts = nan(n,1); - case 'geospatial' - v1_kts = randi(p.Results.vrange1_kts,n,1); -end -switch p.Results.acmodel2 - case 'bayes' - v2_kts = nan(n,1); - case 'geospatial' - v2_kts = randi(p.Results.vrange1_kts,n,1); -end - -%% Sample altitude adjustment for each cpa combination (geospatial only) -% Get AGL altitude -% Calculate the interval of potential altitude adjustments -% Sample the interval and set altitude adjustments for each combination -if p.Results.isSampleAlt1 - below = p.Results.minAlt1_ft_agl-min(ac1Alt_ft_agl); - above = p.Results.maxAlt1_ft_agl-max(ac1Alt_ft_agl); - if below <= above - adjustSpan_ft_agl = below:25:above; - else - adjustSpan_ft_agl = above:-25:(below + above); % Need to bring aircraft down - end - altAdjust1_ft = adjustSpan_ft_agl(randi(numel(adjustSpan_ft_agl),n,1))'; -else - altAdjust1_ft = zeros(n,1); -end - -if p.Results.isSampleAlt2 - below = p.Results.minAlt2_ft_agl-min(ac2Alt_ft_agl); - above = p.Results.maxAlt2_ft_agl-max(ac2Alt_ft_agl); - if below <= above - adjustSpan_ft_agl = below:25:(below + above); - else - adjustSpan_ft_agl = above:-25:below; % Need to bring aircraft down - end - altAdjust2_ft = adjustSpan_ft_agl(randi(numel(adjustSpan_ft_agl),n,1))'; -else - altAdjust2_ft = zeros(n,1); -end - -%% Make sure outputs are columns -if ~iscolumn(v1_kts); v1_kts = v1_kts'; end; -if ~iscolumn(v2_kts); v2_kts = v2_kts'; end; -if ~iscolumn(altAdjust1_ft); altAdjust1_ft = altAdjust1_ft'; end; -if ~iscolumn(altAdjust2_ft); altAdjust2_ft = altAdjust2_ft'; end; \ No newline at end of file diff --git a/startup.m b/startup_pairing_geo.m similarity index 77% rename from startup.m rename to startup_pairing_geo.m index 85f1cbf..e874a73 100644 --- a/startup.m +++ b/startup_pairing_geo.m @@ -1,9 +1,14 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory +% Copyright 2019 - 2021, MIT Lincoln Laboratory % SPDX-License-Identifier: BSD-2-Clause %% Path +% Self +addpath(getenv('AEM_DIR_GEOPAIR')) +addpath(genpath([getenv('AEM_DIR_GEOPAIR') filesep 'code'])) + % Add utilities, such as msl2agl addpath(genpath([getenv('AEM_DIR_CORE') filesep 'matlab'])) -addpath(genpath('waypoint_format')) + +addpath(genpath([getenv('AEM_DIR_BAYES') filesep 'code' filesep 'matlab'])) % Plotting defaults % https://www.mathworks.com/help/matlab/creating_plots/default-property-values.html @@ -19,4 +24,4 @@ set(groot,'defaultFigureColor','white'); % Figure defaults set(groot,'defaultAxesColorOrder',myColorOrder,'defaultAxesFontsize',12,'defaultAxesFontweight','bold','defaultAxesFontName','Arial'); % Axes defaults set(groot,'defaultLineLineWidth',1.5); % Line defaults -set(groot,'defaultStairLineWidth',1.5); % Stair defaults (used in ecdf) \ No newline at end of file +set(groot,'defaultStairLineWidth',1.5); % Stair defaults (used in ecdf) diff --git a/suas_traj_to_wp.m b/suas_traj_to_wp.m deleted file mode 100644 index 5552183..0000000 --- a/suas_traj_to_wp.m +++ /dev/null @@ -1,660 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -function suas_traj_to_wp(inputDir, fout, varargin) - if ~contains(fout, '.dat') - error('Output should be .dat file'); - end - opts = inputParser; - opts.addParameter('numTrialsStart', 3e4, @isnumeric); - opts.addParameter('numTrialsEnd', 1e4, @isnumeric); - opts.addParameter('minSpeedOwn',25, @isnumeric); % kts; CDF resampled until speed is >= minSpeed - opts.addParameter('minSpeedInt',0, @isnumeric); - opts.addParameter('tcpa',80, @isnumeric); % time of cpa - opts.addParameter('speedDistributionFile','suasJcruiseCDF.mat',@ischar); %.mat file containing sUAS speed distributions - opts.addParameter('runPar',true, @isboolean); - opts.addParameter('minAlt',1200, @isnumeric); - opts.addParameter('speed',60, @isnumeric); %in knots - opts.addParameter('minT', 80, @isnumeric); % minimum time of encounters desired - opts.addParameter('maxT',120, @isnumeric); % maximum time of encounters desired - opts.addParameter('tol',5,@isnumeric) % tolerance in seconds for removing encounters with tca not within tol - opts.parse(varargin{:}); - - numTrialsStart = opts.Results.numTrialsStart; - numTrialsEnd = opts.Results.numTrialsEnd; - minSpeedOwn = opts.Results.minSpeedOwn; - minSpeedInt = opts.Results.minSpeedInt; - tcpa = opts.Results.tcpa; - speedDistributionFile = opts.Results.speedDistributionFile; - if ~exist(speedDistributionFile, 'file') - speedDistributionFile = fullfile(mitcas_paths.mitcas_shared, 'encounter_files','sUAS','source','data_files','suasJcruiseCDF.mat'); - end - runPar = opts.Results.runPar; - minAlt = opts.Results.minAlt; - speed = opts.Results.speed; - minT = opts.Results.minT; - maxT = opts.Results.maxT; - tol = opts.Results.tol; - - temp = strsplit(fout,'.'); - fout = temp{1}; - - %% create encounters - fprintf('Creating encounters... \n'); - tic - createEncounters(inputDir, fout, speed, minT, maxT); - toc - - fout = [fout '_WP']; - %% pairs suas trajectories with each other - fprintf('Pairing sUAS trajectories...\n'); - tic - pair_uas2_wp([fout '.dat'], [fout 'Paired.dat'],numTrialsStart, minSpeedOwn, minSpeedInt, tcpa, speedDistributionFile, runPar); - toc - - %% adjust waypoints - fprintf('Adjusting waypoints... \n'); - tic - adjWP([fout 'Paired.dat'], [fout 'Adj.dat'], tcpa, minAlt); - toc - - %% finalize encounter sets - fprintf('Finalizing encounter sets... \n'); - tic - finWP([fout 'Adj.dat'], [fout 'Final.dat'], numTrialsEnd, tcpa, tol); - toc - - fprintf('Encounter set finished!\n'); - -end - -function createEncounters(dirName, fout, speed_kts, minT, maxT) - % This script takes data from .csv files and creates trajectories - - % Inputs - speed = speed_kts * 1.68781; - R = 3959*5280; % radius of earth, in feet (assuming perfect sphere) - foutWP = [fout '_WP.dat']; - foutS = [fout '_S.dat']; - - d = dir(dirName); - fNum = sum(~[d.isdir]); % number of files in directory - filenames = {}; - for ii = 1:numel(d) - if contains(d(ii).name,'.csv') - filenames = [filenames fullfile(d(ii).folder,d(ii).name)]; - end - end - - numAc = 2; - numEnc = fNum; % set to fNum to use all files - if numel(filenames) ~= numEnc - error('Number of filenames doesn''t match numEnc in createEncounters method'); - end - waypoints = struct('initial', cell(numAc, 1), 'update', cell(numAc, 1)); - index = 1; % array index increments separately from main loop - - for i = 1:numel(filenames) - fname = filenames{i}; -% if exist(fname,'file')~=2 % file doesn't exist -% fprintf('File number %g does not exist.\n', i); -% continue; -% end - % Read raw input data - data = csvread(fname, 1, 0); % skips header row - % Remove climb and descend stages - data = data(2:end-1, :); - % Remove duplicate time steps - locs = find(diff(data(:, 1))==0); % locations of duplicates - locs = locs + 1; % we will remove latter of duplicates - data(locs, :) = []; % remove rows with duplicate values - - % Convert raw data to waypoint script - lla = data(:, [1 4 10 11]); %lat/long/altitude at each time - % Express time&lat/long data relative to initial values, set to 0: - dlla = lla; - dlla(:, 1) = lla(:, 1) - lla(1, 1); - dlla(:, 3) = lla(:, 3) - lla(1, 3); - dlla(:, 4) = lla(:, 4) - lla(1, 4); - - - while 1 % Add any part of trajectory > desired time - % End loop if not enough traj remains - if (dlla(end,1) < minT) - break; - end - % Break up trajectories that are too long - if (dlla(end,1) > maxT) - loc = find(dlla(:,1)>=maxT,1); - else - loc = length(dlla(:,1)); - end - dllaT = dlla(1:loc,:); % temporary dlla - llaT = lla(1:loc,:); % temporary lla - - % Convert lat/long to feet relative to initial position of (0,0) - pos = dllaT; - pos(:, 3) = R*dllaT(:, 3)*pi/180; % north/south distance - pos(:, 4) = R*cosd(.5*(llaT(:,3)+llaT(1,3))).*dllaT(:,4)*pi/180; % east/west distance - - % Scale based on desired speed - d = diff(pos); % difference at each time step - r = sqrt(d(:,3).^2+d(:,4).^2); % total distance traveled - rDes = d(:,1)*speed; % desired total distance traveled - ratio = rDes ./ r; % ratio - dadj = [d(:,1:2) d(:,3).*ratio, d(:,4).*ratio]; % adjusted difference - posAdj = [pos(1,:); pos(1,:)+cumsum(dadj)]; % adjusted position - - % Use data to create waypoints - waypoints(1, index).initial = posAdj(1, [3 4 2])'; - waypoints(1, index).update = posAdj(2:end, [1 3 4 2])'; - - a = posAdj; % makes next few lines shorter - if numAc == 2 % create dummy straight-line intruder - waypoints(2, index).initial = [a(end,3) a(end,4) a(1,2)]'; - waypoints(2, index).update = [a(end,1) a(1,3) a(1,4) a(end,2)]'; - end - - % Increment array index if current file was used - index = index + 1; - - % Remove used portion from trajectory - if loc == length(lla(:,1)) % last point used was end of traj - break - end - lla = lla(loc:end,:); - dlla = dlla(loc:end,:); - dlla(:, 1) = dlla(:, 1) - dlla(1, 1); - dlla(:, 3) = dlla(:, 3) - dlla(1, 3); - dlla(:, 4) = dlla(:, 4) - dlla(1, 4); - - end - - - % % Remove trajectories that are too short - % if (dlla(end,1) < minT) - % fprintf('Skipping encounter %g. Time: %g\n', i,dlla(end,1)); - % continue; % skip trajectories shorter than desired time - % end - % % Truncate trajectories that are too long - % if (dlla(end,1) > maxT) - % loc = find(dlla(:,1)>=maxT,1); - % dlla = dlla(1:loc,:); - % lla = lla(1:loc,:); - % end - % % Convert lat/long to feet relative to initial position of (0,0) - % pos = dlla; - % pos(:, 3) = R*dlla(:, 3)*pi/180; % north/south distance - % pos(:, 4) = R*cosd(.5*(lla(:,3)+lla(1,3))).*dlla(:,4)*pi/180; % east/west distance - % - % % Scale based on desired speed - % d = diff(pos); % difference at each time step - % r = sqrt(d(:,3).^2+d(:,4).^2); % total distance traveled - % rDes = d(:,1)*speed; % desired total distance traveled - % ratio = rDes ./ r; % ratio - % dadj = [d(:,1:2) d(:,3).*ratio, d(:,4).*ratio]; % adjusted difference - % posAdj = [pos(1,:); pos(1,:)+cumsum(dadj)]; % adjusted position - % - % % Use data to create waypoints - % waypoints(1, index).initial = posAdj(1, [3 4 2])'; - % waypoints(1, index).update = posAdj(2:end, [1 3 4 2])'; - % - % a = posAdj; % makes next few lines much shorter - % if numAc == 2 % create dummy straight-line intruder - % waypoints(2, index).initial = [a(end,3) a(end,4) a(1,2)]'; - % waypoints(2, index).update = [a(end,1) a(1,3) a(1,4) a(end,2)]'; - % end - % - % % Increment array index if current file was used - % index = index + 1; - - end - - % Create waypoint and script files - save_waypoints(foutWP, waypoints); -% scripts = waypoint2script(waypoints); -% % ensure every last update has zero accel -% for i = 1:length(scripts) -% tf = scripts(1,i).update(1,end); -% scripts(1,i).update = [scripts(1,i).update [tf+1 0 0 0]']; -% scripts(1,i).update(3,1) = 0; -% end -% save_scripts(foutS, scripts); -end - -function pair_uas2_wp(input, fout, numTrials, minSpeedOwn, minSpeedInt, tcpa, speedDistributionFile, runPar) - % Note that the output file will have both aircraft always starting at the - % same position. I use additional scripts to apply desired miss distances - % and perform other postprocessing. - - % Ths script should support parallel processing if the lines under - % "Initialize parallelization" are uncommented and the for loops are - % changed to parfor loops - % sUAS speed distribution - data = load(speedDistributionFile); % gives 'cdfVals', 'cdfSpeeds' - cdfSpeeds = data.cdfSpeeds; - cdfVals = data.cdfVals; - - % sUAS trajectories - wp = load_waypoints(input); - wp = wp(1,:); - numTraj = length(wp); - - % Initialize - ownInitialAll = cell(numTrials,1); - ownUpdateAll = cell(numTrials,1); - intInitialAll = cell(numTrials,1); - intUpdateAll = cell(numTrials,1); - randInt = randi(numTraj,numTrials,1); - randOwn = randi(numTraj,numTrials,1); - - if runPar - % Initialize parallelization - poolobj= gcp('nocreate'); - if(~isempty(poolobj)) - % delete(gcp('nocreate')); - % parpool(); - else - parpool(); - end - parfor n = 1:numTrials - - % Intruder parameters (randomly selected) - trajInt = randInt(n); - intInitial = wp(trajInt).initial; - intUpdates = wp(trajInt).update; - - % Ownship parameters (random from sUAS trajectories) - trajOwn = randOwn(n); - ownInitial = wp(trajOwn).initial; - ownUpdates = wp(trajOwn).update; - - % Find sUAS speeds (random) - while 1 - sInd = find(rand minSpeedOwn - break - end - end - ownSpeed = ownSpeed * 1.68781; % kts to ft/s - % Intruder speed - while 1 - sInd = find(rand minSpeedInt - break - end - end - intSpeed = intSpeed * 1.68781; % kts to ft/s - - % Scale sUAS x-y coords based on desired speed - pos = [0 ownInitial(1:2)'; ownUpdates([1 2 3],:)']; - d = diff(pos); % difference at each time step - r = sqrt(d(:,2).^2+d(:,3).^2); % total distance traveled - rDes = d(:,1)*ownSpeed; % desired total distance traveled - ratio = rDes ./ r; % ratio - dadj = [d(:,1) d(:,2).*ratio, d(:,3).*ratio]; % adjusted difference - posAdj = [pos(1,:); pos(1,:)+cumsum(dadj)]; % adjusted position - ownInitial = [posAdj(1,2:3)'; ownInitial(3)]; - ownUpdates = [posAdj(2:end,:)'; ownUpdates(4,:)]; - - % Repeat for intruder - pos = [0 intInitial(1:2)'; intUpdates([1 2 3],:)']; - d = diff(pos); % difference at each time step - r = sqrt(d(:,2).^2+d(:,3).^2); % total distance traveled - rDes = d(:,1)*intSpeed; % desired total distance traveled - ratio = rDes ./ r; % ratio - dadj = [d(:,1) d(:,2).*ratio, d(:,3).*ratio]; % adjusted difference - posAdj = [pos(1,:); pos(1,:)+cumsum(dadj)]; % adjusted position - intInitial = [posAdj(1,2:3)'; intInitial(3)]; - intUpdates = [posAdj(2:end,:)'; intUpdates(4,:)]; - - % Randomly sample intruder rotation angle - randAngle = rand*2*pi; - % Rotate intruder points about origin (where all tracks should start) - xOld = intUpdates(3,:); % old x (east) points - yOld = intUpdates(2,:); % old y (north) points - intUpdates(3,:) = xOld.*cos(randAngle) - yOld.*sin(randAngle); - intUpdates(2,:) = yOld.*cos(randAngle) + xOld.*sin(randAngle); - - % Find intruder position at time of cpa - index = find(intUpdates(1,:)==tcpa); - if ~isempty(index) % position update exists at tcpa - intNcpa = intUpdates(2,index); - intEcpa = intUpdates(3,index); - intAcpa = intUpdates(4,index); - else % interpolate to approximate position - totalTraj = [[0; intInitial] intUpdates]; % includes t=0 point - intNcpa = interp1(totalTraj(1,:),totalTraj(2,:),tcpa,'pchip'); - intEcpa = interp1(totalTraj(1,:),totalTraj(3,:),tcpa,'pchip'); - intAcpa = interp1(totalTraj(1,:),totalTraj(4,:),tcpa,'pchip'); - end - - % Find o position at time of cpa - index = find(ownUpdates(1,:)==tcpa); - if ~isempty(index) % position update exists at tcpa - ownNcpa = ownUpdates(2,index); - ownEcpa = ownUpdates(3,index); - ownAcpa = ownUpdates(4,index); - else % interpolate - totalTraj = [[0; ownInitial] ownUpdates]; % includes t=0 point - ownNcpa = interp1(totalTraj(1,:),totalTraj(2,:),tcpa,'pchip'); - ownEcpa = interp1(totalTraj(1,:),totalTraj(3,:),tcpa,'pchip'); - ownAcpa = interp1(totalTraj(1,:),totalTraj(4,:),tcpa,'pchip'); - end - - % Move intruder position and uas alt to line up trajectories at tcpa - intInitial(1) = intInitial(1) + (ownNcpa-intNcpa); - intInitial(2) = intInitial(2) + (ownEcpa-intEcpa); - intUpdates(2,:) = intUpdates(2,:) + (ownNcpa-intNcpa); - intUpdates(3,:) = intUpdates(3,:) + (ownEcpa-intEcpa); - ownInitial(3) = ownInitial(3) + (intAcpa-ownAcpa); - ownUpdates(4,:) = ownUpdates(4,:) + (intAcpa-ownAcpa); - - % Output for waypoint variables - ownInitialAll{n} = ownInitial'; - ownUpdateAll{n} = ownUpdates; - intInitialAll{n} = intInitial'; - intUpdateAll{n} = intUpdates; - - end - else - for n = 1:numTrials - - % Intruder parameters (randomly selected) - trajInt = randInt(n); - intInitial = wp(trajInt).initial; - intUpdates = wp(trajInt).update; - - % Ownship parameters (random from sUAS trajectories) - trajOwn = randOwn(n); - ownInitial = wp(trajOwn).initial; - ownUpdates = wp(trajOwn).update; - - % Find sUAS speeds (random) - while 1 - sInd = find(rand minSpeedOwn - break - end - end - ownSpeed = ownSpeed * 1.68781; % kts to ft/s - % Intruder speed - while 1 - sInd = find(rand minSpeedInt - break - end - end - intSpeed = intSpeed * 1.68781; % kts to ft/s - - % Scale sUAS x-y coords based on desired speed - pos = [0 ownInitial(1:2)'; ownUpdates([1 2 3],:)']; - d = diff(pos); % difference at each time step - r = sqrt(d(:,2).^2+d(:,3).^2); % total distance traveled - rDes = d(:,1)*ownSpeed; % desired total distance traveled - ratio = rDes ./ r; % ratio - dadj = [d(:,1) d(:,2).*ratio, d(:,3).*ratio]; % adjusted difference - posAdj = [pos(1,:); pos(1,:)+cumsum(dadj)]; % adjusted position - ownInitial = [posAdj(1,2:3)'; ownInitial(3)]; - ownUpdates = [posAdj(2:end,:)'; ownUpdates(4,:)]; - - % Repeat for intruder - pos = [0 intInitial(1:2)'; intUpdates([1 2 3],:)']; - d = diff(pos); % difference at each time step - r = sqrt(d(:,2).^2+d(:,3).^2); % total distance traveled - rDes = d(:,1)*intSpeed; % desired total distance traveled - ratio = rDes ./ r; % ratio - dadj = [d(:,1) d(:,2).*ratio, d(:,3).*ratio]; % adjusted difference - posAdj = [pos(1,:); pos(1,:)+cumsum(dadj)]; % adjusted position - intInitial = [posAdj(1,2:3)'; intInitial(3)]; - intUpdates = [posAdj(2:end,:)'; intUpdates(4,:)]; - - % Randomly sample intruder rotation angle - randAngle = rand*2*pi; - % Rotate intruder points about origin (where all tracks should start) - xOld = intUpdates(3,:); % old x (east) points - yOld = intUpdates(2,:); % old y (north) points - intUpdates(3,:) = xOld.*cos(randAngle) - yOld.*sin(randAngle); - intUpdates(2,:) = yOld.*cos(randAngle) + xOld.*sin(randAngle); - - % Find intruder position at time of cpa - index = find(intUpdates(1,:)==tcpa); - if ~isempty(index) % position update exists at tcpa - intNcpa = intUpdates(2,index); - intEcpa = intUpdates(3,index); - intAcpa = intUpdates(4,index); - else % interpolate to approximate position - totalTraj = [[0; intInitial] intUpdates]; % includes t=0 point - intNcpa = interp1(totalTraj(1,:),totalTraj(2,:),tcpa,'pchip'); - intEcpa = interp1(totalTraj(1,:),totalTraj(3,:),tcpa,'pchip'); - intAcpa = interp1(totalTraj(1,:),totalTraj(4,:),tcpa,'pchip'); - end - - % Find o position at time of cpa - index = find(ownUpdates(1,:)==tcpa); - if ~isempty(index) % position update exists at tcpa - ownNcpa = ownUpdates(2,index); - ownEcpa = ownUpdates(3,index); - ownAcpa = ownUpdates(4,index); - else % interpolate - totalTraj = [[0; ownInitial] ownUpdates]; % includes t=0 point - ownNcpa = interp1(totalTraj(1,:),totalTraj(2,:),tcpa,'pchip'); - ownEcpa = interp1(totalTraj(1,:),totalTraj(3,:),tcpa,'pchip'); - ownAcpa = interp1(totalTraj(1,:),totalTraj(4,:),tcpa,'pchip'); - end - - % Move intruder position and uas alt to line up trajectories at tcpa - intInitial(1) = intInitial(1) + (ownNcpa-intNcpa); - intInitial(2) = intInitial(2) + (ownEcpa-intEcpa); - intUpdates(2,:) = intUpdates(2,:) + (ownNcpa-intNcpa); - intUpdates(3,:) = intUpdates(3,:) + (ownEcpa-intEcpa); - ownInitial(3) = ownInitial(3) + (intAcpa-ownAcpa); - ownUpdates(4,:) = ownUpdates(4,:) + (intAcpa-ownAcpa); - - % Output for waypoint variables - ownInitialAll{n} = ownInitial'; - ownUpdateAll{n} = ownUpdates; - intInitialAll{n} = intInitial'; - intUpdateAll{n} = intUpdates; - - end - end - - waypoints = struct('initial', cell(2, numTrials), 'update', cell(2, numTrials)); - for n = 1:numTrials - - waypoints(1,n).initial = ownInitialAll{n}; - waypoints(1,n).update = ownUpdateAll{n}; - waypoints(2,n).initial = intInitialAll{n}; - waypoints(2,n).update = intUpdateAll{n}; - - end - - % Save results - fprintf('Simulation complete. Beginning save.\n'); - save_waypoints(fout, waypoints); - fprintf('Save complete\n'); -end - -function adjWP(input, fout, tcpa, minAlt) -% The reason this is done is because, with scripted encounters, it is hard to know aircraft position in the middle of the encounter without actually simulating -% This script makes the following edits: -% Trajectories are moved to have desired miss distances at cpa -% Vertical distance is sampled from a Gaussian(0,100ft) -% Horizontal distance is sampled uniformly from +/-5000 ft -% Trajectories are raised to at least 'minAlt' altitude -% This ensures TCAS and ACAS X will always issue RAs -% Trajectories that start extremely close are thrown out - % Metrics to analyze - m = cell(1,8); - names = cell(1,8); - m{1} = fun.x('x.0',tcpa); - names{1} = ['x_x_0_' num2str(tcpa)]; - m{2} = fun.x('y.0',tcpa); - names{2} = ['x_y_0_' num2str(tcpa)]; - m{3} = fun.x('h.0',tcpa); - names{3} = ['x_h_0_' num2str(tcpa)]; - m{4} = fun.x('x.0',0); - names{4} = 'x_x_0_0'; - m{5} = fun.x('y.0',0); - names{5} = 'x_y_0_0'; - m{6} = fun.x('h.0',0); - names{6} = 'x_h_0_0'; - m{7} = fun.x('x.1',tcpa); - names{7} = ['x_x_1_' num2str(tcpa)]; - m{8} = fun.x('y.1',tcpa); - names{8} = ['x_y_1_' num2str(tcpa)]; - m{9} = fun.x('h.1'); - names{9} = 'x_h_1'; - m{10} = fun.x('dx.1',tcpa); - names{10} = ['x_dx_1_' num2str(tcpa)]; - m{11} = fun.x('dy.1',tcpa); - names{11} = ['x_dy_1_' num2str(tcpa)]; - - c = csim('ss'); - c.encounter = encounter.waypoint_horizontal(input); - c.encounter.max_steps = 120; - - oldWaypoints = load_waypoints(input); - fprintf('Loaded waypoints.\n'); - Own = oldWaypoints(1,:); - Int = oldWaypoints(2,:); - numEnc = length(Own); - edits = false(numEnc,1); - %initX = zeros(numEnc,1); - %initY = zeros(numEnc,1); - %initH = zeros(numEnc,1); - - % Offsets - %horOffset = normrnd(0,0,numEnc,1); - horOffset = rand(numEnc,1)*5000 - 2500; % unif(-5000, 5000) - altOffset = normrnd(0,100,numEnc,1); - - % old initial position - %for i = 1:numEnc - % initX(i) = Int(i).initial(3); - % initY(i) = Int(i).initial(2); - % initH(i) = Int(i).initial(4); - %end - - % Simulate - S = c.eval('all', m); - - % Use results to edit encounters - for n = 1:numEnc - % Old waypoint values - ownI = Own(n).initial; - ownU = Own(n).update; - intI = Int(n).initial; - intU = Int(n).update; - % Find necessary positions - x0 = S.(names{1})(n); - y0 = S.(names{2})(n); - h0 = S.(names{3})(n); - xi0 = S.(names{4})(n); - yi0 = S.(names{5})(n); - hi0 = S.(names{6})(n); - x1 = S.(names{7})(n); - y1 = S.(names{8})(n); - h1 = S.(names{9})(tcpa,n); - hmin = min(S.(names{9})(:,n)); - dx1 = S.(names{10})(n); - dy1 = S.(names{11})(n); - - % Adjust (horizontal offset applied tangential to int flight path at cpa) - ang = atan(dy1/dx1); - %initY(n) = initY(n) + (y0-y1) + horOffset(n)*cos(ang); - %initX(n) = initX(n) + (x0-x1) - horOffset(n)*sin(ang); - %initH(n) = initH(n) + (h0-h1) + altOffset(n); - intI(1) = intI(1) + (y0-y1) + horOffset(n)*cos(ang); - intI(2) = intI(2) + (x0-x1) - horOffset(n)*sin(ang); - intU(2,:) = intU(2,:) + (y0-y1) + horOffset(n)*cos(ang); - intU(3,:) = intU(3,:) + (x0-x1) - horOffset(n)*sin(ang); - hmin = hmin + (h0-h1) + altOffset(n); - if hmin < minAlt % altitude of both aircraft needs to be raised - %initH(n) = initH(n) + minAlt - hmin; - %Own(n).initial(4) = Own(n).initial(4) + minAlt - hmin; - intI(3) = intI(3) + minAlt - hmin; - intU(4,:) = intU(4,:) + minAlt - hmin; - ownI(3) = ownI(3) + minAlt - hmin; - ownU(4,:) = ownU(4,:) + minAlt - hmin; - end - % Ensure aircraft do not start too close - hi1 = intI(3); % new intruder starting height - yi1 = intI(1); % new intruder starting position - xi1 = intI(2); - hi0 = ownI(3); % new ownship starting height - %minRange = 6070; - %minVert = 250; - minSlant = 5000; - %initRange = sqrt((xi0-xi1)^2+(yi0-yi1)^2); - initSlant = sqrt((xi0-xi1)^2+(yi0-yi1)^2+(hi0-hi1)^2); - %if initRange < minRange && abs(hi0-hi1) < minVert - % edits(n) = true; % this will cause encounter to be removed - %end - if initSlant < minSlant - edits(n) = true; - end - - % Recreate waypoints - Own(n).initial = ownI; - Own(n).update = ownU; - Int(n).initial = intI; - Int(n).update = intU; - - end - - - % Recreate intruder script - %for i = 1:numEnc - % Int(i).initial(3) = initX(i); - % Int(i).initial(2) = initY(i); - % Int(i).initial(4) = initH(i); - %end - - % Save Results - waypoints = [Own; Int]; - waypoints = waypoints(:, ~edits); - save_waypoints(fout, waypoints); - fprintf('Save complete.\n'); -end - -function finWP(input, fout, numTrials, tcpa, tol) - steps = 120; % time steps - - c = csim('ss'); - c.encounter = encounter.waypoint_horizontal(input); - c.encounter.max_steps = steps; - - t = c.eval('all', fun.tcaMS()); - t = t.tcaMS; - - index = (ttcpa+tol); - - waypoints = load_waypoints(input); - waypoints = waypoints(:, ~index); - numEnc = length(waypoints); - if numTrials < numEnc % we can't save more encounters than we have - waypoints = waypoints(:, 1:numTrials); - end - save_waypoints(fout, waypoints); -end diff --git a/updatewaypointstruct.m b/updatewaypointstruct.m deleted file mode 100644 index 0369a34..0000000 --- a/updatewaypointstruct.m +++ /dev/null @@ -1,79 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -function [waypoints_struct,encCount,encAdd] = updatewaypointstruct(p,waypoints_struct,encCount,ac1NEU_foward,ac2NEU_foward,ac1NEU_backward,ac2NEU_backward) - -encAdd = false; - -% Adjust timesteps to start at zero -if ~isempty(ac1NEU_foward) - ac1NEU_foward(:,1) = abs(ac1NEU_foward(:,1) - ac1NEU_foward(1,1)); -end -if ~isempty(ac2NEU_foward) - ac2NEU_foward(:,1) = abs(ac2NEU_foward(:,1) - ac2NEU_foward(1,1)); -end -if ~isempty(ac1NEU_backward) - ac1NEU_backward(:,1) = abs(ac1NEU_backward(:,1) - ac1NEU_backward(1,1)); -end -if ~isempty(ac2NEU_backward) - ac2NEU_backward(:,1) = abs(ac2NEU_backward(:,1) - ac2NEU_backward(1,1)); -end - -% CSIM requires an update rate of >= 1 second -% Luis: The spacing has to be at least 1 second or greater. -% I know from experience that if you sample at larger time steps -% the trajectories start looking very different, -% but I have found time spacing between 1-3 seconds -% usually does a pretty good job of capturing what you want -[outTime, outX, outY, outZ] = interpTime(p.Results.timeStep_s,ac1NEU_foward(:,1),ac1NEU_foward(:,2),ac1NEU_foward(:,3),ac1NEU_foward(:,4),'isRoundZ',true); ac1NEU_foward = [outTime,outX,outY,outZ]; -if ~strcmp(p.Results.acmodel1,'bayes') - [outTime, outX, outY, outZ] = interpTime(p.Results.timeStep_s,ac1NEU_backward(:,1),ac1NEU_backward(:,2),ac1NEU_backward(:,3),ac1NEU_backward(:,4),'isRoundZ',true); ac1NEU_backward = [outTime,outX,outY,outZ]; -end - -[outTime, outX, outY, outZ] = interpTime(p.Results.timeStep_s,ac2NEU_foward(:,1),ac2NEU_foward(:,2),ac2NEU_foward(:,3),ac2NEU_foward(:,4),'isRoundZ',true); ac2NEU_foward = [outTime,outX,outY,outZ]; -if ~strcmp(p.Results.acmodel2,'bayes') - [outTime, outX, outY, outZ] = interpTime(p.Results.timeStep_s,ac2NEU_backward(:,1),ac2NEU_backward(:,2),ac2NEU_backward(:,3),ac2NEU_backward(:,4),'isRoundZ',true); ac2NEU_backward = [outTime,outX,outY,outZ]; -end - -% WAYPOINTS STRUCTURE: -% The waypoints structure is an m x n structure matrix, where m is the -% number of aircraft and n is the number of encounters. This structure -% matrix has two fields: initial and update. The initial field is a 3 -% element array specifying the north, east, and altitude. The update -% field is a 4 x n matrix, where n is the number of updates. The rows -% correspond to the time, north, east, and altitude position of the -% waypoints. - -%%%%%%%%%%%%%%%%%%% COMBO 1 -% AC1 forward / AC2 forward -waypoints_ff = neu2wpstruct(ac1NEU_foward,ac2NEU_foward,p.Results.encTime_s,p.Results.initHorz_ft,p.Results.initVert_ft); -if ~isempty(waypoints_ff) - waypoints_struct = [waypoints_struct waypoints_ff]; - encCount = encCount + 1; encAdd = true; -end - -%%%%%%%%%%%%%%%%%%% COMBO 2 -% AC1 backward / AC2 foward -waypoints_bf = neu2wpstruct(ac1NEU_backward,ac2NEU_foward,p.Results.encTime_s,p.Results.initHorz_ft,p.Results.initVert_ft); -if ~isempty(waypoints_bf) - waypoints_struct = [waypoints_struct waypoints_bf]; - encCount = encCount + 1; encAdd = true; -end - -% Bayes trajectories can only go foward due to sampling -if ~strcmp(p.Results.acmodel2,'bayes') - %%%%%%%%%%%%%%%%%%% COMBO 3 - % AC1 foward / AC2 backwards - waypoints_fb = neu2wpstruct(ac1NEU_foward,ac2NEU_backward,p.Results.encTime_s,p.Results.initHorz_ft,p.Results.initVert_ft); - if ~isempty(waypoints_fb) - waypoints_struct = [waypoints_struct waypoints_fb]; - encCount = encCount + 1; encAdd = true; - end - - %%%%%%%%%%%%%%%%%%% COMBO 4 - % AC1 backwards / AC2 backwards - waypoints_bb = neu2wpstruct(ac1NEU_backward,ac2NEU_backward,p.Results.encTime_s,p.Results.initHorz_ft,p.Results.initVert_ft); - if ~isempty(waypoints_bb) - waypoints_struct = [waypoints_struct waypoints_bb]; - encCount = encCount + 1; encAdd = true; - end -end \ No newline at end of file diff --git a/waypoint_format/save_encounters.m b/waypoint_format/save_encounters.m deleted file mode 100644 index ff363bf..0000000 --- a/waypoint_format/save_encounters.m +++ /dev/null @@ -1,98 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -function save_encounters(filename, encounters, varargin) -% function save_encounters(filename, encounters, varargin) -% -% This function is intended as a helper to save_scripts and save_waypoints -% -% Inputs: -% filename: name of output file -% -% encounters: structure of encounters to save to file specified by -% "filename". For a guide to the format of this structure, see -% the help documentation in load_scripts.m. -% -% varargin: parameter/value pairs -% -% + parameter: numupdatetype -% -% value (character string): data type of "num_update" -% variable: the number of updates scripted for some aircraft -% in some encounter (default is 'uint8') -% -% + parameter: append -% -% value (logical): set to true to append encounters to the -% end of the file specified by "filename". Set to false -% (default) to write a new file with the name specified by -% "filename". -% -% + parameter: floattype -% -% value (character string): data type of encounter initial -% conditions and updates (default is 'double') - -%% Handle varargin input -opts = inputParser; -opts.addParamValue('numupdatetype', 'uint8', @ischar); -opts.addParamValue('append', false, @islogical); -opts.addParamValue('floattype', 'double', @ischar); -opts.parse(varargin{:}); - -floattype = opts.Results.floattype; -numupdatetype = opts.Results.numupdatetype; - -%% Append encounters to existing scripts file -if opts.Results.append - fid = fopen(filename, 'r+'); - num_encounters = size(encounters, 2); - num_ac = size(encounters, 1); - - % Error in opening file - if fid == -1 - fid = fopen(filename, 'w'); - fwrite(fid, num_encounters, 'uint32'); - fwrite(fid, num_ac, 'uint32'); - else %no error in opening file - % Go to beginning of file - frewind(fid); - file_num_encounters = fread(fid, 1, 'uint32'); - if isempty(file_num_encounters) - fid = fopen(filename, 'w'); - fwrite(fid, num_encounters, 'uint32'); - fwrite(fid, num_ac, 'uint32'); - else - file_num_ac = fread(fid, 1, 'uint32'); - if file_num_ac ~= num_ac - error('Must have the same number of aircraft when appending encounter files') - end - fseek(fid, 0, 'bof'); - totalencounters = num_encounters + file_num_encounters; - fwrite(fid, totalencounters, 'uint32'); - fseek(fid, 0, 'eof'); - end - end -else % Overwrite existing file or create new one - fid = fopen(filename, 'w'); - num_encounters = size(encounters, 2); - num_ac = size(encounters, 1); - fwrite(fid, num_encounters, 'uint32'); - fwrite(fid, num_ac, 'uint32'); -end - -%% Save encounters -for i = 1:num_encounters - for j = 1:num_ac - fwrite(fid, encounters(j, i).initial, floattype); - end - for j = 1:num_ac - num_update = size(encounters(j, i).update, 2); - if ~isequal(num_update, cast(num_update, numupdatetype)) - error('Number of updates is greater than numupdatetype allows: encounter %0.0f, aircraft %0.0f (see JIRA ticket ACASXCSIM-196)', i, j); - else - fwrite(fid, num_update, numupdatetype); - end - fwrite(fid, encounters(j, i).update, floattype); - end -end -fclose(fid); diff --git a/waypoint_format/save_waypoints.m b/waypoint_format/save_waypoints.m deleted file mode 100644 index a9c0b36..0000000 --- a/waypoint_format/save_waypoints.m +++ /dev/null @@ -1,60 +0,0 @@ -% Copyright 2019 - 2020, MIT Lincoln Laboratory -% SPDX-License-Identifier: BSD-2-Clause -%SAVE_WAYPOINTS Save waypoints to a file. -% SAVE_WAYPOINTS(FILENAME, STRUCTURE) saves a structure holding the -% waypoints to the specified file. -% -% WAYPOINTS FILE: -% The waypoints file contains a set of encounters. Each encounter is -% defined by a set of waypoints associated with a fixed number of -% aircraft. The waypoints are positions in space according to a fixed, -% global coordinate system. All distances are in feet. Time is specified -% in seconds since the beginning of the encounter. The file is organized -% as follows: -% -% [Header] -% uint32 (number of encounters) -% uint32 (number of aircraft) -% [Encounter 1] -% [Initial positions] -% [Aircraft 1] -% double (north position in feet) -% double (east position in feet) -% double (altitude in feet) -% ... -% [Aircraft n] -% double (north position in feet) -% double (east position in feet) -% double (altitude in feet) -% [Updates] -% [Aircraft 1] -% uint16 (number of updates) -% [Update 1] -% double (time in seconds) -% double (north position in feet) -% double (east position in feet) -% double (altitude in feet) -% ... -% [Update m] -% double (time in seconds) -% double (north position in feet) -% double (east position in feet) -% double (altitude in feet) -% ... -% [Aircraft n] -% ... -% ... -% [Encounter k] -% ... -% -% WAYPOINTS STRUCTURE: -% The waypoints structure is an m x n structure matrix, where m is the -% number of aircraft and n is the number of encounters. This structure -% matrix has two fields: initial and update. The initial field is a 3 -% element array specifying the north, east, and altitude. The update -% field is a 4 x n matrix, where n is the number of updates. The rows -% correspond to the time, north, east, and altitude position of the -% waypoints. - -function save_waypoints(filename, waypoints, varargin) -save_encounters(filename, waypoints, 'numupdatetype', 'uint16', varargin{:}); \ No newline at end of file