gusucode.com > Simulink——飞行器轨迹恢复源码程序 > Simulink——飞行器轨迹恢复源码程序/Aerial_recovery_demo/camera.m
function [sys,x0,str,ts] = camera(t,x,u,flag,P) % % modified 11/30/2006 - Randy Beard % modified 05/04/2007 - Randy Beard switch flag, %%%%%%%%%%%%%%%%%% % Initialization % %%%%%%%%%%%%%%%%%% case 0, [sys,x0,str,ts] = mdlInitializeSizes(P); %%%%%%%%%% % Update % %%%%%%%%%% case 2, sys = mdlUpdate(t,x,u,P); %%%%%%%%%% % Output % %%%%%%%%%% case 3, sys = mdlOutput(t,x,u,P); %%%%%%%%%%%%% % Terminate % %%%%%%%%%%%%% case 9, sys = []; % do nothing %%%%%%%%%%%%%%%%%%%% % Unexpected flags % %%%%%%%%%%%%%%%%%%%% otherwise error(['unhandled flag = ',num2str(flag)]); end %end dsfunc % %======================================================================= % mdlInitializeSizes % Return the sizes, initial conditions, and sample times for the S-function. %======================================================================= % function [sys,x0,str,ts] = mdlInitializeSizes(P) sizes = simsizes; sizes.NumContStates = 0; M = size(P.drogue.markers,1); % number of identifying dots sizes.NumDiscStates = 1+... % initialize M+... % camera display handle 3*M; % camera data (eps_x, eps_y, eps_s) sizes.NumOutputs = 3*M; % camera data (eps_x, eps_y, eps_s) sizes.NumInputs = 9+... % drogue states 9; % mav states sizes.DirFeedthrough = 1; x0 = zeros(sizes.NumDiscStates,1); sizes.NumSampleTimes = 1; sys = simsizes(sizes); str = []; ts = [P.mav.T_cam 0]; % end mdlInitializeSizes % %======================================================================= % mdlUpdate % Handle discrete state updates, sample time hits, and major time step % requirements. %==============================2========================================= % function sys = mdlUpdate(t,x,uu,P) M = size(P.drogue.markers,1); % number of markers on drogue initialize = x(1); % initial graphics NN = 1; fig_cam = x(1+NN:M+NN); % target in camera frame NN = 0; drogue.n = uu(1+NN); drogue.e = uu(2+NN); drogue.d = uu(3+NN); drogue.ndot = uu(4+NN); drogue.edot = uu(5+NN); drogue.ddot = uu(6+NN); drogue.phi = uu(7+NN); drogue.theta = uu(8+NN); drogue.psi = uu(9+NN); NN = NN+9; mav.n = uu(1+NN); mav.e = uu(2+NN); mav.d = uu(3+NN); mav.psi = uu(4+NN); mav.V = uu(5+NN); mav.phi = uu(6+NN); mav.gam = uu(7+NN); mav.az = uu(8+NN); mav.el = uu(9+NN); % get pixel location for each drogue marker for i=1:M, tmp = Rot_v_to_b(drogue.phi, drogue.theta, drogue.psi)'*P.drogue.markers(i,:)'; dot.n = drogue.n + tmp(1); dot.e = drogue.e + tmp(2); dot.d = drogue.d + tmp(3); [eps_x(i), eps_y(i), eps_size(i)] = cameraProjection(mav, dot, P); end % plot target in the camera if initialize==0, % initialize the plot initialize = 1; figure(2), clf, hold on % plot camera view h=subplot('position',[0.1, 0.1, 0.8, 0.8]); tmp = P.cam.pix/2; set(h,'XAxisLocation','top','XLim',[-tmp,tmp],'YLim',[-tmp,tmp]); axis ij hold on for i=1:M, fig_cam(i) = blobPlot([eps_x(i), eps_y(i)], eps_size(i), [], 'red', 'normal'); end xlabel('px (pixels)') ylabel('py (pixels)') title('Camera View') else % do this at every time step for i=1:M, blobPlot([eps_x(i), eps_y(i)], eps_size(i), fig_cam(i)); end end sys = [initialize; fig_cam; eps_x'; eps_y'; eps_size']; %end mdlUpdate %======================================================================= % mdlOutput %==============================2========================================= % function sys = mdlOutput(t,x,uu,P) M = size(P.drogue.markers,1); sys = x(1+M+1:end); %======================================================================= % cameraProjection % project the target onto the camera %==============================2========================================= function [eps_x, eps_y, eps_size] = cameraProjection(mav, target, P) % target in the vehicle frame p_obj_v = [target.n; target.e; target.d] - [mav.n; mav.e; mav.d]; % tranform to the camera frame R_v_b = Rot_v_to_b(mav.phi,mav.gam,mav.psi); % vehicle to body R_b_g = Rot_b_to_g(mav.az,mav.el); % body to gimbal R_g_c = [... % gimbal to camera 0, 1, 0;... 0, 0, 1;... 1, 0, 0]; p_obj_c = (R_g_c * R_b_g * R_v_b) * p_obj_v; % convert to pixel location if p_obj_c(3)<.1, eps_x=-9999; eps_y=-9999; eps_size = 0; else eps_x = P.cam.f*(p_obj_c(1)/(p_obj_c(3)))+P.cam.pixelnoise*randn; eps_y = P.cam.f*(p_obj_c(2)/(p_obj_c(3)))+P.cam.pixelnoise*randn; eps_size = P.cam.f*(P.drogue.marker_size/(p_obj_c(3)))+P.cam.pixelnoise*randn; end % snap output of camera to -9999 if outside field of view tmp = P.cam.pix/2+eps_size; if eps_x<-tmp | eps_x>tmp | eps_y<-tmp | eps_y>tmp, eps_x = -9999; eps_y = -9999; eps_size = 0; end %======================================================================= % blobPlot % draw circle %==============================2========================================= function handle=blobPlot(z, size, handle, color, mode); th = 0:.1:2*pi; X = z(1)+ size*cos(th); Y = z(2)+ size*sin(th); if isempty(handle), handle = fill(X, Y, color, 'EraseMode', mode); else set(handle,'XData',X,'YData',Y); drawnow end %%%%%%%%%%%%%%%%%%%%%%% function R = Rot_v_to_b(phi,theta,psi); % Rotation matrix from body coordinates to vehicle coordinates Rot_v_to_v1 = [... cos(psi), sin(psi), 0;... -sin(psi), cos(psi), 0;... 0, 0, 1;... ]; Rot_v1_to_v2 = [... cos(theta), 0, -sin(theta);... 0, 1, 0;... sin(theta), 0, cos(theta);... ]; Rot_v2_to_b = [... 1, 0, 0;... 0, cos(phi), sin(phi);... 0, -sin(phi), cos(phi);... ]; R = Rot_v2_to_b * Rot_v1_to_v2 * Rot_v_to_v1; %%%%%%%%%%%%%%%%%%%%%%% function R = Rot_b_to_g(az,el); % Rotation matrix from body coordinates to gimbal coordinates Rot_b_to_g1 = [... cos(az), sin(az), 0;... -sin(az), cos(az), 0;... 0, 0, 1;... ]; Rot_g1_to_g = [... cos(el), 0, -sin(el);... 0, 1, 0;... sin(el), 0, cos(el);... ]; R = Rot_g1_to_g * Rot_b_to_g1;