mawei
/
tsl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

locationd no GPS (#33029) 

* Pose kf draft

old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b

* Fix it

old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b

* Add translation noise

old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d

* Add gravity to acc

old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613

* Use pyx

old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa

* Indent

old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67

* Reset function

old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc

* Add device_from_ned and ned_from_device transformations

old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae

* Fix rotations

old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6

* kms

old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2

* Centripetal acceleration

old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71

* Rewrite draft

old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c

* Remove old locationd stuff

old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba

* Python process now

old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3

* Process replay fix

old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9

* Add checks for timing and validity

old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36

* Fixes

old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267

* Process replay config fixes

old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e

* static analysis fixes

old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866

* lp in latcontrol

old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca

* Fix SensorEvent name for acceleration

old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd

* Ignore sensor readings from segments with multiple imus

old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0

* Update shebang

old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797

* Replace llk with lp

old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0

* Refactor locationd scenario test

old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84

* Add more debugging tools

old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1

* Param name update

old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337

* Fix expected observations

old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75

* Handle invalid measurements

old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf

* Fix spelling

old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b

* Include observations in debug info too

old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868

* Store error instead of expected observation

old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c

* Renames

old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d

* Zero the yaw

old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179

* New state_dot for orientation

old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7

* Fix the state transformations

old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db

* Update process in test_onroad

old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617

* Test polling on cameraOdometry

old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8

* Keep the copy of x and P returned from predict

old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd

* Remove polling again

old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68

* Remove locationd.cc

old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663

* Optim in _finite_check

old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5

* Access .t once

old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399

* Move the timing check to cam odo code path

old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e

* Call all_checks only once

old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78

* Do not sort

old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025

* Check sm.updated

old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4

* Remove test_params_gps

old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858

* Increase tolerance

old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3

* Fix static

old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490

* Try separate sockets for sensors

old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1

* sensor_all_checks

old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3

* Fix static

old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e

* Set the cpu limit to 25

old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d

* Make it prettier

old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7

* Prettier

old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5

* Increase the cpu budget to 260

old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4

* Change trans std mult. 2 works better

* Update ref commit

* Update ref commit
This commit is contained in:
Kacper Rączy 2024-09-04 02:54:57 -07:00 committed by GitHub
parent 0058ea5817
commit 236dffe400
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
15 changed files with 484 additions and 1285 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cereal/log.capnp
View File

@ -1281,7 +1281,7 @@ struct LivePose {
posenetOK @5 :Bool = false;
sensorsOK @6 :Bool = false;
filterState @7 :FilterState;
debugFilterState @7 :FilterState;
struct XYZMeasurement {
x @0 :Float32;
@ -1297,6 +1297,14 @@ struct LivePose {
value @0 : List(Float64);
std @1 : List(Float64);
valid @2 : Bool;
observations @3 :List(Observation);
struct Observation {
kind @0 :Int32;
value @1 :List(Float32);
error @2 :List(Float32);
}
}
}

1
common/params.cc
View File

@ -157,6 +157,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"LastUpdateTime", PERSISTENT},
{"LiveParameters", PERSISTENT},
{"LiveTorqueParameters", PERSISTENT | DONT_LOG},
{"LocationFilterInitialState", PERSISTENT},
{"LongitudinalPersonality", PERSISTENT},
{"NetworkMetered", PERSISTENT},
{"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},

26
selfdrive/locationd/SConscript
View File

@ -1,17 +1,15 @@
Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
loc_libs = [messaging, common, 'pthread', 'dl']
Import('env', 'rednose')
# build ekf models
rednose_gen_dir = 'models/generated'
rednose_gen_deps = [
"models/constants.py",
]
live_ekf = env.RednoseCompileFilter(
target='live',
filter_gen_script='models/live_kf.py',
pose_ekf = env.RednoseCompileFilter(
target='pose',
filter_gen_script='models/pose_kf.py',
output_dir=rednose_gen_dir,
extra_gen_artifacts=['live_kf_constants.h'],
extra_gen_artifacts=[],
gen_script_deps=rednose_gen_deps,
)
car_ekf = env.RednoseCompileFilter(
@ -21,17 +19,3 @@ car_ekf = env.RednoseCompileFilter(
extra_gen_artifacts=[],
gen_script_deps=rednose_gen_deps,
)
# locationd build
locationd_sources = ["locationd.cc", "models/live_kf.cc"]
lenv = env.Clone()
# ekf filter libraries need to be linked, even if no symbols are used
if arch != "Darwin":
lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations)
lenv.Depends(locationd, rednose)
lenv.Depends(locationd, live_ekf)

716
selfdrive/locationd/locationd.cc
View File

@ -1,716 +0,0 @@
#include "selfdrive/locationd/locationd.h"
#include <sys/time.h>
#include <sys/resource.h>
#include <algorithm>
#include <cmath>
#include <vector>
using namespace EKFS;
using namespace Eigen;
ExitHandler do_exit;
const double ACCEL_SANITY_CHECK = 100.0; // m/s^2
const double ROTATION_SANITY_CHECK = 10.0; // rad/s
const double TRANS_SANITY_CHECK = 200.0; // m/s
const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
const double ALTITUDE_SANITY_CHECK = 10000; // m
const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
const double SANE_GPS_UNCERTAINTY = 1500.0; // m
const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker
const double RESET_TRACKER_DECAY = 0.99995;
const double DECAY = 0.9993; // ~10 secs to resume after a bad input
const double MAX_FILTER_REWIND_TIME = 0.8; // s
const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30;
// TODO: GPS sensor time offsets are empirically calculated
// They should be replaced with synced time from a real clock
const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
const float GPS_POS_STD_THRESHOLD = 50.0;
const float GPS_VEL_STD_THRESHOLD = 5.0;
const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
const float GPS_POS_STD_RESET_THRESHOLD = 2.0;
const float GPS_VEL_STD_RESET_THRESHOLD = 0.5;
const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
const int GPS_ORIENTATION_ERROR_RESET_CNT = 3;
const bool DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0";
template <typename ListType, typename Vector>
static Vector floatlist2vector(const ListType& floatlist) {
Vector res(floatlist.size());
for (int i = 0; i < floatlist.size(); i++) {
res[i] = floatlist[i];
}
return res;
}
template <typename ListType>
static VectorXd float64list2vector(const ListType& floatlist) {
return floatlist2vector<ListType, VectorXd>(floatlist);
}
template <typename ListType>
static VectorXf float32list2vector(const ListType& floatlist) {
return floatlist2vector<ListType, VectorXf>(floatlist);
}
static Vector4d quat2vector(const Quaterniond& quat) {
return Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
}
static Quaterniond vector2quat(const VectorXd& vec) {
return Quaterniond(vec(0), vec(1), vec(2), vec(3));
}
static void init_xyz_measurement(cereal::LivePose::XYZMeasurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) {
meas.setX(val[0]); meas.setY(val[1]); meas.setZ(val[2]);
meas.setXStd(std[0]); meas.setYStd(std[1]); meas.setZStd(std[2]);
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(LocalizerGnssSource gnss_source) {
this->kf = std::make_unique<LiveKalman>();
this->reset_kalman();
this->calib = Vector3d(0.0, 0.0, 0.0);
this->device_from_calib = MatrixXdr::Identity(3, 3);
this->calib_from_device = MatrixXdr::Identity(3, 3);
for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) {
this->posenet_stds.push_back(10.0);
}
VectorXd ecef_pos = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
this->configure_gnss_source(gnss_source);
}
void Localizer::build_live_pose(cereal::LivePose::Builder& fix) {
VectorXd predicted_state = this->kf->get_x();
MatrixXdr predicted_cov = this->kf->get_P();
VectorXd predicted_std = predicted_cov.diagonal().array().sqrt();
VectorXd fix_ecef = predicted_state.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
VectorXd fix_ecef_std = predicted_std.segment<STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START);
VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START);
VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
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 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();
MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
condensed_cov.topLeftCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
condensed_cov.topRightCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
condensed_cov.bottomRightCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
condensed_cov.bottomLeftCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size());
H_input << device_from_ecef_eul, vel_ecef;
MatrixXdr HH = this->kf->H(H_input);
MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose();
VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt();
VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt();
VectorXd nextfix_ecef = fix_ecef + vel_ecef;
VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
VectorXd accDevice = predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
VectorXd accDeviceErr = predicted_std.segment<STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START);
VectorXd angVelocityDevice = predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
VectorXd angVelocityDeviceErr = predicted_std.segment<STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START);
// write measurements to msg
init_xyz_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
init_xyz_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
init_xyz_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode);
init_xyz_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
if (DEBUG) {
cereal::LivePose::FilterState::Builder filter_state_builder = fix.initFilterState();
filter_state_builder.setValue(kj::arrayPtr(predicted_state.data(), predicted_state.size()));
filter_state_builder.setStd(kj::arrayPtr(predicted_std.data(), predicted_std.size()));
filter_state_builder.setValid(true);
}
double old_mean = 0.0, new_mean = 0.0;
int i = 0;
for (double x : this->posenet_stds) {
if (i < POSENET_STD_HIST_HALF) {
old_mean += x;
} else {
new_mean += x;
}
i++;
}
old_mean /= POSENET_STD_HIST_HALF;
new_mean /= POSENET_STD_HIST_HALF;
// experimentally found these values, no false positives in 20k minutes of driving
bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0);
fix.setPosenetOK(!(std_spike && this->car_speed > 5.0));
}
VectorXd Localizer::get_position_geodetic() {
VectorXd fix_ecef = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef);
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();
}
bool Localizer::are_inputs_ok() {
return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid;
}
void Localizer::observation_timings_invalid_reset(){
this->observation_timings_invalid = false;
}
void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) {
// TODO does not yet account for double sensor readings in the log
// Ignore empty readings (e.g. in case the magnetometer had no data ready)
if (log.getTimestamp() == 0) {
return;
}
double sensor_time = 1e-9 * log.getTimestamp();
// sensor time and log time should be close
if (std::abs(current_time - sensor_time) > 0.1) {
LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
this->observation_timings_invalid = true;
return;
} else if (!this->is_timestamp_valid(sensor_time)) {
this->observation_timings_invalid = true;
return;
}
// TODO: handle messages from two IMUs at the same time
if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
return;
}
// Gyro Uncalibrated
if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
auto v = log.getGyroUncalibrated().getV();
auto meas = Vector3d(-v[2], -v[1], -v[0]);
VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]);
float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1];
bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold;
if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
this->observation_values_invalid["gyroscope"] *= DECAY;
} else {
this->observation_values_invalid["gyroscope"] += 1.0;
}
}
// Accelerometer
if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) {
auto v = log.getAcceleration().getV();
// TODO: reduce false positives and re-enable this check
// check if device fell, estimate 10 for g
// 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
// this->device_fell |= (float64list2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
auto meas = Vector3d(-v[2], -v[1], -v[0]);
if (meas.norm() < ACCEL_SANITY_CHECK) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
this->observation_values_invalid["accelerometer"] *= DECAY;
} else {
this->observation_values_invalid["accelerometer"] += 1.0;
}
}
}
void Localizer::input_fake_gps_observations(double current_time) {
// This is done to make sure that the error estimate of the position does not blow up
// when the filter is in no-gps mode
// Steps : first predict -> observe current obs with reasonable STD
this->kf->predict(current_time);
VectorXd current_x = this->kf->get_x();
VectorXd ecef_pos = current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov();
const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov();
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
bool gps_vel_insane = (float64list2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
//this->gps_valid = false;
this->determine_gps_mode(current_time);
return;
}
double sensor_time = current_time - sensor_time_offset;
// Process message
//this->gps_valid = true;
this->gps_mode = true;
Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
this->converter = std::make_unique<LocalCoord>(geodetic);
VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
this->unix_timestamp_millis = log.getUnixTimestampMillis();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef);
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg()));
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
for (int i = 0; i < orientation_error.size(); i++) {
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
if (orientation_error(i) < 0.0) {
orientation_error(i) += 2.0 * M_PI;
}
orientation_error(i) -= M_PI;
}
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
} else if (gps_est_error > 100.0) {
LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
}
this->last_gps_msg = sensor_time;
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
this->determine_gps_mode(current_time);
return;
}
double sensor_time = log.getMeasTime() * 1e-9;
sensor_time -= this->gps_time_offset;
auto ecef_pos_v = log.getPositionECEF().getValue();
VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]);
// indexed at 0 cause all std values are the same MAE
auto ecef_pos_std = log.getPositionECEF().getStd()[0];
MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal();
auto ecef_vel_v = log.getVelocityECEF().getValue();
VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
// indexed at 0 cause all std values are the same MAE
auto ecef_vel_std = log.getVelocityECEF().getStd()[0];
MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef);
LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]};
VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector();
double bearing_rad = atan2(ned_vel[1], ned_vel[0]);
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad);
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
for (int i = 0; i < orientation_error.size(); i++) {
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
if (orientation_error(i) < 0.0) {
orientation_error(i) += 2.0 * M_PI;
}
orientation_error(i) -= M_PI;
}
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
this->determine_gps_mode(current_time);
return;
}
// prevent jumping gnss measurements (covered lots, standstill...)
bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD;
orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD;
orientation_reset &= !this->standstill;
if (orientation_reset) {
this->orientation_reset_count++;
} else {
this->orientation_reset_count = 0;
}
if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
// always reset on first gps message and if the location is off but the accuracy is high
LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
} else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) {
LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
this->orientation_reset_count = 0;
}
this->gps_mode = true;
this->last_gps_msg = sensor_time;
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) {
this->car_speed = std::abs(log.getVEgo());
this->standstill = log.getStandstill();
if (this->standstill) {
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_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) });
}
}
void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) {
VectorXd rot_device = this->device_from_calib * float64list2vector(log.getRot());
VectorXd trans_device = this->device_from_calib * float64list2vector(log.getTrans());
if (!this->is_timestamp_valid(current_time)) {
this->observation_timings_invalid = true;
return;
}
if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
VectorXd rot_calib_std = float64list2vector(log.getRotStd());
VectorXd trans_calib_std = float64list2vector(log.getTransStd());
if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
this->posenet_stds.pop_front();
this->posenet_stds.push_back(trans_calib_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;
MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal();
MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal();
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
{ rot_device }, { rot_device_cov });
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
{ trans_device }, { trans_device_cov });
this->observation_values_invalid["cameraOdometry"] *= DECAY;
this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]);
}
void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
if (!this->is_timestamp_valid(current_time)) {
this->observation_timings_invalid = true;
return;
}
if (log.getRpyCalib().size() > 0) {
auto live_calib = float64list2vector(log.getRpyCalib());
if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
this->observation_values_invalid["liveCalibration"] += 1.0;
return;
}
this->calib = live_calib;
this->device_from_calib = euler2rot(this->calib);
this->calib_from_device = this->device_from_calib.transpose();
this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED;
this->observation_values_invalid["liveCalibration"] *= DECAY;
}
}
void Localizer::reset_kalman(double current_time) {
const VectorXd &init_x = this->kf->get_initial_x();
const MatrixXdr &init_P = this->kf->get_initial_P();
this->reset_kalman(current_time, init_x, init_P);
}
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::time_check(double current_time) {
if (std::isnan(this->last_reset_time)) {
this->last_reset_time = current_time;
}
if (std::isnan(this->first_valid_log_time)) {
this->first_valid_log_time = current_time;
}
double filter_time = this->kf->get_filter_time();
bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10);
if (big_time_gap) {
LOGE("Time gap of over 10s detected, kalman reset");
this->reset_kalman(current_time);
}
}
void Localizer::update_reset_tracker() {
// reset tracker is tuned to trigger when over 1reset/10s over 2min period
if (this->is_gps_ok()) {
this->reset_tracker *= RESET_TRACKER_DECAY;
} else {
this->reset_tracker = 0.0;
}
}
void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) {
// too nonlinear to init on completely wrong
VectorXd current_x = this->kf->get_x();
MatrixXdr current_P = this->kf->get_P();
MatrixXdr init_P = this->kf->get_initial_P();
const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P();
int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
current_x.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START) = init_vel;
current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) = init_pos;
init_P.block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal();
init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START,
STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
this->reset_kalman(current_time, current_x, init_P);
}
void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) {
this->kf->init_state(init_x, init_P, current_time);
this->last_reset_time = current_time;
this->reset_tracker += 1.0;
}
void Localizer::handle_msg_bytes(const char *data, const size_t size) {
AlignedBuffer aligned_buf;
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size));
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
this->handle_msg(event);
}
void Localizer::handle_msg(const cereal::Event::Reader& log) {
double t = log.getLogMonoTime() * 1e-9;
this->time_check(t);
if (log.isAccelerometer()) {
this->handle_sensor(t, log.getAccelerometer());
} else if (log.isGyroscope()) {
this->handle_sensor(t, log.getGyroscope());
} else if (log.isGpsLocation()) {
this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
} else if (log.isGpsLocationExternal()) {
this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
//} else if (log.isGnssMeasurements()) {
// this->handle_gnss(t, log.getGnssMeasurements());
} else if (log.isCarState()) {
this->handle_car_state(t, log.getCarState());
} else if (log.isCameraOdometry()) {
this->handle_cam_odo(t, log.getCameraOdometry());
} else if (log.isLiveCalibration()) {
this->handle_live_calib(t, log.getLiveCalibration());
}
this->finite_check();
this->update_reset_tracker();
}
void Localizer::build_pose_message(
MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool msgValid) {
cereal::Event::Builder evt = msg_builder.initEvent();
evt.setValid(msgValid);
cereal::LivePose::Builder livePose = evt.initLivePose();
this->build_live_pose(livePose);
livePose.setSensorsOK(sensorsOK);
livePose.setInputsOK(inputsOK);
}
bool Localizer::is_gps_ok() {
return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0;
}
bool Localizer::critical_services_valid(const std::map<std::string, double> &critical_services) {
for (auto &kv : critical_services){
if (kv.second >= INPUT_INVALID_THRESHOLD){
return false;
}
}
return true;
}
bool Localizer::is_timestamp_valid(double current_time) {
double filter_time = this->kf->get_filter_time();
if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) {
LOGE("Observation timestamp is older than the max rewind threshold of the filter");
return false;
}
return true;
}
void Localizer::determine_gps_mode(double current_time) {
// 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode
// 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs
// 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is
VectorXd current_pos_std = this->kf->get_P().block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt();
if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){
if (this->gps_mode){
this->gps_mode = false;
this->reset_kalman(current_time);
} else {
this->input_fake_gps_observations(current_time);
}
}
}
void Localizer::configure_gnss_source(const LocalizerGnssSource &source) {
this->gnss_source = source;
if (source == LocalizerGnssSource::UBLOX) {
this->gps_std_factor = 10.0;
this->gps_variance_factor = 1.0;
this->gps_vertical_variance_factor = 1.0;
this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET;
} else {
this->gps_std_factor = 2.0;
this->gps_variance_factor = 0.0;
this->gps_vertical_variance_factor = 3.0;
this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET;
}
}
int Localizer::locationd_thread() {
Params params;
LocalizerGnssSource source;
const char* gps_location_socket;
if (params.getBool("UbloxAvailable")) {
source = LocalizerGnssSource::UBLOX;
gps_location_socket = "gpsLocationExternal";
} else {
source = LocalizerGnssSource::QCOM;
gps_location_socket = "gpsLocation";
}
this->configure_gnss_source(source);
const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
"carState", "accelerometer", "gyroscope"};
SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
PubMaster pm({"livePose"});
uint64_t cnt = 0;
bool filterInitialized = false;
const std::vector<std::string> critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"};
for (std::string service : critical_input_services) {
this->observation_values_invalid.insert({service, 0.0});
}
while (!do_exit) {
sm.update();
if (filterInitialized){
this->observation_timings_invalid_reset();
for (const char* service : service_list) {
if (sm.updated(service) && sm.valid(service)){
const cereal::Event::Reader log = sm[service];
this->handle_msg(log);
}
}
} else {
filterInitialized = sm.allAliveAndValid();
}
const char* trigger_msg = "cameraOdometry";
if (sm.updated(trigger_msg)) {
bool inputsOK = sm.allValid() && this->are_inputs_ok();
bool gpsOK = this->is_gps_ok();
bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
// Log time to first fix
if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) {
this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time);
}
MessageBuilder pose_msg_builder;
this->build_pose_message(pose_msg_builder, inputsOK, sensorsOK, filterInitialized);
kj::ArrayPtr<capnp::byte> pose_bytes = pose_msg_builder.toBytes();
pm.send("livePose", pose_bytes.begin(), pose_bytes.size());
if (cnt % 1200 == 0 && gpsOK) { // once a minute
VectorXd posGeo = this->get_position_geodetic();
std::string lastGPSPosJSON = util::string_format(
"{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
params.putNonBlocking("LastGPSPosition", lastGPSPosJSON);
}
cnt++;
}
}
return 0;
}
int main() {
util::set_realtime_priority(5);
Localizer localizer;
return localizer.locationd_thread();
}

