www.gusucode.com > Simulink——飞行器轨迹恢复源码程序 > Simulink——飞行器轨迹恢复源码程序/Aerial_recovery_demo/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.radius = R;
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.n0(i) = P.mothership.n;
P.cable.link.e0(i) = P.mothership.e;
P.cable.link.d0(i) = P.mothership.d + P.cable.length * (i)/P.cable.N ;
P.cable.link.ndot0(i) = 0;
P.cable.link.edot0(i) = 0;
P.cable.link.ddot0(i) = 0;
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.navigation_ratio = 3.5; % PRONAV navigation constants
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