# initialisation
if i == 0:
x0 = States(p0, v0, q0, bw0, ba0, scale0, p_BC_0, q_BC_0)
cov0 = np.square(np.diag(stdevs0))
num_states = x0.size
num_meas = 7
num_control = len(u0)
kf = Filter(num_states, num_meas, num_control)
kf.dt = 0.
kf.set_states(x0)
kf.set_covariance(cov0)
current_imu = imu_traj.at_index(i)
kf.om_old = current_imu.om
kf.acc_old = current_imu.acc
old_t = t
old_ti = t
continue
# propagate
imu_queue = imu_traj.get_queue(old_t, t)
if imu_queue:
for ii, ti in enumerate(imu_queue.t):
current_imu = imu_queue.at_index(ii)
kf.dt = ti - old_ti
kf.propagate_states(current_imu)