321
selfdrive/locationd/locationd.py Executable file
View File

@ -0,0 +1,321 @@
#!/usr/bin/env python3
import os
import json
import time
import capnp
import numpy as np
from enum import Enum
from collections import defaultdict
from cereal import log, messaging
from openpilot.common.transformations.orientation import rot_from_euler
from openpilot.common.realtime import config_realtime_process
from openpilot.common.params import Params
from openpilot.selfdrive.locationd.helpers import rotate_std
from openpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States
from openpilot.selfdrive.locationd.models.constants import ObservationKind, GENERATED_DIR
ACCEL_SANITY_CHECK = 100.0 # m/s^2
ROTATION_SANITY_CHECK = 10.0 # rad/s
TRANS_SANITY_CHECK = 200.0 # m/s
CALIB_RPY_SANITY_CHECK = 0.5 # rad (+- 30 deg)
MIN_STD_SANITY_CHECK = 1e-5 # m or rad
MAX_FILTER_REWIND_TIME = 0.8 # s
MAX_SENSOR_TIME_DIFF = 0.1 # s
YAWRATE_CROSS_ERR_CHECK_FACTOR = 30
INPUT_INVALID_THRESHOLD = 0.5
INPUT_INVALID_DECAY = 0.9993 # ~10 secs to resume after a bad input
POSENET_STD_INITIAL_VALUE = 10.0
POSENET_STD_HIST_HALF = 20
def init_xyz_measurement(measurement: capnp._DynamicStructBuilder, values: np.ndarray, stds: np.ndarray, valid: bool):
assert len(values) == len(stds) == 3
for i, d in enumerate(("x", "y", "z")):
setattr(measurement, d, float(values[i]))
setattr(measurement, f"{d}Std", float(stds[i]))
measurement.valid = valid
class HandleLogResult(Enum):
SUCCESS = 0
TIMING_INVALID = 1
INPUT_INVALID = 2
SENSOR_SOURCE_INVALID = 3
class LocationEstimator:
def __init__(self, debug: bool):
self.kf = PoseKalman(GENERATED_DIR, MAX_FILTER_REWIND_TIME)
self.debug = debug
self.posenet_stds = [POSENET_STD_INITIAL_VALUE] * (POSENET_STD_HIST_HALF * 2)
self.car_speed = 0.0
self.camodo_yawrate_distribution = np.array([0.0, 10.0]) # mean, std
self.device_from_calib = np.eye(3)
obs_kinds = [ObservationKind.PHONE_ACCEL, ObservationKind.PHONE_GYRO, ObservationKind.CAMERA_ODO_ROTATION, ObservationKind.CAMERA_ODO_TRANSLATION]
self.observations = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds}
self.observation_errors = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds}
def reset(self, t: float, x_initial: np.ndarray = PoseKalman.initial_x, P_initial: np.ndarray = PoseKalman.initial_P):
self.kf.reset(t, x_initial, P_initial)
def _validate_sensor_source(self, source: log.SensorEventData.SensorSource):
# some segments have two IMUs, ignore the second one
return source != log.SensorEventData.SensorSource.bmx055
def _validate_sensor_time(self, sensor_time: float, t: float):
# ignore empty readings
if sensor_time == 0:
return False
# sensor time and log time should be close
sensor_time_invalid = abs(sensor_time - t) > MAX_SENSOR_TIME_DIFF
if sensor_time_invalid:
print("Sensor reading ignored, sensor timestamp more than 100ms off from log time")
return not sensor_time_invalid
def _validate_timestamp(self, t: float):
kf_t = self.kf.t
invalid = not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME
if invalid:
print("Observation timestamp is older than the max rewind threshold of the filter")
return not invalid
def _finite_check(self, t: float, new_x: np.ndarray, new_P: np.ndarray):
all_finite = np.isfinite(new_x).all() and np.isfinite(new_P).all()
if not all_finite:
print("Non-finite values detected, kalman reset")
self.reset(t)
def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> HandleLogResult:
new_x, new_P = None, None
if which == "accelerometer" and msg.which() == "acceleration":
sensor_time = msg.timestamp * 1e-9
if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time):
return HandleLogResult.TIMING_INVALID
if not self._validate_sensor_source(msg.source):
return HandleLogResult.SENSOR_SOURCE_INVALID
v = msg.acceleration.v
meas = np.array([-v[2], -v[1], -v[0]])
if np.linalg.norm(meas) >= ACCEL_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
acc_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_ACCEL, meas)
if acc_res is not None:
_, new_x, _, new_P, _, _, (acc_err,), _, _ = acc_res
self.observation_errors[ObservationKind.PHONE_ACCEL] = np.array(acc_err)
self.observations[ObservationKind.PHONE_ACCEL] = meas
elif which == "gyroscope" and msg.which() == "gyroUncalibrated":
sensor_time = msg.timestamp * 1e-9
if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time):
return HandleLogResult.TIMING_INVALID
if not self._validate_sensor_source(msg.source):
return HandleLogResult.SENSOR_SOURCE_INVALID
v = msg.gyroUncalibrated.v
meas = np.array([-v[2], -v[1], -v[0]])
gyro_bias = self.kf.x[States.GYRO_BIAS]
gyro_camodo_yawrate_err = np.abs((meas[2] - gyro_bias[2]) - self.camodo_yawrate_distribution[0])
gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * self.camodo_yawrate_distribution[1]
gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold
if np.linalg.norm(meas) >= ROTATION_SANITY_CHECK or not gyro_valid:
return HandleLogResult.INPUT_INVALID
gyro_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_GYRO, meas)
if gyro_res is not None:
_, new_x, _, new_P, _, _, (gyro_err,), _, _ = gyro_res
self.observation_errors[ObservationKind.PHONE_GYRO] = np.array(gyro_err)
self.observations[ObservationKind.PHONE_GYRO] = meas
elif which == "carState":
self.car_speed = abs(msg.vEgo)
elif which == "liveCalibration":
if len(msg.rpyCalib) > 0:
calib = np.array(msg.rpyCalib)
if calib.min() < -CALIB_RPY_SANITY_CHECK or calib.max() > CALIB_RPY_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
self.device_from_calib = rot_from_euler(calib)
self.calibrated = msg.calStatus == log.LiveCalibrationData.Status.calibrated
elif which == "cameraOdometry":
if not self._validate_timestamp(t):
return HandleLogResult.TIMING_INVALID
rot_device = np.matmul(self.device_from_calib, np.array(msg.rot))
trans_device = np.matmul(self.device_from_calib, np.array(msg.trans))
if np.linalg.norm(rot_device) > ROTATION_SANITY_CHECK or np.linalg.norm(trans_device) > TRANS_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
rot_calib_std = np.array(msg.rotStd)
trans_calib_std = np.array(msg.transStd)
if rot_calib_std.min() <= MIN_STD_SANITY_CHECK or trans_calib_std.min() <= MIN_STD_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
if np.linalg.norm(rot_calib_std) > 10 * ROTATION_SANITY_CHECK or np.linalg.norm(trans_calib_std) > 10 * TRANS_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
self.posenet_stds.pop(0)
self.posenet_stds.append(trans_calib_std[0])
# Multiply by N to avoid to high certainty in kalman filter because of temporally correlated noise
rot_calib_std *= 10
trans_calib_std *= 2
rot_device_std = rotate_std(self.device_from_calib, rot_calib_std)
trans_device_std = rotate_std(self.device_from_calib, trans_calib_std)
rot_device_noise = rot_device_std ** 2
trans_device_noise = trans_device_std ** 2
cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, rot_device_noise)
cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, trans_device_noise)
self.camodo_yawrate_distribution = np.array([rot_device[2], rot_device_std[2]])
if cam_odo_rot_res is not None:
_, new_x, _, new_P, _, _, (cam_odo_rot_err,), _, _ = cam_odo_rot_res
self.observation_errors[ObservationKind.CAMERA_ODO_ROTATION] = np.array(cam_odo_rot_err)
self.observations[ObservationKind.CAMERA_ODO_ROTATION] = rot_device
if cam_odo_trans_res is not None:
_, new_x, _, new_P, _, _, (cam_odo_trans_err,), _, _ = cam_odo_trans_res
self.observation_errors[ObservationKind.CAMERA_ODO_TRANSLATION] = np.array(cam_odo_trans_err)
self.observations[ObservationKind.CAMERA_ODO_TRANSLATION] = trans_device
if new_x is not None and new_P is not None:
self._finite_check(t, new_x, new_P)
return HandleLogResult.SUCCESS
def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_valid: bool):
state, cov = self.kf.x, self.kf.P
std = np.sqrt(np.diag(cov))
orientation_ned, orientation_ned_std = state[States.NED_ORIENTATION], std[States.NED_ORIENTATION]
velocity_device, velocity_device_std = state[States.DEVICE_VELOCITY], std[States.DEVICE_VELOCITY]
angular_velocity_device, angular_velocity_device_std = state[States.ANGULAR_VELOCITY], std[States.ANGULAR_VELOCITY]
acceleration_device, acceleration_device_std = state[States.ACCELERATION], std[States.ACCELERATION]
msg = messaging.new_message("livePose")
msg.valid = filter_valid
livePose = msg.livePose
init_xyz_measurement(livePose.orientationNED, orientation_ned, orientation_ned_std, filter_valid)
init_xyz_measurement(livePose.velocityDevice, velocity_device, velocity_device_std, filter_valid)
init_xyz_measurement(livePose.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid)
init_xyz_measurement(livePose.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid)
if self.debug:
livePose.debugFilterState.value = state.tolist()
livePose.debugFilterState.std = std.tolist()
livePose.debugFilterState.valid = filter_valid
livePose.debugFilterState.observations = [
{'kind': k, 'value': self.observations[k].tolist(), 'error': self.observation_errors[k].tolist()}
for k in self.observations.keys()
]
old_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST_HALF])
new_mean = np.mean(self.posenet_stds[POSENET_STD_HIST_HALF:])
std_spike = (new_mean / old_mean) > 4.0 and new_mean > 7.0
livePose.inputsOK = inputs_valid
livePose.posenetOK = not std_spike or self.car_speed <= 5.0
livePose.sensorsOK = sensors_valid
return msg
def sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, simulation):
cur_time = time.monotonic()
for which, msgs in [("accelerometer", acc_msgs), ("gyroscope", gyro_msgs)]:
if len(msgs) > 0:
sensor_valid[which] = msgs[-1].valid
sensor_recv_time[which] = cur_time
if not simulation:
sensor_alive[which] = (cur_time - sensor_recv_time[which]) < 0.1
else:
sensor_alive[which] = len(msgs) > 0
return all(sensor_alive.values()) and all(sensor_valid.values())
def main():
config_realtime_process([0, 1, 2, 3], 5)
DEBUG = bool(int(os.getenv("DEBUG", "0")))
SIMULATION = bool(int(os.getenv("SIMULATION", "0")))
pm = messaging.PubMaster(['livePose'])
sm = messaging.SubMaster(['carState', 'liveCalibration', 'cameraOdometry'], poll='cameraOdometry')
# separate sensor sockets for efficiency
sensor_sockets = [messaging.sub_sock(which, timeout=20) for which in ['accelerometer', 'gyroscope']]
sensor_alive, sensor_valid, sensor_recv_time = defaultdict(bool), defaultdict(bool), defaultdict(float)
params = Params()
estimator = LocationEstimator(DEBUG)
filter_initialized = False
critcal_services = ["accelerometer", "gyroscope", "liveCalibration", "cameraOdometry"]
observation_timing_invalid = False
observation_input_invalid = defaultdict(int)
initial_pose = params.get("LocationFilterInitialState")
if initial_pose is not None:
initial_pose = json.loads(initial_pose)
x_initial = np.array(initial_pose["x"], dtype=np.float64)
P_initial = np.diag(np.array(initial_pose["P"], dtype=np.float64))
estimator.reset(None, x_initial, P_initial)
while True:
sm.update()
acc_msgs, gyro_msgs = (messaging.drain_sock(sock) for sock in sensor_sockets)
if filter_initialized:
observation_timing_invalid = False
msgs = []
for msg in acc_msgs + gyro_msgs:
t, valid, which, data = msg.logMonoTime, msg.valid, msg.which(), getattr(msg, msg.which())
msgs.append((t, valid, which, data))
for which, updated in sm.updated.items():
if not updated:
continue
t, valid, data = sm.logMonoTime[which], sm.valid[which], sm[which]
msgs.append((t, valid, which, data))
for log_mono_time, valid, which, msg in sorted(msgs, key=lambda x: x[0]):
if valid:
t = log_mono_time * 1e-9
res = estimator.handle_log(t, which, msg)
if res == HandleLogResult.TIMING_INVALID:
observation_timing_invalid = True
elif res == HandleLogResult.INPUT_INVALID:
observation_input_invalid[which] += 1
else:
observation_input_invalid[which] *= INPUT_INVALID_DECAY
else:
filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
if sm.updated["cameraOdometry"]:
critical_service_inputs_valid = all(observation_input_invalid[s] < INPUT_INVALID_THRESHOLD for s in critcal_services)
inputs_valid = sm.all_valid() and critical_service_inputs_valid and not observation_timing_invalid
sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized)
pm.send("livePose", msg)
if __name__ == "__main__":
main()

