%Mapping from aircraft relative to track relative coordinate system...centered on the radar.

cosR = cos(roll*d2r);sinR = sin(roll*d2r);
cosP = cos(pitch*d2r);sinP = sin(pitch*d2r);
cosD = cos(drift*d2r);sinD = sin(drift*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 = (cosD.*A - sinD*sinTau.*B - sinD.*C);
roty = (sinD.*A + cosD*sinTau.*B + cosD.*C);
rotz = (sinTau*(sinP.*cosTheta - cosP.*sinR.*sinTheta) - cosP.*cosR*cosTau);

Xt = bsxfun(@times,range,rotx);     %km
Yt = bsxfun(@times,range,roty);     %km
Zt = bsxfun(@times,range,rotz);     %km
ihgt = bsxfun(@plus,Zt,ralt);       %km
%End Mapping
