diff --git a/controls/model/logAnalysis.m b/controls/model/logAnalysis.m
index 6433cba477c8487f74f4ac715579d79d5cef6140..ab699fbf2b87a485b3c08b32fab780b2f9637a51 100644
--- a/controls/model/logAnalysis.m
+++ b/controls/model/logAnalysis.m
@@ -131,7 +131,7 @@ linkaxes([ax1, ax2, ax3, ax4], 'x');
%% Plot z control structure
% Plot z controller command
-figure(3); ax1 = subplot(2, 1, 1);
+figure(3);
stairs(time, z_command, '.-'); hold on; grid minor;
stairs(time_model_40ms, z_command_model_data, '.-'); hold off;
title('Z Command');
@@ -139,21 +139,10 @@ xlabel('Time (s)');
ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');
-% Plot z position
-ax2 = subplot(2, 1, 2);
-stairs(time, z_pos_raw, '.-'); hold on; grid minor;
-stairs(time_model_40ms, z_position_model_data, '.-'); hold off;
-title('Z Position');
-xlabel('Time (s)');
-ylabel('Position (m)');
-legend('Log', 'Model', 'location', 'northwest');
-
-linkaxes([ax1, ax2], 'x');
-
%% Plot yaw control structure
% Plot yaw controller output
-figure(4); ax1 = subplot(2, 2, 1);
+figure(4); ax1 = subplot(2, 1, 1);
stairs(time, yawrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_40ms, yawrate_setpoint_model_data, '.-'); hold off;
title('Yaw Controller Output');
@@ -162,7 +151,7 @@ ylabel('d\psi/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest');
% Plot yaw controller command
-ax2 = subplot(2, 2, 2);
+ax2 = subplot(2, 1, 2);
stairs(time, yaw_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, yaw_command_model_data, '.-'); hold off;
title('Yaw Command');
@@ -170,16 +159,7 @@ xlabel('Time (s)');
ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');
-% Plot yaw position
-ax3 = subplot(2, 2, 3);
-stairs(time, yaw_angle_raw, '.-'); hold on; grid minor;
-stairs(time_model_40ms, yaw_value_model_data, '.-'); hold off;
-title('Yaw Position');
-xlabel('Time (s)');
-ylabel('Value (rad)');
-legend('Log', 'Model', 'location', 'northwest');
-
-linkaxes([ax1, ax2, ax3], 'x');
+linkaxes([ax1, ax2], 'x');
%% Plot PWM Commands
figure(5); ax1 = subplot(2, 2, 1);
@@ -241,9 +221,9 @@ linkaxes([ax1, ax2], 'x');
%% Plot VRPN Position
-figure(9); ax1 = subplot(3, 1, 1);
+figure(10); ax1 = subplot(3, 1, 1);
stairs(time, x_pos_raw, '.-'); hold on; grid minor;
-%stairs(time_model_40ms, x_position_model_data, '.-');
+stairs(time_model_40ms, x_position_model_data, '.-');
stairs(time, x_setpoint, '.-');
title('X position');
xlabel('Time (s)');
@@ -252,7 +232,7 @@ legend('X position', 'X position model', 'X setpoint');
ax2 = subplot(3, 1, 2);
stairs(time, y_pos_raw, '.-'); hold on; grid minor;
-%stairs(time_model_40ms, y_position_model_data, '.-');
+stairs(time_model_40ms, y_position_model_data, '.-');
stairs(time, y_setpoint, '.-');
title('Y position');
xlabel('Time (s)');
@@ -262,10 +242,11 @@ legend('Y position', 'Y position model', 'Y setpoint');
ax3 = subplot(3, 1, 3);
stairs(time, z_pos_raw, '.-'); hold on; grid minor;
stairs(time_model_40ms, z_position_model_data, '.-');
+stairs(time, z_setpoint, '.-');
title('Z position');
xlabel('Time (s)');
ylabel('Z position');
-legend('Z position', 'Z position model', 'Y setpoint');
+legend('Z position', 'Z position model', 'Z setpoint');
linkaxes([ax1, ax2, ax3], 'x');
diff --git a/controls/model/modelParameters.m b/controls/model/modelParameters.m
index a2bf10fe3427cc99d218f81d16c12bb12e72c0a1..9a0694366b23315e7dd7c7d36f4e6be5d3c3b01a 100644
--- a/controls/model/modelParameters.m
+++ b/controls/model/modelParameters.m
@@ -3,14 +3,14 @@
addpath(path);
% Log Analysis Toggle
- logAnalysisToggle = 1; % 1 for log analysis, 0 for normal operation
+ 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 = 30;
+ runtime = 55;
% Model Parameters
m = 1.216; % Quadrotor + battery mass
@@ -20,7 +20,7 @@
Jzz = 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
- Kh = 0; % Rotor in-plane drag constant
+ 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
@@ -30,17 +30,18 @@
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
+ Pmin = 0; % Minimum zybo output duty cycle command
+ Pmax = 1; % Maximum zybo output duty cycle command
Tc = 0.04; % Camera system sampling period
Tq = 0.005; % Quad sampling period
tau_c = 0; % Camera system total latency
- Vb = 11.4; % Nominal battery voltage (V)
+ 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
+ delta_T = 4 * [ 0, 0, 2.351e-4 ];
% Define Biquad Filter Parameters
b0 = 0.020083;
@@ -112,7 +113,7 @@ if logAnalysisToggle == 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;
@@ -156,7 +157,7 @@ if logAnalysisToggle == 1
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_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);
@@ -224,38 +225,10 @@ 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
-% height_controlled_o = dataStruct.RC_Throttle_Constant.data(1);
-%
-% % 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 * ...
-% Kv * Kq * Kd);
-% omega1_o = omega0_o;
-% omega2_o = omega0_o;
-% omega3_o = omega0_o;
-%
-% % Set initial positions
-% x_o = x_position(1);
-% y_o = y_position(1);
-% z_o = z_position(1);
-%
-% % 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
-% roll_o = roll_measured_VRPN(1);
-% pitch_o = pitch_measured_VRPN(1);
-% yaw_o = 0;
-%
-% % 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;
+ % 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
@@ -308,15 +281,15 @@ for i = 1:4
% rotor thrust constants
K_T{i} = 1.2007e-05;
% rotor H-force coefficeint
- K_H{i} = 0;
+ K_H{i} = 3.4574e-04;
% rotor drag constant
K_d{i} = 1.4852e-07;
% rotor b_z axis velocity thrust adjustment factor
- Delta_T{i} = 0;
+ Delta_T{i} = 0.5*[ 0 0 2.351e-4 ];
% 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 ];
+ Gamma_H{i} = [ 1 0 0 ; 0 1 0 ; 0 0 0 ];
end
@@ -380,6 +353,6 @@ d_V = 0;
% DISTURBANCE PARAMETERS
-F_D = 0;
-Q_D = 0;
+F_D = 1*randn(3,1);
+Q_D = 0.05*randn(3,1);
end
\ No newline at end of file
diff --git a/controls/model/test_model.slx b/controls/model/test_model.slx
index dd12b5cc5903054e32c16d022fa90f62cff22876..35a8b396d018cd92f55e5327af5b488dbafddcaf 100644
Binary files a/controls/model/test_model.slx and b/controls/model/test_model.slx differ