122
selfdrive/locationd/models/live_kf.cc
View File

@ -1,122 +0,0 @@
#include "selfdrive/locationd/models/live_kf.h"
using namespace EKFS;
using namespace Eigen;
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
}
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
}
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
std::vector<Eigen::Map<Eigen::VectorXd>> res;
for (const Eigen::VectorXd &vec : vec_vec) {
res.push_back(get_mapvec(vec));
}
return res;
}
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
std::vector<Eigen::Map<MatrixXdr>> res;
for (const MatrixXdr &mat : mat_vec) {
res.push_back(get_mapmat(mat));
}
return res;
}
LiveKalman::LiveKalman() {
this->dim_state = live_initial_x.rows();
this->dim_state_err = live_initial_P_diag.rows();
this->initial_x = live_initial_x;
this->initial_P = live_initial_P_diag.asDiagonal();
this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal();
this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal();
this->reset_orientation_P = live_reset_orientation_diag.asDiagonal();
this->Q = live_Q_diag.asDiagonal();
for (auto& pair : live_obs_noise_diag) {
this->obs_noise[pair.first] = pair.second.asDiagonal();
}
// 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<int>{3}, std::vector<std::string>(), 0.8);
}
void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
MatrixXdr covs = covs_diag.asDiagonal();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
void LiveKalman::init_state(const VectorXd &state, double filter_time) {
MatrixXdr covs = this->filter->covs();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
VectorXd LiveKalman::get_x() {
return this->filter->state();
}
MatrixXdr LiveKalman::get_P() {
return this->filter->covs();
}
double LiveKalman::get_filter_time() {
return this->filter->get_filter_time();
}
std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
std::vector<MatrixXdr> R;
for (int i = 0; i < n; i++) {
R.push_back(this->obs_noise[kind]);
}
return R;
}
std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
std::optional<Estimate> r;
if (R.size() == 0) {
R = this->get_R(kind, meas.size());
}
r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
return r;
}
void LiveKalman::predict(double t) {
this->filter->predict(t);
}
const Eigen::VectorXd &LiveKalman::get_initial_x() {
return this->initial_x;
}
const MatrixXdr &LiveKalman::get_initial_P() {
return this->initial_P;
}
const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() {
return this->fake_gps_pos_cov;
}
const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() {
return this->fake_gps_vel_cov;
}
const MatrixXdr &LiveKalman::get_reset_orientation_P() {
return this->reset_orientation_P;
}
MatrixXdr LiveKalman::H(const VectorXd &in) {
assert(in.size() == 6);
Matrix<double, 3, 6, Eigen::RowMajor> res;
this->filter->get_extra_routine("H")((double*)in.data(), res.data());
return res;
}

