new plots for analysis

controls/DataAnalysisTool/Tool/BiquadFilter.m

+function out = BiquadFilter(input, Fs, Fc)
+    K = tan(pi * Fc / Fs);
+    Q = 0.5;
+    
+    norm = 1.0 / (1.0 + K/Q + K*K);
+    a0 = K*K*norm;
+    a1 = 2.0*a0;
+    a2 = a0;
+    b1 = 2.0*(K*K -1) * norm;
+    b2 = (1.0 - K/Q + K*K) * norm;
+    
+    prev = [0, 0];
+    out = zeros(length(input), 1);
+    for n = 1:length(input)
+        leftSum = input(n) - prev(1)*b1 - prev(2)*b2;
+        out(n) = leftSum*a0 + prev(1)*a1 + prev(2)*a2;
+        
+        prev(2) = prev(1);
+        prev(1) = leftSum;
+    end
\ No newline at end of file

controls/DataAnalysisTool/Tool/DataAnalysis.m

@@ -8,7 +8,7 @@
 % window.
 %
 %fname = 'sampleLogFileWithMarker.txt';  
-fname = '../../../groundStation/logs/tune_of_w_velocity_2.txt';  
+fname = '';  
 
 % PLOTTING SWITCHES - set them to 0 or 1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %

controls/DataAnalysisTool/Tool/simplePlots.m

@@ -217,19 +217,52 @@ plot(expData.Time.data, expData.Alt_Setpoint_Constant.data - expData.VRPN_Alt_Co
 title('Z error');
 linkaxes([ax1, ax2, ax3], 'x');
 %%
-ax1 = subplot(2, 1, 1);
+%ax1 = subplot(2, 1, 1);
+figure;
 plot(expData.Time.data, expData.Lidar_Constant.data); hold on;
 plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
-legend('lidar', 'vrpn');
 
-ax2 = subplot(2, 1, 2);
-plot(expData.Time.data, expData.Altitude_PID_Correction.data);
+angle = sqrt(expData.Roll_Constant.data.^2 + expData.VRPN_Pitch_Constant.data.^2);
+corrected = expData.Lidar_Constant.data .* cos(angle);
+%plot(expData.Time.data, corrected);
+legend('lidar', 'vrpn', 'angle corrected');
 
-linkaxes([ax1, ax2], 'x');
+
+%% complementary filter of accelerometer and sonar
+alpha = 0.9975;
+%alpha = 1;
+dt = 0.005;
+fc = 10;
+
+lidar = expData.Lidar_Constant.data;
+% Remove bias and scale to m/s
+z_acceleration = BiquadFilter(9.8 * (expData.accel_z.data + 0.9812), 200, fc);
+
+sonar_comp = zeros(size(lidar));
+sonar_vel = zeros(size(lidar));
+sonar_vel(1) = 0;
+sonar_comp(1) = lidar(1);
+for n = [2:length(lidar)]
+    sonar_vel(n) = alpha * (sonar_vel(n-1) + z_acceleration(n)*dt) + (1 - alpha) * (lidar(n) - lidar(n-1)) / dt;
+    sonar_comp(n) = alpha * (sonar_comp(n-1) + sonar_vel(n)*dt) + (1 - alpha) * (lidar(n));
+end
+
+figure;
+plot(expData.Time.data, sonar_comp); hold on;
+%plot(expData.Time.data, lidar);
+plot(expData.Time.data, expData.VRPN_Alt_Constant.data + 0.025);
+title('Massively filtered sonar vs. VRPN');
+legend('Filtered lidar', 'VRPN');
+ylabel('height (m)');
+xlabel('time (s)');
 %%
 figure;
+
+offsetX = -expData.OF_Integrate_X_Integrated.data(1) - expData.VRPN_X_Constant.data(1);
+offsetY = -expData.OF_Integrate_Y_Integrated.data(1) - expData.VRPN_Y_Constant.data(1);
+
 ax1 = subplot(2, 1, 1);
-plot(expData.Time.data, expData.OF_Integrate_X_Integrated.data); hold on;
+plot(expData.Time.data, -expData.OF_Integrate_X_Integrated.data - offsetX); hold on;
 plot(expData.Time.data, expData.VRPN_X_Constant.data);
 legend('OF X Position', 'VRPN X Position');
 xlabel('Time (s)');
@@ -237,7 +270,7 @@ 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.OF_Integrate_Y_Integrated.data - offsetY); hold on;
 plot(expData.Time.data, expData.VRPN_Y_Constant.data);
 legend('OF Y Position', 'VRPN Y Position');
 xlabel('Time (s)');
@@ -246,3 +279,51 @@ hold off;
 
 linkaxes([ax1, ax2]);
 
+%%
+figure;
+angleOffset = 0.62204 + expData.Yaw_Setpoint_Constant.data;
+
+FlowVelX = expData.Flow_Vel_X_Constant.data .* cos(angleOffset) - expData.Flow_Vel_Y_Constant.data .* sin(angleOffset);
+FlowVelY = expData.Flow_Vel_X_Constant.data .* sin(angleOffset) + expData.Flow_Vel_Y_Constant.data .* cos(angleOffset);
+
+fc = 2;
+FlowVelX = BiquadFilter(FlowVelX, 200, fc);
+FlowVelY = BiquadFilter(FlowVelY, 200, fc);
+
+flowX = zeros(1, length(expData.Time.data));
+
+driftX = 0;
+driftY = 0;
+
+flowX(1) = expData.VRPN_X_Constant.data(1);
+for n = 2:length(flowX)
+    flowX(n) = flowX(n-1) + 0.005*(FlowVelX(n) - driftX);
+end
+
+flowY = zeros(1, length(expData.Time.data));
+flowY(1) = expData.VRPN_Y_Constant.data(1);
+for n = 2:length(flowY)
+    flowY(n) = flowY(n-1) + 0.005*(FlowVelY(n) - driftY);
+end
+
+ax1 = subplot(2, 1, 1);
+plot(expData.Time.data, -flowX - 0.5); hold on;
+plot(expData.Time.data, expData.VRPN_X_Constant.data);
+%legend('OF Integrated X Position');
+legend('OF Integrated X', 'VRPN X');
+%legend('OF Integrated X Position', 'Approximate Max X Position (measured with tape measure)');
+xlabel('Time (s)');
+ylabel('Position (m)');
+hold off;
+
+ax2 = subplot(2, 1, 2);
+plot(expData.Time.data, -flowY - 0.1); hold on;
+plot(expData.Time.data, expData.VRPN_Y_Constant.data);
+%legend('OF Integrated Y Position');
+legend('OF Integrated Y', 'VRPN Y');
+%legend('OF Integrated Y Position', 'Approximate Max Y Position (measured with tape measure)');
+xlabel('Time (s)');
+ylabel('Position (m)');
+hold off;
+
+linkaxes([ax1 ax2]);    
\ No newline at end of file