calc_angle.rar_calc wheel angle

function [ angle ] = calc_angle( map, pos, attitude, lander_pos ) %calc_angle calculates the angle between the measuring ray and Mars surface % using the Lander attitude angles and slope of the interception pixel % relative to its adjacent pixels. % Outputs: % angle is the angle between the measuring ray and Mars surface in % degrees. % Inputs: % map is a structure of data, map related: % map.map is a 2 dimensional matrix where the lines have a direct % correspondence with latitude and columns with longitude, the % values in the matrix are the radius to the center of the planet % for the given line and column. % % map.a is the first distance along the ray to be evaluated if % the ray has already intercepted the surface % % map.m is the number of maximum increments of size map.a that % are calculated, map.a*map.m is the maximum searching distance % for the interception. % % map.LINE_PROJECTION_OFFSET is the line number on which the map % projection origin occurs; i.e., the line number that % corresponds to the center latitude. % % map.SAMPLE_PROJECTION_OFFSET is the sample number on which the % map projection origin occurs; i.e., the sample number that % corresponds to the center longitude. % % map.CENTER_LONGITUDE, map.CENTER_LATITUDE are the longitude and % latitude chosen as the origin of the map projection. % % map.RES is the number of pixels per degree of the map. % % pos is a 1-by-3 vector of spherical coordinates in degrees and % meters ordered as follows: [longitude latitude radius] where % longitude is East longitude. Is the point of interception of the % ray and local mars surface. % % attitude is a 1-by-3 vector of Euler angles in degrees ordered as % follows: [yaw pitch roll] % % lander_pos is a 1-by-3 vector of spherical coordinates in degrees % and meters ordered as follows: [longitude latitude radius] where % longitude is East longitude. Is the origin of the ray, the position % of the lander. %% If there is no footprint, then there is no interception angle: if isnan(pos(1)) || isnan(pos(1)) || isnan(pos(1)) angle = NaN; return end %% Calculation of the interception angle: %West longitudes are not allowed, only East [0 360[: if pos(1) < 0 pos(1) = pos(1) + 360; end lon_dif = (pos(1) - map.CENTER_LONGITUDE); if lon_dif < 0 lon_dif = lon_dif + 360; end %Conversion from longitude and latitude to line and sample of the %interception point: sample = map.SAMPLE_PROJECTION_OFFSET + map.RES * lon_dif; line = map.LINE_PROJECTION_OFFSET - map.RES * (pos(2) - ... map.CENTER_LATITUDE); %Rounding to the nearest integer: isample = round(sample); iline = round(line); %Finding of the north and east neighboring pixels: if line > iline vline = iline + 1; else vline = iline - 1; end if sample > isample hsample = isample + 1; else hsample = isample - 1; end %Conversion from line and sample to latitude and longitude from the center %of the center pixel and two pixel neighbors. Also access to the map data %to retrieve the radius: ilon = (isample - map.SAMPLE_PROJECTION_OFFSET)/map.RES + map.CENTER_LONGITUDE; ilat = -(iline - map.LINE_PROJECTION_OFFSET)/map.RES + map.CENTER_LATITUDE; iR = map.map(iline, isample); vlat = -(vline - map.LINE_PROJECTION_OFFSET)/map.RES + map.CENTER_LATITUDE; vR = map.map(vline,isample); hlon = (hsample - map.SAMPLE_PROJECTION_OFFSET)/map.RES + map.CENTER_LONGITUDE; hR = map.map(iline,hsample); %Conversion from spherical coordinates to cartesian: [i_mcmf(1), i_mcmf(2), i_mcmf(3)] = sph2cart(ilon*pi/180, ilat*pi/180, iR); [v_mcmf(1), v_mcmf(2), v_mcmf(3)] = sph2cart(ilon*pi/180, vlat*pi/180, vR); [h_mcmf(1), h_mcmf(2), h_mcmf(3)] = sph2cart(hlon*pi/180, ilat*pi/180, hR); %Transformation (rotation only)from Mars-Centered Mars-Fixed to NED: v_ned = dcmpcpf2ned([ilat ilon])*(v_mcmf'-i_mcmf'); h_ned = dcmpcpf2ned([ilat ilon])*(h_mcmf'-i_mcmf'); %Vector normal to the plane defined by the three pixel centers: n_ned = cross(h_ned,v_ned); %X Lander body axis in NED coordinates located at the interception pixel: a = dcmbody2ned(attitude)*[1; 0; 0]; a = dcmpcpf2ned([lander_pos(2) lander_pos(1)])\a; a_ned = dcmpcpf2ned([ilat ilon])*a; %Interception angle in degrees taken from the dot product of the lander %attitude vector and surface normal vector: angle = 90 - acosd(dot(n_ned,a_ned)/(sqrt(n_ned(1)^2+n_ned(2)^2+n_ned(3)^2)*sqrt(a_ned(1)^2+a_ned(2)^2+a_ned(3)^2))); if n_ned(3) < 0 angle = -angle; end end