66
selfdrive/locationd/models/live_kf.h
View File

@ -1,66 +0,0 @@
#pragma once
#include <string>
#include <cmath>
#include <memory>
#include <unordered_map>
#include <vector>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include "generated/live_kf_constants.h"
#include "rednose/helpers/ekf_sym.h"
#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth)
using namespace EKFS;
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
class LiveKalman {
public:
LiveKalman();
void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
void init_state(const Eigen::VectorXd &state, double filter_time);
Eigen::VectorXd get_x();
MatrixXdr get_P();
double get_filter_time();
std::vector<MatrixXdr> get_R(int kind, int n);
std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
void predict(double t);
const Eigen::VectorXd &get_initial_x();
const MatrixXdr &get_initial_P();
const MatrixXdr &get_fake_gps_pos_cov();
const MatrixXdr &get_fake_gps_vel_cov();
const MatrixXdr &get_reset_orientation_P();
MatrixXdr H(const Eigen::VectorXd &in);
private:
std::string name = "live";
std::shared_ptr<EKFSym> filter;
int dim_state;
int dim_state_err;
Eigen::VectorXd initial_x;
MatrixXdr initial_P;
MatrixXdr fake_gps_pos_cov;
MatrixXdr fake_gps_vel_cov;
MatrixXdr reset_orientation_P;
MatrixXdr Q; // process noise
std::unordered_map<int, MatrixXdr> obs_noise;
};

