locationd: add filterState (#28340)

* Add filterState to published messages

* Hide it behind DEBUG flag

* Update cereal

* Update cereal

cereal

@@ -1 +1 @@
-Subproject commit 3f950792d896b9c6e1846e522757abb3eae3087d
+Subproject commit 172b9b7dc07fc5e8609a0e7160e525f1eb4b1b68

selfdrive/locationd/locationd.cc

@@ -35,6 +35,8 @@ const float  GPS_VEL_STD_RESET_THRESHOLD = 0.5;
 const float  GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
 const int    GPS_ORIENTATION_ERROR_RESET_CNT = 3;
 
+const bool   DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0";
+
 static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
   VectorXd res(floatlist.size());
   for (int i = 0; i < floatlist.size(); i++) {
@@ -161,6 +163,9 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
   init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
   init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated);
+  if (DEBUG) {
+    init_measurement(fix.initFilterState(), predicted_state, predicted_std, true);
+  }
 
   double old_mean = 0.0, new_mean = 0.0;
   int i = 0;