Merge branch 'optical_flow' of git.ece.iastate.edu:danc/MicroCART_17-18 into optical_flow

controls/DataAnalysisTool/Tool/simplePlots.m

@@ -225,6 +225,24 @@ ax2 = subplot(2, 1, 2);
 plot(expData.Time.data, expData.Altitude_PID_Correction.data);
 
 linkaxes([ax1, ax2], 'x');
+
+%% Sonar
+filtered_sonar = [];
+last_sonar = expData.Flow_Distance_Constant.data(1);
+for i = [1 : length(expData.Flow_Distance_Constant.data)]
+    this_sonar = expData.Flow_Distance_Constant.data(i);
+    if (abs(this_sonar - last_sonar) < 0.4)
+        filtered_sonar(i) = this_sonar;
+        last_sonar = this_sonar;
+    else
+        filtered_sonar(i) = last_sonar;
+    end
+end
+alt_offset = -0.04;
+plot(expData.Time.data, -expData.Flow_Distance_Constant.data + alt_offset); hold on;
+plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
+plot(expData.Time.data, -filtered_sonar + alt_offset);
+legend('sonar', 'vrpn', 'dumb filter');
 %%
 figure;
 ax1 = subplot(2, 1, 1);
@@ -245,3 +263,8 @@ hold off;
 
 linkaxes([ax1, ax2]);
 
+%% Integarted gyro yaw
+figure;
+plot(expData.Time.data, cumtrapz(expData.gyro_z.data + 0.0088)); hold on;
+plot(expData.Time.data, expData.Yaw_Constant.data); hold on;
+legend('Integrated gyro z', 'actual yaw');