g = 9.81;  % m/s^2
rms_err = 10e-3 * g;
[a,t,dt,a_true]=accel(rms_err);

% Kinematics inferred from data.
v = cumsum(a-g)*dt;  % Assuming perfect zero-point calibration of the sensor!
z = cumsum(v)*dt;

% Corrected kinematics.
g_corr = mean(a);
   % We can do this because we know that the initial and final velocities are zero!
v_corr = cumsum(a-g_corr)*dt;
z_corr = cumsum(v_corr)*dt;

% Exact (theoretical) kinematics.
v_true = cumsum(a_true-g)*dt;
z_true = cumsum(v_true)*dt;

height_error = max(z) - max(z_true)
corrected_height_error = max(z_corr) - max(z_true)

figure(1)
hold off
plot(t, a, 'b')
hold on
plot(t,a_true, 'r')
legend('measured','theoretical')
xlabel('t (s)')
ylabel('acceleration (m/s^2)')
title('Accelerometer Data (simulated)')
print('../plots/acceleration.pdf','-dpdf')


figure(2)
hold off
plot(t, v, 'b')
hold on
plot(t,v_corr, 'g')
plot(t,v_true, 'r')
legend('measured','corrected','theoretical')
xlabel('t (s)')
ylabel('velocity (m/s)')
print('../plots/velocity.pdf','-dpdf')

figure(3)
hold off
plot(t, z, 'b')
hold on
plot(t,z_corr, 'g')
plot(t,z_true, 'r')
legend('measured','corrected','theoretical')
xlabel('t (s)')
ylabel('position (m)')
print('../plots/position.pdf','-dpdf')

figure(4)
hold off
plot(t, v - v_true, 'b')
hold on
plot(t, v_corr - v_true, 'g')
legend('measured - theoretical','corrected - theoretical')
xlabel('t (s)')
ylabel('velocity error (m/s)')
print('../plots/velocity_error.pdf','-dpdf')

figure(5)
hold off
plot(t, z - z_true, 'b')
hold on
plot(t, z_corr - z_true, 'g')
legend('measured - theoretical','corrected - theoretical')
xlabel('t (s)')
ylabel('position error (m)')
print('../plots/position_error.pdf','-dpdf')


