From 2f80775b50174070ab463896e73bfef827d00516 Mon Sep 17 00:00:00 2001
From: HaraldSchafer <harald.the.engineer@gmail.com>
Date: Tue, 18 May 2021 11:12:02 -0700
Subject: [PATCH] Filter only initialized if up for 1sec (#20940)

* add time since reset check

* fix time since reset

* update ref

Co-authored-by: Willem Melching <willem.melching@gmail.com>
---
 selfdrive/locationd/locationd.cc         | 12 ++++++++++--
 selfdrive/locationd/locationd.h          |  1 +
 selfdrive/test/process_replay/ref_commit |  2 +-
 3 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc
index b9749bb70d..176aac2380 100755
--- a/selfdrive/locationd/locationd.cc
+++ b/selfdrive/locationd/locationd.cc
@@ -13,6 +13,8 @@ const double TRANS_SANITY_CHECK = 200.0;  // m/s
 const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
 const double ALTITUDE_SANITY_CHECK = 10000; // m
 const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
+const double VALID_TIME_SINCE_RESET = 1.0; // s
+const double VALID_POS_STD = 50.0; // m
 
 static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
   VectorXd res(floatlist.size());
@@ -160,9 +162,11 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   //fix.setGpsTimeOfWeek(this->time.tow);
   fix.setUnixTimestampMillis(this->unix_timestamp_millis);
 
-  if (fix_ecef_std.norm() < 50.0 && this->calibrated) {
+  double time_since_reset = this->kf->get_filter_time() - this->last_reset_time;
+  fix.setTimeSinceReset(time_since_reset);
+  if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) {
     fix.setStatus(cereal::LiveLocationKalman::Status::VALID);
-  } else if (fix_ecef_std.norm() < 50.0) {
+  } else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) {
     fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED);
   } else {
     fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED);
@@ -351,6 +355,9 @@ void Localizer::finite_check(double current_time) {
 }
 
 void Localizer::time_check(double current_time) {
+  if (isnan(this->last_reset_time)) {
+    this->last_reset_time = current_time;
+  }
   double filter_time = this->kf->get_filter_time();
   bool big_time_gap = !isnan(filter_time) && (current_time - filter_time > 10);
   if (big_time_gap){
@@ -367,6 +374,7 @@ void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd
   init_x.head(3) = init_pos;
 
   this->kf->init_state(init_x, init_P, current_time);
+  this->last_reset_time = current_time;
 }
 
 void Localizer::handle_msg_bytes(const char *data, const size_t size) {
diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h
index 58030cd921..f9e3645fb0 100755
--- a/selfdrive/locationd/locationd.h
+++ b/selfdrive/locationd/locationd.h
@@ -55,6 +55,7 @@ private:
   bool calibrated = false;
 
   double car_speed = 0.0;
+  double last_reset_time = NAN;
   std::deque<double> posenet_stds;
 
   std::unique_ptr<LocalCoord> converter;
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index aa4c8594ea..e6cf70ff74 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-80151d0d0e0abb0adf07e7fee01209752827032c
\ No newline at end of file
+1b4fcd6a2a06c406d34787b66725b660001266ed
\ No newline at end of file