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::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);