242
selfdrive/locationd/models/live_kf.py
View File

@ -1,242 +0,0 @@
#!/usr/bin/env python3
import sys
import os
import numpy as np
from openpilot.selfdrive.locationd.models.constants import ObservationKind
import sympy as sp
import inspect
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
from rednose.helpers.ekf_sym import gen_code
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
def numpy2eigenstring(arr):
assert(len(arr.shape) == 1)
arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '')
return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()"
class States:
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters
ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2
ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2
# Error-state has different slices because it is an ESKF
ECEF_POS_ERR = slice(0, 3)
ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error
ECEF_VELOCITY_ERR = slice(6, 9)
ANGULAR_VELOCITY_ERR = slice(9, 12)
GYRO_BIAS_ERR = slice(12, 15)
ACCELERATION_ERR = slice(15, 18)
ACC_BIAS_ERR = slice(18, 21)
class LiveKalman:
name = 'live'
initial_x = np.array([3.88e6, -3.37e6, 3.76e6,
0.42254641, -0.31238054, -0.83602975, -0.15788347, # NED [0,0,0] -> ECEF Quat
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0])
# state covariance
initial_P_diag = np.array([10**2, 10**2, 10**2,
0.01**2, 0.01**2, 0.01**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
1**2, 1**2, 1**2,
100**2, 100**2, 100**2,
0.01**2, 0.01**2, 0.01**2])
# state covariance when resetting midway in a segment
reset_orientation_diag = np.array([1**2, 1**2, 1**2])
# fake observation covariance, to ensure the uncertainty estimate of the filter is under control
fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
# process noise
Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
0.001**2, 0.001**2, 0.001**2,
0.01**2, 0.01**2, 0.01**2,
0.1**2, 0.1**2, 0.1**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
3**2, 3**2, 3**2,
0.005**2, 0.005**2, 0.005**2])
obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])}
@staticmethod
def generate_code(generated_dir):
name = LiveKalman.name
dim_state = LiveKalman.initial_x.shape[0]
dim_state_err = LiveKalman.initial_P_diag.shape[0]
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x, y, z = state[States.ECEF_POS, :]
q = state[States.ECEF_ORIENTATION, :]
v = state[States.ECEF_VELOCITY, :]
vx, vy, vz = v
omega = state[States.ANGULAR_VELOCITY, :]
vroll, vpitch, vyaw = omega
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
acceleration = state[States.ACCELERATION, :]
acc_bias = state[States.ACC_BIAS, :]
dt = sp.Symbol('dt')
# calibration and attitude rotation matrices
quat_rot = quat_rotate(*q)
# Got the quat predict equations from here
# A New Quaternion-Based Kalman Filter for
# Real-Time Attitude Estimation Using the Two-Step
# Geometrically-Intuitive Correction Algorithm
A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
[vroll, 0, vyaw, -vpitch],
[vpitch, -vyaw, 0, vroll],
[vyaw, vpitch, -vroll, 0]])
q_dot = A * q
# Time derivative of the state as a function of state
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.ECEF_POS, :] = v
state_dot[States.ECEF_ORIENTATION, :] = q_dot
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
state_err = sp.Matrix(state_err_sym)
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
# Time derivative of the state error as a function of state error and state
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
state_err_dot[States.ECEF_POS_ERR, :] = v_err
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
f_err_sym = state_err + dt * state_err_dot
# Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
# these error functions are defined so that say there
# is a nominal x and true x:
# true x = err_function(nominal x, delta x)
# delta x = inv_err_function(nominal x, true x)
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
delta_quat = sp.Matrix(np.ones(4))
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
eskf_params = [[err_function_sym, nom_x, delta_x],
[inv_err_function_sym, nom_x, true_x],
H_mod_sym, f_err_sym, state_err_sym]
#
# Observation functions
#
h_gyro_sym = sp.Matrix([
vroll + roll_bias,
vpitch + pitch_bias,
vyaw + yaw_bias])
pos = sp.Matrix([x, y, z])
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
h_acc_sym = (gravity + acceleration + acc_bias)
h_acc_stationary_sym = acceleration
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
h_pos_sym = sp.Matrix([x, y, z])
h_vel_sym = sp.Matrix([vx, vy, vz])
h_orientation_sym = q
h_relative_motion = sp.Matrix(quat_rot.T * v)
obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pos_sym, ObservationKind.ECEF_POS, None],
[h_vel_sym, ObservationKind.ECEF_VEL, None],
[h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
# this returns a sympy routine for the jacobian of the observation function of the local vel
in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz
h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
# write constants to extra header file for use in cpp
live_kf_header = "#pragma once\n\n"
live_kf_header += "#include <unordered_map>\n"
live_kf_header += "#include <eigen3/Eigen/Dense>\n\n"
for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)):
assert(slc.step is None) # unsupported
live_kf_header += f'#define STATE_{state}_START {slc.start}\n'
live_kf_header += f'#define STATE_{state}_END {slc.stop}\n'
live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
live_kf_header += "\n"
for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)):
live_kf_header += f'#define OBSERVATION_{kind} {val}\n'
live_kf_header += "\n"
live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n"
live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n"
live_kf_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {\n"
for kind, noise in LiveKalman.obs_noise_diag.items():
live_kf_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n"
live_kf_header += "};\n\n"
open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header)
if __name__ == "__main__":
generated_dir = sys.argv[2]
LiveKalman.generate_code(generated_dir)

