%Mapping from aircraft relative to earth relative coordinate system...centered on the radar.
cosR = cos(roll*d2r);sinR = sin(roll*d2r);
cosP = cos(pitch*d2r);sinP = sin(pitch*d2r);
cosH = cos(head*d2r);sinH = sin(head*d2r);
cosTheta = cos(azi*d2r);sinTheta = sin(azi*d2r);
cosTau = cos(tau*d2r);sinTau = sin(tau*d2r);

A = cosR.*sinTheta*sinTau - sinR*cosTau;
B = cosP.*cosTheta + sinP.*sinR.*sinTheta;
C = sinP.*cosR*cosTau;

rotx = (cosH.*A + sinH*sinTau.*B + sinH.*C);
roty = (-sinH.*A + cosH*sinTau.*B + cosH.*C);
rotz = (sinTau*(sinP.*cosTheta - cosP.*sinR.*sinTheta) - cosP.*cosR*cosTau);

Xe = bsxfun(@times,range,rotx);     %km
Ye = bsxfun(@times,range,roty);     %km
Ze = bsxfun(@times,range,rotz);     %km

%Lat/Lon/Height
%ilat = bsxfun(@plus,Ye/111,alat);                              %degrees
%ilon = bsxfun(@plus,Xe./(111*cos(ilat*d2r)),alon+360);         %degrees
ihgt = bsxfun(@plus,Ze,ralt);                                   %km
