Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
function [positions, clockError] = SingleSource(pData, pSoundsourcePositions, pCombinationDetails, pStartEndPosition)
%SINGLESOURCE Estimates the trajectory by using only one soundsource.
%
% Parameters:
% pData (struct):
% Data structured by soundsource. The toa is already corrected
% (e.g. doppler correction etc.)
% pSoundsourcePositions (struct):
% The soundsource positions [deg].
% pCombinationDetails (table):
% Contains all information of the current soundsource
% combination.
% pStartEndPosition (double, logical):
% Set to false if it has not been passed by artoa. Contains the
% desired start and end position as a matrix of row vectors.
%
%
% Returns:
% positions (double):
% The calculated list of segment positions as a X x 2 column vector.
% clockError (double):
% The estimated clock error.
%% Check input
referencePositionStart = cellfun(@str2double, strsplit(pCombinationDetails.referencePosition{1}));
referencePositionEnd = cellfun(@str2double, strsplit(pCombinationDetails.referencePositionEnd{1}));
if any(isnan(referencePositionStart)) || any(isnan(referencePositionEnd))
error([mfilename ': SingleSource algorithm requires BOTH (Reference) Positions to be set!']);
end
%% Get required data
soundsourceName = strsplit(pCombinationDetails.soundsources{1});
soundsourceName = soundsourceName{1};
%% Prepare data for calculations
% get a date vector (does not matter which one)
dates = cell2mat(pData.(soundsourceName).date);
distances = cell2mat(pData.(soundsourceName).distance);
soundsourcePosition = pSoundsourcePositions.(soundsourceName);
% initialize return variables
positions = [ ...
referencePositionStart; ...
];
endPosition = str2num(pCombinationDetails.referencePositionEnd{:});
clockError = [];
%% Algorithm
dateLength = length(dates); % number of points to calculate trajectory for
timeStep = diff(dates(1:2))*86400; % time between position fixes %!!! needs genearlization accorind to float's RAFOS schedule
averageHorizontalVelocity = (distance(str2num(pCombinationDetails.referencePositionEnd{:}),...
str2num(pCombinationDetails.referencePosition{:}))*60*1852)...
/ ((pCombinationDetails.combinationEnd-pCombinationDetails.combinationBegin)*86400) % in meter per second
j = 0
figure(10)
for HorizontalVelocity = averageHorizontalVelocity:.05: 5*averageHorizontalVelocity
HorizontalVelocity
j = j+1
%clear positions
posi(j,:,1) = NaN*ones(1,dateLength,1);
posi(j,:,1) = NaN*ones(1,dateLength,1);
for i = 1:dateLength
currentDistance = distances(i); % get current distance [km] according to current toa
[lat1,lon1] = scircle1( ...
soundsourcePosition(1), ...
soundsourcePosition(2), ...
km2deg(currentDistance)); % calculate circle around soundsource
% plot(lon1,lat1,'r'); hold on % calculate circle around last float position
if i == 1
lastFloatPosition(1) = referencePositionStart(1);
lastFloatPosition(2) = referencePositionStart(2);
[dummy,oldbearing] = distance(referencePositionStart(1),referencePositionStart(2),referencePositionEnd(1),referencePositionEnd(2));
else
%continue
end
[lat2,lon2] = scircle1( ...
lastFloatPosition(1), ...
lastFloatPosition(2), ...
km2deg(HorizontalVelocity*timeStep/1000));
% plot(lon2,lat2,'b'); hold on; grid on
[lat,lon] = scxsc(soundsourcePosition(1),soundsourcePosition(2),km2deg(currentDistance),...
lastFloatPosition(1), lastFloatPosition(2),km2deg(HorizontalVelocity*timeStep/1000) );
%% so we have usually 2 solution, which one to select? --> the one closer towards the final point
% [helper] = distance(lat,lon,endPosition(1),endPosition(2)); %%%! besser: der punkt der sich auf der Verbindungsachse zwischen A und B in Richtung B bewegt
[helper,newbearing] = distance( lastFloatPosition(1), lastFloatPosition(2),lat,lon); %%%! besser: der punkt der sich auf der Verbindungsachse zwischen A und B in Richtung B bewegt
if ~isnan(helper(1)) % intersection found
% helperindi = find(helper == min(helper));
helper = abs(newbearing - oldbearing);
helperindi = find(helper == min(helper));
oldbearing = newbearing(helperindi);
lastFloatPosition(1) = lat(helperindi);
lastFloatPosition(2) = lon(helperindi);
posi(j,i,1) = lastFloatPosition(1);
posi(j,i,2) = lastFloatPosition(2);
plot(lastFloatPosition(2),lastFloatPosition(1),'xk')
plot(posi(j,:,2),posi(j,:,1),'-b'); hold on
else isnan(helper(1))
if size(posi,1) == 1
plot(posi(:,2),posi(:,1),'or'); hold on
else
plot(posi(j,:,2),posi(j,:,1),'-r'); hold on
end
% averageHorizontalVelocity = averageHorizontalVelocity + .01*averageHorizontalVelocity
break
end
end
drawnow
end
validdata = find(~isnan(posi(:,end,1)))
helper = distance(referencePositionEnd(1),referencePositionEnd(2),posi(validdata,end,1),posi(validdata,end,2))
jindi = find(helper == min(helper))
positions(1:dateLength,1) = posi(validdata(jindi),:,1)'
positions(1:dateLength,2) = posi(validdata(jindi),:,2)'
clockError = zeros(length(posi),1); % ich vermute mal das dies der Unterschied gemessene toa zu der "von der errechneten Position abgeleiteten toa". der ist null da wir ihn of 0 zwingen.
end