implement state propagation

[mdb15106]

Take the previous loop's state vector, and use it as the initial condition to estimate the current state vector using the IMU's linear acceleration measurements.

Velocity updates should follow the form v_i = v_i_prev + a_i*dt where v_i is the updated i-component of the velocity vector, v_i_prev is the previous loop's i-component of the velocity vector, a_i is the i-component of the IMU's linear acceleration measurement, and dt is the time difference between the current loop and the previous loop.

Position updates should follow the form r_i = r_i_prev + v_i_prev*dt + 0.5*a_i*dt^2.

Attitude Euler angles should be a straightforward overwrite of the current values with the latest IMU estimate, since the IMU handles absolute attitude estimation itself.

Note: We do not need to update the mass estimate throughout the flight.

[mdb15106]

Relevant information:

• This should be implemented in the case (NAVIGATION) block in main::loop.
• The sensor acceleration measurement variable is Workspace::a_body_wrt_inertial_in_inertial.
• The position vector to be updated is Workspace::r_body_in_inertial.
• The velocity vector to be updated is Workspace::v_body_wrt_inertial_in_inertial.
• The Euler angle vector to be updated is Workspace::attitude_euler_angles_RPY (variable name might change depening on the specific Euler angle sequence we need to pass to the controller).
• To access a Workspace variable from main.ino, use ws.variable_name.