spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

expose filter internal states and stds (#22564)

pull/22526/head
Vivek Aithal 2021-10-14 19:44:31 -07:00 committed by GitHub
parent c2a5f0ade2
commit f1546e6552
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
selfdrive/locationd/liblocationd.cc
View File

@ -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());
}
}

8
selfdrive/locationd/locationd.cc
View File

@ -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++) {

2
selfdrive/locationd/locationd.h
View File

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