def predict(self, timestep=1):
"""Predict what the measurement should be in the next timestep.
:param timestep: the number of timesteps in the past that we are predicting forward *from*"""
F = np.identity(self.state_vector_size)
F[0][1] = self.loop_dt
B = np.identity(self.state_vector_size)
self.imu_deque.append(self.get_heading_state())
u = Vision.rad_to_vision_units(self.imu_deque[-timestep] -
self.imu_deque[-timestep - 1])
self.filter.predict(F, u, B)
