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;
