sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Locationd 100hz (#20759)

Browse Source 
* 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: 333313cf0e
commatwo_master
HaraldSchafer 4 years ago committed by GitHub
parent c7cca3b07e
commit cbc2cb24b7
5 changed files with 42 additions and 52 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 78
      selfdrive/locationd/locationd.cc
  2. 8
      selfdrive/locationd/locationd.h
  3. 4
      selfdrive/locationd/test/test_locationd.py
  4. 2
      selfdrive/test/process_replay/ref_commit
  5. 2
      selfdrive/test/test_onroad.py

78
selfdrive/locationd/locationd.cc
Unescape View File

@ -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<LiveKalman>();
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_LEN>(STATE_ACCELERATION_START);
VectorXd acc_calib_std = ((this->calib_from_device * predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(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_LEN, STATE_ACCELERATION_ERR_LEN>(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_LEN>(STATE_ANGULAR_VELOCITY_START);
MatrixXdr vel_angular_err = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(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_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(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_LEN>(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::List<cereal::Se
// Gyro Uncalibrated
if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
this->gyro_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::List<cereal::Se
// 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
this->device_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) {

8
selfdrive/locationd/locationd.h
Unescape View File

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

4
selfdrive/locationd/test/test_locationd.py
Unescape View File

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

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
17a1bec4747f2a449baed294a6fc395ddb745e52
d48ac24ef8432244c8074337666c17bc9bfef442

2
selfdrive/test/test_onroad.py
Unescape View File

@ -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,

Write Preview
Loading…
Cancel
Save