compare carspeed float to epsilon instead of zero (#20714)
* compare carspeed float to epsilon instead of zero * update ref * add ref commit again Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com>albatross
parent
4a710bfc16
commit
d2a2ccfee4
|
@ -239,9 +239,9 @@ void Localizer::handle_car_state(double current_time, const cereal::CarState::Re
|
||||||
this->speed_counter++;
|
this->speed_counter++;
|
||||||
|
|
||||||
if (this->speed_counter % SENSOR_DECIMATION == 0) {
|
if (this->speed_counter % SENSOR_DECIMATION == 0) {
|
||||||
this->kf->predict_and_observe(current_time, OBSERVATION_ODOMETRIC_SPEED, { (VectorXd(1) << log.getVEgo()).finished() });
|
this->kf->predict_and_observe(current_time, OBSERVATION_ODOMETRIC_SPEED, { (VectorXd(1) << log.getVEgoRaw()).finished() });
|
||||||
this->car_speed = std::abs(log.getVEgo());
|
this->car_speed = std::abs(log.getVEgo());
|
||||||
if (log.getVEgo() == 0.0) { // TODO probably never really 0.0
|
if (this->car_speed < 1e-3) {
|
||||||
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
|
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -53,7 +53,7 @@ private:
|
||||||
MatrixXdr calib_from_device;
|
MatrixXdr calib_from_device;
|
||||||
bool calibrated = false;
|
bool calibrated = false;
|
||||||
|
|
||||||
int car_speed = 0;
|
double car_speed = 0.0;
|
||||||
std::deque<double> posenet_stds;
|
std::deque<double> posenet_stds;
|
||||||
|
|
||||||
std::unique_ptr<LocalCoord> converter;
|
std::unique_ptr<LocalCoord> converter;
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
bb1af9fc73c15967a4160d8899f2688b059627bf
|
381c4976c45f232f413b37945fbcd43391174089
|
Loading…
Reference in New Issue