-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathinit_quadsim_params.m
executable file
·121 lines (102 loc) · 5.75 KB
/
init_quadsim_params.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
% init_quadsim_params.m
%
% Initialize parameters structure (P) for quadsim
%
% Inputs:
% None
%
% Outputs:
% P: parameters structure used for quadsim
%
% Developed for JHU EP 525.461, UAV Systems & Control
% Adapted from design project in "Small Unmanned Aircraft: Theory and
% Practice", RWBeard & TWMcClain, Princeton Univ. Press, 2012
%
function P = init_quadsim_params
P = [];
% Acquire UAV vehicle model params
% e.g. P.mass_kg, P.C_L_alpha, etc.
quad_props
P.Ts = 0.01; % Autopilot sample time step, s
P.Tlog = 0.01; % Logging time step, s
P.Tvis = 0.2; % Visualization time step, s
P.manual_flight_flag=0; % Perform manually controlled flight
% environment params
P.gravity = 9.80665; % m/s^2
P.rho = 1.2682; % air density, kg/m^3
P.h0_ASL = 123; % height above sea level of NED reference location, m (used in static pressure measurement)
P.air_temp_F = 72; % air temperature, Fahrenheit (used in static pressure measurement)
% wind parameters
P.wind_n = 0; % North component of steady wind vector, m/s
P.wind_e = 0; % East component of steady wind vector, m/s
P.wind_d = 0; % Down component of steady wind vector, m/s
P.L_wx = 200; % Dryden gusting spatial wavelength along body x, m
P.L_wy = 200; % Dryden gusting spatial wavelength along body y, m
P.L_wz = 50; % Dryden gusting spatial wavelength along body z, m
P.sigma_wx = 1.06; % Dryden intensity along body x, m/s
P.sigma_wy = 1.06; % Dryden intensity along body y, m/s
P.sigma_wz = 0.7; % Dryden intensity along body z, m/s
P.Va_dryden = 10; % Airspeed parameter for modified Dryden, m/s
% control limit parameters
P.delta_e_max = 1; % Max allowable elevator channel command, -1 to 1
P.delta_a_max = 1; % Max allowable aileron channel command, -1 to 1
P.delta_r_max = 1; % Max allowable rudder channel command, -1 to 1
P.theta_max = 25*pi/180; % Max allowable pitch command, rad
P.phi_max = 25*pi/180; % Max allowable roll command, rad
% specify nominal launch height
P.h0_m = 50; % launch altitude, m
% first cut at initial conditions
% Dynamic States
P.pn0 = 0; % initial North position, m
P.pe0 = 0; % initial East position, m
P.pd0 = -P.h0_m; % initial Down position (negative altitude), m
P.u0 = 0; % initial ground-relative velocity along body x-axis, m/s
P.v0 = 0; % initial ground-relative velocity along body y-axis, m/s
P.w0 = 0; % initial ground-relativevelocity along body z-axis, m/s
P.phi0 = 0; % initial roll angle, rad, (+rightWingDown)
P.theta0 = 0; % initial pitch angle, rad (+noseUp)
P.psi0 = 0; % initial yaw angle, rad (+EastOfNorth)
P.p0 = 0; % initial body frame roll rate, rad/s
P.q0 = 0; % initial body frame pitch rate, rad/s
P.r0 = 0; % initial body frame yaw rate, rad/s
P.delta_e0 = 0; % initial elevator channel, -1 to 1
P.delta_a0 = 0; % initial aileron channel, -1 to 1
P.delta_r0 = 0; % initial rudder channel, -1 to 1
P.delta_t0 = 0; % initial throttle value, 0-1
%
% The following parameters will be used later in the course when we
% develop sensor models and state estimation algorithms.
%
% Note: A quad probably doesn't have a pitot tube measuring
% differential pressure (in order to estimate airspeed). But, we've
% left the differential pressure parameters in this file to make it
% easier for students to copy the "sensors" and "estimates" modules
% from UAVSIM.
% Sensor parameters: Gyro and Accelerometer
P.sigma_noise_gyro = .015*sqrt(80)*pi/180; % rad/s
P.sigma_noise_accel = 250*sqrt(100)/1e6*P.gravity; % m/s^2
% Sensor parameters: Pressure Sensors
kPa_to_Npm2 = 1000; % 1 Pa = 1 N/m^2, so 1 kPa = 1000 N/m^2
P.sigma_bias_static_press = 0.04*kPa_to_Npm2; % Static pressure measurement bias std. dev., N/m^2
P.sigma_noise_static_press = 0.01*kPa_to_Npm2; % Static pressure measurement noise std. dev., N/m^2
P.sigma_bias_diff_press = 0.020*kPa_to_Npm2; % Differential pressure measurement bias std. dev., N/m^2
P.sigma_noise_diff_press = 0.002*kPa_to_Npm2; % Differential pressure measurement noise std. dev., N/m^2
% Sensor parameters: Magnetometer
P.sigma_bias_mag = 1*pi/180; % Magnetometer bias std. dev., rad
P.sigma_noise_mag = 0.3*pi/180; % Magnetometer noise std. dev., rad
% Sensor parameters: GPS
P.Ts_gps = 1; % GPS sampling time, s
P.sigma_bias_gps_north = 4.7; % GPS north Gauss-Markov bias parameter, m
P.sigma_bias_gps_east = 4.7; % GPS east Gauss-Markov bias parameter, m
P.sigma_bias_gps_alt = 9.2; % GPS alt Gauss-Markov bias parameter, m
P.sigma_eta_gps_north = 0.21; % GPS north Gauss-Markov noise parameter, m
P.sigma_eta_gps_east = 0.21; % GPS east Gauss-Markov noise parameter, m
P.sigma_eta_gps_alt = 0.40; % GPS alt Gauss-Markov noise parameter, m
P.tau_gps = 1100; % GPS Gauss-Markov time constant, s (In book: k_gps = 1/tau_gps)
P.sigma_noise_gps_speed = 0.1; % GPS groundspeed std. dev., m/s
% State estimation parameters
P.tau_static_press = 0.5; % Time constant for autopilot's static pressure LPF, seconds
P.tau_diff_press = 0.25; % Time constant for autopilot's differential pressure LPF, seconds
P.tau_gyro = 0.05; % Time constant for autopilot's gyros, seconds
P.tau_accel = 0.05; % Time constant for autopilot's accelerometers, seconds
P.tau_mag = 0.2; % Time constant for autopilot's magnetometer, seconds