-
Notifications
You must be signed in to change notification settings - Fork 6
/
signatureAD2CP_beam2xyz_enu.m
228 lines (190 loc) · 8.19 KB
/
signatureAD2CP_beam2xyz_enu.m
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
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
% Function provided by Nortek
% Modified due to modification in Config struct
% Line AD2CP_ was removed
% MGP, April 29th, 2015
% Fixed bug for Two VelZ solution (removed -1)
function [ Data, Config, T_beam2xyz ] = signatureAD2CP_beam2xyz_enu( Data, Config, mode, twoZs )
if nargin == 3
twoZs = 0;
end
ad2cpstr = ''; %This string was removed from the Config struct AD2CP_
if strcmpi( mode, 'avg' )
dataModeWord = 'Average';
configModeWord = 'avg';
elseif strcmpi( mode, 'burst' )
dataModeWord = 'Burst';
configModeWord = 'burst';
end
% make the assumption the beam mapping is the same for all measurements in a data file
activeBeams = Data.( [dataModeWord '_Physicalbeam'] )( 1, : );
activeBeams = activeBeams(find(activeBeams > 0));
numberOfBeams = length( activeBeams );
if numberOfBeams <= 2
print( 'Transformations require at least 3 active beams.' )
T_beam2xyz = nan;
return
end
% assume max number of beams involved is 4, extra rows removed later
beamVectorsSpherical = zeros( 4, 3 );
for i = activeBeams
beamVectorsSpherical( i, : ) = [ 1, ...
Config.( [ad2cpstr 'BeamCfg' num2str( i ) '_theta' ] ), ...
Config.( [ad2cpstr 'BeamCfg' num2str( i ) '_phi' ] ) ];
end
disp(beamVectorsSpherical)
if numberOfBeams == 3
% for a three beam system, translate the beam vectors expressed in spherical coordinates
% into beam vectors in Cartesian coordinates
% first transform from spherical to cartesian coordinates
for i = activeBeams
beamVectorsCartesian( i, : ) = [ ...
sind( beamVectorsSpherical( i, 2 ) ) * cosd( beamVectorsSpherical( i, 3 ) ), ...
sind( beamVectorsSpherical( i, 2 ) ) * sind( beamVectorsSpherical( i, 3 ) ), ...
cosd( beamVectorsSpherical( i, 2 ) ) ];
end
cartesianTransformCheck = sum( beamVectorsCartesian.^2, 2 );
% remove any extra rows for inactive beams
beamVectorsCartesian( cartesianTransformCheck == 0, : ) = [];
T_beam2xyz = inv( beamVectorsCartesian );
elseif numberOfBeams == 4
if twoZs == 0
% for a four beam system, translate the beam vectors expressed in spherical coordinates
% into beam vectors in Cartesian coordinates, using only three basis vectors
for i = 1:numberOfBeams
beamVectorsCartesian( i, : ) = [ ...
sind( beamVectorsSpherical( i, 2 ) ) * cosd( beamVectorsSpherical( i, 3 ) ), ...
sind( beamVectorsSpherical( i, 2 ) ) * sind( beamVectorsSpherical( i, 3 ) ), ...
cosd( beamVectorsSpherical( i, 2 ) ) ];
end
cartesianTransformCheck = sum( beamVectorsCartesian.^2, 2 );
% pseudo inverse needs to be used because beamVectorsCartesian isn't square
T_beam2xyz = pinv( beamVectorsCartesian );
else
% this section makes two estimates of the vertical velocity
for i = 1:numberOfBeams
if i == 1 | i == 3
beamVectorsCartesianzz( i, : ) = [ ...
beamVectorsSpherical( i, 1 ) * sind( beamVectorsSpherical( i, 2 ) ) * cosd( beamVectorsSpherical( i, 3 ) ), ...
1 * beamVectorsSpherical( i, 1 ) * sind( beamVectorsSpherical( i, 2 ) ) * sind( beamVectorsSpherical( i, 3 ) ), ...
beamVectorsSpherical( i, 1 ) * cosd( beamVectorsSpherical( i, 2 ) ), ...
0 ];
else
beamVectorsCartesianzz( i, : ) = [ ...
beamVectorsSpherical( i, 1 ) * sind( beamVectorsSpherical( i, 2 ) ) * cosd( beamVectorsSpherical( i, 3 ) ), ...
1 * beamVectorsSpherical( i, 1 ) * sind( beamVectorsSpherical( i, 2 ) ) * sind( beamVectorsSpherical( i, 3 ) ), ...
0, ...
beamVectorsSpherical( i, 1 ) * cosd( beamVectorsSpherical( i, 2 ) ) ];
end
end
cartesianTransformCheck = sum( beamVectorsCartesianzz.^2, 2 );
% there should be an inverse for this, no pseudoinverse needed
T_beam2xyz = inv( beamVectorsCartesianzz );
% Can also add in a row for the error velocity calculation,
% as the difference between the two vertical velcoities
% T_beam2xyz( end + 1, : ) = T_beam2xyzz( 3, : ) - T_beam2xyzz( 4, : );
% note the addition of the error velocity row changes the inversion of the matrix, it
% needs to be removed to recover the xyz2beam matrix.
end
end
% verify we're not already in 'xyz'
if strcmpi( Config.( [ ad2cpstr configModeWord '_coordSystem' ] ), 'xyz' )
disp( 'Velocity data is already in xyz coordinate system.' )
return
end
xAllCells = zeros( length( Data.( [ dataModeWord '_TimeStamp' ] ) ), Config.( [ad2cpstr configModeWord '_nCells' ] ) );
yAllCells = zeros( length( Data.( [ dataModeWord '_TimeStamp' ] ) ), Config.( [ad2cpstr configModeWord '_nCells' ] ) );
zAllCells = zeros( length( Data.( [ dataModeWord '_TimeStamp' ] ) ), Config.( [ad2cpstr configModeWord '_nCells' ] ) );
if twoZs == 1
z2AllCells = zeros( length( Data.( [ dataModeWord '_TimeStamp' ] ) ), Config.( [ad2cpstr configModeWord '_nCells' ] ) );
end
xyz = zeros( size( T_beam2xyz, 2 ), length( Data.( [ dataModeWord '_TimeStamp' ] ) ) );
beam = zeros( size( T_beam2xyz, 2 ), length( Data.( [ dataModeWord '_TimeStamp' ] ) ) );
for nCell = 1:Config.( [ad2cpstr configModeWord '_nCells' ] )
for i = 1:numberOfBeams
beam( i, : ) = Data.( [ dataModeWord '_VelBeam' num2str( Data.( [ dataModeWord '_Physicalbeam' ] )( 1, i ) ) ] )( :, nCell )';
end
xyz = T_beam2xyz * beam;
xAllCells( :, nCell ) = xyz( 1, : )';
yAllCells( :, nCell ) = xyz( 2, : )';
zAllCells( :, nCell ) = xyz( 3, : )';
if twoZs == 1
z2AllCells( :, nCell ) = xyz( 4, : )';
end
end
Config.( [ad2cpstr configModeWord '_coordSystem' ] ) = 'xyz';
Data.( [ dataModeWord '_VelX' ] ) = xAllCells;
Data.( [ dataModeWord '_VelY' ] ) = yAllCells;
if twoZs == 1
Data.( [ dataModeWord '_VelZ1' ] ) = zAllCells;
Data.( [ dataModeWord '_VelZ2' ] ) = z2AllCells;
else
Data.( [ dataModeWord '_VelZ' ] ) = zAllCells;
end
% verify we're not already in 'enu'
if strcmpi( Config.( [ad2cpstr configModeWord '_coordSystem' ] ), 'enu' )
disp( 'Velocity data is already in enu coordinate system.' )
return
end
K = 3;
EAllCells = zeros( length( Data.( [dataModeWord '_TimeStamp' ] ) ), Config.( [ad2cpstr configModeWord '_nCells' ] ) );
NAllCells = zeros( length( Data.( [dataModeWord '_TimeStamp' ] ) ), Config.( [ad2cpstr configModeWord '_nCells' ] ) );
UAllCells = zeros( length( Data.( [dataModeWord '_TimeStamp' ] ) ), Config.( [ad2cpstr configModeWord '_nCells' ] ) );
if twoZs == 1
U2AllCells = zeros( length( Data.( [dataModeWord '_TimeStamp' ] ) ), Config.( [ad2cpstr configModeWord '_nCells' ] ) );
K = 4;
end
Name = ['X','Y','Z'];
ENU = zeros( K, Config.([ad2cpstr configModeWord '_nCells' ]));
xyz = zeros( K, Config.([ad2cpstr configModeWord '_nCells' ]));
for sampleIndex = 1:length(Data.( [dataModeWord '_Error' ]));
if (bitand(bitshift(uint32(Data.( [dataModeWord '_Status' ])(sampleIndex)), -25),7) == 5)
signXYZ=[1 -1 -1 -1];
else
signXYZ=[1 1 1 1];
end
hh = pi*(Data.([dataModeWord '_Heading'])(sampleIndex)-90)/180;
pp = pi*Data.([dataModeWord '_Pitch'])(sampleIndex)/180;
rr = pi*Data.([dataModeWord '_Roll'])(sampleIndex)/180;
% Make heading matrix
H = [cos(hh) sin(hh) 0; -sin(hh) cos(hh) 0; 0 0 1];
% Make tilt matrix
P = [cos(pp) -sin(pp)*sin(rr) -cos(rr)*sin(pp);...
0 cos(rr) -sin(rr); ...
sin(pp) sin(rr)*cos(pp) cos(pp)*cos(rr)];
% Make resulting transformation matrix
xyz2enu = H*P;
if (twoZs == 1)
xyz2enu(1,3) = xyz2enu(1,3)/2;
xyz2enu(1,4) = xyz2enu(1,3);
xyz2enu(2,3) = xyz2enu(2,3)/2;
xyz2enu(2,4) = xyz2enu(2,3);
xyz2enu(4,:) = xyz2enu(3,:);
xyz2enu(3,4) = 0;
xyz2enu(4,4) = xyz2enu(3,3);
xyz2enu(4,3) = 0;
end
for i = 1:K
if (twoZs == 1) && (i >= 3)
axs = [ Name(3) num2str((i-2),1) ];
else
axs = Name(i);
end
xyz( i, : ) = signXYZ(i) * Data.( [ dataModeWord '_Vel' axs] )( sampleIndex, : )';
end
ENU = xyz2enu * xyz;
EAllCells( sampleIndex, : ) = ENU( 1, : )';
NAllCells( sampleIndex, : ) = ENU( 2, : )';
UAllCells( sampleIndex, : ) = ENU( 3, : )';
if twoZs == 1
U2AllCells( sampleIndex, : ) = ENU( 4, : )';
end
end
Config.( [ad2cpstr configModeWord '_coordSystem' ] ) = 'enu';
Data.( [ dataModeWord '_VelEast' ] ) = EAllCells;
Data.( [ dataModeWord '_VelNorth' ] ) = NAllCells;
if twoZs == 1
Data.( [ dataModeWord '_VelUp1' ] ) = UAllCells;
Data.( [ dataModeWord '_VelUp2' ] ) = U2AllCells;
else
Data.( [ dataModeWord '_VelUp' ] ) = UAllCells;
end