Something went wrong on our end
-
Austin Rohlfing authoredAustin Rohlfing authored
modelParameters.m 13.75 KiB
% Contains all the measurable parameters of the quadrotor. Must be run
% before running the simulation or any other functions in this directory
% Add Library Blocks and 3D animation to Path
path_1 = genpath('Library_blocks');
path_2 = genpath('3D_Animation');
addpath(path_1, path_2);
% Log Analysis Toggle
logAnalysisToggle = 0; % 1 for log analysis, 0 for normal operation
% Physics Verification Toggle
physicsVerificationToggle = 1;
% Define Simulink Runtime (if logAnalysisToggle is selected, this will be
% automatically set based on the log files time)
runtime = 40;
% Model Parameters
m = 1.216; % Quadrotor + battery mass
g = 9.81; % Acceleration of gravity
Jxx = 0.0110;%0.0130; % Quadrotor and battery motor of inertia around bx (pitch)
Jyy = 0.0116;%0.0140; % Quadrotor and battery motor of inertia around by (roll)
Jzz = 0.0223;%0.0285; % Quadrotor and battery motor of inertia around bz (yaw)
Jreq = 4.2012e-05; % Rotor and motor moment of inertia around axis of rotation
Kt = 1.2007e-05; % Rotor thrust constant
delta_T = [ 0, 0, 9.404e-04 ]; % Thrust constant adjustment factor
Kh = 3.4574e-04; % Rotor in-plane drag constant
Kd = 1.4852e-07; % Rotor drag constant
rhx = 0.16; % X-axis distance from center of mass to a rotor hub
rhy = 0.16; % Y-axis distance from center of mass to a rotor hub
rhz = 0.03; % Z-axis distance from center of mass to a rotor hub
r_oc = [0; 0; 0]; % Vector from origin to center of mass
Rm = 0.235; % Motor resistance
Kq = 96.3422; % Motor torque constant
Kv = 96.3422; % Motor back emf constant
If = 0.3836; % Motor internal friction current
Pmin = 0; % Minimum zybo output duty cycle command
Pmax = 1; % Maximum zybo output duty cycle command
Tc = 0.01;%0.04; % Camera system sampling period
Tq = 0.005; % Quad sampling period
tau_c = 0; % Camera system total latency
Vb = 12.3; % Nominal battery voltage (V)
x_controlled_o = 0; % Equilibrium lateral controller output
y_controlled_o = 0; % Equilibrium longitudinal controller output
yaw_controlled_o = 0; % Equilibrium yaw controller output
Kp_mahony = 0.4; % Proportional term for mahony filter
Ki_mahony = 0.001; % Integral term for mahony filter
% Define Biquad Filter Parameters
accel_Fc = 10;
Fs = 1/Tq;
accel_K = tan(pi * accel_Fc / Fs);
Q = 0.707;
accel_norm = 1.0 / (1.0 + accel_K/Q + accel_K*accel_K);
accel_b0 = accel_K*accel_K*accel_norm;
accel_b1 = 2.0*accel_b0;
accel_b2 = accel_b0;
accel_a0 = 1;
accel_a1 = 2.0*(accel_K*accel_K -1) * accel_norm;
accel_a2 = (1.0 - accel_K/Q + accel_K*accel_K) * accel_norm;
accel_SOS_Matrix = [accel_b0, accel_b1, accel_b2, accel_a0, accel_a1, accel_a2];
OF_Fc = 7;
OF_K = tan(pi * OF_Fc / Fs);
OF_norm = 1.0 / (1.0 + OF_K/Q + OF_K*OF_K);
OF_b0 = OF_K*OF_K*OF_norm;
OF_b1 = 2.0*OF_b0;
OF_b2 = OF_b0;
OF_a0 = 1;
OF_a1 = 2.0*(OF_K*OF_K -1) * OF_norm;
OF_a2 = (1.0 - OF_K/Q + OF_K * OF_K) * OF_norm;
OF_SOS_Matrix = [OF_b0, OF_b1, OF_b2, OF_a0, OF_a1, OF_a2];
% Determine Initial Conditions
% Position Initial Conditions
x_o = 0;
y_o = 0;
z_o = 0;
% Velocity Initial Conditions
x_vel_o = 0;
y_vel_o = 0;
z_vel_o = 0;
% Euler Angle Initial Conditions
roll_o = 0;
pitch_o = 0;
yaw_o = 0;
% Euler Rate Initial Conditions
rollrate_o = 0;
pitchrate_o = 0;
yawrate_o = 0;
% Equilibrium rotor speeds
omega0_o = sqrt((m*g)/(4*Kt));
omega1_o = sqrt((m*g)/(4*Kt));
omega2_o = sqrt((m*g)/(4*Kt));
omega3_o = sqrt((m*g)/(4*Kt));
% Equilibrium height controller output
height_controlled_o = (((Rm*If ...
+ (((omega0_o * 2 * Rm * Kv * Kq ...
* Kd + 1)^2) - 1)/(4* Rm*Kv^2*Kq ...
* Kd))/Vb)*(Pmax - Pmin) + Pmin);
if logAnalysisToggle == 1
%%%%%% Commented out section until logging is .txt file based %%%%%%
% FNAME
% if you know the name of the log file that you want to parse, set the it
% here only if it is in the working directory. Otherwise, you may leave it
% blank. You will be able to choose the file to parse through an explorer
% window.
%
%fname = 'sampleLogFile.txt';
fname = '';
fpath = 'C:\Users\Andy\Documents\School\MicroCART\GitRepo\MicroCART_17-18\controls\model\logFiles\';
if(isempty(fname))
[fname] = uigetfile('.txt','Select log file', fpath);
end
params.file.name = fname; % file name only
params.file.path = fpath; % file path only
params.file.pathName = [fpath fname]; % file path with file name
[dataStruct, headers] = parse_log_model(params.file.pathName);
time = dataStruct.Time.data;
time = time - time(1);
time_indices = length(time);
runtime = max(time);
% Determine x position error
x_setpoint = dataStruct.X_Setpoint_Constant.data;
x_pos_raw = dataStruct.VRPN_X_Constant.data;
x_error = timeseries(x_setpoint - x_pos_raw, time);
x_position = timeseries(x_pos_raw, time);
% Determine y position error
y_setpoint = dataStruct.Y_Setpoint_Constant.data;
y_pos_raw = dataStruct.VRPN_Y_Constant.data;
y_error = timeseries(y_setpoint - y_pos_raw, time);
y_position = timeseries(y_pos_raw, time);
% Determine z position error
z_setpoint = dataStruct.Alt_Setpoint_Constant.data;
z_pos_raw = dataStruct.VRPN_Alt_Constant.data;
z_error = timeseries(z_setpoint - z_pos_raw, time);
z_position = timeseries(z_pos_raw, time);
% Determine x velocity error
x_vel_setpoint = dataStruct.X_pos_PID_Correction.data;
x_vel_raw = dataStruct.X_Vel_Correction.data;
x_vel_error = timeseries(x_vel_setpoint - x_vel_raw, time);
x_vel = timeseries(x_vel_raw, time);
% Determine y velocity error
y_vel_setpoint = dataStruct.Y_pos_PID_Correction.data;
y_vel_raw = dataStruct.Y_Vel_Correction.data;
y_vel_error = timeseries(y_vel_setpoint - y_vel_raw, time);
y_vel = timeseries(y_vel_raw, time);
% Determine pitch error
pitch_setpoint = dataStruct.X_Vel_PID_Correction.data;
pitch_angle_raw = dataStruct.Pitch_trim_add_Sum.data;
pitch_error = timeseries(pitch_setpoint - pitch_angle_raw, time);
pitch_angle = timeseries(pitch_angle_raw, time);
% Determine roll error
roll_setpoint = dataStruct.Y_Vel_PID_Correction.data;
roll_angle_raw = dataStruct.Roll_Constant.data;
roll_error = timeseries(roll_setpoint - roll_angle_raw, time);
roll_angle = timeseries(roll_angle_raw, time);
% Determine yaw error
yaw_setpoint = dataStruct.Yaw_Setpoint_Constant.data;
yaw_angle_raw = dataStruct.Yaw_Constant.data;
yaw_error = timeseries(yaw_setpoint - yaw_angle_raw, time);
yaw_angle = timeseries(yaw_angle_raw, time);
% Determine pitch rate error
pitchrate_setpoint = dataStruct.Pitch_PID_Correction.data;
pitchrate_raw = dataStruct.gyro_y.data;
pitchrate_error = timeseries(pitchrate_setpoint - pitchrate_raw, time);
pitchrate = timeseries(pitchrate_raw, time);
% Determine roll rate error
rollrate_setpoint = dataStruct.Roll_PID_Correction.data;
rollrate_raw = dataStruct.gyro_x.data;
rollrate_error = timeseries(rollrate_setpoint - rollrate_raw, time);
rollrate = timeseries(rollrate_raw, time);
% Determine yaw rate error
yawrate_setpoint = dataStruct.Yaw_PID_Correction.data;
yawrate_raw = dataStruct.gyro_z.data;
yawrate_error = timeseries(yawrate_setpoint - yawrate_raw, time);
yawrate = timeseries(yawrate_raw, time);
% Pull motor commands from log
x_command = dataStruct.Pitch_Rate_PID_Correction.data;
y_command = dataStruct.Roll_Rate_PID_Correction.data; z_command = dataStruct.Altitude_PID_Correction.data;
yaw_command = dataStruct.Yaw_Rate_PID_Correction.data;
% Determine signal mix PWM values
PWM0 = dataStruct.Signal_Mixer_MOTOR_0.data;
PWM1 = dataStruct.Signal_Mixer_MOTOR_1.data;
PWM2 = dataStruct.Signal_Mixer_MOTOR_2.data;
PWM3 = dataStruct.Signal_Mixer_MOTOR_3.data;
PWM_arr = ...
[PWM0, PWM1, PWM2, PWM3];
motor_commands = timeseries(PWM_arr, time);
% Pull the measurements from the acceleratometer
raw_accel_data_x = dataStruct.accel_x.data;
raw_accel_data_y = dataStruct.accel_y.data;
raw_accel_data_z = dataStruct.accel_z.data;
raw_accel_data_arr = ...
[ raw_accel_data_x , raw_accel_data_y , raw_accel_data_z ];
raw_accel_data = timeseries( raw_accel_data_arr , time );
% Pull the measurements from the gyroscope
raw_gyro_data_x = dataStruct.gyro_x.data;
raw_gyro_data_y = dataStruct.gyro_y.data;
raw_gyro_data_z = dataStruct.gyro_z.data;
raw_gyro_data_arr = ...
[ raw_gyro_data_x , raw_gyro_data_y , raw_gyro_data_z ];
raw_gyro_data = timeseries( raw_gyro_data_arr , time );
% Pull the measurements from the complimentary filter
pitch_measured_IMU = dataStruct.Pitch_trim_add_Sum.data;
roll_measured_IMU = dataStruct.Roll_Constant.data;
IMU_angle_arr = ...
[roll_measured_IMU, pitch_measured_IMU];
IMU_angle_data = timeseries( IMU_angle_arr, time);
% Pull VRPN pitch and roll
pitch_measured_VRPN = dataStruct.Pitch_trim_add_Sum.data;
% Define setpoint timeseries
x_setpoint_model = timeseries(x_setpoint, time);
y_setpoint_model = timeseries(y_setpoint, time);
z_setpoint_model = timeseries(z_setpoint, time);
end
if physicsVerificationToggle == 1
% PHYSICAL SYSTEM PARAMETERS
% quadrotor + battery inertia tensor
J = diag([ Jxx Jyy Jzz ]);
% quadrotor + battery body frame origin to c.o.g. error radius
r_oc = [ 0 ; 0 ; 0 ];
% mass of rigidly attached load
m_L = 0;
% dimensions of assuemd rectangular load
x_L = 0; y_L = 0; z_L = 0;
% inertia tensor of load
J_L = m_L/12*diag([y_L^2+z_L^2,x_L^2+z_L^2,x_L^2+y_L^2]);
% location of load center of mass in body frame
r_oL = [0; 0; 0];%[ 0.12 ; 0.12 ; -0.04];%(0.04 + z_L/2) ];
%r_oL = [ .115 ; .115 ; 0.04];
% acceleration of gravity
g = 9.81;
% initial dynamics states conditions
u0 = 0;
v0 = 0;
w0 = 0;
p0 = 0;
q0 = 0; r0 = 0;
x0 = 0;
y0 = 0;
z0 = 0;
phi0 = 0;
theta0 = 0;
psi0 = 0;
% ROTOR PARAMETERS
% nominally equal rotor parameters
K_T = cell(1,4); K_H = cell(1,4); K_d = cell(1,4);
Delta_T = cell(1,4);
%Delta_T = [0 0 0];
J_r = cell(1,4); Gamma_H = cell(1,4);
for i = 1:4
% rotor thrust constants
K_T{i} = Kt;
% rotor H-force coefficeint
K_H{i} = Kh;
% rotor drag constant
K_d{i} = Kd;
% rotor b_z axis velocity thrust adjustment factor
Delta_T{i} = 0.5*[ 0 0 2.351e-4 ];
% equivalent rotor + motor moment of inertia
J_r{i} = Jreq;
% h-force xy-plane selection matrix
Gamma_H{i} = [ 1 0 0 ; 0 1 0 ; 0 0 0 ];
end
% ESC-MOTOR PARAMETERS
have_motor_transient = 1;
% nominally equal motor and eletronic speed control parameters
R_m = cell(1,4); K_Q = cell(1,4); K_V = cell(1,4);
i_0 = cell(1,4); omega_0 = cell(1,4); K_E = cell(1,4);
p_0 = cell(1,4); p_max = cell(1,4);
for i = 1:4
% motor resistance
R_m{i} = 0.235;
% motor torque constant
K_Q{i} = 96.3422;
% motor back-emf constant
K_V{i} = 96.3422;
% motor no-load current
i_0{i} = 0.3836;
% initial motor/rotor speed
omega_0{i} = (m*g/4/K_T{1})^0.5;
% ESC normalized pwm input factor
K_E{i} = 1;
% ESC turn-on duty cycle
p_0{i} = 0;
% ESC maximum duty cycle (as calibrated)
p_max{i} = 1;
end
% ROTOR LOCATION/ORIENTATION PARAMETERS
% locatin of hub 1 in body frame
r_h{1} = [ rhx ; rhy ; -rhz ];
% rotor 1 thrust unit vector
Gamma_T{1} = [ 0 ; 0 ; -1 ];
% rotor 1 rotation unit vector
Gamma_Omega{1} = [ 0 ; 0 ; -1 ];
r_h{2} = [ -rhx ; rhy ; -rhz ];
Gamma_T{2} = [ 0 ; 0 ; -1 ];
Gamma_Omega{2} = [ 0 ; 0 ; 1 ];
r_h{3} = [ -rhx ; -rhy ; -rhz ];
Gamma_T{3} = [ 0 ; 0 ; -1 ];
Gamma_Omega{3} = [ 0 ; 0 ; -1 ];
r_h{4} = [ rhx ; -rhy ; -rhz ];
Gamma_T{4} = [ 0 ; 0 ; -1 ];
Gamma_Omega{4} = [ 0 ; 0 ; 1 ];
% BATTERY PARAMETERS
% starting/operating battery voltage
V_0 = 11.4; %12.4; %11.4;
% approximate hovering rate of battery discharge
d_V = 0;
% DISTURBANCE PARAMETERS
F_D = 1*randn(3,1);
Q_D = 0.05*randn(3,1);
end
% Signal mixer using rotor layout
% 1 2
% 3 4
signal_mixer = [ 1, -1, -1, -1
1, -1, 1, 1
1, 1, -1, 1
1, 1, 1, -1 ];
% Linearize model, load preferred weight set, and build LQR model
[ssA, ssB] = linearization(m, g, Kt, Kh, Kd, delta_T(3), rhx, rhy, rhz, J, Jreq, Vb, Rm, Kv, Kq, If, signal_mixer);
load('weights_modern.mat');
% Set control period to equal period of camera system
T_LQR = Tc;
[lqr_K, lqr_S, lqr_E] = initial_lqr(ssA, ssB, state_weights, command_weights, T_LQR);