138
selfdrive/locationd/models/pose_kf.py Executable file
View File

@ -0,0 +1,138 @@
#!/usr/bin/env python3
import sys
import numpy as np
from openpilot.selfdrive.locationd.models.constants import ObservationKind
if __name__=="__main__":
import sympy as sp
from rednose.helpers.ekf_sym import gen_code
from rednose.helpers.sympy_helpers import euler_rotate, rot_to_euler
else:
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
EARTH_G = 9.81
class States:
NED_ORIENTATION = slice(0, 3) # roll, pitch, yaw in rad
DEVICE_VELOCITY = slice(3, 6) # ned velocity in m/s
ANGULAR_VELOCITY = slice(6, 9) # roll, pitch and yaw rates in rad/s
GYRO_BIAS = slice(9, 12) # roll, pitch and yaw gyroscope biases in rad/s
ACCELERATION = slice(12, 15) # acceleration in device frame in m/s**2
ACCEL_BIAS = slice(15, 18) # Acceletometer bias in m/s**2
class PoseKalman:
name = "pose"
# state
initial_x = np.array([0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0])
# state covariance
initial_P = np.diag([0.01**2, 0.01**2, 0.01**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
1**2, 1**2, 1**2,
100**2, 100**2, 100**2,
0.01**2, 0.01**2, 0.01**2])
# process noise
Q = np.diag([0.001**2, 0.001**2, 0.001**2,
0.01**2, 0.01**2, 0.01**2,
0.1**2, 0.1**2, 0.1**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
3**2, 3**2, 3**2,
0.005**2, 0.005**2, 0.005**2])
obs_noise = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.CAMERA_ODO_TRANSLATION: np.array([0.5**2, 0.5**2, 0.5**2]),
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2])}
@staticmethod
def generate_code(generated_dir):
name = PoseKalman.name
dim_state = PoseKalman.initial_x.shape[0]
dim_state_err = PoseKalman.initial_P.shape[0]
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
roll, pitch, yaw = state[States.NED_ORIENTATION, :]
velocity = state[States.DEVICE_VELOCITY, :]
angular_velocity = state[States.ANGULAR_VELOCITY, :]
vroll, vpitch, vyaw = angular_velocity
gyro_bias = state[States.GYRO_BIAS, :]
acceleration = state[States.ACCELERATION, :]
acc_bias = state[States.ACCEL_BIAS, :]
dt = sp.Symbol('dt')
ned_from_device = euler_rotate(roll, pitch, yaw)
device_from_ned = ned_from_device.T
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.DEVICE_VELOCITY, :] = acceleration
f_sym = state + dt * state_dot
device_from_device_t1 = euler_rotate(dt*vroll, dt*vpitch, dt*vyaw)
ned_from_device_t1 = ned_from_device * device_from_device_t1
f_sym[States.NED_ORIENTATION, :] = rot_to_euler(ned_from_device_t1)
centripetal_acceleration = angular_velocity.cross(velocity)
gravity = sp.Matrix([0, 0, -EARTH_G])
h_gyro_sym = angular_velocity + gyro_bias
h_acc_sym = device_from_ned * gravity + acceleration + centripetal_acceleration + acc_bias
h_phone_rot_sym = angular_velocity
h_relative_motion_sym = velocity
obs_eqs = [
[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_relative_motion_sym, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err)
def __init__(self, generated_dir, max_rewind_age):
dim_state, dim_state_err = PoseKalman.initial_x.shape[0], PoseKalman.initial_P.shape[0]
self.filter = EKF_sym_pyx(generated_dir, self.name, PoseKalman.Q, PoseKalman.initial_x, PoseKalman.initial_P,
dim_state, dim_state_err, max_rewind_age=max_rewind_age)
@property
def x(self):
return self.filter.state()
@property
def P(self):
return self.filter.covs()
@property
def t(self):
return self.filter.get_filter_time()
def predict_and_observe(self, t, kind, data, obs_noise=None):
data = np.atleast_2d(data)
if obs_noise is None:
obs_noise = self.obs_noise[kind]
R = self._get_R(len(data), obs_noise)
return self.filter.predict_and_update_batch(t, kind, data, R)
def reset(self, t, x_init, P_init):
self.filter.init_state(x_init, P_init, t)
def _get_R(self, n, obs_noise):
dim = obs_noise.shape[0]
R = np.zeros((n, dim, dim))
for i in range(n):
R[i, :, :] = np.diag(obs_noise)
return R
if __name__ == "__main__":
generated_dir = sys.argv[2]
PoseKalman.generate_code(generated_dir)

33
selfdrive/locationd/test/test_locationd.py
View File

@ -1,13 +1,7 @@
import pytest
import json
import random
import time
import capnp
import cereal.messaging as messaging
from cereal.services import SERVICE_LIST
from openpilot.common.params import Params
from openpilot.common.transformations.coordinates import ecef2geodetic
from openpilot.system.manager.process_config import managed_processes
@ -60,30 +54,3 @@ class TestLocationdProc:
msg.logMonoTime = t
msg.valid = True
return msg
def test_params_gps(self):
random.seed(123489234)
self.params.remove('LastGPSPosition')
self.x = -2710700 + (random.random() * 1e5)
self.y = -4280600 + (random.random() * 1e5)
self.z = 3850300 + (random.random() * 1e5)
self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
# get fake messages at the correct frequency, listed in services.py
msgs = []
for sec in range(65):
for name in self.LLD_MSGS:
for j in range(int(SERVICE_LIST[name].frequency)):
msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9)))
for msg in sorted(msgs, key=lambda x: x.logMonoTime):
self.pm.send(msg.which(), msg)
if msg.which() == "cameraOdometry":
self.pm.wait_for_readers_to_update(msg.which(), 0.1, dt=0.005)
time.sleep(1) # wait for async params write
lastGPS = json.loads(self.params.get('LastGPSPosition'))
assert lastGPS['latitude'] == pytest.approx(self.lat, abs=0.001)
assert lastGPS['longitude'] == pytest.approx(self.lon, abs=0.001)
assert lastGPS['altitude'] == pytest.approx(self.alt, abs=0.001)

