www.gusucode.com > Simulink,飞行器控制源码程序 > Simulink,飞行器控制源码程序/Aerial_recovery_demo/cable_drogue_dynamics.m
function [sys,x0,str,ts] = cable_drogue_dynamics(t,x,u,flag,P) persistent G; persistent drogue_CD; % Simple UAV Dynamics switch flag, %%%%%%%%%%%%%%%%%% % Initialization % %%%%%%%%%%%%%%%%%% case 0, [sys,x0,str,ts, G,drogue_CD]=mdlInitializeSizes(P); %%%%%%%%%%%%%%% % Derivatives % %%%%%%%%%%%%%%% case 1, [sys,drogue_CD]=mdlDerivatives(t,x,u, P, G,drogue_CD); %%%%%%%%%% % Update % %%%%%%%%%% case 2, sys=mdlUpdate(t,x,u); %%%%%%%%%%% % Outputs % %%%%%%%%%%% case 3, sys=mdlOutputs(t,x,u,P,drogue_CD); %%%%%%%%%%%%%%%%%%%%%%% % GetTimeOfNextVarHit % %%%%%%%%%%%%%%%%%%%%%%% case 4, sys=mdlGetTimeOfNextVarHit(t,x,u); %%%%%%%%%%%%% % Terminate % %%%%%%%%%%%%% case 9, sys=mdlTerminate(t,x,u); %%%%%%%%%%%%%%%%%%%% % Unexpected flags % %%%%%%%%%%%%%%%%%%%% otherwise error(['Unhandled flag = ',num2str(flag)]); end % end sfuntmpl % %============================================================================= % mdlInitializeSizes % Return the sizes, initial conditions, and sample times for the S-function. %========================================================================== %=== % function [sys,x0,str,ts, G,drogue_CD]=mdlInitializeSizes(P) % % call simsizes for a sizes structure, fill it in and convert it to a % sizes array. % % Note that in this example, the values are hard coded. This is not a % recommended practice as the characteristics of the block are typically % defined by the S-function parameters. % sizes = simsizes; sizes.NumContStates = 3*(P.cable.N -1) ... % cable position + 3*(P.cable.N -1) ... % cable velocity + 3 ... % drogue position, + 3 ... % drogue velocity, + 2; % drogue roll, pitch, sizes.NumDiscStates = 0; sizes.NumOutputs = 9 +... % drogue states + 6*(P.cable.N-1) ... % cable states + 1 ...% cable length +1 ; % drogue drag coefficient sizes.NumInputs = 9 ... % mothership states + 3; % drogue control inputs (roll rate, pull-up force, velocity) sizes.DirFeedthrough = 1; sizes.NumSampleTimes = 1; % at least one sample time is needed sys = simsizes(sizes); % % initialize the initial conditions % x0 = []; if (P.cable.N>=2) for i=1:(P.cable.N-1) x0 = [x0;... P.cable.link.n0(i);... P.cable.link.e0(i);... P.cable.link.d0(i);... P.cable.link.ndot0(i);... P.cable.link.edot0(i);... P.cable.link.ddot0(i);... ]; end end x0 = [x0;... P.drogue.n;... P.drogue.e;... P.drogue.d;... P.drogue.ndot;... P.drogue.edot;... P.drogue.ddot;... P.drogue.phi;... P.drogue.theta;... ]; % Gauss's Principle parameters G.A = zeros(P.cable.N,3*P.cable.N); G.M = diag([P.cable.mass/P.cable.N*ones(3*(P.cable.N-1),1);P.drogue.mass*ones(3,1)]); G.a_u = zeros(3*P.cable.N, 1); G.b = zeros(P.cable.N, 1); G.phi = zeros(P.cable.N, 1); G.phi_diff = zeros(3*P.cable.N, P.cable.N); G.psi = zeros(P.cable.N, 1); G.psi_diff = zeros(3*P.cable.N, P.cable.N); drogue_CD = P.drogue.CD_0; % % str is always an empty matrix % str = []; % % initialize the array of sample times % ts = [0 0]; % end mdlInitializeSizes % %============================================================================= % mdlDerivatives % Return the derivatives for the continuous states. %============================================================================= % function [sys,drogue_CD]=mdlDerivatives(t,x,uu,P,G,drogue_CD) sys = []; % mothership states NN = 0; mothership.n = uu(1+NN); mothership.e = uu(2+NN); mothership.d = uu(3+NN); mothership.chi = uu(4+NN); mothership.phi = uu(5+NN); mothership.gam = uu(6+NN); mothership.nddot = uu(7+NN); mothership.eddot = uu(8+NN); mothership.dddot = uu(9+NN); % control inputs to drogue NN = 9; u_phi = uu(1+NN); u_theta = uu(2+NN); u_delta_CD = uu(3+NN); % drogue states NN = 6*(P.cable.N -1); drogue.n = x(1+NN); drogue.e = x(2+NN); drogue.d = x(3+NN); drogue.vn = x(4+NN); drogue.ve = x(5+NN); drogue.vd = x(6+NN); drogue.phi = x(7+NN); drogue.theta = x(8+NN); if (P.cable.N>=2) % multiple links are used % cable states for i = 1:(P.cable.N-1) NN = 6*(i-1); cable.n(i) = x(1+NN); cable.e(i) = x(2+NN); cable.d(i) = x(3+NN); cable.vn(i) = x(4+NN); cable.ve(i) = x(5+NN); cable.vd(i) = x(6+NN); end %%%%%%%%%%%% dynamics of the 1st link which connects to the mothership NN = 1; ell = [... cable.n(NN) - mothership.n;... cable.e(NN) - mothership.e;... cable.d(NN) - mothership.d;... ]; G.phi(NN) = norm(ell)^2 - P.cable.constraint_length^2; G.phi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = ell/norm(ell); % line-of-sight rate elldot = [... cable.vn(NN) - P.mothership.V*cos(mothership.chi)*cos(mothership.gam);... cable.ve(NN) - P.mothership.V*sin(mothership.chi)*cos(mothership.gam);... cable.vd(NN) + P.mothership.V*sin(mothership.gam);... ]; G.psi(NN) = ell'/norm(ell) * elldot; G.psi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = elldot; % unconstrained drogue acceleration due to gravity g = [0; 0; P.g]; F_straighten = [0;0;0]; % aerodynamic forces F_aeroforce = aeroforce(P.rho, -ell, P.cable.d, [cable.vn(NN);cable.ve(NN);cable.vd(NN)], P); % totally applied forces F_total = F_straighten + F_aeroforce; % vector "A" of the Gauss's Principle G.A(NN,(3*(NN-1)+1):(3*(NN-1)+3)) = ell'; % unconstrained acceleration G.a_u((3*(NN-1)+1):(3*(NN-1)+3),1) = g + F_total/P.cable.mass/P.cable.N; % vector "b" of the Gauss's Principle G.b (NN,1) = - norm(elldot)^2 +... ell'*[mothership.nddot;mothership.eddot;mothership.dddot]; %%%%%%%%%%%%%%% dynamics of the 2nd-nth links if P.cable.N == 2 %%% the nth joint is drogue NN = 2; ell = [... drogue.n- cable.n(NN-1);... drogue.e- cable.e(NN-1);... drogue.d- cable.d(NN-1)]; G.phi(NN) = norm(ell)^2 - P.cable.constraint_length^2; G.phi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = ell/norm(ell); G.phi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -ell/norm(ell); elldot = [... drogue.vn- cable.vn(NN-1);... drogue.ve- cable.ve(NN-1);... drogue.vd- cable.vd(NN-1)]; G.psi(NN) = ell'/norm(ell) * elldot; G.psi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = elldot; G.psi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -elldot; F_straighten = [0;0;0]; % drag force on drogue V = norm([drogue.vn; drogue.ve; drogue.vd]); drogue_CD = drogue_CD + u_delta_CD; drogue_CD = sat2side(drogue_CD,P.drogue.CD_bar,P.drogue.CD_bar_lower); D_drogue = 0.5*P.rho*V*[drogue.vn; drogue.ve; drogue.vd]*P.drogue.S * (drogue_CD); % lift force on drogue psi = atan2(ell(2),ell(1)); u_F = 0; L_drogue = Rot_v_to_b(drogue.phi,drogue.theta,psi)'*[0; 0; -u_F]; % unconstrained acceleration G.a_u((3*(NN-1)+1):(3*(NN-1)+3),1) = g + F_straighten/P.cable.mass/P.cable.N + ... 1/P.drogue.mass*(L_drogue - D_drogue); % vector "A" of the Gauss's Principle G.A(NN,(3*(NN-2)+1):(3*(NN-2)+3)) = - ell'; G.A(NN,(3*(NN-1)+1):(3*(NN-1)+3)) = ell'; % vector "b" of the Gauss's Principle G.b (NN,1) = - norm(elldot)^2; else %%%% dynamics of the second link NN = 2; ell = [... cable.n(NN)- cable.n(NN-1);... cable.e(NN)- cable.e(NN-1);... cable.d(NN)- cable.d(NN-1)]; G.phi(NN) = norm(ell)^2 - P.cable.constraint_length^2; G.phi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = ell/norm(ell); G.phi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -ell/norm(ell); elldot = [... cable.vn(NN)- cable.vn(NN-1);... cable.ve(NN)- cable.ve(NN-1);... cable.vd(NN)- cable.vd(NN-1)]; G.psi(NN) = ell'/norm(ell) * elldot; G.psi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = elldot; G.psi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -elldot; if (P.cable.N - NN) <2 F_straighten = [0;0;0]; elseif (P.cable.N - NN) ==2 strait_dir_1 = [... cable.n(NN) - mothership.n;... cable.e(NN) - mothership.e;... cable.d(NN) - mothership.d;... ]; strait_dir_2 = [... cable.n(NN)- drogue.n;... cable.e(NN)- drogue.e;... cable.d(NN)- drogue.d]; F_straighten = P.cable.straighten * (strait_dir_1/norm(strait_dir_1)... + strait_dir_2/norm(strait_dir_2)); else strait_dir_1 = [... cable.n(NN) - mothership.n;... cable.e(NN) - mothership.e;... cable.d(NN) - mothership.d;... ]; strait_dir_2 = [... cable.n(NN)- cable.n(NN+2);... cable.e(NN)- cable.n(NN+2);... cable.d(NN)- cable.n(NN+2)]; F_straighten = P.cable.straighten * (strait_dir_1/norm(strait_dir_1)... + strait_dir_2/norm(strait_dir_2)); end % aerodynamic forces F_aeroforce = aeroforce(P.rho, -ell, P.cable.d, [cable.vn(NN);cable.ve(NN);cable.vd(NN)], P); % totally applied forces F_total = F_straighten + F_aeroforce; % unconstrained acceleration G.a_u((3*(NN-1)+1):(3*(NN-1)+3),1) = g + F_total/P.cable.mass/P.cable.N; % vector "A" of the Gauss's Principle G.A(NN,(3*(NN-2)+1):(3*(NN-2)+3)) = - ell'; G.A(NN,(3*(NN-1)+1):(3*(NN-1)+3)) = ell'; % vector "b" of the Gauss's Principle G.b (NN,1) = - norm(elldot)^2; %%%%%%%%%%% dynamics of the 3rd~(n-1)th links for i = 3:(P.cable.N-1) NN = i; ell = [... cable.n(NN)- cable.n(NN-1);... cable.e(NN)- cable.e(NN-1);... cable.d(NN)- cable.d(NN-1)]; G.phi(NN) = norm(ell)^2 - P.cable.constraint_length^2; G.phi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = ell/norm(ell); G.phi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -ell/norm(ell); elldot = [... cable.vn(NN)- cable.vn(NN-1);... cable.ve(NN)- cable.ve(NN-1);... cable.vd(NN)- cable.vd(NN-1)]; G.psi(NN) = ell'/norm(ell) * elldot; G.psi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = elldot; G.psi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -elldot; if (P.cable.N - NN) <2 F_straighten = [0;0;0]; elseif (P.cable.N - NN) ==2 strait_dir_1 = [... cable.n(NN)- cable.n(NN-2);... cable.e(NN)- cable.n(NN-2);... cable.d(NN)- cable.n(NN-2)]; strait_dir_2 = [... cable.n(NN)- drogue.n;... cable.e(NN)- drogue.e;... cable.d(NN)- drogue.d]; F_straighten = P.cable.straighten * (strait_dir_1/norm(strait_dir_1)... + strait_dir_2/norm(strait_dir_2)); else strait_dir_1 = [... cable.n(NN)- cable.n(NN-2);... cable.e(NN)- cable.n(NN-2);... cable.d(NN)- cable.n(NN-2)]; strait_dir_2 = [... cable.n(NN)- cable.n(NN+2);... cable.e(NN)- cable.n(NN+2);... cable.d(NN)- cable.n(NN+2)]; F_straighten = P.cable.straighten * (strait_dir_1/norm(strait_dir_1)... + strait_dir_2/norm(strait_dir_2)); end if NN ~= (P.cable.N-1) % aerodynamic forces F_aeroforce = aeroforce(P.rho, -ell, P.cable.d, [cable.vn(NN);cable.ve(NN);cable.vd(NN)], P); else % enlarge the drag of the last joint F_aeroforce = aeroforce(P.rho, -ell, P.cable.d, [cable.vn(NN);cable.ve(NN);cable.vd(NN)], P); end %_last_joint % totally applied forces F_total = F_straighten + F_aeroforce; % unconstrained acceleration G.a_u((3*(NN-1)+1):(3*(NN-1)+3),1) = g + F_total/P.cable.mass/P.cable.N; % vector "A" of the Gauss's Principle G.A(NN,(3*(NN-2)+1):(3*(NN-2)+3)) = - ell'; G.A(NN,(3*(NN-1)+1):(3*(NN-1)+3)) = ell'; % vector "b" of the Gauss's Principle G.b (NN,1) = - norm(elldot)^2; end %%%%% dynamics of the nth link i.e. the drogue NN = P.cable.N; ell = [... drogue.n- cable.n(NN-1);... drogue.e- cable.e(NN-1);... drogue.d- cable.d(NN-1)]; G.phi(NN) = norm(ell)^2 - P.cable.constraint_length^2; G.phi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = ell/norm(ell); G.phi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -ell/norm(ell); elldot = [... drogue.vn- cable.vn(NN-1);... drogue.ve- cable.ve(NN-1);... drogue.vd- cable.vd(NN-1)]; G.psi(NN) = ell'/norm(ell) * elldot; G.psi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = elldot; G.psi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -elldot; F_straighten = [0;0;0]; % drag force on drogue V = norm([drogue.vn; drogue.ve; drogue.vd]); drogue_CD = drogue_CD+u_delta_CD; drogue_CD = sat2side(drogue_CD,P.drogue.CD_bar,P.drogue.CD_bar_lower); D_drogue = 0.5*P.rho*V*[drogue.vn; drogue.ve; drogue.vd]*P.drogue.S * (drogue_CD); % lift force on drogue psi = atan2(ell(2),ell(1)); u_F = 0.5*P.rho*V^2*P.drogue.S * (P.drogue.CL_0 ); % + u_delta_CL L_drogue = Rot_v_to_b(drogue.phi,drogue.theta,psi)'*[0; 0; -u_F]; % unconstrained acceleration G.a_u((3*(NN-1)+1):(3*(NN-1)+3),1) = g + F_straighten/P.cable.mass/P.cable.N + ... 1/P.drogue.mass*(L_drogue - D_drogue); % vector "A" of the Gauss's Principle G.A(NN,(3*(NN-2)+1):(3*(NN-2)+3)) = - ell'; G.A(NN,(3*(NN-1)+1):(3*(NN-1)+3)) = ell'; % vector "b" of the Gauss's Principle G.b (NN,1) = - norm(elldot)^2; end phidot = lim_sat(drogue.phi, u_phi, P.drogue.phibar, P.drogue.phidotbar); % u_phi is the commanded roll rate thetadot = lim_sat(drogue.theta, u_theta, P.drogue.thetabar, P.drogue.thetadotbar); %-u_F/(V+.1)/P.drogue.mass %%%% compute the matrix that makes sure the constraints not violate constrained_acceleration = G.a_u + G.M^(-.5) * pinv(G.A*G.M^(-.5)) * (G.b - G.A * G.a_u) - P.cable_spring_1* G.phi_diff * G.phi ... - P.cable_spring_2* G.psi_diff * G.psi; for i = 1:(P.cable.N-1) sys(6*(i-1)+1:6*(i-1)+6,1) = [cable.vn(i);cable.ve(i);cable.vd(i);constrained_acceleration(3*(i-1)+1:3*(i-1)+3)]; end i = P.cable.N; sys(6*(i-1)+1:6*(i-1)+6,1) = [drogue.vn;drogue.ve;drogue.vd;constrained_acceleration(3*(i-1)+1:3*(i-1)+3)]; sys = [sys;phidot;thetadot]; else %% single link is used n = drogue.n; e = drogue.e; d = drogue.d; vn = drogue.vn; ve = drogue.ve; vd = drogue.vd; phi = drogue.phi; theta = drogue.theta; % compute airspeed V = norm([vn; ve; vd]); % angular_rate = V / norm([n,e]) % line of sight vector ell = [... mothership.n - n;... mothership.e - e;... mothership.d - d;... ]; % line-of-sight rate elldot = [... P.mothership.V*cos(mothership.chi)*cos(mothership.gam) - vn;... P.mothership.V*sin(mothership.chi)*cos(mothership.gam) - ve;... -P.mothership.V*sin(mothership.gam) - vd;... ]; % unconstrained drogue acceleration due to gravity g = [0; 0; P.g]; % lift force on drogue psi = atan2(ell(2),ell(1)); u_F = 0.5*P.rho*V^2*P.drogue.S * (P.drogue.CL_0 ); % + u_delta_CL L_drogue = Rot_v_to_b(phi,theta,psi)'*[0; 0; -u_F]; % drag force on drogue drogue_CD = drogue_CD+u_delta_CD; drogue_CD = sat2side(drogue_CD,P.drogue.CD_bar,P.drogue.CD_bar_lower); D_drogue = 0.5*P.rho*V*[vn; ve; vd]*P.drogue.S * (drogue_CD); % -u_delta_CD % unconstrained acceleration of drogue (i.e., without cable) a_unconstrained = g + 1/P.drogue.mass*(L_drogue - D_drogue) ; % constrained acceleration of the drogue based on Gauss's Principle a_constrained = (eye(3)-ell*ell'/P.cable.length^2)*a_unconstrained... + ell*ell'/P.cable.length^2*[mothership.nddot; mothership.eddot; mothership.dddot]... + ell/P.cable.length^2*norm(elldot)^2; %norm(ell)^2 % drogue dynamics ndot = vn; edot = ve; ddot = vd; nddot = a_constrained(1); eddot = a_constrained(2); dddot = a_constrained(3); phidot = lim_sat(phi, u_phi, P.drogue.phibar, P.drogue.phidotbar); % u_phi is the commanded roll rate thetadot = lim_sat(theta, u_theta, P.drogue.thetabar, P.drogue.thetadotbar); %-u_F/(V+.1)/P.drogue.mass % The commanded pitch rate is u_F/V/m sys = [ndot; edot; ddot; nddot; eddot; dddot; phidot; thetadot]; end % end mdlDerivatives % %============================================================================= % mdlUpdate % Handle discrete state updates, sample time hits, and major time step % requirements. %============================================================================= % function sys=mdlUpdate(t,x,u) sys = []; % end mdlUpdate % %============================================================================= % mdlOutputs % Return the block outputs. %============================================================================= % function sys=mdlOutputs(t,x,uu,P,drogue_CD) % second last joint states NN = 6*(P.cable.N -2); n = x(1+NN); e = x(2+NN); d = x(3+NN); NN = 6*(P.cable.N -1); % drogue states drogue.n = x(1+NN); drogue.e = x(2+NN); drogue.d = x(3+NN); % line of sight vector ell = [... drogue.n - n;... drogue.e - e;... drogue.d - d;... ]; drogue_psi = atan2(ell(2),ell(1)); sys = [x;drogue_psi;norm(ell);drogue_CD]; % output all UAV states % end mdlOutputs % %============================================================================= % mdlGetTimeOfNextVarHit % Return the time of the next hit for this block. Note that the result is % absolute time. Note that this function is only used when you specify a % variable discrete-time sample time [-2 0] in the sample time array in % mdlInitializeSizes. %============================================================================= % function sys=mdlGetTimeOfNextVarHit(t,x,u) sampleTime = 1; % Example, set the next hit to be one second later. sys = t + sampleTime; % end mdlGetTimeOfNextVarHit % %============================================================================= % mdlTerminate % Perform any end of simulation tasks. %============================================================================= % function sys=mdlTerminate(t,x,u) sys = []; % end mdlTerminate % %============================================================================= % lim_sat % the integration variable is limited to be within a certain interval and % the rate of increase is saturated %============================================================================= % function state_dot = lim_sat(state, input, state_limit, rate_limit) if ((state>=state_limit)&(input>0))|((state<=-state_limit)&( input<0)) state_dot = 0; else state_dot = sat(input, rate_limit); end % %============================================================================= % sat % saturate the input u at the constraint c %============================================================================= % function out = sat(u,c) if u>c, out=c; elseif u<-c, out=-c; else out=u; end % function out = sat2side(u,upper_limit,lower_limit) if u > upper_limit out = upper_limit; elseif u<lower_limit out = lower_limit; else out = u; end % %============================================================================= % aeroforce % compute the aerodynamic lift and aerodynamic drag of the cable %============================================================================= % function out = aeroforce(rho, L, d, v, P) if v~=0 length = norm(L); attack_angle = acos(dot(L,v)/(length*norm(v))); mach = norm(v)/P.sound_speed; mach_p = mach * cos(attack_angle); mach_n = mach * sin(attack_angle); if mach_p<0.4 Cf = 0.038 - 0.0425*mach_p; else Cf = 0.013 + 0.0395*(mach_p-0.085)^2; end Cn = 1.17 + mach_n/40 - mach_n^2/4 + 5*mach_n^3/8; CD = Cf + Cn*(sin(attack_angle))^3; CL = Cn * sin(attack_angle)^2 * cos(attack_angle); temp = cross(cross(v,L),v); e_L = - temp/norm(temp); aerodrag = - .5 * rho * length * d * norm(v)*v * CD; aerolift = .5 * rho * length * d * norm(v)^2 * e_L * CL; out = aerodrag + aerolift; else out = [0;0;0]; end %============================================================================= % aeroforce % compute the aerodynamic lift and aerodynamic drag of the cable %============================================================================= % function out = aeroforce_last_joint(rho, L, d, v, P) if v~=0 length = norm(L); attack_angle = acos(dot(L,v)/(length*norm(v))); CD = P.Cf_last_joint + P.Cn_last_joint*(sin(attack_angle))^3; CL = P.Cn_last_joint * sin(attack_angle)^2 * cos(attack_angle); temp = cross(cross(v,L),v); e_L = - temp/norm(temp); aerodrag = - .5 * rho * length * d * norm(v)*v * CD; aerolift = .5 * rho * length * d * norm(v)^2 * e_L * CL; out = aerodrag + aerolift; else out = [0;0;0]; end %%%%%%%%%%%%%%%%%%%%%% function R = Rot_v_to_b(phi,theta,psi); % Rotation matrix from vehicle coordinates to body 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_w_to_i(gam,chi) % Rotation matrix from wind frame to inertial frame temp_a = [cos(chi) -sin(chi) 0;... sin(chi) cos(chi) 0;... 0 0 1]; temp_b = [cos(gam) 0 sin(gam);... 0 1 0;... -sin(gam) 0 cos(gam)]; R = temp_a * temp_b;