1 #====================================================================== |
1 #====================================================================== |
2 # R D I _ C O O R D S . P L |
2 # R D I _ C O O R D S . P L |
3 # doc: Sun Jan 19 17:57:53 2003 |
3 # doc: Sun Jan 19 17:57:53 2003 |
4 # dlm: Thu May 26 17:40:20 2016 |
4 # dlm: Sun Jul 31 13:54:26 2016 |
5 # (c) 2003 A.M. Thurnherr |
5 # (c) 2003 A.M. Thurnherr |
6 # uE-Info: 530 14 NIL 0 0 72 2 2 4 NIL ofnI |
6 # uE-Info: 568 0 NIL 0 0 72 10 2 4 NIL ofnI |
7 #====================================================================== |
7 #====================================================================== |
8 |
8 |
9 # RDI Workhorse Coordinate Transformations |
9 # RDI Workhorse Coordinate Transformations |
10 |
10 |
11 # HISTORY: |
11 # HISTORY: |
44 # Feb 29, 2016: - debugged & verified velEarthToInstrument(), velInstrumentToBeam() |
44 # Feb 29, 2016: - debugged & verified velEarthToInstrument(), velInstrumentToBeam() |
45 # - added velBeamToEarth() |
45 # - added velBeamToEarth() |
46 # May 19, 2016: - begin implemeting bin interpolation |
46 # May 19, 2016: - begin implemeting bin interpolation |
47 # May 25, 2016: - continued |
47 # May 25, 2016: - continued |
48 # May 26, 2016: - made it work |
48 # May 26, 2016: - made it work |
|
49 # May 30, 2016: - begin implementing 2nd order attitude transformations |
|
50 # Jun 6, 2016: - toEarth transformation in beamToBPEarth was but crude approximation; |
|
51 # updated with transformation taken from Lohrman et al. (JAOT 1990) |
|
52 # - BUG: v34 sign was inconsistent with RDI coord manual |
|
53 # Jun 8, 2016: - added $ens as arg to velInstrumentToBeam() for consistency |
|
54 # Jul 7, 2016: - added velEarthToBPw() with algorithm debugged and verified |
|
55 # by Paul Wanis from TRDI |
49 |
56 |
50 use strict; |
57 use strict; |
51 use POSIX; |
58 use POSIX; |
52 |
59 |
53 my($PI) = 3.14159265358979; |
60 my($PI) = 3.14159265358979; |
57 |
64 |
58 #---------------------------------------------------------------------- |
65 #---------------------------------------------------------------------- |
59 # Tweakables |
66 # Tweakables |
60 #---------------------------------------------------------------------- |
67 #---------------------------------------------------------------------- |
61 |
68 |
62 $RDI_Coords::minValidVels = 3; # 3-beam solutions ok (velBeamToInstrument) |
69 $RDI_Coords::minValidVels = 3; # 3-beam solutions ok (velBeamToInstrument) |
63 $RDI_Coords::binMapping = 'linterp'; # 'linterp' or 'none' (earthVels, BPearthVels) |
70 $RDI_Coords::binMapping = 'linterp'; # 'linterp' or 'none' (earthVels, BPearthVels) |
64 |
71 $RDI_Coords::beamTransformation = 'LHR90'; # set to 'RDI' to use 1st order transformations from RDI manual |
65 #---------------------------------------------------------------------- |
72 |
66 # beam to earth transformation (from RDI coord trans manual) |
73 #---------------------------------------------------------------------- |
|
74 # beam to earth transformation |
67 #---------------------------------------------------------------------- |
75 #---------------------------------------------------------------------- |
68 |
76 |
69 $RDI_Coords::threeBeam_1 = 0; # stats from velBeamToInstrument |
77 $RDI_Coords::threeBeam_1 = 0; # stats from velBeamToInstrument |
70 $RDI_Coords::threeBeam_2 = 0; |
78 $RDI_Coords::threeBeam_2 = 0; |
71 $RDI_Coords::threeBeam_3 = 0; |
79 $RDI_Coords::threeBeam_3 = 0; |
121 $v1*$B2I[2][0]+$v2*$B2I[2][1]+$v3*$B2I[2][2]+$v4*$B2I[2][3], |
129 $v1*$B2I[2][0]+$v2*$B2I[2][1]+$v3*$B2I[2][2]+$v4*$B2I[2][3], |
122 $v1*$B2I[3][0]+$v2*$B2I[3][1]+$v3*$B2I[3][2]+$v4*$B2I[3][3]); |
130 $v1*$B2I[3][0]+$v2*$B2I[3][1]+$v3*$B2I[3][2]+$v4*$B2I[3][3]); |
123 } |
131 } |
124 } # STATIC SCOPE |
132 } # STATIC SCOPE |
125 |
133 |
|
134 #---------------------------------------------------------------------- |
|
135 # velInstrumentToEarth(\%ADCP,ens,v1,v2,v3,v4) => (u,v,w,e) |
|
136 # - $RDI_Coords::beamTransformation = 'LHR90' |
|
137 # - from Lohrmann, Hackett & Roet (J. Tech., 1990) |
|
138 # - eq A1 maps to RDI matrix M (sec 5.6) with |
|
139 # alpha = roll |
|
140 # beta = gimball_pitch |
|
141 # psi = calculation_pitch |
|
142 # psi = asin{sin(beta) cos(alpha) / sqrt[1- sin(alpha)^2 sin(beta)^2]} |
|
143 # - (I only checked for 0 heading, but this is sufficient) |
|
144 # - $RDI_Coords::beamTransformation = 'RDI' |
|
145 # - default prior to LADCP_w V1.3 |
|
146 # - from RDI manual |
|
147 # - 99% accurate for p/r<8deg |
|
148 # => 1cm/s error for 1m/s winch speed! |
|
149 #---------------------------------------------------------------------- |
|
150 |
126 { # STATIC SCOPE |
151 { # STATIC SCOPE |
127 my($hdg,$pitch,$roll,@I2E); |
152 my($hdg,$pitch,$roll,@I2E); |
128 |
153 |
129 sub velInstrumentToEarth(@) |
154 sub velInstrumentToEarth(@) |
130 { |
155 { |
143 $hdg = $ADCP->{ENSEMBLE}[$ens]->{HEADING} - $ADCP->{HEADING_BIAS} |
168 $hdg = $ADCP->{ENSEMBLE}[$ens]->{HEADING} - $ADCP->{HEADING_BIAS} |
144 if defined($ADCP->{ENSEMBLE}[$ens]->{HEADING}); |
169 if defined($ADCP->{ENSEMBLE}[$ens]->{HEADING}); |
145 $pitch = $ADCP->{ENSEMBLE}[$ens]->{PITCH}; |
170 $pitch = $ADCP->{ENSEMBLE}[$ens]->{PITCH}; |
146 $roll = $ADCP->{ENSEMBLE}[$ens]->{ROLL}; |
171 $roll = $ADCP->{ENSEMBLE}[$ens]->{ROLL}; |
147 my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll))); |
172 my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll))); |
|
173 my($rad_calc_pitch) = ($RDI_Coords::beamTransformation eq 'RDI') ? $rad_gimbal_pitch : |
|
174 asin(sin($rad_gimbal_pitch)*cos(rad($roll)) / |
|
175 sqrt(1-sin(rad($roll))**2*sin($rad_gimbal_pitch)**2)); |
148 my($sh,$ch) = (sin(rad($hdg)),cos(rad($hdg))) |
176 my($sh,$ch) = (sin(rad($hdg)),cos(rad($hdg))) |
149 if defined($hdg); |
177 if defined($hdg); |
150 my($sp,$cp) = (sin($rad_gimbal_pitch),cos($rad_gimbal_pitch)); |
178 my($sp,$cp) = (sin($rad_calc_pitch),cos($rad_calc_pitch)); |
151 my($sr,$cr) = (sin(rad($roll)), cos(rad($roll))); |
179 my($sr,$cr) = (sin(rad($roll)), cos(rad($roll))); |
152 @I2E = $ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP} |
180 @I2E = $ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP} |
153 ? ( |
181 ? ( |
154 [-$ch*$cr-$sh*$sp*$sr, $sh*$cp,-$ch*$sr+$sh*$sp*$cr], |
182 [-$ch*$cr-$sh*$sp*$sr, $sh*$cp,-$ch*$sr+$sh*$sp*$cr], |
155 [-$ch*$sp*$sr+$sh*$cr, $ch*$cp, $sh*$sr+$ch*$sp*$cr], |
183 [-$ch*$sp*$sr+$sh*$cr, $ch*$cp, $sh*$sr+$ch*$sp*$cr], |
180 |
208 |
181 |
209 |
182 #---------------------------------------------------------------------- |
210 #---------------------------------------------------------------------- |
183 # velEarthToInstrument() transforms earth to instrument coordinates |
211 # velEarthToInstrument() transforms earth to instrument coordinates |
184 # - based on manually inverted rotation matrix M (Sec 5.6 in coord-trans manual) |
212 # - based on manually inverted rotation matrix M (Sec 5.6 in coord-trans manual) |
|
213 # - Paul Wanis from TRDI pointed out that M is orthonormal, which |
|
214 # implies that M^-1 = M' (where M' is the transpose), confirming |
|
215 # the (unnecessary) derivation |
185 # - code was verified for both down- and uplookers |
216 # - code was verified for both down- and uplookers |
186 # - missing heading data (IMP) causes undef beam velocities |
217 # - missing heading data (IMP) causes undef beam velocities |
187 #---------------------------------------------------------------------- |
218 #---------------------------------------------------------------------- |
188 |
219 |
189 { # STATIC SCOPE |
220 { # STATIC SCOPE |
216 $u*$E2I[1][0]+$v*$E2I[1][1]+$w*$E2I[1][2], |
247 $u*$E2I[1][0]+$v*$E2I[1][1]+$w*$E2I[1][2], |
217 $u*$E2I[2][0]+$v*$E2I[2][1]+$w*$E2I[2][2], |
248 $u*$E2I[2][0]+$v*$E2I[2][1]+$w*$E2I[2][2], |
218 $ev) |
249 $ev) |
219 : (undef,undef,undef,undef); |
250 : (undef,undef,undef,undef); |
220 |
251 |
221 } |
252 } # velEarthToIntrument() |
222 } # STATIC SCOPE |
253 } # STATIC SCOPE |
223 |
254 |
224 #---------------------------------------------------------------------- |
255 #---------------------------------------------------------------------- |
225 # velInstrumentToBeam() transforms instrument to beam coordinates |
256 # velInstrumentToBeam() transforms instrument to beam coordinates |
226 # - based on manually solved eq system in sec 5.3 of coord manual |
257 # - based on manually solved eq system in sec 5.3 of coord manual |
232 { # STATIC SCOPE |
263 { # STATIC SCOPE |
233 my($a,$b,$c,$d); |
264 my($a,$b,$c,$d); |
234 |
265 |
235 sub velInstrumentToBeam(@) |
266 sub velInstrumentToBeam(@) |
236 { |
267 { |
237 my($ADCP,$x,$y,$z,$ev) = @_; |
268 my($ADCP,$ens,$x,$y,$z,$ev) = @_; |
238 return undef unless (defined($x) + defined($y) + |
269 return undef unless (defined($x) + defined($y) + |
239 defined($z) + defined($ev) == 4); |
270 defined($z) + defined($ev) == 4); |
240 |
271 |
241 unless (defined($a)) { |
272 unless (defined($a)) { |
242 $a = 1 / (2 * sin(rad($ADCP->{BEAM_ANGLE}))); |
273 $a = 1 / (2 * sin(rad($ADCP->{BEAM_ANGLE}))); |
258 #---------------------------------------------------------------------- |
289 #---------------------------------------------------------------------- |
259 |
290 |
260 sub velEarthToBeam(@) |
291 sub velEarthToBeam(@) |
261 { |
292 { |
262 my($ADCP,$ens,$u,$v,$w,$ev) = @_; |
293 my($ADCP,$ens,$u,$v,$w,$ev) = @_; |
263 return velInstrumentToBeam($ADCP, |
294 return velInstrumentToBeam($ADCP,$ens, |
264 velEarthToInstrument($ADCP,$ens,$u,$v,$w,$ev)); |
295 velEarthToInstrument($ADCP,$ens,$u,$v,$w,$ev)); |
|
296 } |
|
297 |
|
298 #---------------------------------------------------------------------- |
|
299 # velEarthToBPw() returns w12 and w34 for beam-coordinate data |
|
300 # - I am grateful for Paul Wanis from TRDI who corrected a |
|
301 # bug in my transformation (fixed in V1.3). [The bug did not |
|
302 # affect the final w profiles significantly, because w12 and w34 |
|
303 # are used only as diagnostics.] |
|
304 # - algorithm: |
|
305 # 1) rotate into instrument coordinates |
|
306 # 2) w12 = w + e*tan(beam_angle)/sqrt(2) |
|
307 # w34 = w - e*tan(beam_angle)/sqrt(2) |
|
308 # 3) rotate into horizontal coords (earth coords w/o |
|
309 # considering heading, i.e. same as earth coords |
|
310 # in case of w |
|
311 #---------------------------------------------------------------------- |
|
312 |
|
313 sub velEarthToBPw(@) |
|
314 { |
|
315 my($ADCP,$ens,$u,$v,$w,$ev) = @_; |
|
316 my(@iv) = velEarthToInstrument(@_); |
|
317 my(@iv12) = my(@iv34) = @iv; |
|
318 $iv12[2] += $iv[3] * tan(rad($ADCP->{BEAM_ANGLE}))/sqrt(2); |
|
319 $iv34[2] -= $iv[3] * tan(rad($ADCP->{BEAM_ANGLE}))/sqrt(2); |
|
320 my(@ev12) = velInstrumentToEarth($ADCP,$ens,@iv12); |
|
321 my(@ev34) = velInstrumentToEarth($ADCP,$ens,@iv34); |
|
322 return ($ev12[2],$ev34[2]); |
265 } |
323 } |
266 |
324 |
267 #====================================================================== |
325 #====================================================================== |
268 # velBeamToBPEarth(@) calculates the vertical- and horizontal vels |
326 # velBeamToBPEarth(@) calculates the vertical- and horizontal vels |
269 # from the two beam pairs separately. Note that (w1+w2)/2 is |
327 # from the two beam pairs separately. Note that (w1+w2)/2 is |
270 # identical to the w estimated according to RDI without 3-beam |
328 # identical to the w estimated according to RDI (ignoring 3-beam |
271 # solutions. |
329 # solutions). |
272 #====================================================================== |
330 #====================================================================== |
273 |
331 |
274 { # STATIC SCOPE |
332 { # STATIC SCOPE |
275 my($TwoCosBAngle,$TwoSinBAngle); |
333 my($TwoCosBAngle,$TwoSinBAngle); |
276 |
334 |
285 |
343 |
286 unless (defined($TwoCosBAngle)) { |
344 unless (defined($TwoCosBAngle)) { |
287 $TwoCosBAngle = 2 * cos(rad($ADCP->{BEAM_ANGLE})); |
345 $TwoCosBAngle = 2 * cos(rad($ADCP->{BEAM_ANGLE})); |
288 $TwoSinBAngle = 2 * sin(rad($ADCP->{BEAM_ANGLE})); |
346 $TwoSinBAngle = 2 * sin(rad($ADCP->{BEAM_ANGLE})); |
289 } |
347 } |
290 my($roll) = rad($ADCP->{ENSEMBLE}[$ens]->{ROLL}); |
348 my($rad_roll) = rad($ADCP->{ENSEMBLE}[$ens]->{ROLL}); |
291 my($sr) = sin($roll); my($cr) = cos($roll); |
349 my($sr) = sin($rad_roll); my($cr) = cos($rad_roll); |
292 my($pitch) = atan(tan(rad($ADCP->{ENSEMBLE}[$ens]->{PITCH})) * $cr); # gimbal pitch |
350 my($rad_gimbal_pitch) = atan(tan(rad($ADCP->{ENSEMBLE}[$ens]->{PITCH})) * $cr); # gimbal pitch |
293 my($sp) = sin($pitch); my($cp) = cos($pitch); |
351 my($rad_calc_pitch) = ($RDI_Coords::beamTransformation eq 'RDI') ? $rad_gimbal_pitch : |
|
352 asin(sin($rad_gimbal_pitch)*cos($rad_roll) / |
|
353 sqrt(1-sin($rad_roll)**2*sin($rad_gimbal_pitch)**2)); |
|
354 my($sp) = sin($rad_calc_pitch); my($cp) = cos($rad_calc_pitch); |
294 |
355 |
295 # Sign convention: |
356 # Sign convention: |
296 # - refer to Coord manual Fig. 3 |
357 # - refer to Coord manual Fig. 3 |
297 # - v12 is horizontal velocity from beam1 to beam2, i.e. westward for upward-looking ADCP |
358 # - v12 is horizontal velocity from beam1 to beam2, i.e. westward for upward-looking ADCP |
298 # with beam 3 pointing north (heading = 0) |
359 # with beam 3 pointing north (heading = 0) |
299 # - w is +ve upward, regardless of instrument orientation |
360 |
300 |
361 my($v12_ic) = ($b1-$b2)/$TwoSinBAngle; # instrument coords |
301 my($v12_ic) = ($b1-$b2)/$TwoSinBAngle; # instrument coords with w vertical up |
362 my($v34_ic) = ($b4-$b3)/$TwoSinBAngle; # consistent with RDI Coords |
302 my($w12_ic) = ($b1+$b2)/$TwoCosBAngle; |
363 my($w12_ic) = ($b1+$b2)/$TwoCosBAngle; |
303 $w12_ic *= -1 if ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}); |
|
304 my($v34_ic) = ($b3-$b4)/$TwoSinBAngle; |
|
305 my($w34_ic) = ($b3+$b4)/$TwoCosBAngle; |
364 my($w34_ic) = ($b3+$b4)/$TwoCosBAngle; |
306 $w34_ic *= -1 if ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}); |
365 my($w_ic) = ($w12_ic+$w34_ic) / 2; |
307 |
366 |
308 if ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}) { # beampair Earth coords |
367 if ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_DOWN}) { # beampair Earth coords |
309 $w12 = $w12_ic*$cr + $v12_ic*$sr - $v34_ic*$sp; |
368 $v12 = $v12_ic*$cr + $v34_ic*0 + $w_ic*$sr; # Lohrman et al. (1990) A1 |
310 $v12 = $v12_ic*$cr - $w12_ic*$sr + $w34_ic*$sp; |
369 $v34 = $v12_ic*$sp*$sr + $v34_ic*$cp - $w_ic*$sp*$cr; # - defined for z upward => DL |
311 $w34 = $w34_ic*$cp - $v34_ic*$sp + $v12_ic*$sr; |
370 $w12 =-$v12_ic*$cp*$sr + $v34_ic*$sp + $w12_ic*$cp*$cr; |
312 $v34 = $v34_ic*$cp + $w34_ic*$sp - $w12_ic*$sr; |
371 $w34 =-$v12_ic*$cp*$sr + $v34_ic*$sp + $w34_ic*$cp*$cr; |
313 } else { |
372 } else { # RDI Coord trans manual |
314 $w12 = $w12_ic*$cr - $v12_ic*$sr - $v34_ic*$sp; |
373 $v12 =-$v12_ic*$cr + $v34_ic*0 - $w_ic*$sr; # - as above with 1st & 3rd cols negated |
315 $v12 = $v12_ic*$cr + $w12_ic*$sr + $w34_ic*$sp; |
374 $v34 =-$v12_ic*$sp*$sr + $v34_ic*$cp + $w_ic*$sp*$cr; |
316 $w34 = $w34_ic*$cp - $v34_ic*$sp - $v12_ic*$sr; |
375 $w12 = $v12_ic*$cp*$sr + $v34_ic*$sp - $w12_ic*$cp*$cr; |
317 $v34 = $v34_ic*$cp + $w34_ic*$sp + $w12_ic*$sr; |
376 $w34 = $v12_ic*$cp*$sr + $v34_ic*$sp - $w34_ic*$cp*$cr; |
318 } |
377 } |
319 |
378 |
320 $v12=$w12=undef unless (defined($b1) && defined($b2)); |
379 $v12=$w12=undef unless (defined($b1) && defined($b2)); |
321 $v34=$w34=undef unless (defined($b3) && defined($b4)); |
380 $v34=$w34=undef unless (defined($b3) && defined($b4)); |
322 |
381 |
325 } |
384 } |
326 |
385 |
327 #=================================================================== |
386 #=================================================================== |
328 # velBeamToBPInstrument(@) calculates the instrument-coordinate vels |
387 # velBeamToBPInstrument(@) calculates the instrument-coordinate vels |
329 # from the two beam pairs separately. |
388 # from the two beam pairs separately. |
|
389 # - in spite of the function name, the output is in ship |
|
390 # coordinates (instr coords with w up) |
330 #=================================================================== |
391 #=================================================================== |
331 |
392 |
332 { # STATIC SCOPE |
393 { # STATIC SCOPE |
333 my($TwoCosBAngle,$TwoSinBAngle); |
394 my($TwoCosBAngle,$TwoSinBAngle); |
334 |
395 |
355 $v12 = ($b1-$b2)/$TwoSinBAngle; |
416 $v12 = ($b1-$b2)/$TwoSinBAngle; |
356 $w12 = ($b1+$b2)/$TwoCosBAngle; |
417 $w12 = ($b1+$b2)/$TwoCosBAngle; |
357 $w12 *= -1 if ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}); |
418 $w12 *= -1 if ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}); |
358 } |
419 } |
359 if (defined($b3) && defined($b4)) { |
420 if (defined($b3) && defined($b4)) { |
360 $v34 = ($b3-$b4)/$TwoSinBAngle; |
421 $v34 = ($b4-$b3)/$TwoSinBAngle; |
361 $w34 = ($b3+$b4)/$TwoCosBAngle; |
422 $w34 = ($b3+$b4)/$TwoCosBAngle; |
362 $w34 *= -1 if ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}); |
423 $w34 *= -1 if ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}); |
363 } |
424 } |
364 |
425 |
365 return ($v12,$w12,$v34,$w34); |
426 return ($v12,$w12,$v34,$w34); |