retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Locationd 100 Hz (#20816) 

* 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>
albatross
Willem Melching 2021-05-06 11:01:58 +02:00 committed by GitHub
parent 930fe1a5ea
commit e9db5723ef
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 61 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
rednose_repo

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

69
selfdrive/locationd/locationd.cc
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
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
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
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
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
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
View File

@ -1 +1 @@
f1515b6efca6de7305d411355f286d2c6b9fb19b
fcfadaaad7f452fdbb69c1057058327b0d2004bf

2
selfdrive/test/test_onroad.py
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,