superrob/IQ.Pilot
1
0
Fork 0
forked from IQ.Lvbs/IQ.Pilot
Code Pull Requests Activity

IQ.Pilot Release Commit @ b6534c0

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-08-27 20:17:33 -05:00
commit 00f07cac48
4706 changed files with 1257146 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
iqpilot/selfdrive/iqlocd/SConscript Normal file
View File

@@ -0,0 +1,8 @@
Import('env', 'arch', 'common', 'messaging', 'transformations')
loc_libs = [messaging, common, 'pthread', 'dl']
iqlocd_sources = ["atlas_loc_core.cc", "models/orbit_kf.cc"]
lenv = env.Clone()
iqlocd = lenv.Program("iqlocd", iqlocd_sources, LIBS=loc_libs + transformations)

0
iqpilot/selfdrive/iqlocd/__init__.py Normal file
View File

753
iqpilot/selfdrive/iqlocd/atlas_loc_core.cc Normal file
View File

@@ -0,0 +1,753 @@
#include "iqpilot/selfdrive/iqlocd/atlas_loc_core.h"
#include <sys/time.h>
#include <sys/resource.h>
#include <algorithm>
#include <cmath>
#include <vector>
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 fill_vector_sample(cereal::IQLiveLocation::VectorSample::Builder sample,
const VectorXd& values, const VectorXd& deviations, bool is_valid) {
sample.setValues(kj::arrayPtr(values.data(), values.size()));
sample.setDeviations(kj::arrayPtr(deviations.data(), deviations.size()));
sample.setIsValid(is_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();
}
AtlasLocator::AtlasLocator(AtlasGnssMode gnss_source) {
this->kf = std::make_unique<OrbitKalman>();
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->tune_gnss_source(gnss_source);
}
void AtlasLocator::populate_location_packet(cereal::IQLiveLocation::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->current_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
fill_vector_sample(fix.initGeodeticPosition(), fix_pos_geo_vec, nans, this->gps_mode);
fill_vector_sample(fix.initEcefPosition(), fix_ecef, fix_ecef_std, this->gps_mode);
fill_vector_sample(fix.initEcefVelocity(), vel_ecef, vel_ecef_std, this->gps_mode);
fill_vector_sample(fix.initNedVelocity(), ned_vel, nans, this->gps_mode);
fill_vector_sample(fix.initBodyVelocity(), vel_device, vel_device_std, true);
fill_vector_sample(fix.initBodyAcceleration(), accDevice, accDeviceErr, true);
fill_vector_sample(fix.initEcefOrientation(), orientation_ecef, orientation_ecef_std, this->gps_mode);
fill_vector_sample(fix.initAlignedOrientationEcef(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
fill_vector_sample(fix.initNedOrientation(), orientation_ned, orientation_ned_std, this->gps_mode);
fill_vector_sample(fix.initAlignedOrientationNed(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
fill_vector_sample(fix.initBodyAngularRate(), angVelocityDevice, angVelocityDeviceErr, true);
fill_vector_sample(fix.initAlignedVelocity(), vel_calib, vel_calib_std, this->calibrated);
fill_vector_sample(fix.initAlignedAngularRate(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
fill_vector_sample(fix.initAlignedAcceleration(), acc_calib, acc_calib_std, this->calibrated);
if (DEBUG) {
fill_vector_sample(fix.initDebugState(), 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.setVisionHealthy(!(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.setSecondsSinceReset(time_since_reset);
if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) {
fix.setSolutionState(cereal::IQLiveLocation::SolutionState::READY);
} else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) {
fix.setSolutionState(cereal::IQLiveLocation::SolutionState::COARSE);
} else {
fix.setSolutionState(cereal::IQLiveLocation::SolutionState::BOOTING);
}
}
VectorXd AtlasLocator::current_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 AtlasLocator::current_state_vector() {
return this->kf->get_x();
}
VectorXd AtlasLocator::current_sigma_vector() {
return this->kf->get_P().diagonal().array().sqrt();
}
bool AtlasLocator::inputs_are_ready() {
return this->critical_services_ok(this->observation_values_invalid) && !this->observation_timings_invalid;
}
void AtlasLocator::clear_observation_timing_fault(){
this->observation_timings_invalid = false;
}
void AtlasLocator::consume_sensor_frame(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->timestamp_ok(sensor_time)) {
this->observation_timings_invalid = true;
return;
}
// TODO: handle messages from two IMUs at the same time
if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
return;
}
// Gyro Uncalibrated
if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
auto v = log.getGyroUncalibrated().getV();
auto meas = Vector3d(-v[2], -v[1], -v[0]);
VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]);
float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1];
bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold;
if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
this->observation_values_invalid["gyroscope"] *= DECAY;
} else {
this->observation_values_invalid["gyroscope"] += 1.0;
}
}
// Accelerometer
if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) {
auto v = log.getAcceleration().getV();
// TODO: reduce false positives and re-enable this check
// check if device fell, estimate 10 for g
// 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
// this->device_fell |= (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 AtlasLocator::seed_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 AtlasLocator::consume_gps_frame(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->refresh_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 AtlasLocator::consume_gnss_frame(double current_time, const cereal::GnssMeasurements::Reader& log) {
if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
this->refresh_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->refresh_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 AtlasLocator::consume_car_state_frame(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 AtlasLocator::consume_camera_odometry(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->timestamp_ok(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 AtlasLocator::consume_live_calibration(double current_time, const cereal::ExtrinsicsCalibration::Reader& log) {
if (!this->timestamp_ok(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["extrinsicsCalibration"] += 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::ExtrinsicsCalibration::Status::CALIBRATED;
this->observation_values_invalid["extrinsicsCalibration"] *= DECAY;
}
}
void AtlasLocator::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 AtlasLocator::run_finite_guard(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 AtlasLocator::run_time_guard(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 AtlasLocator::cool_reset_tracker() {
// reset tracker is tuned to trigger when over 1reset/10s over 2min period
if (this->gps_ready()) {
this->reset_tracker *= RESET_TRACKER_DECAY;
} else {
this->reset_tracker = 0.0;
}
}
void AtlasLocator::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 AtlasLocator::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 AtlasLocator::consume_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->consume_event(event);
}
void AtlasLocator::consume_event(const cereal::Event::Reader& log) {
double t = log.getLogMonoTime() * 1e-9;
this->run_time_guard(t);
if (log.isAccelerometer()) {
this->consume_sensor_frame(t, log.getAccelerometer());
} else if (log.isGyroscope()) {
this->consume_sensor_frame(t, log.getGyroscope());
} else if (log.isGpsLocation()) {
this->consume_gps_frame(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
} else if (log.isGpsLocationExternal()) {
this->consume_gps_frame(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
//} else if (log.isGnssMeasurements()) {
// this->consume_gnss_frame(t, log.getGnssMeasurements());
} else if (log.isCarState()) {
this->consume_car_state_frame(t, log.getCarState());
} else if (log.isCameraOdometry()) {
this->consume_camera_odometry(t, log.getCameraOdometry());
} else if (log.isExtrinsicsCalibration()) {
this->consume_live_calibration(t, log.getExtrinsicsCalibration());
}
this->run_finite_guard();
this->cool_reset_tracker();
}
kj::ArrayPtr<capnp::byte> AtlasLocator::pack_state_message(MessageBuilder& msg_builder, bool inputsOK,
bool sensorsOK, bool gpsOK, bool msgValid) {
cereal::Event::Builder evt = msg_builder.initEvent();
evt.setValid(msgValid);
cereal::IQLiveLocation::Builder iq_loc = evt.initIqLiveLocation();
this->populate_location_packet(iq_loc);
iq_loc.setSensorsHealthy(sensorsOK);
iq_loc.setGpsHealthy(gpsOK);
iq_loc.setInputsHealthy(inputsOK);
return msg_builder.toBytes();
}
bool AtlasLocator::gps_ready() {
return this->last_gps_msg > 0.0 && (this->kf->get_filter_time() - this->last_gps_msg) < 2.0;
}
bool AtlasLocator::critical_services_ok(const std::map<std::string, double> &critical_services) {
for (auto &kv : critical_services){
if (kv.second >= INPUT_INVALID_THRESHOLD){
return false;
}
}
return true;
}
bool AtlasLocator::timestamp_ok(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 AtlasLocator::refresh_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->seed_fake_gps_observations(current_time);
}
}
}
void AtlasLocator::tune_gnss_source(const AtlasGnssMode &source) {
this->gnss_source = source;
if (source == AtlasGnssMode::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 AtlasLocator::run() {
Params params;
AtlasGnssMode source;
const char* gps_location_socket;
if (params.getBool("UbloxAvailable")) {
source = AtlasGnssMode::UBLOX;
gps_location_socket = "gpsLocationExternal";
} else {
source = AtlasGnssMode::QCOM;
gps_location_socket = "gpsLocation";
}
this->tune_gnss_source(source);
const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "extrinsicsCalibration",
"carState", "accelerometer", "gyroscope"};
SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
PubMaster pm({"iqLiveLocation"});
bool filterInitialized = false;
const std::vector<std::string> critical_input_services = {"cameraOdometry", "extrinsicsCalibration", "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->clear_observation_timing_fault();
for (const char* service : service_list) {
if (sm.updated(service) && sm.valid(service)){
const cereal::Event::Reader log = sm[service];
this->consume_event(log);
}
}
} else {
filterInitialized = sm.allAliveAndValid();
}
const char* trigger_msg = "cameraOdometry";
if (sm.updated(trigger_msg)) {
bool inputsOK = sm.allValid() && this->inputs_are_ready();
bool gpsOK = this->gps_ready();
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->pack_state_message(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
pm.send("iqLiveLocation", bytes.begin(), bytes.size());
double current_time = sm[trigger_msg].getLogMonoTime() * 1e-9;
if (gpsOK && (std::isnan(this->last_gps_param_time) || current_time - this->last_gps_param_time >= 60.0)) {
VectorXd posGeo = this->current_geodetic();
std::string lastGPSPosJSON = util::string_format(
"{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
int result = params.put("LastGPSPositionIQLoc", lastGPSPosJSON);
if (result != 0) {
LOGE("Failed to persist LastGPSPositionIQLoc: %d", result);
}
this->last_gps_param_time = current_time;
}
}
}
return 0;
}
int main() {
util::set_realtime_priority(5);
AtlasLocator engine;
return engine.run();
}

101
iqpilot/selfdrive/iqlocd/atlas_loc_core.h Normal file
View File

@@ -0,0 +1,101 @@
#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 "iqpilot/common/transformations/coordinates.hpp"
#include "iqpilot/common/transformations/orientation.hpp"
#include "iqpilot/selfdrive/iqlocd/models/orbit_kf.h"
#include "iqpilot/selfdrive/iqlocd/sensor_event_constants.h"
#define VISION_DECIMATION 2
#define SENSOR_DECIMATION 10
#define POSENET_STD_HIST_HALF 20
enum AtlasGnssMode {
UBLOX, QCOM
};
class AtlasLocator {
public:
AtlasLocator(AtlasGnssMode gnss_source = AtlasGnssMode::UBLOX);
int run();
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 run_finite_guard(double current_time = NAN);
void run_time_guard(double current_time = NAN);
void cool_reset_tracker();
bool gps_ready();
bool critical_services_ok(const std::map<std::string, double> &critical_services);
bool timestamp_ok(double current_time);
void refresh_gps_mode(double current_time);
bool inputs_are_ready();
void clear_observation_timing_fault();
kj::ArrayPtr<capnp::byte> pack_state_message(MessageBuilder& msg_builder,
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
void populate_location_packet(cereal::IQLiveLocation::Builder& fix);
Eigen::VectorXd current_geodetic();
Eigen::VectorXd current_state_vector();
Eigen::VectorXd current_sigma_vector();
void consume_bytes(const char *data, const size_t size);
void consume_event(const cereal::Event::Reader& log);
void consume_sensor_frame(double current_time, const cereal::SensorEventData::Reader& log);
void consume_gps_frame(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset);
void consume_gnss_frame(double current_time, const cereal::GnssMeasurements::Reader& log);
void consume_car_state_frame(double current_time, const cereal::CarState::Reader& log);
void consume_camera_odometry(double current_time, const cereal::CameraOdometry::Reader& log);
void consume_live_calibration(double current_time, const cereal::ExtrinsicsCalibration::Reader& log);
void seed_fake_gps_observations(double current_time);
private:
std::unique_ptr<OrbitKalman> 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;
double last_gps_param_time = NAN;
AtlasGnssMode 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 tune_gnss_source(const AtlasGnssMode &source);
};

0
iqpilot/selfdrive/iqlocd/models/__init__.py Normal file
View File

94
iqpilot/selfdrive/iqlocd/models/car_kf.py Normal file
View File

@@ -0,0 +1,94 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import math
from typing import Any
import numpy as np
from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from iqpilot.selfdrive.iqlocd.models.constants import ObservationKind
from iqpilot.selfdrive.state_estimation import EstimatorModel, ModelDefinition, StateEstimator
try:
from iqpilot.selfdrive.state_estimation.native_binding_pyx import car_predict, car_update
except ModuleNotFoundError:
car_predict = None
car_update = None
class States:
STIFFNESS = slice(0, 1)
STEER_RATIO = slice(1, 2)
ANGLE_OFFSET = slice(2, 3)
ANGLE_OFFSET_FAST = slice(3, 4)
VELOCITY = slice(4, 6)
YAW_RATE = slice(6, 7)
STEER_ANGLE = slice(7, 8)
ROAD_ROLL = slice(8, 9)
def _transition(state: np.ndarray, dt: float, values: dict[str, float]) -> np.ndarray:
result = state.copy()
stiffness = state[0]
steer_ratio = state[1]
angle = state[7] - state[2] - state[3]
speed, lateral_speed = state[4:6]
yaw_rate = state[6]
mass = values["mass"]
inertia = values["rotational_inertia"]
front = values["center_to_front"]
rear = values["center_to_rear"]
front_stiffness = stiffness * values["stiffness_front"]
rear_stiffness = stiffness * values["stiffness_rear"]
lateral_dot = -(front_stiffness + rear_stiffness) * lateral_speed / (mass * speed)
lateral_dot += (-(front_stiffness * front - rear_stiffness * rear) / (mass * speed) - speed) * yaw_rate
lateral_dot += front_stiffness * angle / (mass * steer_ratio) - ACCELERATION_DUE_TO_GRAVITY * state[8]
yaw_dot = -(front_stiffness * front - rear_stiffness * rear) * lateral_speed / (inertia * speed)
yaw_dot -= (front_stiffness * front**2 + rear_stiffness * rear**2) * yaw_rate / (inertia * speed)
yaw_dot += front_stiffness * front * angle / (inertia * steer_ratio)
result[5] += dt * lateral_dot
result[6] += dt * yaw_dot
return result
class CarKalman(EstimatorModel):
name = "car"
initial_x = np.array([1.0, 15.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0])
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),
}
def __init__(self):
self.native_parameters = np.zeros(6)
measurements = {
ObservationKind.ROAD_FRAME_YAW_RATE: lambda state, _: state[6:7],
ObservationKind.ROAD_FRAME_XY_SPEED: lambda state, _: state[4:6],
ObservationKind.ROAD_FRAME_X_SPEED: lambda state, _: state[4:5],
ObservationKind.STEER_ANGLE: lambda state, _: state[7:8],
ObservationKind.ANGLE_OFFSET_FAST: lambda state, _: state[3:4],
ObservationKind.STEER_RATIO: lambda state, _: state[1:2],
ObservationKind.STIFFNESS: lambda state, _: state[0:1],
ObservationKind.ROAD_ROLL: lambda state, _: state[8:9],
}
def native_predict(state, covariance, dt, process_noise, _):
car_predict(state, covariance, process_noise, dt, self.native_parameters)
model = ModelDefinition(9, 9, _transition, measurements, self.Q, self.obs_noise,
native_predict=native_predict if car_predict is not None else None, native_update=car_update)
super().__init__(StateEstimator(model, self.initial_x, self.P_initial, max_rewind_age=0.8))
def set_globals(self, mass: float, rotational_inertia: float, center_to_front: float, center_to_rear: float,
stiffness_front: float, stiffness_rear: float) -> None:
self.native_parameters[:] = mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear
for name, value in locals().copy().items():
if name != "self":
self.filter.set_global(name, value)

88
iqpilot/selfdrive/iqlocd/models/constants.py Normal file
View File

@@ -0,0 +1,88 @@
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]

225
iqpilot/selfdrive/iqlocd/models/orbit_kf.cc Normal file
View File

@@ -0,0 +1,225 @@
/*
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
*/
#include "iqpilot/selfdrive/iqlocd/models/orbit_kf.h"
#include <cmath>
using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector3d;
using Eigen::VectorXd;
using iqpilot::state_estimation::ModelDefinition;
using iqpilot::state_estimation::StateEstimator;
namespace {
constexpr double EARTH_GM = 3.986005e14;
Matrix3d rotation(const VectorXd &state) {
return Quaterniond(state(3), state(4), state(5), state(6)).normalized().toRotationMatrix();
}
Matrix3d skew(const Vector3d &value) {
Matrix3d result;
result << 0.0, -value.z(), value.y(), value.z(), 0.0, -value.x(), -value.y(), value.x(), 0.0;
return result;
}
VectorXd transition(const VectorXd &state, double dt) {
VectorXd result = state;
const Quaterniond orientation(state(3), state(4), state(5), state(6));
const Vector3d omega = state.segment<3>(10);
const Quaterniond derivative(0.0, omega.x(), omega.y(), omega.z());
const Quaterniond rate = orientation * derivative;
result.segment<3>(0) += dt * state.segment<3>(7);
result.segment<4>(3) += 0.5 * dt * (VectorXd(4) << rate.w(), rate.x(), rate.y(), rate.z()).finished();
result.segment<3>(7) += dt * rotation(state) * state.segment<3>(16);
return result;
}
VectorXd normalize(const VectorXd &state) {
VectorXd result = state;
result.segment<4>(3) /= result.segment<4>(3).norm();
return result;
}
VectorXd inject(const VectorXd &state, const VectorXd &delta) {
VectorXd result = state;
result.segment<3>(0) += delta.segment<3>(0);
const Quaterniond orientation(state(3), state(4), state(5), state(6));
Quaterniond error(1.0, 0.5 * delta(3), 0.5 * delta(4), 0.5 * delta(5));
const Quaterniond updated = error * orientation;
result.segment<4>(3) << updated.w(), updated.x(), updated.y(), updated.z();
result.segment(7, 15) += delta.segment(6, 15);
return normalize(result);
}
MatrixXdr error_projection(const VectorXd &state) {
MatrixXdr projection = MatrixXdr::Zero(22, 21);
projection.block<3, 3>(0, 0).setIdentity();
const double w = state(3);
const double x = state(4);
const double y = state(5);
const double z = state(6);
projection.block<4, 3>(3, 3) << -0.5 * x, -0.5 * y, -0.5 * z,
0.5 * w, 0.5 * z, -0.5 * y,
-0.5 * z, 0.5 * w, 0.5 * x,
0.5 * y, -0.5 * x, 0.5 * w;
projection.block(7, 6, 15, 15).setIdentity();
return projection;
}
MatrixXdr orbit_error_transition(const VectorXd &state, double dt) {
MatrixXdr result = MatrixXdr::Identity(21, 21);
const Matrix3d transform = rotation(state);
result.block<3, 3>(0, 6) = Matrix3d::Identity() * dt;
result.block<3, 3>(3, 3) += -dt * skew(transform * state.segment<3>(10));
result.block<3, 3>(3, 9) = dt * transform;
result.block<3, 3>(6, 3) = -dt * skew(transform * state.segment<3>(16));
result.block<3, 3>(6, 15) = dt * transform;
return result;
}
MatrixXdr selected_jacobian(int start) {
MatrixXdr result = MatrixXdr::Zero(3, 21);
result.block<3, 3>(0, start).setIdentity();
return result;
}
VectorXd phone_acceleration(const VectorXd &state) {
const Vector3d position = state.segment<3>(0);
const Vector3d gravity = rotation(state).transpose() * (EARTH_GM * position / std::pow(position.squaredNorm(), 1.5));
return gravity + state.segment<3>(16) + state.segment<3>(19);
}
MatrixXdr diagonal(std::initializer_list<double> values) {
VectorXd vector(values.size());
int index = 0;
for (double value : values) vector(index++) = value;
return vector.asDiagonal();
}
}
OrbitKalman::OrbitKalman() {
initial_x.resize(22);
initial_x << 3.88e6, -3.37e6, 3.76e6, 0.42254641, -0.31238054, -0.83602975, -0.15788347,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
initial_P = diagonal({100.0, 100.0, 100.0, 0.0001, 0.0001, 0.0001, 100.0, 100.0, 100.0,
1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 10000.0, 10000.0, 10000.0, 0.0001, 0.0001, 0.0001});
fake_gps_pos_cov = diagonal({1e6, 1e6, 1e6});
fake_gps_vel_cov = diagonal({100.0, 100.0, 100.0});
reset_orientation_P = diagonal({1.0, 1.0, 1.0});
obs_noise = {
{OBSERVATION_PHONE_GYRO, diagonal({0.000625, 0.000625, 0.000625})},
{OBSERVATION_PHONE_ACCEL, diagonal({0.25, 0.25, 0.25})},
{OBSERVATION_CAMERA_ODO_ROTATION, diagonal({0.0025, 0.0025, 0.0025})},
{OBSERVATION_CAMERA_ODO_TRANSLATION, diagonal({0.25, 0.25, 0.25})},
{OBSERVATION_NO_ROT, diagonal({0.000025, 0.000025, 0.000025})},
{OBSERVATION_NO_ACCEL, diagonal({0.0025, 0.0025, 0.0025})},
{OBSERVATION_ECEF_POS, diagonal({25.0, 25.0, 25.0})},
{OBSERVATION_ECEF_VEL, diagonal({0.25, 0.25, 0.25})},
{OBSERVATION_ECEF_ORIENTATION_FROM_GPS, diagonal({0.04, 0.04, 0.04, 0.04})},
};
const MatrixXdr process_noise = diagonal({0.0009, 0.0009, 0.0009, 0.000001, 0.000001, 0.000001,
0.0001, 0.0001, 0.0001, 0.01, 0.01, 0.01,
2.5e-9, 2.5e-9, 2.5e-9, 9.0, 9.0, 9.0, 0.000025, 0.000025, 0.000025});
std::unordered_map<int, std::function<VectorXd(const VectorXd &)>> measurements = {
{OBSERVATION_PHONE_GYRO, [](const VectorXd &state) { return state.segment<3>(10) + state.segment<3>(13); }},
{OBSERVATION_NO_ROT, [](const VectorXd &state) { return state.segment<3>(10); }},
{OBSERVATION_PHONE_ACCEL, phone_acceleration},
{OBSERVATION_ECEF_POS, [](const VectorXd &state) { return state.segment<3>(0); }},
{OBSERVATION_ECEF_VEL, [](const VectorXd &state) { return state.segment<3>(7); }},
{OBSERVATION_ECEF_ORIENTATION_FROM_GPS, [](const VectorXd &state) { return state.segment<4>(3); }},
{OBSERVATION_CAMERA_ODO_TRANSLATION, [](const VectorXd &state) { return rotation(state).transpose() * state.segment<3>(7); }},
{OBSERVATION_CAMERA_ODO_ROTATION, [](const VectorXd &state) { return state.segment<3>(10); }},
{OBSERVATION_NO_ACCEL, [](const VectorXd &state) { return state.segment<3>(16); }},
};
std::unordered_map<int, std::function<MatrixXdr(const VectorXd &)>> observation_jacobians = {
{OBSERVATION_PHONE_GYRO, [](const VectorXd &) {
MatrixXdr result = selected_jacobian(9);
result.block<3, 3>(0, 12).setIdentity();
return result;
}},
{OBSERVATION_NO_ROT, [](const VectorXd &) { return selected_jacobian(9); }},
{OBSERVATION_PHONE_ACCEL, [](const VectorXd &state) {
MatrixXdr result = MatrixXdr::Zero(3, 21);
const Vector3d position = state.segment<3>(0);
const double radius_squared = position.squaredNorm();
const double radius = std::sqrt(radius_squared);
const Vector3d gravity = EARTH_GM * position / (radius_squared * radius);
result.block<3, 3>(0, 0) = rotation(state).transpose() * EARTH_GM *
(Matrix3d::Identity() / (radius_squared * radius) -
3.0 * position * position.transpose() / (radius_squared * radius_squared * radius));
result.block<3, 3>(0, 3) = rotation(state).transpose() * skew(gravity);
result.block<3, 3>(0, 15).setIdentity();
result.block<3, 3>(0, 18).setIdentity();
return result;
}},
{OBSERVATION_ECEF_POS, [](const VectorXd &) { return selected_jacobian(0); }},
{OBSERVATION_ECEF_VEL, [](const VectorXd &) { return selected_jacobian(6); }},
{OBSERVATION_ECEF_ORIENTATION_FROM_GPS, [](const VectorXd &state) { return error_projection(state).block(3, 0, 4, 21); }},
{OBSERVATION_CAMERA_ODO_TRANSLATION, [](const VectorXd &state) {
MatrixXdr result = MatrixXdr::Zero(3, 21);
result.block<3, 3>(0, 3) = rotation(state).transpose() * skew(state.segment<3>(7));
result.block<3, 3>(0, 6) = rotation(state).transpose();
return result;
}},
{OBSERVATION_CAMERA_ODO_ROTATION, [](const VectorXd &) { return selected_jacobian(9); }},
{OBSERVATION_NO_ACCEL, [](const VectorXd &) { return selected_jacobian(15); }},
};
ModelDefinition model{22, 21, transition, measurements, process_noise, obs_noise, inject, error_projection, normalize,
orbit_error_transition, observation_jacobians};
filter = std::make_shared<StateEstimator>(std::move(model), initial_x, initial_P);
}
void OrbitKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
filter->init_state(state, covs_diag.asDiagonal(), filter_time);
}
void OrbitKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
filter->init_state(state, covs, filter_time);
}
void OrbitKalman::init_state(const VectorXd &state, double filter_time) {
filter->init_state(state, filter->covariance(), filter_time);
}
VectorXd OrbitKalman::get_x() { return filter->state(); }
MatrixXdr OrbitKalman::get_P() { return filter->covariance(); }
double OrbitKalman::get_filter_time() { return filter->time(); }
std::vector<MatrixXdr> OrbitKalman::get_R(int kind, int n) {
return std::vector<MatrixXdr>(n, obs_noise.at(kind));
}
std::optional<Estimate> OrbitKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
return filter->predict_and_observe(t, kind, meas, R);
}
void OrbitKalman::predict(double t) { filter->predict(t); }
const VectorXd &OrbitKalman::get_initial_x() { return initial_x; }
const MatrixXdr &OrbitKalman::get_initial_P() { return initial_P; }
const MatrixXdr &OrbitKalman::get_fake_gps_pos_cov() { return fake_gps_pos_cov; }
const MatrixXdr &OrbitKalman::get_fake_gps_vel_cov() { return fake_gps_vel_cov; }
const MatrixXdr &OrbitKalman::get_reset_orientation_P() { return reset_orientation_P; }
MatrixXdr OrbitKalman::H(const VectorXd &in) {
if (in.size() != 6) throw std::invalid_argument("local velocity input dimension mismatch");
auto function = [](const VectorXd &value) {
const Matrix3d transform = (Eigen::AngleAxisd(value(2), Vector3d::UnitZ()) * Eigen::AngleAxisd(value(1), Vector3d::UnitY()) *
Eigen::AngleAxisd(value(0), Vector3d::UnitX())).toRotationMatrix();
return transform.transpose() * value.segment<3>(3);
};
MatrixXdr result(3, 6);
for (int index = 0; index < 6; ++index) {
const double step = std::cbrt(Eigen::NumTraits<double>::epsilon()) * std::max(1.0, std::abs(in(index)));
VectorXd upper = in;
VectorXd lower = in;
upper(index) += step;
lower(index) -= step;
result.col(index) = (function(upper) - function(lower)) / (2.0 * step);
}
return result;
}

46
iqpilot/selfdrive/iqlocd/models/orbit_kf.h Normal file
View File

@@ -0,0 +1,46 @@
/*
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
*/
#pragma once
#include <memory>
#include <optional>
#include <unordered_map>
#include <vector>
#include <eigen3/Eigen/Dense>
#include "iqpilot/selfdrive/iqlocd/models/orbit_kf_constants.h"
#include "iqpilot/selfdrive/state_estimation/estimator.h"
using MatrixXdr = iqpilot::state_estimation::Matrix;
using Estimate = iqpilot::state_estimation::Estimate;
class OrbitKalman {
public:
OrbitKalman();
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 = {});
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::shared_ptr<iqpilot::state_estimation::StateEstimator> filter;
Eigen::VectorXd initial_x;
MatrixXdr initial_P;
MatrixXdr fake_gps_pos_cov;
MatrixXdr fake_gps_vel_cov;
MatrixXdr reset_orientation_P;
std::unordered_map<int, MatrixXdr> obs_noise;
};

42
iqpilot/selfdrive/iqlocd/models/orbit_kf_constants.h Normal file
View File

@@ -0,0 +1,42 @@
/*
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
*/
#pragma once
#define STATE_ECEF_POS_START 0
#define STATE_ECEF_POS_LEN 3
#define STATE_ECEF_ORIENTATION_START 3
#define STATE_ECEF_ORIENTATION_LEN 4
#define STATE_ECEF_VELOCITY_START 7
#define STATE_ECEF_VELOCITY_LEN 3
#define STATE_ANGULAR_VELOCITY_START 10
#define STATE_ANGULAR_VELOCITY_LEN 3
#define STATE_GYRO_BIAS_START 13
#define STATE_GYRO_BIAS_LEN 3
#define STATE_ACCELERATION_START 16
#define STATE_ACCELERATION_LEN 3
#define STATE_ACC_BIAS_START 19
#define STATE_ACC_BIAS_LEN 3
#define STATE_ECEF_POS_ERR_START 0
#define STATE_ECEF_POS_ERR_LEN 3
#define STATE_ECEF_ORIENTATION_ERR_START 3
#define STATE_ECEF_ORIENTATION_ERR_LEN 3
#define STATE_ECEF_VELOCITY_ERR_START 6
#define STATE_ECEF_VELOCITY_ERR_LEN 3
#define STATE_ANGULAR_VELOCITY_ERR_START 9
#define STATE_ANGULAR_VELOCITY_ERR_LEN 3
#define STATE_GYRO_BIAS_ERR_START 12
#define STATE_GYRO_BIAS_ERR_LEN 3
#define STATE_ACCELERATION_ERR_START 15
#define STATE_ACCELERATION_ERR_LEN 3
#define STATE_ACC_BIAS_ERR_START 18
#define STATE_ACC_BIAS_ERR_LEN 3
#define OBSERVATION_PHONE_GYRO 4
#define OBSERVATION_NO_ROT 9
#define OBSERVATION_PHONE_ACCEL 10
#define OBSERVATION_ECEF_POS 12
#define OBSERVATION_CAMERA_ODO_TRANSLATION 13
#define OBSERVATION_CAMERA_ODO_ROTATION 14
#define OBSERVATION_ECEF_ORIENTATION_FROM_GPS 32
#define OBSERVATION_NO_ACCEL 33
#define OBSERVATION_ECEF_VEL 35

17
iqpilot/selfdrive/iqlocd/sensor_event_constants.h Normal file
View File

@@ -0,0 +1,17 @@
#pragma once
#define SENSOR_ACCELEROMETER 1
#define SENSOR_MAGNETOMETER 2
#define SENSOR_MAGNETOMETER_UNCALIBRATED 3
#define SENSOR_GYRO 4
#define SENSOR_GYRO_UNCALIBRATED 5
#define SENSOR_LIGHT 7
#define SENSOR_TYPE_ACCELEROMETER 1
#define SENSOR_TYPE_GEOMAGNETIC_FIELD 2
#define SENSOR_TYPE_GYROSCOPE 4
#define SENSOR_TYPE_LIGHT 5
#define SENSOR_TYPE_AMBIENT_TEMPERATURE 13
#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14
#define SENSOR_TYPE_MAGNETIC_FIELD SENSOR_TYPE_GEOMAGNETIC_FIELD
#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16

0
iqpilot/selfdrive/iqlocd/tests/__init__.py Normal file
View File

114
iqpilot/selfdrive/iqlocd/tests/test_iqlocd.py Normal file
View File

@@ -0,0 +1,114 @@
import pytest
import json
import os
import random
import subprocess
import time
import capnp
from pathlib import Path
import iqpilot.cereal.messaging as messaging
from iqpilot.cereal.services import SERVICE_LIST
from iqpilot.common.params import Params
from iqpilot.common.transformations.coordinates import ecef2geodetic
from iqpilot.common.basedir import BASEDIR
@pytest.mark.linux
class TestIQLocdProc:
LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'extrinsicsCalibration',
'accelerometer', 'gyroscope']
@pytest.fixture(autouse=True)
def setup_iqlocd(self, openpilot_function_fixture):
self.pm = messaging.PubMaster(self.LLD_MSGS)
self.sm = messaging.SubMaster(['iqLiveLocation'])
self.params = Params()
assert self.params.get_param_path().endswith(os.environ['OPENPILOT_PREFIX'])
self.params.put_bool("UbloxAvailable", True)
iqlocd_dir = Path(BASEDIR) / 'iqpilot/selfdrive/iqlocd'
self.proc = subprocess.Popen(['./iqlocd'], cwd=iqlocd_dir, env=os.environ.copy())
yield
self.proc.terminate()
self.proc.wait(timeout=5)
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":
gps = getattr(msg, name)
gps.flags = 1
gps.hasFix = True
gps.source = 'ublox'
gps.horizontalAccuracy = 1.0
gps.verticalAccuracy = 1.0
gps.speedAccuracy = 1.0
gps.bearingAccuracyDeg = 1.0
gps.vNED = [0.0, 0.0, 0.0]
gps.latitude = float(self.lat)
gps.longitude = float(self.lon)
gps.unixTimestampMillis = t // 1_000_000
gps.altitude = float(self.alt)
elif name == 'cameraOdometry':
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
msg.cameraOdometry.rotStd = [0.01, 0.01, 0.01]
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
msg.cameraOdometry.transStd = [0.01, 0.01, 0.01]
elif name == 'extrinsicsCalibration':
msg.extrinsicsCalibration.calStatus = 'calibrated'
msg.extrinsicsCalibration.rpyCalib = [0.0, 0.0, 0.0]
elif name == 'accelerometer':
msg.accelerometer.sensor = 1
msg.accelerometer.type = 1
msg.accelerometer.timestamp = t
msg.accelerometer.init('acceleration').v = [0.0, 0.0, 9.81]
elif name == 'gyroscope':
msg.gyroscope.sensor = 5
msg.gyroscope.type = 16
msg.gyroscope.timestamp = t
msg.gyroscope.init('gyroUncalibrated').v = [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('LastGPSPositionIQLoc')
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])
msgs = []
for sec in range(1, 4):
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)
self.sm.update(0)
time.sleep(0.001)
deadline = time.monotonic() + 5.0
last_gps_raw = None
while time.monotonic() < deadline and last_gps_raw is None:
last_gps_raw = self.params.get('LastGPSPositionIQLoc')
self.sm.update(0)
time.sleep(0.05)
assert self.proc.poll() is None
location = self.sm['iqLiveLocation']
assert last_gps_raw is not None, {
'gpsHealthy': location.gpsHealthy,
'inputsHealthy': location.inputsHealthy,
'sensorsHealthy': location.sensorsHealthy,
'isolatedPath': self.params.get_param_path(),
}
lastGPS = json.loads(last_gps_raw)
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.2)