From 80600a77dc4bbb6af8de1d292e0814f1e946e378 Mon Sep 17 00:00:00 2001
From: Vivek Aithal <vivekaithal44@gmail.com>
Date: Thu, 14 Oct 2021 19:44:31 -0700
Subject: [PATCH] expose filter internal states and stds (#22564)

old-commit-hash: f1546e6552fdf51b99cb44eafff5f88b1de536f1
---
 selfdrive/locationd/liblocationd.cc | 8 ++++++++
 selfdrive/locationd/locationd.cc    | 8 ++++++++
 selfdrive/locationd/locationd.h     | 2 ++
 3 files changed, 18 insertions(+)

diff --git a/selfdrive/locationd/liblocationd.cc b/selfdrive/locationd/liblocationd.cc
index f324649aee..250d90b382 100755
--- a/selfdrive/locationd/liblocationd.cc
+++ b/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());
+  }
+
 }
diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc
index 87657853b3..8844b37f52 100755
--- a/selfdrive/locationd/locationd.cc
+++ b/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++) {
diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h
index 04284ba3b3..60fed112cf 100755
--- a/selfdrive/locationd/locationd.h
+++ b/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);