diff --git a/controls/model/Library_Blocks/Derivative.slx b/controls/model/Library_Blocks/Derivative.slx
new file mode 100644
index 0000000000000000000000000000000000000000..6367f56fe201f61ea4e48f3cdb2a452a5fc7d0ed
Binary files /dev/null and b/controls/model/Library_Blocks/Derivative.slx differ
diff --git a/controls/model/logAnalysis.m b/controls/model/logAnalysis.m
index 8875fdb5cd7795e672a071bfd660023edcfd0815..6433cba477c8487f74f4ac715579d79d5cef6140 100644
--- a/controls/model/logAnalysis.m
+++ b/controls/model/logAnalysis.m
@@ -25,7 +25,7 @@ y_command_model_data = y_command_model.signals.values(indices_5ms);
y_position_model_data = y_position_model.signals.values(indices_40ms);
% Pull z control structure data
-z_command_model_data = z_command_model.signals.values(indices_5ms);
+z_command_model_data = z_command_model.signals.values(indices_40ms);
z_position_model_data = z_position_model.signals.values(indices_40ms);
% Pull yaw control structure data
@@ -51,7 +51,7 @@ roll_accel_mahony = mahony_reading.signals.values(indices_5ms, 1);
% Plot x position controller output
figure(1); ax1 = subplot(2, 2, 1);
-stairs(time, x_vel_raw, '.-'); hold on; grid minor;
+stairs(time, x_vel_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, x_vel_setpoint_model_data, '.-'); hold off;
title('X Position Controller Output');
xlabel('Time (s)');
@@ -91,7 +91,7 @@ linkaxes([ax1, ax2, ax3, ax4], 'x');
% Plot y position controller output
figure(2); ax1 = subplot(2, 2, 1);
-stairs(time, y_vel_raw, '.-'); hold on; grid minor;
+stairs(time, y_vel_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, y_vel_setpoint_model_data, '.-'); hold off;
title('Y Position Controller Output');
xlabel('Time (s)');
@@ -133,7 +133,7 @@ linkaxes([ax1, ax2, ax3, ax4], 'x');
% Plot z controller command
figure(3); ax1 = subplot(2, 1, 1);
stairs(time, z_command, '.-'); hold on; grid minor;
-stairs(time_model_5ms, z_command_model_data, '.-'); hold off;
+stairs(time_model_40ms, z_command_model_data, '.-'); hold off;
title('Z Command');
xlabel('Time (s)');
ylabel('Command');
@@ -184,7 +184,7 @@ linkaxes([ax1, ax2, ax3], 'x');
%% Plot PWM Commands
figure(5); ax1 = subplot(2, 2, 1);
stairs(time, PWM0,'.-'); hold on; grid minor;
-%stairs(time_model_5ms, PWM0_model, '.-'); hold off;
+stairs(time_model_5ms, PWM0_model, '.-'); hold off;
title('PWM0 Value');
xlabel('Time (s)');
ylabel('PWM0 Command');
@@ -192,7 +192,7 @@ legend('Log', 'Model', 'location', 'northwest');
ax2 = subplot(2, 2, 2);
stairs(time, PWM1,'.-'); hold on; grid minor;
-%stairs(time_model_5ms, PWM1_model, '.-'); hold off;
+stairs(time_model_5ms, PWM1_model, '.-'); hold off;
title('PWM1 Value');
xlabel('Time (s)');
ylabel('PWM1 Command');
@@ -200,7 +200,7 @@ legend('Log', 'Model', 'location', 'northwest');
ax3 = subplot(2, 2, 3);
stairs(time, PWM2,'.-'); hold on; grid minor;
-%stairs(time_model_5ms, PWM2_model, '.-'); hold off;
+stairs(time_model_5ms, PWM2_model, '.-'); hold off;
title('PWM2 Value');
xlabel('Time (s)');
ylabel('PWM2 Command');
@@ -208,7 +208,7 @@ legend('Log', 'Model', 'location', 'northwest');
ax4 = subplot(2, 2, 4);
stairs(time, PWM3,'.-'); hold on; grid minor;
-%stairs(time_model_5ms, PWM3_model, '.-'); hold off;
+stairs(time_model_5ms, PWM3_model, '.-'); hold off;
title('PWM3 Value');
xlabel('Time (s)');
ylabel('PWM3 Command');
@@ -219,26 +219,23 @@ linkaxes([ax1, ax2, ax3, ax4], 'xy');
%% Plot output of complimentary filter
figure(8); ax1 = subplot(2, 1, 1);
-%stairs(time, pitch_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
-stairs(time, (pitch_measured_IMU) * (180/pi), '.-'); hold on; grid minor;
-stairs(time_model_5ms, pitch_accel * (180/pi), '.-');
-%stairs(time_model_5ms, pitch_accel_mahony * (180/pi), '.-'); hold off;
+stairs(time, pitch_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
+stairs(time, (pitch_measured_IMU) * (180/pi), '.-');
+stairs(time_model_5ms, (pitch_accel + 0.045) * (180/pi), '.-');
title('Pitch Complementary Filter Output');
xlabel('Time (s)');
ylabel('Pitch Angle (degrees)');
-%legend('VRPN','IMU', 'Model', 'Mahony', 'location', 'northwest');
-legend('IMU', 'Model','location', 'northwest');
+legend('VRPN','IMU', 'Model', 'location', 'northwest');
ax2 = subplot(2, 1, 2);
-%stairs(time, roll_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
-stairs(time, roll_measured_IMU * (180/pi), '.-'); hold on; grid minor;
+stairs(time, roll_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
+stairs(time, roll_measured_IMU * (180/pi), '.-');
stairs(time_model_5ms, roll_accel * (180/pi), '.-');
-%stairs(time_model_5ms, roll_accel_mahony * (180/pi), '.-'); hold off;
title('Roll Complementary Filter Output');
xlabel('Time (s)');
ylabel('Roll Angle (degrees)');
-%legend('VRPN','IMU', 'Model', 'Mahony', 'location', 'northwest');
-legend('IMU', 'Model','location', 'northwest');
+legend('VRPN','IMU', 'Model', 'location', 'northwest');
+
linkaxes([ax1, ax2], 'x');
diff --git a/controls/model/modelParameters.m b/controls/model/modelParameters.m
index 0b7971269fcd3f3bef9de1f2b0e2f69a8a83d203..a2bf10fe3427cc99d218f81d16c12bb12e72c0a1 100644
--- a/controls/model/modelParameters.m
+++ b/controls/model/modelParameters.m
@@ -1,9 +1,16 @@
+% Add Library Blocks to Path
+ path = genpath('Library_blocks');
+ addpath(path);
+
% Log Analysis Toggle
logAnalysisToggle = 1; % 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 = 20;
+ runtime = 30;
% Model Parameters
m = 1.216; % Quadrotor + battery mass
@@ -103,7 +110,7 @@ if logAnalysisToggle == 1
time = dataStruct.Time.data;
time = time - time(1);
time_indices = length(time);
-
+
runtime = max(time);
% Determine x position error
@@ -137,20 +144,20 @@ if logAnalysisToggle == 1
y_vel = timeseries(y_vel_raw, time);
% Determine pitch error
- pitch_setpoint = dataStruct.X_pos_PID_Correction.data;
+ 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_pos_PID_Correction.data;
+ 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 = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW SETPOINT
- yaw_angle_raw = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW VALUE
+ yaw_angle_raw = dataStruct.Yaw_Constant.data;
yaw_error = timeseries(yaw_setpoint - yaw_angle_raw, time);
yaw_angle = timeseries(yaw_angle_raw, time);
@@ -205,7 +212,6 @@ if logAnalysisToggle == 1
% Create time series object for z command
throttle_command = timeseries(dataStruct.RC_Throttle_Constant.data, time);
- %z_command = dataStruct.RC_Throttle_Constant.data;
% Pull the measurements from the complimentary filter
pitch_measured_IMU = dataStruct.Pitch_trim_add_Sum.data;
@@ -218,11 +224,11 @@ if logAnalysisToggle == 1
pitch_measured_VRPN = dataStruct.VRPN_Pitch_Constant.data;
roll_measured_VRPN = dataStruct.VRPN_Roll_Constant.data;
-% Set height_controlled_o to initial throttle command
+% % Set height_controlled_o to initial throttle command
% height_controlled_o = dataStruct.RC_Throttle_Constant.data(1);
%
-% Set rotor speed initial conditions to there calculated values based on
-% initial throttle control
+% % Set rotor speed initial conditions to there calculated values based on
+% % initial throttle control
% omega0_o = (sqrt(((height_controlled_o ...
% - Pmin)/(Pmax - Pmin)* Vb - Rm * If) * 4 * ...
% Rm * Kv^2 * Kq * Kd + 1) - 1) / (2 * Rm * ...
@@ -231,24 +237,149 @@ if logAnalysisToggle == 1
% omega2_o = omega0_o;
% omega3_o = omega0_o;
%
-% Set initial positions
+% % Set initial positions
% x_o = x_position(1);
% y_o = y_position(1);
% z_o = z_position(1);
%
-% Set initial velocities
+% % Set initial velocities
% x_vel_o = (x_position(1) - x_position(2))/(time(1) - time(2));
% y_vel_o = (y_position(1) - y_position(2))/(time(1) - time(2));
% z_vel_o = (z_position(1) - z_position(2))/(time(1) - time(2));
%
-% Equilibrium angles
+% % Equilibrium angles
% roll_o = roll_measured_VRPN(1);
% pitch_o = pitch_measured_VRPN(1);
% yaw_o = 0;
%
-% Equilibrium angular rates
+% % Equilibrium angular rates
% rollrate_o = (roll_measured_VRPN(1) - roll_measured_VRPN(2))/(time(1) - time(2));
% pitchrate_o = (pitch_measured_VRPN(1) - pitch_measured_VRPN(2))/(time(1) - time(2));
% yawrate_o = 0;
+end
+
+if physicsVerificationToggle == 1
+ % PHYSICAL SYSTEM PARAMETERS
+
+% quadrotor + battery mass
+m = 1.216;
+% quadrotor + battery inertia tensor
+J = diag([ 0.0130 0.0140 0.0285 ]);
+% 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;
+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} = 1.2007e-05;
+ % rotor H-force coefficeint
+ K_H{i} = 0;
+ % rotor drag constant
+ K_d{i} = 1.4852e-07;
+ % rotor b_z axis velocity thrust adjustment factor
+ Delta_T{i} = 0;
+ % equivalent rotor + motor moment of inertia
+ J_r{i} = 4.2012e-05;
+ % h-force xy-plane selection matrix
+ Gamma_H{i} = [ 0 0 0 ; 0 0 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 = 0;
+Q_D = 0;
end
\ No newline at end of file
diff --git a/controls/model/test_model.slx b/controls/model/test_model.slx
index d5bc5cf9bf30f0bb5a79b4167ac79bdb8b1be8a8..dd12b5cc5903054e32c16d022fa90f62cff22876 100644
Binary files a/controls/model/test_model.slx and b/controls/model/test_model.slx differ