0001 function [exyz]=depthxyz(depthRaw,draft,pitchRaw,roll,heading,beamAngle,...
0002 unitsID,x,y,elev,ens);
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
0024
0025
0026
0027
0028
0029
0030
0031
0032
0033 if unitsID=='ft'
0034 draft=double(draft)*0.0328083;
0035 else
0036 draft=double(draft)*.01;
0037 end;
0038
0039
0040
0041 if ischar(elev); elev = str2num(elev); end;
0042
0043
0044 geo=[x,y,-1.*repmat(draft,size(y))+elev];
0045
0046
0047 if ischar(beamAngle)
0048 beamAngleR=str2double(beamAngle).*pi/180;
0049 else
0050 beamAngleR=beamAngle.*pi/180;
0051 end
0052 range=(depthRaw-draft)./cos(beamAngleR);
0053
0054
0055 rollR=roll.*pi/180;
0056 pitchRawR=pitchRaw.*pi/180;
0057 headingR=heading.*pi/180;
0058 pitchR=atan(tan(pitchRawR).*cos(rollR));
0059
0060
0061 ch=cos(headingR);
0062 sh=sin(headingR);
0063 cp=cos(pitchR);
0064 sp=sin(pitchR);
0065 cr=cos(rollR);
0066 sr=sin(rollR);
0067
0068
0069 trans2e=[ch.*cr+sh.*sp.*sr sh.*cp ch.*sr-sh.*sp.*cr];
0070 trans2n=[-sh.*cr+ch.*sp.*sr ch.*cp -sh.*sr-ch.*sp.*cr];
0071 trans2u=[-cp.*sr sp cp.*cr];
0072
0073
0074 rng2xyz=[-sin(beamAngleR) 0 -cos(beamAngleR);...
0075 sin(beamAngleR) 0 -cos(beamAngleR);...
0076 0 sin(beamAngleR) -cos(beamAngleR);...
0077 0 -sin(beamAngleR) -cos(beamAngleR)];
0078
0079
0080 beam1xyz=repmat(rng2xyz(1,:),size(range,1),1).*repmat(range(:,1),1,3);
0081 beam2xyz=repmat(rng2xyz(2,:),size(range,1),1).*repmat(range(:,2),1,3);
0082 beam3xyz=repmat(rng2xyz(3,:),size(range,1),1).*repmat(range(:,3),1,3);
0083 beam4xyz=repmat(rng2xyz(4,:),size(range,1),1).*repmat(range(:,4),1,3);
0084
0085
0086 beam1enu(:,1)=ens;
0087 beam1enu(:,2)=sum(trans2e.*beam1xyz,2);
0088 beam1enu(:,3)=sum(trans2n.*beam1xyz,2);
0089 beam1enu(:,4)=sum(trans2u.*beam1xyz,2);
0090 beam1enu(:,2:4)=beam1enu(:,2:4)+geo;
0091
0092
0093 beam2enu(:,1)=ens;
0094 beam2enu(:,2)=sum(trans2e.*beam2xyz,2);
0095 beam2enu(:,3)=sum(trans2n.*beam2xyz,2);
0096 beam2enu(:,4)=sum(trans2u.*beam2xyz,2);
0097 beam2enu(:,2:4)=beam2enu(:,2:4)+geo;
0098
0099
0100 beam3enu(:,1)=ens;
0101 beam3enu(:,2)=sum(trans2e.*beam3xyz,2);
0102 beam3enu(:,3)=sum(trans2n.*beam3xyz,2);
0103 beam3enu(:,4)=sum(trans2u.*beam3xyz,2);
0104 beam3enu(:,2:4)=beam3enu(:,2:4)+geo;
0105
0106
0107 beam4enu(:,1)=ens;
0108 beam4enu(:,2)=sum(trans2e.*beam4xyz,2);
0109 beam4enu(:,3)=sum(trans2n.*beam4xyz,2);
0110 beam4enu(:,4)=sum(trans2u.*beam4xyz,2);
0111 beam4enu(:,2:4)=beam4enu(:,2:4)+geo;
0112
0113
0114 exyz=[beam1enu;beam2enu;beam3enu;beam4enu];
0115 exyz=sortrows(exyz,1);
0116