73
selfdrive/locationd/test/test_locationd_scenarios.py
View File

@ -21,10 +21,6 @@ JUNK_IDX = 100
class Scenario(Enum):
BASE = 'base'
GPS_OFF = 'gps_off'
GPS_OFF_MIDWAY = 'gps_off_midway'
GPS_ON_MIDWAY = 'gps_on_midway'
GPS_TUNNEL = 'gps_tunnel'
GYRO_OFF = 'gyro_off'
GYRO_SPIKE_MIDWAY = 'gyro_spike_midway'
ACCEL_OFF = 'accel_off'
@ -51,24 +47,6 @@ def run_scenarios(scenario, logs):
if scenario == Scenario.BASE:
pass
elif scenario == Scenario.GPS_OFF:
logs = sorted([x for x in logs if x.which() not in GPS_MESSAGES], key=lambda x: x.logMonoTime)
elif scenario == Scenario.GPS_OFF_MIDWAY:
non_gps = [x for x in logs if x.which() not in GPS_MESSAGES]
gps = [x for x in logs if x.which() in GPS_MESSAGES]
logs = sorted(non_gps + gps[: len(gps) // 2], key=lambda x: x.logMonoTime)
elif scenario == Scenario.GPS_ON_MIDWAY:
non_gps = [x for x in logs if x.which() not in GPS_MESSAGES]
gps = [x for x in logs if x.which() in GPS_MESSAGES]
logs = sorted(non_gps + gps[len(gps) // 2:], key=lambda x: x.logMonoTime)
elif scenario == Scenario.GPS_TUNNEL:
non_gps = [x for x in logs if x.which() not in GPS_MESSAGES]
gps = [x for x in logs if x.which() in GPS_MESSAGES]
logs = sorted(non_gps + gps[:len(gps) // 4] + gps[-len(gps) // 4:], key=lambda x: x.logMonoTime)
elif scenario == Scenario.GYRO_OFF:
logs = sorted([x for x in logs if x.which() != 'gyroscope'], key=lambda x: x.logMonoTime)
@ -116,52 +94,7 @@ class TestLocationdScenarios:
- roll: unchanged
"""
orig_data, replayed_data = run_scenarios(Scenario.BASE, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
def test_gps_off(self):
"""
Test: no GPS message for the entire segment
Expected Result:
- yaw_rate: unchanged
- roll:
- gpsOK: False
"""
orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
def test_gps_off_midway(self):
"""
Test: no GPS message for the second half of the segment
Expected Result:
- yaw_rate: unchanged
- roll:
"""
orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF_MIDWAY, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
def test_gps_on_midway(self):
"""
Test: no GPS message for the first half of the segment
Expected Result:
- yaw_rate: unchanged
- roll:
"""
orig_data, replayed_data = run_scenarios(Scenario.GPS_ON_MIDWAY, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(1.5))
def test_gps_tunnel(self):
"""
Test: no GPS message for the middle section of the segment
Expected Result:
- yaw_rate: unchanged
- roll:
"""
orig_data, replayed_data = run_scenarios(Scenario.GPS_TUNNEL, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
def test_gyro_off(self):
@ -186,7 +119,7 @@ class TestLocationdScenarios:
- inputsOK: False for some time after the spike, True for the rest
"""
orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
assert np.diff(replayed_data['inputs_flag'])[499] == -1.0
assert np.diff(replayed_data['inputs_flag'])[696] == 1.0
@ -211,5 +144,5 @@ class TestLocationdScenarios:
Expected Result: Right now, the kalman filter is not robust to small spikes like it is to gyroscope spikes.
"""
orig_data, replayed_data = run_scenarios(Scenario.ACCEL_SPIKE_MIDWAY, self.logs)
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.25))
assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))

13
selfdrive/test/process_replay/process_replay.py
View File

@ -459,13 +459,6 @@ def controlsd_config_callback(params, cfg, lr):
cfg.pubs = set(cfg.pubs) - sub_keys
def locationd_config_pubsub_callback(params, cfg, lr):
ublox = params.get_bool("UbloxAvailable")
sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", })
cfg.pubs = set(cfg.pubs) - sub_keys
CONFIGS = [
ProcessConfig(
proc_name="controlsd",
@ -531,13 +524,13 @@ CONFIGS = [
ProcessConfig(
proc_name="locationd",
pubs=[
"cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal",
"liveCalibration", "carState", "gpsLocation"
"cameraOdometry", "accelerometer", "gyroscope", "liveCalibration", "carState"
],
subs=["livePose"],
ignore=["logMonoTime"],
config_callback=locationd_config_pubsub_callback,
should_recv_callback=MessageBasedRcvCallback("cameraOdometry"),
tolerance=NUMPY_TOLERANCE,
unlocked_pubs=["accelerometer", "gyroscope"],
),
ProcessConfig(
proc_name="paramsd",

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
bf2bc833e238ec36c0519162edcaad743fad6dc5
c2022c388da6eb2e26bb23ad6009be9d5314c0be

4
selfdrive/test/test_onroad.py
View File

@ -32,7 +32,7 @@ CPU usage budget
* total CPU usage of openpilot (sum(PROCS.values())
should not exceed MAX_TOTAL_CPU
"""
MAX_TOTAL_CPU = 250. # total for all 8 cores
MAX_TOTAL_CPU = 260. # total for all 8 cores
PROCS = {
# Baseline CPU usage by process
"selfdrive.controls.controlsd": 32.0,
@ -40,7 +40,6 @@ PROCS = {
"./loggerd": 14.0,
"./encoderd": 17.0,
"./camerad": 14.5,
"./locationd": 11.0,
"selfdrive.controls.plannerd": 11.0,
"./ui": 18.0,
"selfdrive.locationd.paramsd": 9.0,
@ -51,6 +50,7 @@ PROCS = {
"system.hardware.hardwared": 3.87,
"selfdrive.locationd.calibrationd": 2.0,
"selfdrive.locationd.torqued": 5.0,
"selfdrive.locationd.locationd": 25.0,
"selfdrive.ui.soundd": 3.5,
"selfdrive.monitoring.dmonitoringd": 4.0,
"./proclogd": 1.54,

2
system/manager/process_config.py
View File

@ -59,7 +59,7 @@ procs = [
NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC),
NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)),
PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad),
NativeProcess("locationd", "selfdrive/locationd", ["./locationd"], only_onroad),
PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad),
NativeProcess("pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False),
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),
PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad),