m-loay / imu-and-lidar-fusion-to-localize-vehicle Goto Github PK

for i in range(len(gnss.t)):
    if abs(gnss.t[i] - imu_f.t[k]) < 0.01:
        p_est[k], v_est[k], q_est[k], p_cov[k] = measurement_update(var_gnss, p_cov[k],
                                                gnss.data[i], p_est[k], v_est[k], q_est[k])
for i in range(len(lidar.t)):
    if abs(lidar.t[i] - imu_f.t[k]) < 0.01:
        p_est[k], v_est[k], q_est[k], p_cov[k] = measurement_update(var_lidar, p_cov[k],
                                                lidar.data[i], p_est[k], v_est[k], q_est[k])