Code covered by the BSD License

# Aerial Recovery Concept Demo (Gauss's Principle)

### Liang Sun (view profile)

28 Feb 2013 (Updated )

Demonstrate the concept of aerial recovery of miniature aerial vehicles using a towed-cable system.

param.m
```clear all
clc

P.ts = 0.2;

%=========================================================================
% physical parameters
P.g = 9.8;  % (m/s) gravity constant
P.rho  = 1.268200;  % air density
P.sound_speed = 340;   % sound speed (m/s)

%=========================================================================
% wind parameters  (currently not working with wind)
P.wind.n = 0; % North component of wind
P.wind.e = 0; % East component of wind

%=========================================================================
% mothership configuration

P.mothership.dir = -1; % -1=conter-clockwise; 1=clockwise
V = 50;  % mothership airspeed in m/s
R = 300;  % mothership radius in m
c = [0; 0]; % center of mothership orbit in m
h = 1000;  % altitude of mothership in m
P.mothership.n = c(1) + R; % initial North position of mothership
P.mothership.e = c(2);     % initial East position of mothership
P.mothership.d = -h;       % initial Down position of mothership
P.mothership.V = V;        % initial airspeed of mothership
P.mothership.chi = -pi/2;  % initial heading set up for counterclockwise orbit
P.mothership.phi = 0;      % initial roll angle
P.mothership.gam = 0;      % intial flight path angle
P.mothership.phibar = 45*(pi/180);           % (rad) roll angle constraint
P.mothership.phidotbar = 100*(pi/180);       % (rad/sec) roll rate constraint
P.mothership.R = V^2/P.g/tan(P.mothership.phibar)+5; % (m) min turn radius
P.mothership.gambar = 20*(pi/180);           % (rad) flight path constraint
P.mothership.gamdotbar = 60*(pi/180);        % (rad/sec) flight path rate constraint
P.mothership.T_guidance = P.ts;               % sample rate for mothership guidance loop
P.mothership.k_roll = 10;          % roll attitude hold proportional gain
P.mothership.k_h    = 1;           % proportional gain for altitude
P.mothership.k_hdot = 1;           % derivative gain for altitude
P.mothership.k_orbit = 0.01;  % proportional gain for tracking oribits
P.mothership.size = 0.2;   % size for display

%=========================================================================
% cable parameters

P.cable.N = 15;  % number of the cable links

P.cable.length = 900;  % cabel length in meters
P.cable_spring_1 = 0.05;  % spring constant of cable
P.cable_spring_2 = 0.001;  % velocity constraint spring constant of cable
P.cable.mass   = 1;    % cable mass
P.cable.straighten = 50; % force constatn that straightens the cable

P.cable.constraint_length = P.cable.length/P.cable.N;

if (P.cable.N>=2)
for i=1:(P.cable.N-1)
P.cable.link.d0(i) = P.mothership.d + P.cable.length * (i)/P.cable.N ;
end
end

P.Cf_last_joint = 0.5;
P.Cn_last_joint = 0.27;

%P.cable.rho = 10;   %cable density
P.cable.d   = 0.01;  %cable diameter
%=========================================================================

% drogue configuration
P.drogue.n = P.mothership.n;
P.drogue.e = P.mothership.e;
P.drogue.d = P.mothership.d + P.cable.length;
P.drogue.ndot = 0;
P.drogue.edot = 0;
P.drogue.ddot = 0;
P.drogue.phi = 0;
P.drogue.theta = 0;
P.drogue.psi = P.mothership.chi;
P.drogue.S  = 0.258900;  % wing area of drogue
P.drogue.mass = 5;         % 5 mass of drogue in kg
P.drogue.phibar = 45*pi/180; % maximum roll angle of drogue  30*pi/180
P.drogue.phidotbar = 10*pi/180; % maximum roll rate of drogue
P.drogue.thetabar = 20*pi/180; % 20 maximum pitch angle of drogue
P.drogue.thetadotbar = 5*pi/180; % maximum pitch rate of drogue
P.drogue.Vdotbar = 1;   % maximum acceleration of drogue

P.drogue.size = 0.15;   % size for display
P.drogue.markers = [...  % visual markers in the body frame
0, 0, 0;...
0, 50*P.drogue.size, 0;...
0, -50*P.drogue.size, 0;...
0, 0, -50*P.drogue.size/2;...
];
P.drogue.marker_size = 1;

P.drogue.phi_d   = 15*pi/180;
P.drogue.theta_d = 15*pi/180;
P.drogue.k_phi   = 5;
P.drogue.k_theta = 5;

%=========================================================================
% drogue autopilot parameters
P.drogue.T_guidance = P.ts;       % sample rate for guidance algorithm

%=========================================================================
% drogue aerodynamic coefficients for linear force model
P.drogue.CD_ctrl_flag = 0; % flag of using drag coefficient control
P.drogue.CL_0   = 0.280000;
P.drogue.CD_0   = 0.5;   %0.030000;
P.drogue.CDdotbar = 0.05;
P.drogue.CD_bar = 1.2;
P.drogue.CD_bar_lower = 0.3;
P.drogue.k_CD = 0.01;

%=========================================================================
% MAV configuration
D = 500;
P.mav.n   = P.mothership.n - D*cos(P.mothership.chi);
P.mav.e   = P.mothership.e - D*sin(P.mothership.chi);
P.mav.d   = P.drogue.d;
P.mav.chi = P.mothership.chi;
P.mav.V   = P.mothership.V/3;
P.mav.phi = 0;
P.mav.gam = 0;
P.mav.az  = 0;
P.mav.el  = 0;
% MAV performance parameters
P.mav.Vbar_up = 17;                   % (m/s) upper limit on velocity
P.mav.Vbar_low = 10;                  % (m/s) lower limit on velocity
P.mav.Vdotbar = 1;                    % (m/s^2) limit on velocity increase
P.mav.phibar = 45*(pi/180);           % (rad) roll angle constraint
P.mav.phidotbar = 100*(pi/180);       % (rad/sec) roll rate constraint
P.mav.R = P.mav.V^2/P.g/tan(P.mav.phibar)+5; % (m) min turn radius
P.mav.gambar = 20*(pi/180);           % (rad) flight path constraint
P.mav.gamdotbar = 60*(pi/180);        % (rad/sec) flight path rate constraint
% MAV gimbal parameters
P.mav.gim.elbar = 60*(pi/180);      % maximum gimbal elevation angle
P.mav.gim.eldotbar = 660*(pi/180);  % maximum rate of change of the gimbal elevation angle
P.mav.gim.azbar = 60*(pi/180);      % maximum gimbal azimuth angle
P.mav.gim.azdotbar = 330*(pi/180);  % maximum rate of change of the gimbal azimuth angle
P.mav.gim.az    = 0*(pi/180);       % strapdown azimuth angle
P.mav.gim.el    = 0*(pi/180);       % strapdown elevation angle
% size for display
P.mav.size = 0.1;

%=========================================================================
% camera parameters (on MAV)
P.cam.pix = 480;                                  % size of (square) pixel array
P.cam.fov   = 70*(pi/180);                        % field of view of camera
P.cam.f = (P.cam.pix/2)/tan(P.cam.fov/2);         % focal range
P.cam.pixelnoise = 0;                             % (pixels) - variance of the pixel noise
%
%=========================================================================
% MAV autopilot parameters
P.mav.T_guidance = P.ts;       % sample rate for guidance algorithm
P.mav.T_cam = 1*P.mav.T_guidance;  % sample rate for camera (needs to be integer value of T_guidance in current implimentation)
P.mav.k_V    = 10;             % proportional constant for airspeed
P.mav.k_roll = 10;             % roll attitude hold proportional gain
P.mav.k_pitch = 10;             % pitch gain used in direct pursuit algorithm
P.mav.k_yaw   = 1;             % yaw gain used in direct pursuit algorithm
P.mav.k_az   = 1;              % proportional constant for azimuth angle
P.mav.k_el   = 1;              % proportional constant for elevation angle
P.mav.sigmoid_gain = 0.2;      % sigmoid gain for roll control
P.mav.tau = 1/1;               % bandwidth of differentiator for line of sight rate and distance rate

%---------------------------------------------------------------------
% MAV Kalman filter parameters
P.mav.ekf.paz0 = .1;  % initial estimate of estimation error of eta_az in radians
P.mav.ekf.pel0 = .1;  % initial estimate of estimation error of eta_el in radians
P.mav.ekf.q_az = .01;  % process noise on eta_az
P.mav.ekf.q_el = .01;  % process noise on eta_el
P.mav.ekf.r_eps_x = 1; % measurement noise on x-pixel
P.mav.ekf.r_eps_y = 1; % measurement noise on y-pixel

%=========================================================================
% parameters for drawing UAVs
P.drawuav.scale = 0.2; % plot scale for UAV

% the origin is at the center of the fuselage
P.drawuav.h_fuse = 6.5;           % fuselage height (cm)
P.drawuav.w_fuse = 9.5;           % fuselage width (cm)
P.drawuav.l_fuse = 55.5;          % fuselage length (cm)
% wing coefficients
P.drawuav.w_recess = 6.5;
P.drawuav.w_height = 5.5;
P.drawuav.w_width = 71.5;
P.drawuav.w_tipwidth = 25;
P.drawuav.w_back = 30;
% rudder coefficients
P.drawuav.r_height = 21;
P.drawuav.r_top = 10;

%%%%%%%% parameters for drawing uav %%%%%%%%%%%%%%%%
P.uavscale = 0.2; % plot scale for UAV

% the origin is at the center of the fuselage
P.h_fuse = 6.5;           % fuselage height (cm)
P.w_fuse = 9.5;           % fuselage width (cm)
P.l_fuse = 55.5;          % fuselage length (cm)
% wing coefficients
P.w_recess = 6.5;
P.w_height = 5.5;
P.w_width = 71.5;
P.w_tipwidth = 25;
P.w_back = 30;
% rudder coefficients
P.r_height = 21;
P.r_top = 10;
% colors
P.myred = [1, 0, 0];
P.mygreen = [0, 1, 0];
P.myblue = [0, 0, 1];
P.myyellow = [1,1,0];
P.mymagenta   = [0, 1, 1];

% Plot parameters
P.fig_cable = zeros(1,P.cable.N);
P.fig_joint = zeros(1,(P.cable.N-1));

P.joint_size = 5;

H = 350;
P.x_max = H;
P.x_min = -H;
P.y_max = H;
P.y_min = -H;
P.z_max = 1050;
P.z_min = 50;   % 0

```