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

Locationd 100 Hz (#20816)

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

* move normalization into core

* add quat idxs

* add big eps

* this is not safe in the filter

* more sensible

* updates to rednose

* not tested

* normalize in python too

* update rednose

* nan check

* check for infs too

* all should be finite

* update ref

* rednose pr now in master

Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com>
old-commit-hash: e9db5723ef
vw-mqb-aeb
Willem Melching 5 years ago committed by GitHub
parent 01f1d04f98
commit b4263a43fc
9 changed files with 61 additions and 64 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. 2
      rednose_repo
  2. 69
      selfdrive/locationd/locationd.cc
  3. 9
      selfdrive/locationd/locationd.h
  4. 6
      selfdrive/locationd/models/live_kf.cc
  5. 4
      selfdrive/locationd/models/live_kf.py
  6. 9
      selfdrive/locationd/models/loc_kf.py
  7. 4
      selfdrive/locationd/test/test_locationd.py
  8. 2
      selfdrive/test/process_replay/ref_commit
  9. 2
      selfdrive/test/test_onroad.py

2
rednose_repo

@ -1 +1 @@
Subproject commit f069de89f6ccd277a557fb14590f8cb0fb069a8a
Subproject commit 7fff192e234cd91f50deed12adb3616d96f12d3e

69
selfdrive/locationd/locationd.cc
Unescape View File

@ -30,6 +30,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();
@ -64,11 +75,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();
@ -88,7 +100,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
@ -170,12 +182,9 @@ 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]) });
}
}
// Accelerometer
if (sensor_reading.getSensor() == SENSOR_ACCELEROMETER && sensor_reading.getType() == SENSOR_TYPE_ACCELEROMETER) {
@ -185,12 +194,9 @@ 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]) });
}
}
}
}
void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) {
@ -239,36 +245,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->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++;
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 trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
VectorXd trans_device_std = this->device_from_calib * floatlist2vector(log.getTransStd());
VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
this->posenet_stds.pop_front();
this->posenet_stds.push_back(trans_device_std[0]);
this->posenet_stds.push_back(trans_calib_std[0]);
trans_device_std *= 10.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);
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) {
@ -285,6 +288,14 @@ void Localizer::reset_kalman(double current_time) {
this->reset_kalman(current_time, init_x.segment<4>(3), init_x.head(3));
}
void Localizer::finite_check(double current_time) {
bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all();
if (!all_finite){
LOGE("Non-finite values detected, kalman reset");
this->reset_kalman(current_time);
}
}
void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos) {
// too nonlinear to init on completely wrong
VectorXd init_x = this->kf->get_initial_x();
@ -293,11 +304,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) {
@ -322,6 +328,7 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
} else if (log.isLiveCalibration()) {
this->handle_live_calib(t, log.getLiveCalibration());
}
this->finite_check();
}
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,

9
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 {
@ -30,6 +27,7 @@ public:
void reset_kalman(double current_time = NAN);
void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos);
void finite_check(double current_time = NAN);
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
bool inputsOK, bool sensorsOK, bool gpsOK);
@ -61,9 +59,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;
};

6
selfdrive/locationd/models/live_kf.cc
Unescape View File

@ -39,8 +39,9 @@ LiveKalman::LiveKalman() {
}
// init filter
this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x), get_mapmat(initial_P),
this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(), std::vector<std::string>(), 0.2);
this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
std::vector<int>{3}, std::vector<std::string>(), 0.2);
}
void LiveKalman::init_state(VectorXd& state, VectorXd& covs_diag, double filter_time) {
@ -92,7 +93,6 @@ std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, std:
r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
break;
}
this->filter->normalize_state(STATE_ECEF_ORIENTATION_START, STATE_ECEF_ORIENTATION_END);
return r;
}

4
selfdrive/locationd/models/live_kf.py
Unescape View File

@ -55,8 +55,8 @@ class LiveKalman():
# state covariance
initial_P_diag = np.array([1e16, 1e16, 1e16,
1e6, 1e6, 1e6,
1e4, 1e4, 1e4,
10**2, 10**2, 10**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
0.05**2, 0.05**2, 0.05**2,
0.02**2,

9
selfdrive/locationd/models/loc_kf.py
Unescape View File

@ -382,9 +382,11 @@ class LocKalman():
self.computer = LstSqComputer(generated_dir, N)
self.max_tracks = max_tracks
self.quaternion_idxs = [3,] + [(self.dim_main + i*self.dim_augment + 3)for i in range(self.N)]
# init filter
self.filter = EKF_sym(generated_dir, name, Q, x_initial, P_initial, self.dim_main, self.dim_main_err,
N, self.dim_augment, self.dim_augment_err, self.maha_test_kinds)
N, self.dim_augment, self.dim_augment_err, self.maha_test_kinds, self.quaternion_idxs)
@property
def x(self):
@ -453,11 +455,6 @@ class LocKalman():
# Should not continue if the quats behave this weirdly
if not 0.1 < quat_norm < 10:
raise RuntimeError("Sir! The filter's gone all wobbly!")
self.filter.normalize_state(3, 7)
for i in range(self.N):
d1 = self.dim_main
d3 = self.dim_augment
self.filter.normalize_state(d1 + d3 * i + 3, d1 + d3 * i + 7)
return r
def get_R(self, kind, n):

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 @@
f1515b6efca6de7305d411355f286d2c6b9fb19b
fcfadaaad7f452fdbb69c1057058327b0d2004bf

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