|
|
@ -423,7 +423,7 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements: |
|
|
|
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); |
|
|
|
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
this->last_gps_msg = current_time; |
|
|
|
this->last_gps_msg = sensor_time; |
|
|
|
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); |
|
|
|
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); |
|
|
|
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); |
|
|
|
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); |
|
|
|
} |
|
|
|
} |
|
|
|