From cbc2cb24b7f4ce53b29936b4b096d9c8abb2c6f0 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Wed, 28 Apr 2021 15:29:14 -0700 Subject: [PATCH] Locationd 100hz (#20759) * fix std transform * 100Hz * new ref * no more decimation * clean up confusing maths * static typing * Revert "static typing" This reverts commit 23d87337de648e629fbd35dd8c04a740bbefca47. * 100Hz costs more old-commit-hash: 333313cf0e6819767d8dcc4a55bea70f0354579f --- selfdrive/locationd/locationd.cc | 78 +++++++++++----------- selfdrive/locationd/locationd.h | 8 --- selfdrive/locationd/test/test_locationd.py | 4 +- selfdrive/test/process_replay/ref_commit | 2 +- selfdrive/test/test_onroad.py | 2 +- 5 files changed, 42 insertions(+), 52 deletions(-) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 00996b3198..8423339b59 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -27,6 +27,17 @@ static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder me meas.setValid(valid); } + +static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) { + // To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix + return ((rot_matrix * cov_in) * rot_matrix.transpose()); +} + +static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) { + // Stds cannot be rotated like values, only covariances can be rotated + return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt(); +} + Localizer::Localizer() { this->kf = std::make_unique(); this->reset_kalman(); @@ -61,11 +72,12 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { VectorXd calibrated_orientation_ecef = rot2euler(this->calib_from_device * device_from_ecef); VectorXd acc_calib = this->calib_from_device * predicted_state.segment(STATE_ACCELERATION_START); - VectorXd acc_calib_std = ((this->calib_from_device * predicted_cov.block(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START)) * this->calib_from_device.transpose()).diagonal().array().sqrt(); + MatrixXdr acc_calib_cov = predicted_cov.block(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START); + VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt(); VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment(STATE_ANGULAR_VELOCITY_START); - MatrixXdr vel_angular_err = predicted_cov.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START); - VectorXd ang_vel_calib_std = ((this->calib_from_device * vel_angular_err) * this->calib_from_device.transpose()).diagonal().array().sqrt(); + MatrixXdr vel_angular_cov = predicted_cov.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START); + VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt(); VectorXd vel_device = device_from_ecef * vel_ecef; VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))).transpose(); @@ -85,7 +97,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt(); VectorXd vel_calib = this->calib_from_device * vel_device; - VectorXd vel_calib_std = ((this->calib_from_device * vel_device_cov) * this->calib_from_device.transpose()).diagonal().array().sqrt(); + VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt(); VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef); //orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned @@ -167,11 +179,8 @@ void Localizer::handle_sensors(double current_time, const capnp::Listgyro_counter++; - if (this->gyro_counter % SENSOR_DECIMATION == 0) { - auto v = sensor_reading.getGyroUncalibrated().getV(); - this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) }); - } + auto v = sensor_reading.getGyroUncalibrated().getV(); + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) }); } // Accelerometer @@ -182,10 +191,7 @@ void Localizer::handle_sensors(double current_time, const capnp::Listdevice_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0; - this->acc_counter++; - if (this->acc_counter % SENSOR_DECIMATION == 0) { - this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { Vector3d(-v[2], -v[1], -v[0]) }); - } + this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { Vector3d(-v[2], -v[1], -v[0]) }); } } } @@ -236,36 +242,33 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R } void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) { - this->speed_counter++; - - if (this->speed_counter % SENSOR_DECIMATION == 0) { - this->kf->predict_and_observe(current_time, OBSERVATION_ODOMETRIC_SPEED, { (VectorXd(1) << log.getVEgoRaw()).finished() }); - this->car_speed = std::abs(log.getVEgo()); - if (this->car_speed < 1e-3) { - this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) }); - } + this->kf->predict_and_observe(current_time, OBSERVATION_ODOMETRIC_SPEED, { (VectorXd(1) << log.getVEgoRaw()).finished() }); + this->car_speed = std::abs(log.getVEgo()); + if (this->car_speed < 1e-3) { + this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) }); } } void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) { - this->cam_counter++; + VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); + VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); - if (this->cam_counter % VISION_DECIMATION == 0) { - VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); - VectorXd rot_device_std = (this->device_from_calib * floatlist2vector(log.getRotStd())) * 10.0; - this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION, - { (VectorXd(rot_device.rows() + rot_device_std.rows()) << rot_device, rot_device_std).finished() }); + VectorXd rot_calib_std = floatlist2vector(log.getRotStd()); + VectorXd trans_calib_std = floatlist2vector(log.getTransStd()); - VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); - VectorXd trans_device_std = this->device_from_calib * floatlist2vector(log.getTransStd()); + this->posenet_stds.pop_front(); + this->posenet_stds.push_back(trans_calib_std[0]); - this->posenet_stds.pop_front(); - this->posenet_stds.push_back(trans_device_std[0]); + // Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise + trans_calib_std *= 10.0; + rot_calib_std *= 10.0; + VectorXd rot_device_std = rotate_std(this->device_from_calib, rot_calib_std); + VectorXd trans_device_std = rotate_std(this->device_from_calib, trans_calib_std); - trans_device_std *= 10.0; - this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION, - { (VectorXd(trans_device.rows() + trans_device_std.rows()) << trans_device, trans_device_std).finished() }); - } + this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION, + { (VectorXd(rot_device.rows() + rot_device_std.rows()) << rot_device, rot_device_std).finished() }); + this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION, + { (VectorXd(trans_device.rows() + trans_device_std.rows()) << trans_device, trans_device_std).finished() }); } void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { @@ -290,11 +293,6 @@ void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_x.head(3) = init_pos; this->kf->init_state(init_x, init_P, current_time); - - this->gyro_counter = 0; - this->acc_counter = 0; - this->speed_counter = 0; - this->cam_counter = 0; } void Localizer::handle_msg_bytes(const char *data, const size_t size) { diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 6c00a43a10..551957737b 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -17,9 +17,6 @@ #include "models/live_kf.h" -#define VISION_DECIMATION 2 -#define SENSOR_DECIMATION 10 - #define POSENET_STD_HIST_HALF 20 class Localizer { @@ -61,9 +58,4 @@ private: int64_t unix_timestamp_millis = 0; double last_gps_fix = 0; bool device_fell = false; - - int gyro_counter = 0; - int acc_counter = 0; - int speed_counter = 0; - int cam_counter = 0; }; diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 37fbe87812..ac8aa6496a 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -12,8 +12,8 @@ from common.params import Params from selfdrive.manager.process_config import managed_processes -SENSOR_DECIMATION = 10 -VISION_DECIMATION = 2 +SENSOR_DECIMATION = 1 +VISION_DECIMATION = 1 LIBLOCATIOND_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '../liblocationd.so')) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 3393779b37..1a93c3eb8c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -17a1bec4747f2a449baed294a6fc395ddb745e52 \ No newline at end of file +d48ac24ef8432244c8074337666c17bc9bfef442 \ No newline at end of file diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index ef93d6003e..078b1cb4b0 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -20,7 +20,7 @@ from tools.lib.logreader import LogReader PROCS = { "selfdrive.controls.controlsd": 50.0, "./loggerd": 45.0, - "./locationd": 3.5, + "./locationd": 9.1, "selfdrive.controls.plannerd": 20.0, "./_ui": 15.0, "selfdrive.locationd.paramsd": 9.1,