convert locationd to c++ (#20622)
* live_kf to c++ * first locationd code * Running in process_replay * locationd handle cam_odo and live_calib * log event handlers * working message receiving * compiling message sending * correctly sending some messages * correct receiving and sending * update ref_commit with some all_alive_and_valid being false, minor fixes * fix std abs * linking on device fix * fix cpu usage test * generate kf constants and defines * fix replay test * replay without acks, cleanup * operate on bytearray messages * cleanup * send msg fix * small sleep, less flaky test * remove python locationd * review feedback * bump rednosepull/20710/head
Joost Wooning
GitHub
parent
96836eb1e5
commit
3420707ad5
16
SConstruct
16
SConstruct
|
|
@ -366,19 +366,19 @@ Export('common', 'gpucommon', 'visionipc')
|
|||
rednose_config = {
|
||||
'generated_folder': '#selfdrive/locationd/models/generated',
|
||||
'to_build': {
|
||||
'live': ('#selfdrive/locationd/models/live_kf.py', True),
|
||||
'car': ('#selfdrive/locationd/models/car_kf.py', True),
|
||||
'live': ('#selfdrive/locationd/models/live_kf.py', True, ['live_kf_constants.h']),
|
||||
'car': ('#selfdrive/locationd/models/car_kf.py', True, []),
|
||||
},
|
||||
}
|
||||
|
||||
if arch != "aarch64":
|
||||
rednose_config['to_build'].update({
|
||||
'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True),
|
||||
'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True),
|
||||
'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False),
|
||||
'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False),
|
||||
'feature_handler_5': ('#rednose/helpers/feature_handler.py', False),
|
||||
'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True),
|
||||
'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []),
|
||||
'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, []),
|
||||
'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, []),
|
||||
'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, []),
|
||||
'feature_handler_5': ('#rednose/helpers/feature_handler.py', False, []),
|
||||
'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True, []),
|
||||
})
|
||||
|
||||
Export('rednose_config')
|
||||
|
|
|
|||
|
|
@ -6,8 +6,6 @@
|
|||
|
||||
#include "coordinates.hpp"
|
||||
|
||||
#define DEG2RAD(x) ((x) * M_PI / 180.0)
|
||||
#define RAD2DEG(x) ((x) * 180.0 / M_PI)
|
||||
|
||||
|
||||
double a = 6378137; // lgtm [cpp/short-global-name]
|
||||
|
|
|
|||
|
|
@ -1,5 +1,8 @@
|
|||
#pragma once
|
||||
|
||||
#define DEG2RAD(x) ((x) * M_PI / 180.0)
|
||||
#define RAD2DEG(x) ((x) * 180.0 / M_PI)
|
||||
|
||||
struct ECEF {
|
||||
double x, y, z;
|
||||
Eigen::Vector3d to_vector(){
|
||||
|
|
@ -9,6 +12,9 @@ struct ECEF {
|
|||
|
||||
struct NED {
|
||||
double n, e, d;
|
||||
Eigen::Vector3d to_vector(){
|
||||
return Eigen::Vector3d(n, e, d);
|
||||
}
|
||||
};
|
||||
|
||||
struct Geodetic {
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
Subproject commit a0eb4d249e4739cae7e4da4c38ab81c4cafa5df9
|
||||
Subproject commit f069de89f6ccd277a557fb14590f8cb0fb069a8a
|
||||
|
|
@ -289,12 +289,15 @@ selfdrive/locationd/generated/ubx.h
|
|||
selfdrive/locationd/generated/gps.cpp
|
||||
selfdrive/locationd/generated/gps.h
|
||||
|
||||
selfdrive/locationd/locationd.py
|
||||
selfdrive/locationd/locationd.h
|
||||
selfdrive/locationd/locationd.cc
|
||||
selfdrive/locationd/paramsd.py
|
||||
selfdrive/locationd/models/.gitignore
|
||||
selfdrive/locationd/models/live_kf.py
|
||||
selfdrive/locationd/models/car_kf.py
|
||||
selfdrive/locationd/models/constants.py
|
||||
selfdrive/locationd/models/live_kf.h
|
||||
selfdrive/locationd/models/live_kf.cc
|
||||
|
||||
selfdrive/locationd/calibrationd.py
|
||||
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
ubloxd
|
||||
ubloxd_test
|
||||
params_learner
|
||||
paramsd
|
||||
paramsd
|
||||
locationd
|
||||
|
|
|
|||
|
|
@ -1,8 +1,7 @@
|
|||
Import('env', 'common', 'cereal', 'messaging')
|
||||
Import('env', 'common', 'cereal', 'messaging', 'libkf', 'transformations')
|
||||
|
||||
loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'kaitai', 'pthread']
|
||||
|
||||
|
||||
if GetOption('kaitai'):
|
||||
generated = Dir('generated').srcnode().abspath
|
||||
cmd = f"kaitai-struct-compiler --target cpp_stl --outdir {generated} $SOURCES"
|
||||
|
|
@ -10,3 +9,10 @@ if GetOption('kaitai'):
|
|||
env.Command(['generated/gps.cpp', 'generated/gps.h'], 'gps.ksy', cmd)
|
||||
|
||||
env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp"], LIBS=loc_libs)
|
||||
|
||||
ekf_sym_cc = env.Object("#rednose/helpers/ekf_sym.cc")
|
||||
lenv = env.Clone()
|
||||
lenv["_LIBFLAGS"] += f' {libkf[0].get_labspath()}'
|
||||
locationd = lenv.Program("locationd", ["locationd.cc", "models/live_kf.cc", ekf_sym_cc],
|
||||
LIBS=loc_libs + transformations)
|
||||
lenv.Depends(locationd, libkf)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,378 @@
|
|||
#include "locationd.h"
|
||||
|
||||
using namespace EKFS;
|
||||
using namespace Eigen;
|
||||
|
||||
ExitHandler do_exit;
|
||||
|
||||
static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
|
||||
VectorXd res(floatlist.size());
|
||||
for (int i = 0; i < floatlist.size(); i++) {
|
||||
res[i] = floatlist[i];
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
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_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) {
|
||||
meas.setValue(kj::arrayPtr(val.data(), val.size()));
|
||||
meas.setStd(kj::arrayPtr(std.data(), std.size()));
|
||||
meas.setValid(valid);
|
||||
}
|
||||
|
||||
Localizer::Localizer() {
|
||||
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] });
|
||||
}
|
||||
|
||||
void Localizer::build_live_location(cereal::LiveLocationKalman::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 fix_pos_geo_vec = this->get_position_geodetic();
|
||||
//fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo)
|
||||
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 device_from_ecef = quat2rot(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
|
||||
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();
|
||||
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();
|
||||
|
||||
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 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 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
|
||||
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();
|
||||
//ned_vel_std = self.converter->ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter->ecef2ned(fix_ecef + vel_ecef)
|
||||
|
||||
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);
|
||||
|
||||
Vector3d nans = Vector3d(NAN, NAN, NAN);
|
||||
|
||||
// write measurements to msg
|
||||
init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, true);
|
||||
init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, true);
|
||||
init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, true);
|
||||
init_measurement(fix.initVelocityNED(), ned_vel, nans, true);
|
||||
init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
|
||||
init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
|
||||
init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, true);
|
||||
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated);
|
||||
init_measurement(fix.initOrientationNED(), orientation_ned, nans, true);
|
||||
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
|
||||
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
|
||||
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
|
||||
init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated);
|
||||
|
||||
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));
|
||||
fix.setDeviceStable(!this->device_fell);
|
||||
this->device_fell = false;
|
||||
|
||||
//fix.setGpsWeek(this->time.week);
|
||||
//fix.setGpsTimeOfWeek(this->time.tow);
|
||||
fix.setUnixTimestampMillis(this->unix_timestamp_millis);
|
||||
|
||||
if (fix_ecef_std.norm() < 50.0 && this->calibrated) {
|
||||
fix.setStatus(cereal::LiveLocationKalman::Status::VALID);
|
||||
} else if (fix_ecef_std.norm() < 50.0) {
|
||||
fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED);
|
||||
} else {
|
||||
fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED);
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
void Localizer::handle_sensors(double current_time, const capnp::List<cereal::SensorEventData, capnp::Kind::STRUCT>::Reader& log) {
|
||||
// TODO does not yet account for double sensor readings in the log
|
||||
for (int i = 0; i < log.size(); i++) {
|
||||
const cereal::SensorEventData::Reader& sensor_reading = log[i];
|
||||
double sensor_time = 1e-9 * sensor_reading.getTimestamp();
|
||||
// TODO: handle messages from two IMUs at the same time
|
||||
if (sensor_reading.getSource() == cereal::SensorEventData::SensorSource::LSM6DS3) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 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) {
|
||||
auto v = sensor_reading.getAcceleration().getV();
|
||||
|
||||
// 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 |= (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) {
|
||||
// ignore the message if the fix is invalid
|
||||
if (log.getFlags() % 2 == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
this->last_gps_fix = current_time;
|
||||
|
||||
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;
|
||||
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(3.0 * log.getVerticalAccuracy(), 2)).asDiagonal();
|
||||
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy(), 2)).asDiagonal();
|
||||
|
||||
this->unix_timestamp_millis = log.getTimestamp();
|
||||
double gps_est_error = (this->kf->get_x().head(3) - 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);
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
|
||||
} else if (gps_est_error > 50.0) {
|
||||
LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
|
||||
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos);
|
||||
}
|
||||
|
||||
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_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.getVEgo()).finished() });
|
||||
this->car_speed = std::abs(log.getVEgo());
|
||||
if (log.getVEgo() == 0.0) { // TODO probably never really 0.0
|
||||
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());
|
||||
|
||||
this->posenet_stds.pop_front();
|
||||
this->posenet_stds.push_back(trans_device_std[0]);
|
||||
|
||||
trans_device_std *= 10.0;
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
|
||||
{ (VectorXd(trans_device.rows() + trans_device_std.rows()) << trans_device, trans_device_std).finished() });
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
|
||||
if (log.getRpyCalib().size() > 0) {
|
||||
this->calib = floatlist2vector(log.getRpyCalib());
|
||||
this->device_from_calib = euler2rot(this->calib);
|
||||
this->calib_from_device = this->device_from_calib.transpose();
|
||||
this->calibrated = log.getCalStatus() == 1;
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::reset_kalman(double current_time) {
|
||||
VectorXd init_x = this->kf->get_initial_x();
|
||||
this->reset_kalman(current_time, init_x.segment<4>(3), init_x.head(3));
|
||||
}
|
||||
|
||||
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();
|
||||
MatrixXdr init_P = this->kf->get_initial_P();
|
||||
init_x.segment<4>(3) = init_orient;
|
||||
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) {
|
||||
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;
|
||||
if (log.isSensorEvents()) {
|
||||
this->handle_sensors(t, log.getSensorEvents());
|
||||
} else if (log.isGpsLocationExternal()) {
|
||||
this->handle_gps(t, log.getGpsLocationExternal());
|
||||
} 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());
|
||||
}
|
||||
}
|
||||
|
||||
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK)
|
||||
{
|
||||
cereal::Event::Builder evt = msg_builder.initEvent();
|
||||
evt.setLogMonoTime(logMonoTime);
|
||||
cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
|
||||
this->build_live_location(liveLoc);
|
||||
liveLoc.setInputsOK(inputsOK);
|
||||
liveLoc.setSensorsOK(sensorsOK);
|
||||
liveLoc.setGpsOK(gpsOK);
|
||||
return msg_builder.toBytes();
|
||||
}
|
||||
|
||||
int Localizer::locationd_thread() {
|
||||
const std::initializer_list<const char *> service_list =
|
||||
{ "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" };
|
||||
SubMaster sm(service_list, nullptr, { "gpsLocationExternal" });
|
||||
PubMaster pm({ "liveLocationKalman" });
|
||||
|
||||
Params params;
|
||||
|
||||
while (!do_exit) {
|
||||
sm.update();
|
||||
for (const char* service : service_list) {
|
||||
if (sm.updated(service) && sm.valid(service)) {
|
||||
const cereal::Event::Reader log = sm[service];
|
||||
this->handle_msg(log);
|
||||
}
|
||||
}
|
||||
|
||||
if (sm.updated("cameraOdometry")) {
|
||||
uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime();
|
||||
bool inputsOK = sm.allAliveAndValid();
|
||||
bool sensorsOK = sm.alive("sensorEvents") && sm.valid("sensorEvents");
|
||||
bool gpsOK = (logMonoTime / 1e9) - this->last_gps_fix < 1.0;
|
||||
|
||||
MessageBuilder msg_builder;
|
||||
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK);
|
||||
pm.send("liveLocationKalman", bytes.begin(), bytes.size());
|
||||
|
||||
if (sm.frame % 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.put("LastGPSPosition", lastGPSPosJSON); // TODO write async
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main() {
|
||||
Localizer localizer;
|
||||
return localizer.locationd_thread();
|
||||
}
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
#pragma once
|
||||
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#include "messaging.hpp"
|
||||
#include "common/params.h"
|
||||
#include "common/util.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
#include "common/transformations/coordinates.hpp"
|
||||
#include "common/transformations/orientation.hpp"
|
||||
#include "selfdrive/sensord/sensors/constants.hpp"
|
||||
|
||||
#include "models/live_kf.h"
|
||||
|
||||
#define VISION_DECIMATION 2
|
||||
#define SENSOR_DECIMATION 10
|
||||
|
||||
#define POSENET_STD_HIST_HALF 20
|
||||
|
||||
class Localizer {
|
||||
public:
|
||||
Localizer();
|
||||
|
||||
int locationd_thread();
|
||||
|
||||
void reset_kalman(double current_time = NAN);
|
||||
void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos);
|
||||
|
||||
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK);
|
||||
void build_live_location(cereal::LiveLocationKalman::Builder& fix);
|
||||
|
||||
Eigen::VectorXd get_position_geodetic();
|
||||
|
||||
void handle_msg_bytes(const char *data, const size_t size);
|
||||
void handle_msg(const cereal::Event::Reader& log);
|
||||
void handle_sensors(double current_time, const capnp::List<cereal::SensorEventData, capnp::Kind::STRUCT>::Reader& log);
|
||||
void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log);
|
||||
void handle_car_state(double current_time, const cereal::CarState::Reader& log);
|
||||
void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
|
||||
void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log);
|
||||
|
||||
private:
|
||||
std::unique_ptr<LiveKalman> kf;
|
||||
|
||||
Eigen::VectorXd calib;
|
||||
MatrixXdr device_from_calib;
|
||||
MatrixXdr calib_from_device;
|
||||
bool calibrated = false;
|
||||
|
||||
int car_speed = 0;
|
||||
std::deque<double> posenet_stds;
|
||||
|
||||
std::unique_ptr<LocalCoord> converter;
|
||||
|
||||
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;
|
||||
};
|
||||
|
|
@ -1,353 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
import json
|
||||
import gc
|
||||
import numpy as np
|
||||
import sympy as sp
|
||||
import cereal.messaging as messaging
|
||||
from cereal import log
|
||||
from common.params import Params
|
||||
import common.transformations.coordinates as coord
|
||||
from common.transformations.orientation import ecef_euler_from_ned, \
|
||||
euler_from_quat, \
|
||||
ned_euler_from_ecef, \
|
||||
quat_from_euler, euler_from_rot, \
|
||||
rot_from_quat, rot_from_euler
|
||||
from rednose.helpers import KalmanError
|
||||
from selfdrive.locationd.models.live_kf import LiveKalman, States, ObservationKind
|
||||
from selfdrive.locationd.models.constants import GENERATED_DIR
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
#from datetime import datetime
|
||||
#from laika.gps_time import GPSTime
|
||||
|
||||
from sympy.utilities.lambdify import lambdify
|
||||
from rednose.helpers.sympy_helpers import euler_rotate
|
||||
|
||||
SensorSource = log.SensorEventData.SensorSource
|
||||
|
||||
|
||||
VISION_DECIMATION = 2
|
||||
SENSOR_DECIMATION = 10
|
||||
POSENET_STD_HIST = 40
|
||||
|
||||
|
||||
def to_float(arr):
|
||||
return [float(arr[0]), float(arr[1]), float(arr[2])]
|
||||
|
||||
|
||||
def get_H():
|
||||
# this returns a function to eval the jacobian
|
||||
# of the observation function of the local vel
|
||||
roll = sp.Symbol('roll')
|
||||
pitch = sp.Symbol('pitch')
|
||||
yaw = sp.Symbol('yaw')
|
||||
vx = sp.Symbol('vx')
|
||||
vy = sp.Symbol('vy')
|
||||
vz = sp.Symbol('vz')
|
||||
|
||||
h = euler_rotate(roll, pitch, yaw).T*(sp.Matrix([vx, vy, vz]))
|
||||
H = h.jacobian(sp.Matrix([roll, pitch, yaw, vx, vy, vz]))
|
||||
H_f = lambdify([roll, pitch, yaw, vx, vy, vz], H)
|
||||
return H_f
|
||||
|
||||
|
||||
class Localizer():
|
||||
def __init__(self, disabled_logs=None, dog=None):
|
||||
if disabled_logs is None:
|
||||
disabled_logs = []
|
||||
|
||||
self.kf = LiveKalman(GENERATED_DIR)
|
||||
self.reset_kalman()
|
||||
self.max_age = .1 # seconds
|
||||
self.disabled_logs = disabled_logs
|
||||
self.calib = np.zeros(3)
|
||||
self.device_from_calib = np.eye(3)
|
||||
self.calib_from_device = np.eye(3)
|
||||
self.calibrated = False
|
||||
self.H = get_H()
|
||||
|
||||
self.posenet_invalid_count = 0
|
||||
self.posenet_speed = 0
|
||||
self.car_speed = 0
|
||||
self.posenet_stds = 10*np.ones((POSENET_STD_HIST))
|
||||
|
||||
self.converter = coord.LocalCoord.from_ecef(self.kf.x[States.ECEF_POS])
|
||||
|
||||
self.unix_timestamp_millis = 0
|
||||
self.last_gps_fix = 0
|
||||
self.device_fell = False
|
||||
|
||||
@staticmethod
|
||||
def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov, calibrated):
|
||||
predicted_std = np.sqrt(np.diagonal(predicted_cov))
|
||||
|
||||
fix_ecef = predicted_state[States.ECEF_POS]
|
||||
fix_ecef_std = predicted_std[States.ECEF_POS_ERR]
|
||||
vel_ecef = predicted_state[States.ECEF_VELOCITY]
|
||||
vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR]
|
||||
fix_pos_geo = coord.ecef2geodetic(fix_ecef)
|
||||
#fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo)
|
||||
orientation_ecef = euler_from_quat(predicted_state[States.ECEF_ORIENTATION])
|
||||
orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR]
|
||||
device_from_ecef = rot_from_quat(predicted_state[States.ECEF_ORIENTATION]).T
|
||||
calibrated_orientation_ecef = euler_from_rot(calib_from_device.dot(device_from_ecef))
|
||||
|
||||
acc_calib = calib_from_device.dot(predicted_state[States.ACCELERATION])
|
||||
acc_calib_std = np.sqrt(np.diagonal(calib_from_device.dot(
|
||||
predicted_cov[States.ACCELERATION_ERR, States.ACCELERATION_ERR]).dot(
|
||||
calib_from_device.T)))
|
||||
ang_vel_calib = calib_from_device.dot(predicted_state[States.ANGULAR_VELOCITY])
|
||||
ang_vel_calib_std = np.sqrt(np.diagonal(calib_from_device.dot(
|
||||
predicted_cov[States.ANGULAR_VELOCITY_ERR, States.ANGULAR_VELOCITY_ERR]).dot(
|
||||
calib_from_device.T)))
|
||||
|
||||
vel_device = device_from_ecef.dot(vel_ecef)
|
||||
device_from_ecef_eul = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]).T
|
||||
idxs = list(range(States.ECEF_ORIENTATION_ERR.start, States.ECEF_ORIENTATION_ERR.stop)) + \
|
||||
list(range(States.ECEF_VELOCITY_ERR.start, States.ECEF_VELOCITY_ERR.stop))
|
||||
condensed_cov = predicted_cov[idxs][:, idxs]
|
||||
HH = H(*list(np.concatenate([device_from_ecef_eul, vel_ecef])))
|
||||
vel_device_cov = HH.dot(condensed_cov).dot(HH.T)
|
||||
vel_device_std = np.sqrt(np.diagonal(vel_device_cov))
|
||||
|
||||
vel_calib = calib_from_device.dot(vel_device)
|
||||
vel_calib_std = np.sqrt(np.diagonal(calib_from_device.dot(
|
||||
vel_device_cov).dot(calib_from_device.T)))
|
||||
|
||||
orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef)
|
||||
#orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
|
||||
ned_vel = converter.ecef2ned(fix_ecef + vel_ecef) - converter.ecef2ned(fix_ecef)
|
||||
#ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef)
|
||||
|
||||
fix = messaging.log.LiveLocationKalman.new_message()
|
||||
|
||||
# write measurements to msg
|
||||
measurements = [
|
||||
# measurement field, value, std, valid
|
||||
(fix.positionGeodetic, fix_pos_geo, np.nan*np.zeros(3), True),
|
||||
(fix.positionECEF, fix_ecef, fix_ecef_std, True),
|
||||
(fix.velocityECEF, vel_ecef, vel_ecef_std, True),
|
||||
(fix.velocityNED, ned_vel, np.nan*np.zeros(3), True),
|
||||
(fix.velocityDevice, vel_device, vel_device_std, True),
|
||||
(fix.accelerationDevice, predicted_state[States.ACCELERATION], predicted_std[States.ACCELERATION_ERR], True),
|
||||
(fix.orientationECEF, orientation_ecef, orientation_ecef_std, True),
|
||||
(fix.calibratedOrientationECEF, calibrated_orientation_ecef, np.nan*np.zeros(3), calibrated),
|
||||
(fix.orientationNED, orientation_ned, np.nan*np.zeros(3), True),
|
||||
(fix.angularVelocityDevice, predicted_state[States.ANGULAR_VELOCITY], predicted_std[States.ANGULAR_VELOCITY_ERR], True),
|
||||
(fix.velocityCalibrated, vel_calib, vel_calib_std, calibrated),
|
||||
(fix.angularVelocityCalibrated, ang_vel_calib, ang_vel_calib_std, calibrated),
|
||||
(fix.accelerationCalibrated, acc_calib, acc_calib_std, calibrated),
|
||||
]
|
||||
|
||||
for field, value, std, valid in measurements:
|
||||
# TODO: can we write the lists faster?
|
||||
field.value = to_float(value)
|
||||
field.std = to_float(std)
|
||||
field.valid = valid
|
||||
|
||||
return fix
|
||||
|
||||
def liveLocationMsg(self):
|
||||
fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P, self.calibrated)
|
||||
# experimentally found these values, no false positives in 20k minutes of driving
|
||||
old_mean, new_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST//2]), np.mean(self.posenet_stds[POSENET_STD_HIST//2:])
|
||||
std_spike = new_mean/old_mean > 4 and new_mean > 7
|
||||
|
||||
fix.posenetOK = not (std_spike and self.car_speed > 5)
|
||||
fix.deviceStable = not self.device_fell
|
||||
self.device_fell = False
|
||||
|
||||
#fix.gpsWeek = self.time.week
|
||||
#fix.gpsTimeOfWeek = self.time.tow
|
||||
fix.unixTimestampMillis = self.unix_timestamp_millis
|
||||
|
||||
if np.linalg.norm(fix.positionECEF.std) < 50 and self.calibrated:
|
||||
fix.status = 'valid'
|
||||
elif np.linalg.norm(fix.positionECEF.std) < 50:
|
||||
fix.status = 'uncalibrated'
|
||||
else:
|
||||
fix.status = 'uninitialized'
|
||||
return fix
|
||||
|
||||
def update_kalman(self, time, kind, meas, R=None):
|
||||
try:
|
||||
self.kf.predict_and_observe(time, kind, meas, R)
|
||||
except KalmanError:
|
||||
cloudlog.error("Error in predict and observe, kalman reset")
|
||||
self.reset_kalman()
|
||||
|
||||
def handle_gps(self, current_time, log):
|
||||
# ignore the message if the fix is invalid
|
||||
if log.flags % 2 == 0:
|
||||
return
|
||||
|
||||
self.last_gps_fix = current_time
|
||||
|
||||
self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude])
|
||||
ecef_pos = self.converter.ned2ecef([0, 0, 0])
|
||||
ecef_vel = self.converter.ned2ecef(np.array(log.vNED)) - ecef_pos
|
||||
ecef_pos_R = np.diag([(3*log.verticalAccuracy)**2]*3)
|
||||
ecef_vel_R = np.diag([(log.speedAccuracy)**2]*3)
|
||||
|
||||
#self.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3))
|
||||
self.unix_timestamp_millis = log.timestamp
|
||||
gps_est_error = np.sqrt((self.kf.x[0] - ecef_pos[0])**2 +
|
||||
(self.kf.x[1] - ecef_pos[1])**2 +
|
||||
(self.kf.x[2] - ecef_pos[2])**2)
|
||||
|
||||
orientation_ecef = euler_from_quat(self.kf.x[States.ECEF_ORIENTATION])
|
||||
orientation_ned = ned_euler_from_ecef(ecef_pos, orientation_ecef)
|
||||
orientation_ned_gps = np.array([0, 0, np.radians(log.bearingDeg)])
|
||||
orientation_error = np.mod(orientation_ned - orientation_ned_gps - np.pi, 2*np.pi) - np.pi
|
||||
initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps))
|
||||
if np.linalg.norm(ecef_vel) > 5 and np.linalg.norm(orientation_error) > 1:
|
||||
cloudlog.error("Locationd vs ubloxLocation orientation difference too large, kalman reset")
|
||||
self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat)
|
||||
self.update_kalman(current_time, ObservationKind.ECEF_ORIENTATION_FROM_GPS, initial_pose_ecef_quat)
|
||||
elif gps_est_error > 50:
|
||||
cloudlog.error("Locationd vs ubloxLocation position difference too large, kalman reset")
|
||||
self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat)
|
||||
|
||||
self.update_kalman(current_time, ObservationKind.ECEF_POS, ecef_pos, R=ecef_pos_R)
|
||||
self.update_kalman(current_time, ObservationKind.ECEF_VEL, ecef_vel, R=ecef_vel_R)
|
||||
|
||||
def handle_car_state(self, current_time, log):
|
||||
self.speed_counter += 1
|
||||
|
||||
if self.speed_counter % SENSOR_DECIMATION == 0:
|
||||
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, [log.vEgo])
|
||||
self.car_speed = abs(log.vEgo)
|
||||
if log.vEgo == 0:
|
||||
self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0])
|
||||
|
||||
def handle_cam_odo(self, current_time, log):
|
||||
self.cam_counter += 1
|
||||
|
||||
if self.cam_counter % VISION_DECIMATION == 0:
|
||||
rot_device = self.device_from_calib.dot(log.rot)
|
||||
rot_device_std = self.device_from_calib.dot(log.rotStd)
|
||||
self.update_kalman(current_time,
|
||||
ObservationKind.CAMERA_ODO_ROTATION,
|
||||
np.concatenate([rot_device, 10*rot_device_std]))
|
||||
trans_device = self.device_from_calib.dot(log.trans)
|
||||
trans_device_std = self.device_from_calib.dot(log.transStd)
|
||||
self.posenet_speed = np.linalg.norm(trans_device)
|
||||
self.posenet_stds[:-1] = self.posenet_stds[1:]
|
||||
self.posenet_stds[-1] = trans_device_std[0]
|
||||
self.update_kalman(current_time,
|
||||
ObservationKind.CAMERA_ODO_TRANSLATION,
|
||||
np.concatenate([trans_device, 10*trans_device_std]))
|
||||
|
||||
def handle_sensors(self, current_time, log):
|
||||
# TODO does not yet account for double sensor readings in the log
|
||||
for sensor_reading in log:
|
||||
sensor_time = 1e-9 * sensor_reading.timestamp
|
||||
# TODO: handle messages from two IMUs at the same time
|
||||
if sensor_reading.source == SensorSource.lsm6ds3:
|
||||
continue
|
||||
|
||||
# Gyro Uncalibrated
|
||||
if sensor_reading.sensor == 5 and sensor_reading.type == 16:
|
||||
self.gyro_counter += 1
|
||||
if self.gyro_counter % SENSOR_DECIMATION == 0:
|
||||
v = sensor_reading.gyroUncalibrated.v
|
||||
self.update_kalman(sensor_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]])
|
||||
|
||||
# Accelerometer
|
||||
if sensor_reading.sensor == 1 and sensor_reading.type == 1:
|
||||
# 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
|
||||
self.device_fell = self.device_fell or (np.linalg.norm(np.array(sensor_reading.acceleration.v) - np.array([10, 0, 0])) > 40)
|
||||
|
||||
self.acc_counter += 1
|
||||
if self.acc_counter % SENSOR_DECIMATION == 0:
|
||||
v = sensor_reading.acceleration.v
|
||||
self.update_kalman(sensor_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]])
|
||||
|
||||
def handle_live_calib(self, current_time, log):
|
||||
if len(log.rpyCalib):
|
||||
self.calib = log.rpyCalib
|
||||
self.device_from_calib = rot_from_euler(self.calib)
|
||||
self.calib_from_device = self.device_from_calib.T
|
||||
self.calibrated = log.calStatus == 1
|
||||
|
||||
def reset_kalman(self, current_time=None, init_orient=None, init_pos=None):
|
||||
self.filter_time = current_time
|
||||
init_x = LiveKalman.initial_x.copy()
|
||||
# too nonlinear to init on completely wrong
|
||||
if init_orient is not None:
|
||||
init_x[3:7] = init_orient
|
||||
if init_pos is not None:
|
||||
init_x[:3] = init_pos
|
||||
self.kf.init_state(init_x, covs=np.diag(LiveKalman.initial_P_diag), filter_time=current_time)
|
||||
|
||||
self.observation_buffer = []
|
||||
|
||||
self.gyro_counter = 0
|
||||
self.acc_counter = 0
|
||||
self.speed_counter = 0
|
||||
self.cam_counter = 0
|
||||
|
||||
|
||||
def locationd_thread(sm, pm, disabled_logs=None):
|
||||
gc.disable()
|
||||
|
||||
if disabled_logs is None:
|
||||
disabled_logs = []
|
||||
|
||||
if sm is None:
|
||||
socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration', 'carState']
|
||||
sm = messaging.SubMaster(socks, ignore_alive=['gpsLocationExternal'], ignore_avg_freq=['sensorEvents'])
|
||||
if pm is None:
|
||||
pm = messaging.PubMaster(['liveLocationKalman'])
|
||||
|
||||
params = Params()
|
||||
localizer = Localizer(disabled_logs=disabled_logs)
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
for sock, updated in sm.updated.items():
|
||||
if updated and sm.valid[sock]:
|
||||
t = sm.logMonoTime[sock] * 1e-9
|
||||
if sock == "sensorEvents":
|
||||
localizer.handle_sensors(t, sm[sock])
|
||||
elif sock == "gpsLocationExternal":
|
||||
localizer.handle_gps(t, sm[sock])
|
||||
elif sock == "carState":
|
||||
localizer.handle_car_state(t, sm[sock])
|
||||
elif sock == "cameraOdometry":
|
||||
localizer.handle_cam_odo(t, sm[sock])
|
||||
elif sock == "liveCalibration":
|
||||
localizer.handle_live_calib(t, sm[sock])
|
||||
|
||||
if sm.updated['cameraOdometry']:
|
||||
t = sm.logMonoTime['cameraOdometry']
|
||||
msg = messaging.new_message('liveLocationKalman')
|
||||
msg.logMonoTime = t
|
||||
|
||||
msg.liveLocationKalman = localizer.liveLocationMsg()
|
||||
msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid()
|
||||
msg.liveLocationKalman.sensorsOK = sm.alive['sensorEvents'] and sm.valid['sensorEvents']
|
||||
|
||||
gps_age = (t / 1e9) - localizer.last_gps_fix
|
||||
msg.liveLocationKalman.gpsOK = gps_age < 1.0
|
||||
pm.send('liveLocationKalman', msg)
|
||||
|
||||
if sm.frame % 1200 == 0 and msg.liveLocationKalman.gpsOK: # once a minute
|
||||
location = {
|
||||
'latitude': msg.liveLocationKalman.positionGeodetic.value[0],
|
||||
'longitude': msg.liveLocationKalman.positionGeodetic.value[1],
|
||||
'altitude': msg.liveLocationKalman.positionGeodetic.value[2],
|
||||
}
|
||||
params.put("LastGPSPosition", json.dumps(location))
|
||||
|
||||
|
||||
def main(sm=None, pm=None):
|
||||
locationd_thread(sm, pm)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import os
|
||||
os.environ["OMP_NUM_THREADS"] = "1"
|
||||
main()
|
||||
|
|
@ -0,0 +1,140 @@
|
|||
#include "live_kf.h"
|
||||
|
||||
using namespace EKFS;
|
||||
using namespace Eigen;
|
||||
|
||||
Eigen::Map<Eigen::VectorXd> get_mapvec(Eigen::VectorXd& vec) {
|
||||
return Eigen::Map<Eigen::VectorXd>(vec.data(), vec.rows(), vec.cols());
|
||||
}
|
||||
|
||||
Eigen::Map<MatrixXdr> get_mapmat(MatrixXdr& mat) {
|
||||
return Eigen::Map<MatrixXdr>(mat.data(), mat.rows(), mat.cols());
|
||||
}
|
||||
|
||||
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(std::vector<Eigen::VectorXd>& vec_vec) {
|
||||
std::vector<Eigen::Map<Eigen::VectorXd>> res;
|
||||
for (Eigen::VectorXd& vec : vec_vec) {
|
||||
res.push_back(get_mapvec(vec));
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(std::vector<MatrixXdr>& mat_vec) {
|
||||
std::vector<Eigen::Map<MatrixXdr>> res;
|
||||
for (MatrixXdr& mat : mat_vec) {
|
||||
res.push_back(get_mapmat(mat));
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
LiveKalman::LiveKalman() {
|
||||
this->dim_state = 23;
|
||||
this->dim_state_err = 22;
|
||||
|
||||
this->initial_x = live_initial_x;
|
||||
this->initial_P = live_initial_P_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<std::string>(), 0.2);
|
||||
}
|
||||
|
||||
void LiveKalman::init_state(VectorXd& state, 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(VectorXd& state, MatrixXdr& covs, double filter_time) {
|
||||
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
|
||||
}
|
||||
|
||||
void LiveKalman::init_state(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();
|
||||
}
|
||||
|
||||
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, std::vector<VectorXd> meas, std::vector<MatrixXdr> R) {
|
||||
std::optional<Estimate> r;
|
||||
switch (kind) {
|
||||
case OBSERVATION_CAMERA_ODO_TRANSLATION:
|
||||
r = this->predict_and_update_odo_trans(meas, t, kind);
|
||||
break;
|
||||
case OBSERVATION_CAMERA_ODO_ROTATION:
|
||||
r = this->predict_and_update_odo_rot(meas, t, kind);
|
||||
break;
|
||||
case OBSERVATION_ODOMETRIC_SPEED:
|
||||
r = this->predict_and_update_odo_speed(meas, t, kind);
|
||||
break;
|
||||
default:
|
||||
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));
|
||||
break;
|
||||
}
|
||||
this->filter->normalize_state(STATE_ECEF_ORIENTATION_START, STATE_ECEF_ORIENTATION_END);
|
||||
return r;
|
||||
}
|
||||
|
||||
std::optional<Estimate> LiveKalman::predict_and_update_odo_speed(std::vector<VectorXd> speed, double t, int kind) {
|
||||
std::vector<MatrixXdr> R;
|
||||
R.assign(speed.size(), (MatrixXdr(1, 1) << std::pow(0.2, 2)).finished().asDiagonal());
|
||||
return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(speed), get_vec_mapmat(R));
|
||||
}
|
||||
|
||||
std::optional<Estimate> LiveKalman::predict_and_update_odo_trans(std::vector<VectorXd> trans, double t, int kind) {
|
||||
std::vector<VectorXd> z;
|
||||
std::vector<MatrixXdr> R;
|
||||
for (VectorXd& trns : trans) {
|
||||
assert(trns.size() == 6); // TODO remove
|
||||
z.push_back(trns.head(3));
|
||||
R.push_back(trns.segment<3>(3).array().square().matrix().asDiagonal());
|
||||
}
|
||||
return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(z), get_vec_mapmat(R));
|
||||
}
|
||||
|
||||
std::optional<Estimate> LiveKalman::predict_and_update_odo_rot(std::vector<VectorXd> rot, double t, int kind) {
|
||||
std::vector<VectorXd> z;
|
||||
std::vector<MatrixXdr> R;
|
||||
for (VectorXd& rt : rot) {
|
||||
assert(rt.size() == 6); // TODO remove
|
||||
z.push_back(rt.head(3));
|
||||
R.push_back(rt.segment<3>(3).array().square().matrix().asDiagonal());
|
||||
}
|
||||
return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(z), get_vec_mapmat(R));
|
||||
}
|
||||
|
||||
Eigen::VectorXd LiveKalman::get_initial_x() {
|
||||
return this->initial_x;
|
||||
}
|
||||
|
||||
MatrixXdr LiveKalman::get_initial_P() {
|
||||
return this->initial_P;
|
||||
}
|
||||
|
||||
MatrixXdr LiveKalman::H(VectorXd in) {
|
||||
assert(in.size() == 6);
|
||||
Matrix<double, 3, 6, Eigen::RowMajor> res;
|
||||
this->filter->get_extra_routine("H")(in.data(), res.data());
|
||||
return res;
|
||||
}
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
|
||||
#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(Eigen::VectorXd& vec);
|
||||
Eigen::Map<MatrixXdr> get_mapmat(MatrixXdr& mat);
|
||||
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(std::vector<Eigen::VectorXd>& vec_vec);
|
||||
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(std::vector<MatrixXdr>& mat_vec);
|
||||
|
||||
class LiveKalman {
|
||||
public:
|
||||
LiveKalman();
|
||||
|
||||
void init_state(Eigen::VectorXd& state, Eigen::VectorXd& covs_diag, double filter_time);
|
||||
void init_state(Eigen::VectorXd& state, MatrixXdr& covs, double filter_time);
|
||||
void init_state(Eigen::VectorXd& state, double filter_time);
|
||||
|
||||
Eigen::VectorXd get_x();
|
||||
MatrixXdr get_P();
|
||||
std::vector<MatrixXdr> get_R(int kind, int n);
|
||||
|
||||
std::optional<Estimate> predict_and_observe(double t, int kind, 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);
|
||||
|
||||
Eigen::VectorXd get_initial_x();
|
||||
MatrixXdr get_initial_P();
|
||||
|
||||
MatrixXdr H(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 Q; // process noise
|
||||
std::unordered_map<int, MatrixXdr> obs_noise;
|
||||
};
|
||||
|
|
@ -1,21 +1,25 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
import os
|
||||
import numpy as np
|
||||
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.locationd.models.constants import ObservationKind
|
||||
|
||||
if __name__ == '__main__': # Generating sympy
|
||||
import sympy as sp
|
||||
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
|
||||
from rednose.helpers.ekf_sym import gen_code
|
||||
else:
|
||||
from rednose.helpers.ekf_sym_pyx import EKF_sym # pylint: disable=no-name-in-module, import-error
|
||||
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
|
||||
|
|
@ -60,14 +64,24 @@ class LiveKalman():
|
|||
(0.01)**2, (0.01)**2, (0.01)**2])
|
||||
|
||||
# process noise
|
||||
Q = np.diag([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,
|
||||
(0.02 / 100)**2,
|
||||
3**2, 3**2, 3**2,
|
||||
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
|
||||
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,
|
||||
(0.02 / 100)**2,
|
||||
3**2, 3**2, 3**2,
|
||||
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
|
||||
|
||||
obs_noise_diag = {ObservationKind.ODOMETRIC_SPEED: np.array([0.2**2]),
|
||||
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.IMU_FRAME: 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.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):
|
||||
|
|
@ -193,96 +207,37 @@ class LiveKalman():
|
|||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
|
||||
[h_imu_frame_sym, ObservationKind.IMU_FRAME, None]]
|
||||
|
||||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params)
|
||||
# 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])]
|
||||
|
||||
def __init__(self, generated_dir):
|
||||
self.dim_state = self.initial_x.shape[0]
|
||||
self.dim_state_err = self.initial_P_diag.shape[0]
|
||||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
|
||||
|
||||
self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
|
||||
ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
|
||||
ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]),
|
||||
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.NO_ROT: np.diag([0.005**2, 0.005**2, 0.005**2]),
|
||||
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2]),
|
||||
ObservationKind.ECEF_VEL: np.diag([.5**2, .5**2, .5**2]),
|
||||
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.diag([.2**2, .2**2, .2**2, .2**2])}
|
||||
# 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: type(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"
|
||||
|
||||
# init filter
|
||||
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err, max_rewind_age=0.2, logger=cloudlog)
|
||||
for kind, val in inspect.getmembers(ObservationKind, lambda x: type(x) == int):
|
||||
live_kf_header += f'#define OBSERVATION_{kind} {val}\n'
|
||||
live_kf_header += "\n"
|
||||
|
||||
@property
|
||||
def x(self):
|
||||
return self.filter.state()
|
||||
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_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"
|
||||
|
||||
@property
|
||||
def t(self):
|
||||
return self.filter.get_filter_time()
|
||||
|
||||
@property
|
||||
def P(self):
|
||||
return self.filter.covs()
|
||||
|
||||
def rts_smooth(self, estimates):
|
||||
return self.filter.rts_smooth(estimates, norm_quats=True)
|
||||
|
||||
def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
|
||||
if covs_diag is not None:
|
||||
P = np.diag(covs_diag)
|
||||
elif covs is not None:
|
||||
P = covs
|
||||
else:
|
||||
P = self.filter.covs()
|
||||
self.filter.init_state(state, P, filter_time)
|
||||
|
||||
def predict_and_observe(self, t, kind, meas, R=None):
|
||||
if len(meas) > 0:
|
||||
meas = np.atleast_2d(meas)
|
||||
if kind == ObservationKind.CAMERA_ODO_TRANSLATION:
|
||||
r = self.predict_and_update_odo_trans(meas, t, kind)
|
||||
elif kind == ObservationKind.CAMERA_ODO_ROTATION:
|
||||
r = self.predict_and_update_odo_rot(meas, t, kind)
|
||||
elif kind == ObservationKind.ODOMETRIC_SPEED:
|
||||
r = self.predict_and_update_odo_speed(meas, t, kind)
|
||||
else:
|
||||
if R is None:
|
||||
R = self.get_R(kind, len(meas))
|
||||
elif len(R.shape) == 2:
|
||||
R = R[None]
|
||||
r = self.filter.predict_and_update_batch(t, kind, meas, R)
|
||||
|
||||
self.filter.normalize_state(States.ECEF_ORIENTATION.start, States.ECEF_ORIENTATION.stop)
|
||||
return r
|
||||
|
||||
def get_R(self, kind, n):
|
||||
obs_noise = self.obs_noise[kind]
|
||||
dim = obs_noise.shape[0]
|
||||
R = np.zeros((n, dim, dim))
|
||||
for i in range(n):
|
||||
R[i, :, :] = obs_noise
|
||||
return R
|
||||
|
||||
def predict_and_update_odo_speed(self, speed, t, kind):
|
||||
z = np.array(speed)
|
||||
R = np.zeros((len(speed), 1, 1))
|
||||
for i, _ in enumerate(z):
|
||||
R[i, :, :] = np.diag([0.2**2])
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R)
|
||||
|
||||
def predict_and_update_odo_trans(self, trans, t, kind):
|
||||
z = trans[:, :3]
|
||||
R = np.zeros((len(trans), 3, 3))
|
||||
for i, _ in enumerate(z):
|
||||
R[i, :, :] = np.diag(trans[i, 3:]**2)
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R)
|
||||
|
||||
def predict_and_update_odo_rot(self, rot, t, kind):
|
||||
z = rot[:, :3]
|
||||
R = np.zeros((len(rot), 3, 3))
|
||||
for i, _ in enumerate(z):
|
||||
R[i, :, :] = np.diag(rot[i, 3:]**2)
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R)
|
||||
open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
|||
|
|
@ -18,11 +18,11 @@ procs = [
|
|||
NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC, persistent=EON, sigkill=EON),
|
||||
NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)),
|
||||
NativeProcess("ui", "selfdrive/ui", ["./ui"], persistent=True, watchdog_max_dt=(10 if TICI else None)),
|
||||
NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]),
|
||||
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd"),
|
||||
PythonProcess("controlsd", "selfdrive.controls.controlsd"),
|
||||
PythonProcess("deleter", "selfdrive.loggerd.deleter", persistent=True),
|
||||
PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), driverview=True),
|
||||
PythonProcess("locationd", "selfdrive.locationd.locationd"),
|
||||
PythonProcess("logmessaged", "selfdrive.logmessaged", persistent=True),
|
||||
PythonProcess("pandad", "selfdrive.pandad", persistent=True),
|
||||
PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
|
||||
|
|
|
|||
|
|
@ -432,6 +432,7 @@ def cpp_replay_process(cfg, lr):
|
|||
if not len(resp_sockets):
|
||||
while not pm.all_readers_updated(msg.which()):
|
||||
time.sleep(0)
|
||||
time.sleep(0.0001)
|
||||
|
||||
managed_processes[cfg.proc_name].stop()
|
||||
return log_msgs
|
||||
|
|
|
|||
|
|
@ -74,13 +74,11 @@ def profile(proc, func, car='vw'):
|
|||
if __name__ == '__main__':
|
||||
from selfdrive.controls.controlsd import main as controlsd_thread
|
||||
from selfdrive.controls.radard import radard_thread
|
||||
from selfdrive.locationd.locationd import locationd_thread
|
||||
from selfdrive.locationd.paramsd import main as paramsd_thread
|
||||
|
||||
procs = {
|
||||
'radard': radard_thread,
|
||||
'controlsd': controlsd_thread,
|
||||
'locationd': locationd_thread,
|
||||
'paramsd': paramsd_thread,
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@ from tools.lib.logreader import LogReader
|
|||
PROCS = {
|
||||
"selfdrive.controls.controlsd": 50.0,
|
||||
"./loggerd": 45.0,
|
||||
"selfdrive.locationd.locationd": 32.8,
|
||||
"./locationd": 3.5,
|
||||
"selfdrive.controls.plannerd": 20.0,
|
||||
"./_ui": 15.0,
|
||||
"selfdrive.locationd.paramsd": 9.1,
|
||||
|
|
@ -44,7 +44,6 @@ if TICI:
|
|||
"./loggerd": 60.0,
|
||||
"selfdrive.controls.controlsd": 26.0,
|
||||
"./camerad": 25.0,
|
||||
"selfdrive.locationd.locationd": 21.0,
|
||||
"selfdrive.controls.plannerd": 12.0,
|
||||
"selfdrive.locationd.paramsd": 5.0,
|
||||
"./_dmonitoringmodeld": 10.0,
|
||||
|
|
|
|||
Loading…
Reference in New Issue