sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
github-actions[bot] committed
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
@@ -0,0 +1,3 @@
|
||||
params_learner
|
||||
paramsd
|
||||
locationd
|
||||
Whitespace-only changes.
BIN
Binary file not shown.
@@ -0,0 +1,750 @@
|
||||
#include "sunnypilot/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 VALID_TIME_SINCE_RESET = 1.0; // s
|
||||
const double VALID_POS_STD = 50.0; // m
|
||||
const double MAX_RESET_TRACKER = 5.0;
|
||||
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";
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
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_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();
|
||||
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();
|
||||
VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
|
||||
|
||||
VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
|
||||
MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
|
||||
VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt();
|
||||
VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
|
||||
|
||||
MatrixXdr vel_angular_cov = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START);
|
||||
VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt();
|
||||
|
||||
VectorXd vel_device = device_from_ecef * vel_ecef;
|
||||
VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
|
||||
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 = rotate_cov(this->calib_from_device, 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 calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
|
||||
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);
|
||||
|
||||
Vector3d nans = Vector3d(NAN, NAN, NAN);
|
||||
|
||||
// TODO fill in NED and Calibrated stds
|
||||
// write measurements to msg
|
||||
init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
|
||||
init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
|
||||
init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
|
||||
init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
|
||||
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, this->gps_mode);
|
||||
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
|
||||
init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode);
|
||||
init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
|
||||
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);
|
||||
if (DEBUG) {
|
||||
init_measurement(fix.initFilterState(), predicted_state, predicted_std, 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));
|
||||
fix.setDeviceStable(!this->device_fell);
|
||||
fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER);
|
||||
fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff);
|
||||
this->device_fell = false;
|
||||
|
||||
//fix.setGpsWeek(this->time.week);
|
||||
//fix.setGpsTimeOfWeek(this->time.tow);
|
||||
fix.setUnixTimestampMillis(this->unix_timestamp_millis);
|
||||
|
||||
double time_since_reset = this->kf->get_filter_time() - this->last_reset_time;
|
||||
fix.setTimeSinceReset(time_since_reset);
|
||||
if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) {
|
||||
fix.setStatus(cereal::LiveLocationKalman::Status::VALID);
|
||||
} else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) {
|
||||
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);
|
||||
}
|
||||
|
||||
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.which() == cereal::SensorEventData::GYRO_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.which() == cereal::SensorEventData::ACCELERATION) {
|
||||
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 |= (floatlist2vector(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 = (floatlist2vector(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 * floatlist2vector(log.getRot());
|
||||
VectorXd trans_device = this->device_from_calib * floatlist2vector(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 = floatlist2vector(log.getRotStd());
|
||||
VectorXd trans_calib_std = floatlist2vector(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 = floatlist2vector(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();
|
||||
}
|
||||
|
||||
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK,
|
||||
bool sensorsOK, bool gpsOK, bool msgValid) {
|
||||
cereal::Event::Builder evt = msg_builder.initEvent();
|
||||
evt.setValid(msgValid);
|
||||
cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
|
||||
this->build_live_location(liveLoc);
|
||||
liveLoc.setSensorsOK(sensorsOK);
|
||||
liveLoc.setGpsOK(gpsOK);
|
||||
liveLoc.setInputsOK(inputsOK);
|
||||
return msg_builder.toBytes();
|
||||
}
|
||||
|
||||
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({"liveLocationKalman"});
|
||||
|
||||
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 msg_builder;
|
||||
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
|
||||
pm.send("liveLocationKalman", bytes.begin(), 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("LastGPSPositionLLK", lastGPSPosJSON);
|
||||
}
|
||||
cnt++;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main() {
|
||||
util::set_realtime_priority(5);
|
||||
|
||||
Localizer localizer;
|
||||
return localizer.locationd_thread();
|
||||
}
|
||||
@@ -0,0 +1,100 @@
|
||||
#pragma once
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <deque>
|
||||
#include <fstream>
|
||||
#include <memory>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "common/params.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
#include "common/util.h"
|
||||
|
||||
#include "sunnypilot/common/transformations/coordinates.hpp"
|
||||
#include "sunnypilot/common/transformations/orientation.hpp"
|
||||
#include "sunnypilot/system/sensord/sensors/constants.h"
|
||||
#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
|
||||
|
||||
#define VISION_DECIMATION 2
|
||||
#define SENSOR_DECIMATION 10
|
||||
#define POSENET_STD_HIST_HALF 20
|
||||
|
||||
enum LocalizerGnssSource {
|
||||
UBLOX, QCOM
|
||||
};
|
||||
|
||||
class Localizer {
|
||||
public:
|
||||
Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX);
|
||||
|
||||
int locationd_thread();
|
||||
|
||||
void reset_kalman(double current_time = NAN);
|
||||
void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R);
|
||||
void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P);
|
||||
void finite_check(double current_time = NAN);
|
||||
void time_check(double current_time = NAN);
|
||||
void update_reset_tracker();
|
||||
bool is_gps_ok();
|
||||
bool critical_services_valid(const std::map<std::string, double> &critical_services);
|
||||
bool is_timestamp_valid(double current_time);
|
||||
void determine_gps_mode(double current_time);
|
||||
bool are_inputs_ok();
|
||||
void observation_timings_invalid_reset();
|
||||
|
||||
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
|
||||
void build_live_location(cereal::LiveLocationKalman::Builder& fix);
|
||||
|
||||
Eigen::VectorXd get_position_geodetic();
|
||||
Eigen::VectorXd get_state();
|
||||
Eigen::VectorXd get_stdev();
|
||||
|
||||
void handle_msg_bytes(const char *data, const size_t size);
|
||||
void handle_msg(const cereal::Event::Reader& log);
|
||||
void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log);
|
||||
void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset);
|
||||
void handle_gnss(double current_time, const cereal::GnssMeasurements::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);
|
||||
|
||||
void input_fake_gps_observations(double current_time);
|
||||
|
||||
private:
|
||||
std::unique_ptr<LiveKalman> kf;
|
||||
|
||||
Eigen::VectorXd calib;
|
||||
MatrixXdr device_from_calib;
|
||||
MatrixXdr calib_from_device;
|
||||
bool calibrated = false;
|
||||
|
||||
double car_speed = 0.0;
|
||||
double last_reset_time = NAN;
|
||||
std::deque<double> posenet_stds;
|
||||
|
||||
std::unique_ptr<LocalCoord> converter;
|
||||
|
||||
int64_t unix_timestamp_millis = 0;
|
||||
double reset_tracker = 0.0;
|
||||
bool device_fell = false;
|
||||
bool gps_mode = false;
|
||||
double first_valid_log_time = NAN;
|
||||
double ttff = NAN;
|
||||
double last_gps_msg = 0;
|
||||
LocalizerGnssSource gnss_source;
|
||||
bool observation_timings_invalid = false;
|
||||
std::map<std::string, double> observation_values_invalid;
|
||||
bool standstill = true;
|
||||
int32_t orientation_reset_count = 0;
|
||||
float gps_std_factor;
|
||||
float gps_variance_factor;
|
||||
float gps_vertical_variance_factor;
|
||||
double gps_time_offset;
|
||||
Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std
|
||||
|
||||
void configure_gnss_source(const LocalizerGnssSource &source);
|
||||
};
|
||||
@@ -0,0 +1 @@
|
||||
generated/
|
||||
Whitespace-only changes.
+180
@@ -0,0 +1,180 @@
|
||||
#!/usr/bin/env python3
|
||||
import math
|
||||
import sys
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
|
||||
from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
from rednose.helpers.kalmanfilter import KalmanFilter
|
||||
|
||||
if __name__ == '__main__': # Generating sympy
|
||||
import sympy as sp
|
||||
from rednose.helpers.ekf_sym import gen_code
|
||||
else:
|
||||
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
|
||||
|
||||
|
||||
i = 0
|
||||
|
||||
def _slice(n):
|
||||
global i
|
||||
s = slice(i, i + n)
|
||||
i += n
|
||||
|
||||
return s
|
||||
|
||||
|
||||
class States:
|
||||
# Vehicle model params
|
||||
STIFFNESS = _slice(1) # [-]
|
||||
STEER_RATIO = _slice(1) # [-]
|
||||
ANGLE_OFFSET = _slice(1) # [rad]
|
||||
ANGLE_OFFSET_FAST = _slice(1) # [rad]
|
||||
|
||||
VELOCITY = _slice(2) # (x, y) [m/s]
|
||||
YAW_RATE = _slice(1) # [rad/s]
|
||||
STEER_ANGLE = _slice(1) # [rad]
|
||||
ROAD_ROLL = _slice(1) # [rad]
|
||||
|
||||
|
||||
class CarKalman(KalmanFilter):
|
||||
name = 'car'
|
||||
|
||||
initial_x = np.array([
|
||||
1.0,
|
||||
15.0,
|
||||
0.0,
|
||||
0.0,
|
||||
|
||||
10.0, 0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
])
|
||||
|
||||
# process noise
|
||||
Q = np.diag([
|
||||
(.05 / 100)**2,
|
||||
.01**2,
|
||||
math.radians(0.02)**2,
|
||||
math.radians(0.25)**2,
|
||||
|
||||
.1**2, .01**2,
|
||||
math.radians(0.1)**2,
|
||||
math.radians(0.1)**2,
|
||||
math.radians(1)**2,
|
||||
])
|
||||
P_initial = Q.copy()
|
||||
|
||||
obs_noise: dict[int, Any] = {
|
||||
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
|
||||
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
|
||||
ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
|
||||
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
|
||||
ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
|
||||
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
|
||||
}
|
||||
|
||||
global_vars = [
|
||||
'mass',
|
||||
'rotational_inertia',
|
||||
'center_to_front',
|
||||
'center_to_rear',
|
||||
'stiffness_front',
|
||||
'stiffness_rear',
|
||||
]
|
||||
|
||||
@staticmethod
|
||||
def generate_code(generated_dir):
|
||||
dim_state = CarKalman.initial_x.shape[0]
|
||||
name = CarKalman.name
|
||||
|
||||
# Linearized single-track lateral dynamics, equations 7.211-7.213
|
||||
# Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
|
||||
# Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6
|
||||
|
||||
# globals
|
||||
global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
|
||||
m, j, aF, aR, cF_orig, cR_orig = global_vars
|
||||
|
||||
# make functions and jacobians with sympy
|
||||
# state variables
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1)
|
||||
state = sp.Matrix(state_sym)
|
||||
|
||||
# Vehicle model constants
|
||||
sf = state[States.STIFFNESS, :][0, 0]
|
||||
|
||||
cF, cR = sf * cF_orig, sf * cR_orig
|
||||
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
|
||||
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
|
||||
theta = state[States.ROAD_ROLL, :][0, 0]
|
||||
sa = state[States.STEER_ANGLE, :][0, 0]
|
||||
|
||||
sR = state[States.STEER_RATIO, :][0, 0]
|
||||
u, v = state[States.VELOCITY, :]
|
||||
r = state[States.YAW_RATE, :][0, 0]
|
||||
|
||||
A = sp.Matrix(np.zeros((2, 2)))
|
||||
A[0, 0] = -(cF + cR) / (m * u)
|
||||
A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
|
||||
A[1, 0] = -(cF * aF - cR * aR) / (j * u)
|
||||
A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
|
||||
|
||||
B = sp.Matrix(np.zeros((2, 1)))
|
||||
B[0, 0] = cF / m / sR
|
||||
B[1, 0] = (cF * aF) / j / sR
|
||||
|
||||
C = sp.Matrix(np.zeros((2, 1)))
|
||||
C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
|
||||
C[1, 0] = 0
|
||||
|
||||
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
|
||||
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
|
||||
|
||||
dt = sp.Symbol('dt')
|
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
|
||||
state_dot[States.YAW_RATE.start, 0] = x_dot[1]
|
||||
|
||||
# Basic descretization, 1st order integrator
|
||||
# Can be pretty bad if dt is big
|
||||
f_sym = state + dt * state_dot
|
||||
|
||||
#
|
||||
# Observation functions
|
||||
#
|
||||
obs_eqs = [
|
||||
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
|
||||
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
|
||||
[sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
|
||||
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
|
||||
[sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
|
||||
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
|
||||
[sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
|
||||
[sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
|
||||
]
|
||||
|
||||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
|
||||
|
||||
def __init__(self, generated_dir):
|
||||
dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
|
||||
self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
|
||||
dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
|
||||
|
||||
def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
|
||||
self.filter.set_global("mass", mass)
|
||||
self.filter.set_global("rotational_inertia", rotational_inertia)
|
||||
self.filter.set_global("center_to_front", center_to_front)
|
||||
self.filter.set_global("center_to_rear", center_to_rear)
|
||||
self.filter.set_global("stiffness_front", stiffness_front)
|
||||
self.filter.set_global("stiffness_rear", stiffness_rear)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
generated_dir = sys.argv[2]
|
||||
CarKalman.generate_code(generated_dir)
|
||||
@@ -0,0 +1,92 @@
|
||||
import os
|
||||
|
||||
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
|
||||
|
||||
class ObservationKind:
|
||||
UNKNOWN = 0
|
||||
NO_OBSERVATION = 1
|
||||
GPS_NED = 2
|
||||
ODOMETRIC_SPEED = 3
|
||||
PHONE_GYRO = 4
|
||||
GPS_VEL = 5
|
||||
PSEUDORANGE_GPS = 6
|
||||
PSEUDORANGE_RATE_GPS = 7
|
||||
SPEED = 8
|
||||
NO_ROT = 9
|
||||
PHONE_ACCEL = 10
|
||||
ORB_POINT = 11
|
||||
ECEF_POS = 12
|
||||
CAMERA_ODO_TRANSLATION = 13
|
||||
CAMERA_ODO_ROTATION = 14
|
||||
ORB_FEATURES = 15
|
||||
MSCKF_TEST = 16
|
||||
FEATURE_TRACK_TEST = 17
|
||||
LANE_PT = 18
|
||||
IMU_FRAME = 19
|
||||
PSEUDORANGE_GLONASS = 20
|
||||
PSEUDORANGE_RATE_GLONASS = 21
|
||||
PSEUDORANGE = 22
|
||||
PSEUDORANGE_RATE = 23
|
||||
ECEF_VEL = 35
|
||||
ECEF_ORIENTATION_FROM_GPS = 32
|
||||
NO_ACCEL = 33
|
||||
ORB_FEATURES_WIDE = 34
|
||||
|
||||
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
|
||||
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
|
||||
STEER_ANGLE = 26 # [rad]
|
||||
ANGLE_OFFSET_FAST = 27 # [rad]
|
||||
STIFFNESS = 28 # [-]
|
||||
STEER_RATIO = 29 # [-]
|
||||
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
|
||||
ROAD_ROLL = 31 # [rad]
|
||||
|
||||
names = [
|
||||
'Unknown',
|
||||
'No observation',
|
||||
'GPS NED',
|
||||
'Odometric speed',
|
||||
'Phone gyro',
|
||||
'GPS velocity',
|
||||
'GPS pseudorange',
|
||||
'GPS pseudorange rate',
|
||||
'Speed',
|
||||
'No rotation',
|
||||
'Phone acceleration',
|
||||
'ORB point',
|
||||
'ECEF pos',
|
||||
'camera odometric translation',
|
||||
'camera odometric rotation',
|
||||
'ORB features',
|
||||
'MSCKF test',
|
||||
'Feature track test',
|
||||
'Lane ecef point',
|
||||
'imu frame eulers',
|
||||
'GLONASS pseudorange',
|
||||
'GLONASS pseudorange rate',
|
||||
'pseudorange',
|
||||
'pseudorange rate',
|
||||
|
||||
'Road Frame x,y speed',
|
||||
'Road Frame yaw rate',
|
||||
'Steer Angle',
|
||||
'Fast Angle Offset',
|
||||
'Stiffness',
|
||||
'Steer Ratio',
|
||||
'Road Frame x speed',
|
||||
'Road Roll',
|
||||
'ECEF orientation from GPS',
|
||||
'NO accel',
|
||||
'ORB features wide camera',
|
||||
'ECEF_VEL',
|
||||
]
|
||||
|
||||
@classmethod
|
||||
def to_string(cls, kind):
|
||||
return cls.names[kind]
|
||||
|
||||
|
||||
SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
|
||||
ObservationKind.PSEUDORANGE_RATE_GPS,
|
||||
ObservationKind.PSEUDORANGE_GLONASS,
|
||||
ObservationKind.PSEUDORANGE_RATE_GLONASS]
|
||||
Binary file not shown.
File diff suppressed because it is too large.
Load diff
@@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
#include "rednose/helpers/ekf.h"
|
||||
extern "C" {
|
||||
void live_update_4(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_update_9(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_update_12(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_update_35(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_H(double *in_vec, double *out_6746369553526897517);
|
||||
void live_err_fun(double *nom_x, double *delta_x, double *out_2356689388845686605);
|
||||
void live_inv_err_fun(double *nom_x, double *true_x, double *out_7416532235122030660);
|
||||
void live_H_mod_fun(double *state, double *out_7744797074742978621);
|
||||
void live_f_fun(double *state, double dt, double *out_8063025216892748525);
|
||||
void live_F_fun(double *state, double dt, double *out_1924980074135776075);
|
||||
void live_h_4(double *state, double *unused, double *out_7366302340587907635);
|
||||
void live_H_4(double *state, double *unused, double *out_8678952100085115055);
|
||||
void live_h_9(double *state, double *unused, double *out_1116014595619586497);
|
||||
void live_H_9(double *state, double *unused, double *out_2480573038359989091);
|
||||
void live_h_10(double *state, double *unused, double *out_15153629602772458);
|
||||
void live_H_10(double *state, double *unused, double *out_4583653000591918572);
|
||||
void live_h_12(double *state, double *unused, double *out_9071809923999704409);
|
||||
void live_H_12(double *state, double *unused, double *out_4748335565592474766);
|
||||
void live_h_35(double *state, double *unused, double *out_7011637724721731008);
|
||||
void live_H_35(double *state, double *unused, double *out_2002772533267461057);
|
||||
void live_h_32(double *state, double *unused, double *out_8005805873262674095);
|
||||
void live_H_32(double *state, double *unused, double *out_6269965131311088716);
|
||||
void live_h_13(double *state, double *unused, double *out_7868428076142185420);
|
||||
void live_H_13(double *state, double *unused, double *out_4177806446556115282);
|
||||
void live_h_14(double *state, double *unused, double *out_1116014595619586497);
|
||||
void live_H_14(double *state, double *unused, double *out_2480573038359989091);
|
||||
void live_h_33(double *state, double *unused, double *out_5108012817780654932);
|
||||
void live_H_33(double *state, double *unused, double *out_1147784471371396547);
|
||||
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
#pragma once
|
||||
|
||||
#include <unordered_map>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#define STATE_ACCELERATION_START 16
|
||||
#define STATE_ACCELERATION_END 19
|
||||
#define STATE_ACCELERATION_LEN 3
|
||||
#define STATE_ACCELERATION_ERR_START 15
|
||||
#define STATE_ACCELERATION_ERR_END 18
|
||||
#define STATE_ACCELERATION_ERR_LEN 3
|
||||
#define STATE_ACC_BIAS_START 19
|
||||
#define STATE_ACC_BIAS_END 22
|
||||
#define STATE_ACC_BIAS_LEN 3
|
||||
#define STATE_ACC_BIAS_ERR_START 18
|
||||
#define STATE_ACC_BIAS_ERR_END 21
|
||||
#define STATE_ACC_BIAS_ERR_LEN 3
|
||||
#define STATE_ANGULAR_VELOCITY_START 10
|
||||
#define STATE_ANGULAR_VELOCITY_END 13
|
||||
#define STATE_ANGULAR_VELOCITY_LEN 3
|
||||
#define STATE_ANGULAR_VELOCITY_ERR_START 9
|
||||
#define STATE_ANGULAR_VELOCITY_ERR_END 12
|
||||
#define STATE_ANGULAR_VELOCITY_ERR_LEN 3
|
||||
#define STATE_ECEF_ORIENTATION_START 3
|
||||
#define STATE_ECEF_ORIENTATION_END 7
|
||||
#define STATE_ECEF_ORIENTATION_LEN 4
|
||||
#define STATE_ECEF_ORIENTATION_ERR_START 3
|
||||
#define STATE_ECEF_ORIENTATION_ERR_END 6
|
||||
#define STATE_ECEF_ORIENTATION_ERR_LEN 3
|
||||
#define STATE_ECEF_POS_START 0
|
||||
#define STATE_ECEF_POS_END 3
|
||||
#define STATE_ECEF_POS_LEN 3
|
||||
#define STATE_ECEF_POS_ERR_START 0
|
||||
#define STATE_ECEF_POS_ERR_END 3
|
||||
#define STATE_ECEF_POS_ERR_LEN 3
|
||||
#define STATE_ECEF_VELOCITY_START 7
|
||||
#define STATE_ECEF_VELOCITY_END 10
|
||||
#define STATE_ECEF_VELOCITY_LEN 3
|
||||
#define STATE_ECEF_VELOCITY_ERR_START 6
|
||||
#define STATE_ECEF_VELOCITY_ERR_END 9
|
||||
#define STATE_ECEF_VELOCITY_ERR_LEN 3
|
||||
#define STATE_GYRO_BIAS_START 13
|
||||
#define STATE_GYRO_BIAS_END 16
|
||||
#define STATE_GYRO_BIAS_LEN 3
|
||||
#define STATE_GYRO_BIAS_ERR_START 12
|
||||
#define STATE_GYRO_BIAS_ERR_END 15
|
||||
#define STATE_GYRO_BIAS_ERR_LEN 3
|
||||
|
||||
#define OBSERVATION_ANGLE_OFFSET_FAST 27
|
||||
#define OBSERVATION_CAMERA_ODO_ROTATION 14
|
||||
#define OBSERVATION_CAMERA_ODO_TRANSLATION 13
|
||||
#define OBSERVATION_ECEF_ORIENTATION_FROM_GPS 32
|
||||
#define OBSERVATION_ECEF_POS 12
|
||||
#define OBSERVATION_ECEF_VEL 35
|
||||
#define OBSERVATION_FEATURE_TRACK_TEST 17
|
||||
#define OBSERVATION_GPS_NED 2
|
||||
#define OBSERVATION_GPS_VEL 5
|
||||
#define OBSERVATION_IMU_FRAME 19
|
||||
#define OBSERVATION_LANE_PT 18
|
||||
#define OBSERVATION_MSCKF_TEST 16
|
||||
#define OBSERVATION_NO_ACCEL 33
|
||||
#define OBSERVATION_NO_OBSERVATION 1
|
||||
#define OBSERVATION_NO_ROT 9
|
||||
#define OBSERVATION_ODOMETRIC_SPEED 3
|
||||
#define OBSERVATION_ORB_FEATURES 15
|
||||
#define OBSERVATION_ORB_FEATURES_WIDE 34
|
||||
#define OBSERVATION_ORB_POINT 11
|
||||
#define OBSERVATION_PHONE_ACCEL 10
|
||||
#define OBSERVATION_PHONE_GYRO 4
|
||||
#define OBSERVATION_PSEUDORANGE 22
|
||||
#define OBSERVATION_PSEUDORANGE_GLONASS 20
|
||||
#define OBSERVATION_PSEUDORANGE_GPS 6
|
||||
#define OBSERVATION_PSEUDORANGE_RATE 23
|
||||
#define OBSERVATION_PSEUDORANGE_RATE_GLONASS 21
|
||||
#define OBSERVATION_PSEUDORANGE_RATE_GPS 7
|
||||
#define OBSERVATION_ROAD_FRAME_XY_SPEED 24
|
||||
#define OBSERVATION_ROAD_FRAME_X_SPEED 30
|
||||
#define OBSERVATION_ROAD_FRAME_YAW_RATE 25
|
||||
#define OBSERVATION_ROAD_ROLL 31
|
||||
#define OBSERVATION_SPEED 8
|
||||
#define OBSERVATION_STEER_ANGLE 26
|
||||
#define OBSERVATION_STEER_RATIO 29
|
||||
#define OBSERVATION_STIFFNESS 28
|
||||
#define OBSERVATION_UNKNOWN 0
|
||||
|
||||
static const Eigen::VectorXd live_initial_x = (Eigen::VectorXd(22) << 3.8800000e+06,-3.3700000e+06,3.7600000e+06,4.2254641e-01,-3.1238054e-01,-8.3602975e-01,-1.5788347e-01,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00).finished();
|
||||
static const Eigen::VectorXd live_initial_P_diag = (Eigen::VectorXd(21) << 1.e+02,1.e+02,1.e+02,1.e-04,1.e-04,1.e-04,1.e+02,1.e+02,1.e+02,1.e+00,1.e+00,1.e+00,1.e+00,1.e+00,1.e+00,1.e+04,1.e+04,1.e+04,1.e-04,1.e-04,1.e-04).finished();
|
||||
static const Eigen::VectorXd live_fake_gps_pos_cov_diag = (Eigen::VectorXd(3) << 1000000,1000000,1000000).finished();
|
||||
static const Eigen::VectorXd live_fake_gps_vel_cov_diag = (Eigen::VectorXd(3) << 100,100,100).finished();
|
||||
static const Eigen::VectorXd live_reset_orientation_diag = (Eigen::VectorXd(3) << 1,1,1).finished();
|
||||
static const Eigen::VectorXd live_Q_diag = (Eigen::VectorXd(21) << 8.9999999999999998e-04,8.9999999999999998e-04,8.9999999999999998e-04,9.9999999999999995e-07,9.9999999999999995e-07,9.9999999999999995e-07,1.0000000000000000e-04,1.0000000000000000e-04,1.0000000000000000e-04,1.0000000000000002e-02,1.0000000000000002e-02,1.0000000000000002e-02,2.5000000000000001e-09,2.5000000000000001e-09,2.5000000000000001e-09,9.0000000000000000e+00,9.0000000000000000e+00,9.0000000000000000e+00,2.5000000000000001e-05,2.5000000000000001e-05,2.5000000000000001e-05).finished();
|
||||
static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {
|
||||
{ 4, (Eigen::VectorXd(3) << 0.0006250000000000001,0.0006250000000000001,0.0006250000000000001).finished() },
|
||||
{ 10, (Eigen::VectorXd(3) << 0.25,0.25,0.25).finished() },
|
||||
{ 14, (Eigen::VectorXd(3) << 0.0025000000000000005,0.0025000000000000005,0.0025000000000000005).finished() },
|
||||
{ 9, (Eigen::VectorXd(3) << 2.5e-05,2.5e-05,2.5e-05).finished() },
|
||||
{ 33, (Eigen::VectorXd(3) << 0.0025000000000000005,0.0025000000000000005,0.0025000000000000005).finished() },
|
||||
{ 12, (Eigen::VectorXd(3) << 25,25,25).finished() },
|
||||
{ 35, (Eigen::VectorXd(3) << 0.25,0.25,0.25).finished() },
|
||||
{ 32, (Eigen::VectorXd(4) << 0.04000000000000001,0.04000000000000001,0.04000000000000001,0.04000000000000001).finished() },
|
||||
};
|
||||
|
||||
@@ -0,0 +1,122 @@
|
||||
#include "sunnypilot/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;
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
#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
@@ -0,0 +1,242 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
import os
|
||||
import numpy as np
|
||||
|
||||
from openpilot.sunnypilot.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)
|
||||
@@ -0,0 +1 @@
|
||||
out/
|
||||
Whitespace-only changes.
@@ -0,0 +1,94 @@
|
||||
import pytest
|
||||
import platform
|
||||
import json
|
||||
import random
|
||||
import time
|
||||
import capnp
|
||||
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.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
|
||||
|
||||
|
||||
if platform.system() == 'Darwin':
|
||||
pytest.skip("Skipping locationd test on macOS due to unsupported msgq.", allow_module_level=True)
|
||||
|
||||
|
||||
class TestLocationdProc:
|
||||
LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration',
|
||||
'accelerometer', 'gyroscope']
|
||||
|
||||
def setup_method(self):
|
||||
self.pm = messaging.PubMaster(self.LLD_MSGS)
|
||||
|
||||
self.params = Params()
|
||||
self.params.put_bool("UbloxAvailable", True)
|
||||
managed_processes['locationd_llk'].prepare()
|
||||
managed_processes['locationd_llk'].start()
|
||||
|
||||
def teardown_method(self):
|
||||
managed_processes['locationd_llk'].stop()
|
||||
|
||||
def get_msg(self, name, t):
|
||||
try:
|
||||
msg = messaging.new_message(name)
|
||||
except capnp.lib.capnp.KjException:
|
||||
msg = messaging.new_message(name, 0)
|
||||
|
||||
if name == "gpsLocationExternal":
|
||||
msg.gpsLocationExternal.flags = 1
|
||||
msg.gpsLocationExternal.hasFix = True
|
||||
msg.gpsLocationExternal.verticalAccuracy = 1.0
|
||||
msg.gpsLocationExternal.speedAccuracy = 1.0
|
||||
msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
|
||||
msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
|
||||
msg.gpsLocationExternal.latitude = float(self.lat)
|
||||
msg.gpsLocationExternal.longitude = float(self.lon)
|
||||
msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
|
||||
msg.gpsLocationExternal.altitude = float(self.alt)
|
||||
#if name == "gnssMeasurements":
|
||||
# msg.gnssMeasurements.measTime = t
|
||||
# msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
|
||||
# msg.gnssMeasurements.positionECEF.std = [0,0,0]
|
||||
# msg.gnssMeasurements.positionECEF.valid = True
|
||||
# msg.gnssMeasurements.velocityECEF.value = []
|
||||
# msg.gnssMeasurements.velocityECEF.std = [0,0,0]
|
||||
# msg.gnssMeasurements.velocityECEF.valid = True
|
||||
elif name == 'cameraOdometry':
|
||||
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
|
||||
msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
|
||||
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
|
||||
msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
|
||||
msg.logMonoTime = t
|
||||
msg.valid = True
|
||||
return msg
|
||||
|
||||
def test_params_gps(self):
|
||||
random.seed(123489234)
|
||||
self.params.remove('LastGPSPositionLLK')
|
||||
|
||||
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('LastGPSPositionLLK'))
|
||||
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)
|
||||
@@ -0,0 +1,65 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car.structs import car
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
|
||||
RELAXED_MIN_BUCKET_POINTS = np.array([1, 200, 300, 500, 500, 300, 200, 1])
|
||||
|
||||
ALLOWED_CARS = ['toyota', 'hyundai', 'rivian', 'honda']
|
||||
|
||||
|
||||
class TorqueEstimatorExt:
|
||||
def __init__(self, CP: car.CarParams):
|
||||
self.CP = CP
|
||||
self._params = Params()
|
||||
self.frame = -1
|
||||
|
||||
self.enforce_torque_control_toggle = self._params.get_bool("EnforceTorqueControl") # only during init
|
||||
self.use_params = self.CP.brand in ALLOWED_CARS and self.CP.lateralTuning.which() == 'torque'
|
||||
self.use_live_torque_params = self._params.get_bool("LiveTorqueParamsToggle")
|
||||
self.custom_torque_params = self._params.get_bool("CustomTorqueParams")
|
||||
self.torque_override_enabled = self._params.get_bool("TorqueParamsOverrideEnabled")
|
||||
self.min_bucket_points = RELAXED_MIN_BUCKET_POINTS
|
||||
self.factor_sanity = 0.0
|
||||
self.friction_sanity = 0.0
|
||||
self.offline_latAccelFactor = 0.0
|
||||
self.offline_friction = 0.0
|
||||
|
||||
def initialize_custom_params(self, decimated=False):
|
||||
self.update_use_params()
|
||||
|
||||
if self.enforce_torque_control_toggle:
|
||||
if self._params.get_bool("LiveTorqueParamsRelaxedToggle"):
|
||||
self.min_bucket_points = RELAXED_MIN_BUCKET_POINTS / (10 if decimated else 1)
|
||||
self.factor_sanity = 0.5 if decimated else 1.0
|
||||
self.friction_sanity = 0.8 if decimated else 1.0
|
||||
|
||||
if self._params.get_bool("CustomTorqueParams"):
|
||||
self.offline_latAccelFactor = float(self._params.get("TorqueParamsOverrideLatAccelFactor", return_default=True))
|
||||
self.offline_friction = float(self._params.get("TorqueParamsOverrideFriction", return_default=True))
|
||||
|
||||
def _update_params(self):
|
||||
if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
|
||||
self.use_live_torque_params = self._params.get_bool("LiveTorqueParamsToggle")
|
||||
self.custom_torque_params = self._params.get_bool("CustomTorqueParams")
|
||||
self.torque_override_enabled = self._params.get_bool("TorqueParamsOverrideEnabled")
|
||||
|
||||
def update_use_params(self):
|
||||
self._update_params()
|
||||
|
||||
if self.enforce_torque_control_toggle:
|
||||
if self.custom_torque_params and self.torque_override_enabled:
|
||||
self.use_params = False
|
||||
else:
|
||||
self.use_params = self.use_live_torque_params
|
||||
|
||||
self.frame += 1
|
||||
Reference in new issue
Block a user