Something went wrong on our end
simplePlots.m 10.11 KiB
%%
plot(Time, Cam_Meas_Roll*(180/pi)); hold on;
plot(Time, Quad_Meas_Roll*(180/pi));
%%
plot(Time, pitch_velocity*(180/pi)); hold on;
plot(Time, roll_velocity*(180/pi));
plot(Time, yaw_velocity*(180/pi));
%%
figure;
plot(expData.Time.data, expData.VRPN_Pitch_Constant.data*(180/pi)); hold on;
plot(expData.Time.data, expData.Pitch_trim_add_Sum.data*(180/pi) + 2.2);
legend('Camera Pitch', 'Quad Pitch');
xlabel('seconds'); ylabel('degrees');
%%
figure;
plot(time, (VRPNRollConstant + pi)*(180/pi)); hold on;
plot(time, RollConstant*(180/pi));
legend('Camera Roll', 'Quad Roll');
xlabel('seconds'); ylabel('degrees');
%%
%plot(Time, X_setpoint); hold on;
markerSize = 3;
ax1 = subplot(2,1,1);
plot(time, (XSetpointConstant - VRPNXConstant), '-o', 'MarkerSize', markerSize); hold on;
plot(time, -XposPIDCorrection * (180/pi), '-o', 'MarkerSize', markerSize); hold off;
legend('X Error', 'X PID output');
ax2 = subplot(2,1,2);
plot(time, (180/pi)*PitchConstant, '-o', 'MarkerSize', markerSize); hold on;
plot(time, (180/pi)*PitchPIDCorrection, '-o', 'MarkerSize', markerSize);
legend('Pitch Error', 'Pitch PID output');
linkaxes([ax1, ax2], 'x');
%%
ax2 = subplot(2,2,1);
plot(expData.Time.data, expData.X_Setpoint_Constant.data - expData.VRPN_X_Constant.data);
title('X error');
ax1 = subplot(2,2,2);
plot(expData.Time.data, expData.X_pos_PID_Correction.data);
title('x output');
ax3 = subplot(2,2,3);
plot(expData.Time.data, expData.Pitch_PID_Correction.data); hold on;
plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* 10);
title('pitch output');
legend('output', 'Pitch x10');
ax4 = subplot(2,2,4);
plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
plot(expData.Time.data, expData.gyro_y.data .* 100000);
legend('output', 'Pitch rate x100000');
title('pitch rate output');
linkaxes([ax1, ax2, ax3, ax4], 'x');
%% X pos controller flow
figure;
ax2 = subplot(2,2,1);
plot(expData.Time.data, expData.X_Setpoint_Constant.data - expData.OF_Integrate_X_Integrated.data);
title('X error');
ax1 = subplot(2,2,2);
plot(expData.Time.data, expData.X_pos_PID_Correction.data);
title('x output');
ax3 = subplot(2,2,3);
plot(expData.Time.data, expData.Pitch_PID_Correction.data); hold on;
plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* 10);
title('pitch output');
legend('output', 'Pitch x10');
ax4 = subplot(2,2,4);
plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
plot(expData.Time.data, expData.gyro_y.data);
legend('output', 'Pitch rate');
title('pitch rate output');
linkaxes([ax1, ax2, ax3, ax4], 'x');
%%
plot(time, 1044.26 .* (PitchPIDCorrection - gyro_y));hold on;
%plot(time, PitchRatePIDCorrection);
%%
ax2 = subplot(2, 1, 1);
plot(time, YawConstant);
ax1 = subplot(2, 1, 2);
plot(time, gyro_z); hold on;
plot(time, YawRatePIDCorrection);
linkaxes([ax1, ax2], 'x');
%%
all_motors = expData.Signal_Mixer_MOTOR_0.data + expData.Signal_Mixer_MOTOR_1.data + ...
expData.Signal_Mixer_MOTOR_2.data + expData.Signal_Mixer_MOTOR_3.data;
ax1 = subplot(1, 2, 1);
plot(expData.Time.data, all_motors ./ 4); hold on;
plot(expData.Time.data, expData.RC_Throttle_Constant.data); hold on;
plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
plot(expData.Time.data, expData.Roll_Rate_PID_Correction.data); hold on;
plot(expData.Time.data, expData.Yaw_Rate_PID_Correction.data);
legend('average motors', 'throttle', 'pitch', 'roll', 'yaw');
ax2 = subplot(1, 2, 2);
plot(expData.Time.data, -expData.VRPN_Alt_Constant.data);
legend('Z, meters');
linkaxes([ax1, ax2], 'x');
%%
ax1 = subplot(1, 2, 1);
plot(expData.Time.data, expData.Pitch_Constant.data .* (180 / pi)); hold on; grid minor
plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* (180 / pi));
legend('imu', 'vrpn');
ax2 = subplot(1, 2, 2);
plot(expData.Time.data, expData.Roll_Constant.data .* (180 / pi)); hold on; grid minor
plot(expData.Time.data, expData.VRPN_Roll_Constant.data .* (180 / pi));
legend('imu', 'vrpn');
linkaxes([ax1, ax2], 'x');
%%
ax1 = subplot(3, 1, 1);
plot(expData.Time.data, expData.accel_x.data);
ax2 = subplot(3, 1, 2);
plot(expData.Time.data, expData.accel_y.data);
ax3 = subplot(3, 1, 3);
plot(expData.Time.data, expData.accel_z.data);
linkaxes([ax1, ax2, ax3], 'x');
%%
ax1 = subplot(3, 1, 1);
plot(expData.Time.data, expData.gyro_x.data);
ax2 = subplot(3, 1, 2);
plot(expData.Time.data, expData.gyro_y.data);
ax3 = subplot(3, 1, 3);
plot(expData.Time.data, expData.gyro_z.data);
linkaxes([ax1, ax2, ax3], 'x');
%%
figure;
ax2 = subplot(2,2,1);
raw_derivative = -diff(expData.VRPN_X_Constant.data) / 0.04;plot(expData.Time.data, expData.X_Vel_Correction.data - (expData.RC_Pitch_Constant.data * 5)); hold on;
%plot(expData.Time.data, expData.X_Vel_Correction.data); hold on;
%plot(expData.Time.data, [0; raw_derivative]);
title('X velocity error');
ax1 = subplot(2,2,2);
plot(expData.Time.data, expData.X_Vel_PID_Correction.data);
title('x vel output');
ax3 = subplot(2,2,3);
plot(expData.Time.data, expData.Pitch_PID_Correction.data); hold on;
plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* 10);
title('pitch output');
legend('output', 'Pitch x10');
ax4 = subplot(2,2,4);
plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
plot(expData.Time.data, expData.gyro_y.data .* 100000);
legend('output', 'Pitch rate x100000');
title('pitch rate output');
linkaxes([ax1, ax2, ax3, ax4], 'x');
%% vel flow
figure;
ax2 = subplot(2,2,1);
plot(expData.Time.data, expData.OF_Offset_Angle_Rotated_X.data - expData.RC_Pitch_Constant.data); hold on;
plot(expData.Time.data, expData.OF_Offset_Angle_Rotated_X.data); hold on;
%plot(expData.Time.data, expData.X_Vel_Correction.data); hold on;
%plot(expData.Time.data, [0; raw_derivative]);
title('X velocity error');
ax1 = subplot(2,2,2);
plot(expData.Time.data, expData.X_Vel_PID_Correction.data);
title('x vel output');
ax3 = subplot(2,2,3);
plot(expData.Time.data, expData.Pitch_PID_Correction.data); hold on;
plot(expData.Time.data, expData.Pitch_trim_add_Sum.data .* 10);
title('pitch output');
legend('output', 'Pitch x10');
ax4 = subplot(2,2,4);
plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
plot(expData.Time.data, expData.gyro_y.data .* 100000);
legend('output', 'Pitch rate x100000');
title('pitch rate output');
linkaxes([ax1, ax2, ax3, ax4], 'x');
%%
ax1 = subplot(2, 1, 1);
plot(expData.Time.data, expData.Alt_Setpoint_Constant.data - expData.VRPN_Alt_Constant.data); hold on;
plot(expData.Time.data, expData.Alt_Setpoint_Constant.data); hold on;
plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
legend('z error', 'z setpoint', 'z position');
xlabel('time (s)');
ylabel('meters');
ax2 = subplot(2, 1, 2);
plot(expData.Time.data, expData.Altitude_PID_Correction.data);
linkaxes([ax1, ax2], 'x');
legend('z PID output');
xlabel('time (s)');
ylabel('1e-8 seconds');
%%
ax1 = subplot(3, 1, 1);
plot(expData.Time.data, expData.X_Setpoint_Constant.data - expData.VRPN_X_Constant.data);
title('X error');
ax2 = subplot(3, 1, 2);
plot(expData.Time.data, expData.Y_Setpoint_Constant.data - expData.VRPN_Y_Constant.data);
title('Y error');
ax3 = subplot(3, 1, 3);
plot(expData.Time.data, expData.Alt_Setpoint_Constant.data - expData.VRPN_Alt_Constant.data);
title('Z error');
linkaxes([ax1, ax2, ax3], 'x');
%%
ax1 = subplot(2, 1, 1);
plot(expData.Time.data, expData.Lidar_Constant.data); hold on;
plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
legend('lidar', 'vrpn');
title('Z axis');
ylabel('meters');
xlabel('time (s)');
ax2 = subplot(2, 1, 2);
plot(expData.Time.data, expData.Altitude_PID_Correction.data);
title('Altitude PID Correction');
ylabel('PWM values');
xlabel('time (s)');
linkaxes([ax1, ax2], 'x');
%%
figure;
ax1 = subplot(2, 1, 1);
plot(expData.Time.data, expData.OF_Integrate_X_Integrated.data); hold on;
plot(expData.Time.data, expData.VRPN_X_Constant.data);
legend('OF X Position', 'VRPN X Position');
xlabel('Time (s)');
ylabel('Displacement (m)');
hold off;
ax2 = subplot(2, 1, 2);
plot(expData.Time.data, expData.OF_Integrate_Y_Integrated.data); hold on;
plot(expData.Time.data, expData.VRPN_Y_Constant.data);
legend('OF Y Position', 'VRPN Y Position');
xlabel('Time (s)');
ylabel('Displacement (m)');
hold off;
linkaxes([ax1, ax2]);
%% Remove sonar outliers
sonar = expData.Flow_Distance_Constant.data;
filtered_sonar = zeros(length(sonar), 1);
last_sonar = sonar(1);
for i = 1:length(sonar)
this_sonar = sonar(i);
if abs(this_sonar - last_sonar) < 0.3
filtered_sonar(i) = this_sonar;
last_sonar = this_sonar;
else
filtered_sonar(i) = last_sonar;
end
end
plot(expData.Time.data, sonar); hold on;
plot(expData.Time.data, filtered_sonar);
%% Better sonar outlier removal
sonar = expData.Flow_Distance_Constant.data;
running_med = ApproxMedFilt(sonar, 0.001);
filtered_sonar = zeros(size(sonar));
for i = 1:length(sonar)
if abs(sonar(i) - running_med(i)) < 0.15
filtered_sonar(i) = sonar(i);
else
filtered_sonar(i) = filtered_sonar(i-1);
end
endplot(expData.Time.data, sonar); hold on;
%plot(expData.Time.data, running_med);
plot(expData.Time.data, filtered_sonar);
legend('Raw', 'Median thresholded');
%% complementary filter of accelerometer and sonar
alpha = 0.9975;
dt = 0.005;
fc = 15;
% Remove bias and scale to m/s
z_acceleration = BiquadFilter(-9.8 * (expData.accel_z.data + 0.9968), 200, fc);
sonar_comp = zeros(size(sonar));
sonar_vel = zeros(size(sonar));
sonar_vel(1) = 0;
sonar_comp(1) = filtered_sonar(1);
for n = [2:length(sonar)]
sonar_vel(n) = alpha * (sonar_vel(n-1) + z_acceleration(n)*dt) + (1 - alpha) * (filtered_sonar(n) - filtered_sonar(n-1)) / dt;
sonar_comp(n) = alpha * (sonar_comp(n-1) + sonar_vel(n)*dt) + (1 - alpha) * (filtered_sonar(n));
end
figure;
plot(expData.Time.data, sonar_comp); hold on;
plot(expData.Time.data, -expData.VRPN_Alt_Constant.data - 0.025);
title('Massively filtered sonar vs. VRPN');
legend('Filtered sonar', 'VRPN');
ylabel('height (m)');
xlabel('time (s)');