expose filter internal states and stds (#22564)

selfdrive/locationd/liblocationd.cc

@@ -19,4 +19,12 @@ extern "C" {
   void localizer_handle_msg_bytes(Localizer *localizer, const char *data, size_t size) {
     localizer->handle_msg_bytes(data, size);
   }
+
+  void get_filter_internals(Localizer *localizer, double *state_buff, double *std_buff){
+    Eigen::VectorXd state = localizer->get_state();
+    memcpy(state_buff, state.data(), sizeof(double) * state.size());
+    Eigen::VectorXd stdev = localizer->get_stdev();
+    memcpy(std_buff, stdev.data(), sizeof(double) * stdev.size());
+  }
+
 }

selfdrive/locationd/locationd.cc

@@ -187,6 +187,14 @@ VectorXd Localizer::get_position_geodetic() {
   return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt);
 }
 
+VectorXd Localizer::get_state() {
+  return this->kf->get_x();
+}
+
+VectorXd Localizer::get_stdev() {
+  return this->kf->get_P().diagonal().array().sqrt();
+}
+
 void Localizer::handle_sensors(double current_time, const capnp::List<cereal::SensorEventData, capnp::Kind::STRUCT>::Reader& log) {
   // TODO does not yet account for double sensor readings in the log
   for (int i = 0; i < log.size(); i++) {

selfdrive/locationd/locationd.h

@@ -38,6 +38,8 @@ public:
   void build_live_location(cereal::LiveLocationKalman::Builder& fix);
 
   Eigen::VectorXd get_position_geodetic();
+  Eigen::VectorXd get_state();
+  Eigen::VectorXd get_stdev();
 
   void handle_msg_bytes(const char *data, const size_t size);
   void handle_msg(const cereal::Event::Reader& log);