Skip to content
Snippets Groups Projects
Commit 3c8e18ef authored by Andy Snawerdt's avatar Andy Snawerdt
Browse files

Added mahony filter and plots for log analysis

parent d824b14f
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
controls/model/logAnalysis.m
+ 87
− 26
View file @ 3c8e18ef
......@@ -37,16 +37,20 @@ PWM1_model = motorCommands.signals.values(indices_5ms, 2);
PWM2_model = motorCommands.signals.values(indices_5ms, 3);
PWM3_model = motorCommands.signals.values(indices_5ms, 4);
%Pull accelerometer readings from model
% Pull accelerometer readings from model
pitch_accel = angle_IMU_reading.signals.values(2, 1, indices_5ms);
pitch_accel = reshape(pitch_accel, [length(pitch_accel) , 1] );
roll_accel = angle_IMU_reading.signals.values(1, 1, indices_5ms);
roll_accel = reshape(roll_accel, [length(roll_accel) , 1] );
% Pull mahony filter data
pitch_accel_mahony = mahony_reading.signals.values(indices_5ms, 2);
roll_accel_mahony = mahony_reading.signals.values(indices_5ms, 1);
%% Plot x control structure
% Plot lateral controller output
figure(1); subplot(2, 2, 1);
figure(1); ax1 = subplot(2, 2, 1);
stairs(time, pitch_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, pitch_setpoint_model_data, '.-'); hold off;
title('Lateral Controller Output');
......@@ -55,7 +59,7 @@ ylabel('\theta (rad)');
legend('Log', 'Model', 'location', 'northwest');
% Plot pitch controller output
subplot(2, 2, 2);
ax2 = subplot(2, 2, 2);
stairs(time, pitchrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_5ms, pitchrate_setpoint_model_data, '.-'); hold off;
title('Pitch Controller Output');
......@@ -64,16 +68,16 @@ ylabel('d\theta/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest');
% Plot x controller command
subplot(2, 2, 3);
ax3 = subplot(2, 2, 3);
stairs(time, x_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, x_command_model_data, '.-'); hold off;
title('X Command');
title('Pitch Rate Controller Output');
xlabel('Time (s)');
ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');
% Plot x position
subplot(2, 2, 4);
ax4 = subplot(2, 2, 4);
stairs(time, x_position, '.-'); hold on; grid minor;
stairs(time_model_40ms, x_position_model_data, '.-'); hold off;
title('X Position');
......@@ -81,10 +85,12 @@ xlabel('Time (s)');
ylabel('Position (m)');
legend('Log', 'Model', 'location', 'northwest');
linkaxes([ax1, ax2, ax3, ax4], 'x');
%% Plot y control structure
% Plot longitude controller output
figure(2); subplot(2, 2, 1);
figure(2); ax1 = subplot(2, 2, 1);
stairs(time, roll_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, roll_setpoint_model_data, '.-'); hold off;
title('Longitude Controller Output ');
......@@ -93,7 +99,7 @@ ylabel('\phi (rad)');
legend('Log', 'Model', 'location', 'northwest');
% Plot roll controller output
subplot(2, 2, 2);
ax2 = subplot(2, 2, 2);
stairs(time, rollrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_5ms, rollrate_setpoint_model_data, '.-'); hold off;
title('Roll Controller Output');
......@@ -102,7 +108,7 @@ ylabel('d\phi/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest');
% Plot y controller command
subplot(2, 2, 3);
ax3 = subplot(2, 2, 3);
stairs(time, y_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, y_command_model_data, '.-'); hold off;
title('Y Command');
......@@ -111,7 +117,7 @@ ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');
% Plot y position
subplot(2, 2, 4);
ax4 = subplot(2, 2, 4);
stairs(time, y_position, '.-'); hold on; grid minor;
stairs(time_model_40ms, y_position_model_data, '.-'); hold off;
title('Y Position');
......@@ -119,10 +125,12 @@ xlabel('Time (s)');
ylabel('Position (m)');
legend('Log', 'Model', 'location', 'northwest');
linkaxes([ax1, ax2, ax3, ax4], 'x');
%% Plot z control structure
% Plot z controller command
figure(3); subplot(2, 1, 1);
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;
title('Z Command');
......@@ -131,7 +139,7 @@ ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');
% Plot z position
subplot(2, 1, 2);
ax2 = subplot(2, 1, 2);
stairs(time, z_position, '.-'); hold on; grid minor;
stairs(time_model_40ms, z_position_model_data, '.-'); hold off;
title('Z Position');
......@@ -139,10 +147,12 @@ 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); subplot(2, 2, 1);
figure(4); ax1 = subplot(2, 2, 1);
stairs(time, yawrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_40ms, yawrate_setpoint_model_data, '.-'); hold off;
title('Yaw Controller Output');
......@@ -151,7 +161,7 @@ ylabel('d\psi/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest');
% Plot yaw controller command
subplot(2, 2, 2);
ax2 = subplot(2, 2, 2);
stairs(time, yaw_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, yaw_command_model_data, '.-'); hold off;
title('Yaw Command');
......@@ -160,7 +170,7 @@ ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');
% Plot yaw position
subplot(2, 2, 3);
ax3 = subplot(2, 2, 3);
stairs(time, yaw_value, '.-'); hold on; grid minor;
stairs(time_model_40ms, yaw_value_model_data, '.-'); hold off;
title('Yaw Position');
......@@ -168,10 +178,12 @@ xlabel('Time (s)');
ylabel('Value (rad)');
legend('Log', 'Model', 'location', 'northwest');
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');
......@@ -179,7 +191,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');
......@@ -187,7 +199,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');
......@@ -195,7 +207,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');
......@@ -205,29 +217,34 @@ linkaxes([ax1, ax2, ax3, ax4], 'xy');
%% Plot output of complimentary filter
figure(8); subplot(2, 1, 1);
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), '.-');
stairs(time_model_5ms, pitch_accel * (180/pi), '.-'); hold off;
stairs(time, (pitch_measured_IMU + 0.02) * (180/pi), '.-');
%stairs(time_model_5ms, pitch_accel * (180/pi), '.-');
stairs(time_model_5ms, pitch_accel_mahony * (180/pi), '.-'); hold off;
title('Pitch Complementary Filter Output');
xlabel('Time (s)');
ylabel('Pitch Angle (degrees)');
legend('VRPN','IMU', 'Model', 'location', 'northwest');
subplot(2, 1, 2);
ax2 = subplot(2, 1, 2);
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), '.-'); hold off;
%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', 'location', 'northwest');
linkaxes([ax1, ax2], 'x');
%% Plot VRPN Position
figure(9); ax1 = subplot(3, 1, 1);
stairs(time, x_position, '.-'); 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)');
ylabel('X position');
......@@ -235,7 +252,9 @@ legend('X position', 'X position model', 'X setpoint');
ax2 = subplot(3, 1, 2);
stairs(time, y_position, '.-'); hold on; grid minor;
stairs(time_model_40ms, y_position_model_data, '.-');
%stairs(time_model_40ms, y_position_model_data, '.-');
stairs(time, x_setpoint, '.-');
title('Y position');
xlabel('Time (s)');
ylabel('Y position');
......@@ -250,3 +269,45 @@ ylabel('Z position');
legend('Z position', 'Z position model', 'Y setpoint');
linkaxes([ax1, ax2, ax3], 'x');
%% Plot Gyro Data
figure(10); ax1 = subplot(3, 1, 1);
stairs(time, raw_gyro_data_x, '.-'); hold on; grid minor;
title('gyro x');
xlabel('Time (s)');
ylabel('p (rad/s)');
ax2 = subplot(3, 1, 2);
stairs(time, raw_gyro_data_y, '.-'); hold on; grid minor;
title('gyro y');
xlabel('Time (s)');
ylabel('q (rad/s)');
ax3 = subplot(3, 1, 3);
stairs(time, raw_gyro_data_z, '.-'); hold on; grid minor;
title('gyro z');
xlabel('Time (s)');
ylabel('r (rad/s)');
linkaxes([ax1, ax2, ax3], 'x');
%% Plot Accel Data
figure(11); ax1 = subplot(3, 1, 1);
stairs(time, raw_accel_data_x, '.-'); hold on; grid minor;
title('accel x');
xlabel('Time (s)');
ylabel('accel x (g)');
ax2 = subplot(3, 1, 2);
stairs(time, raw_gyro_data_y, '.-'); hold on; grid minor;
title('accel y');
xlabel('Time (s)');
ylabel('accel y (g)');
ax3 = subplot(3, 1, 3);
stairs(time, raw_gyro_data_z, '.-'); hold on; grid minor;
title('accel z');
xlabel('Time (s)');
ylabel('accel (z)');
linkaxes([ax1, ax2, ax3], 'x');
\ No newline at end of file
controls/model/modelParameters.m
+ 38
− 35
View file @ 3c8e18ef
......@@ -18,6 +18,7 @@
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.2308; % Motor resistance
Kq = 96.3422; % Motor torque constant
Kv = 96.3422; % Motor back emf constant
......@@ -31,6 +32,8 @@
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.2; % Proportional term for mahony filter
Ki_mahony = 0.001; % Integral term for mahony filter
% Determine Initial Conditions
......@@ -114,7 +117,7 @@ if logAnalysisToggle == 1
% Determine pitch error
pitch_setpoint = dataStruct.X_pos_PID_Correction.data;
pitch_value = dataStruct.Pitch_Constant.data;
pitch_value = dataStruct.Pitch_trim_add_Sum.data;
pitch_error = timeseries(pitch_setpoint - pitch_value, time);
% Determine roll error
......@@ -175,10 +178,10 @@ 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;
%z_command = dataStruct.RC_Throttle_Constant.data;
% Pull the measurements from the complimentary filter
pitch_measured_IMU = dataStruct.Pitch_Constant.data;
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];
......@@ -188,37 +191,37 @@ 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;
% % 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;
end
\ No newline at end of file
controls/model/test_model.slx
+ 0
− 0
View file @ 3c8e18ef
No preview for this file type
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment