merged simplePlots.m, added sonar

controls/DataAnalysisTool/Tool/simplePlots.m

@@ -221,9 +221,15 @@ 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');
 %%
@@ -246,3 +252,57 @@ 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
+end
+plot(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)');