Skip to content

Commit 516fc96

Browse files
t-shermt-sherm
committed
Add matlab analysis files
1 parent b6d4a78 commit 516fc96

File tree

4 files changed

+487
-0
lines changed

4 files changed

+487
-0
lines changed

‎MATLAB Analysis Code/CA_Avoid_new.m

+129
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,129 @@
1+
function [ lat, lon ] = CA_Avoid( aircraftA, aircraftB, timeToCollision)
2+
% Version of collision avoidance function to test and debug
3+
%
4+
%
5+
% INPUT:
6+
% aircraftA/B: struct
7+
% 1. lat: latitude of current poition
8+
% 2. lon: longitude of current poition
9+
% 3. safetyBubble: radius of safety bubble at predicted collision
10+
% point
11+
%
12+
13+
%aircraftInfo struct (code uses this)
14+
15+
% aircraftInfo() {
16+
% //Initializes all values to zero
17+
% //Descriptions are in the aircraftInfo struct
18+
% lat [3] = {0};
19+
% lon [3] = {0};
20+
% alt [3] = {0};
21+
% velocityX [2] = {0};
22+
% velocityY [2] = {0};
23+
% xAcc = 0;
24+
% yAcc = 0;
25+
% futureDistx [3] = {0};
26+
% futureDisty [3] = {0};
27+
% Hdg = 0;
28+
% safetyBubble = 10; //Roughly 60 ft
29+
% priority = 0;
30+
31+
32+
avoidBufferSize = 100; % From autopilot_interface.h
33+
% makeCircle = 0;
34+
% headOnCollision = 0;
35+
% approachingFromRight = 0;
36+
% % mavlink_mission_item_t avoidWaypoint;
37+
% turnRight = false;
38+
39+
% //----------------------------------------------------
40+
% //Store mission waypoints inside a vector
41+
% //----------------------------------------------------
42+
%
43+
% Request_Waypoints();
44+
45+
% //-------------------------------------------------------
46+
% //Calculate avoid waypoint
47+
% //-------------------------------------------------------
48+
49+
50+
%
51+
% (our airplane)
52+
%
53+
% A-----------------B (other airplane)
54+
% | /
55+
% | /
56+
% | /
57+
% | ^ /
58+
% | / /
59+
% | K /
60+
% | / /
61+
% | v /
62+
% | /
63+
% | /
64+
% | /
65+
% | /
66+
% |/
67+
% C (Avoid waypoint)
68+
69+
RADIUS_EARTH = 6371e3; % meters
70+
71+
latA = aircraftA(1);
72+
lonA = aircraftA(2);
73+
buffA = aircraftA(3);
74+
75+
latB = aircraftB(1);
76+
lonB = aircraftB(2);
77+
buffB = aircraftB(3);
78+
79+
%//This will allow the distance of the new waypoint to be smaller or larger depending on the timeToCollision
80+
AVOID_BUFFER = avoidBufferSize + (buffB / timeToCollision);
81+
82+
if (timeToCollision < 1)
83+
AVOID_BUFFER = avoidBufferSize;
84+
end
85+
86+
% Length k = distance needed between avoidWaypoint and B (meters)
87+
k = buffB + AVOID_BUFFER;
88+
% Angular distance
89+
Ad = k/RADIUS_EARTH;
90+
%addToFile(convertToString(k), "k: distance between waypoint and B");
91+
92+
93+
94+
% Guard against division by zero when finding slopes
95+
%!!!!!Increment value(.00000001) IS YET TO BE DETERMINED!!!!!
96+
if (lonB - lonA == 0)
97+
lonB = lonB + .00000001;
98+
end
99+
if (latB - latA == 0)
100+
latB = latB + .00000001;
101+
end
102+
103+
% Bearing from A to B: from https://www.igismap.com/formula-to-find-bearing-or-heading-angle-between-two-points-latitude-longitude/
104+
dLon = lonB - lonA;
105+
dLat = latB - latA;
106+
x = cosd(latB) * sind(dLon);
107+
y = cosd(latA) * sind(latB) - sind(latA) * cosd(latB) * cosd(dLon);
108+
109+
110+
% Special cases in dealing with NED frame
111+
bearingAB = atan2d(y,x);
112+
if and(dLon < 0, dLat > 0) || and(dLon > 0, dLat < 0)
113+
bearingAB = bearingAB + 180;
114+
end
115+
116+
% Slope of line BC: right now it is simply a 80 degree turn
117+
bearingBC = bearingAB + 80;
118+
119+
% displacement in meters of new waypoint in x direction (North/South)
120+
lat = asind(sind(latA) * cos(Ad) + cosd(latA)* sin(Ad) * cosd(bearingBC));
121+
% Longitude displacement
122+
lon_disp = atan2d( sind(bearingBC) * sin(Ad) * cosd(latA), cos(Ad) - sind(latA) * sind(latB));
123+
lon = lonA + lon_disp;
124+
125+
126+
% insert_waypoint( avoidWaypoint, currentWaypoint);
127+
% [ lat , lon ] = NewAvoidWaypoint(xDisplacement, yDisplacement, [aircraftA(1),aircraftB(2)]);
128+
129+
end

‎MATLAB Analysis Code/CA_analysis.m

+68
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
% NGCP 2018-2019
2+
% Code to analyze collision avoidance log output
3+
% Created by Tristan Sherman
4+
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~
5+
6+
7+
% !!NOTE!! to use wmmarker the MATLAB mapping toolbox must be installed
8+
9+
10+
11+
12+
%Clear variables
13+
% if exist('log','var')
14+
% clearvars -except log
15+
% else
16+
clear
17+
%end
18+
clc
19+
wmclose %Close map
20+
21+
% Pick the file name to analyze
22+
filename = 'logging_file_Test2.txt';
23+
[log, collisionPoints] = parseCAlog(filename); %Parse log file into variable
24+
obstacleLat = 34.059249;
25+
obstacleLon = -117.820915;
26+
27+
28+
29+
30+
% Choose which timestep you want to display
31+
position = 136;
32+
position = (position * 2) - 1; % Column of desired position
33+
34+
35+
36+
37+
% Store the three GPS positions provided by the GPS
38+
lastPoint= [log(1,position), log(1,position+1)];
39+
secondPoint = [log(2,position), log(2,position+1)];
40+
currentPoint = [log(3,position), log(3,position+1)];
41+
42+
43+
44+
45+
% for p = 65:length(log(1,:))
46+
% wmmarker(log(3,(p*2)-1),log(3,p*2));
47+
% end
48+
49+
50+
% The current lat, lon will be displayed as (0,0) in the plot
51+
x(1) = 0;
52+
y(1) = 0;
53+
predictPointPosition = 5; % This is the row of the first predicted point
54+
endPosition = 24; % The row of the final predicted point in log
55+
56+
for i = predictPointPosition:endPosition-10
57+
58+
if log(i,position) ~=0
59+
nextPoint = [log(i,position), log(i,position+1)];
60+
wmmarker(log(i,position),log(i,position+1)); %Display point on web map
61+
end
62+
[dx,dy] = GPStoCartesian(currentPoint,nextPoint);
63+
x(i) = dx;
64+
y(i) = dy;
65+
end
66+
67+
plot(x,y,'-o')
68+
arrow([0,0],[log(4,position),log(4,position+1)], 'Length', 5) % This was a function downloaded from an online source

‎MATLAB Analysis Code/GPStoCartesian.m

+68
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
function [dx,dy] = GPStoCartesian(currentPos, nextPos)
2+
% NGCP 2018-2019
3+
% Tristan Sherman
4+
% Find x,y distance between gps coordinates and output them in cartesian
5+
% frame
6+
7+
% INPUT: CURRENT POSITION - [lat, lon]
8+
% NEXT POS - [lat, lon]
9+
10+
% Note: Input is in NED frame meaning North = +x and East = +y
11+
% Output: distance in feet
12+
13+
14+
15+
% conversion to radians
16+
TO_RADIANS = pi/180;
17+
TO_FT = 1000 * 3.2808399;
18+
RADIUS_EARTH = 6378.037; % km
19+
20+
currentLat = currentPos(1);
21+
currentLon = currentPos(2);
22+
nextLat = nextPos(1);
23+
nextLon = nextPos(2);
24+
25+
% %Convert to radians
26+
latRad = currentLat * TO_RADIANS;
27+
lonRad = currentLon * TO_RADIANS;
28+
29+
%Find delta in positions
30+
deltaLat = (nextLat - currentLat) * TO_RADIANS;
31+
deltaLon = (nextLon - currentLon) * TO_RADIANS;
32+
33+
% Math from: http://www.movable-type.co.uk/scripts/latlong.html
34+
%? is latitude, ? is longitude, R is earth’s radius (mean radius = 6,371km);
35+
% note that angles need to be in radians to pass to trig functions!
36+
% var R = 6371e3; // metres
37+
% var ?1 = lat1.toRadians();
38+
% var ?2 = lat2.toRadians();
39+
% var ?? = (lat2-lat1).toRadians();
40+
% var ?? = (lon2-lon1).toRadians();
41+
% var a = Math.sin(??/2) * Math.sin(??/2) +
42+
% Math.cos(?1) * Math.cos(?2) *
43+
% Math.sin(??/2) * Math.sin(??/2);
44+
% var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
45+
46+
% Using the above math to calculate dlon, we assume dlat = 0. This
47+
% simplifies the equation
48+
% Calculate x cartesian distance (longitude distance): no longer NED frame
49+
50+
varDlon = (cos(latRad))^2 * (sin(deltaLon/2))^2;
51+
dx = 2 * atan2(sqrt(varDlon),sqrt(1-varDlon)) * RADIUS_EARTH * TO_FT;
52+
53+
if deltaLon < 0
54+
dx = -dx;
55+
end
56+
57+
%Calculate dlat. Assume dlon = 0
58+
varDlat = sin(deltaLat/2)^2;
59+
dy = 2 * atan2(sqrt(varDlat),sqrt(1-varDlat)) * RADIUS_EARTH * TO_FT;
60+
61+
if deltaLat < 0
62+
dy = -dy;
63+
end
64+
%
65+
% c = 2 * atan2(sqrt(var), sqrt(1 - var));
66+
% dx = deltaX * TO_FT;
67+
% dy = deltaY * TO_FT;
68+
end

0 commit comments

Comments
 (0)