locationd no GPS (#33029)
* Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commitpull/33463/head
Kacper Rączy
8 months ago
committed by
GitHub
parent
0058ea5817
commit
236dffe400
15 changed files with 484 additions and 1285 deletions
@ -1,716 +0,0 @@ |
||||
#include "selfdrive/locationd/locationd.h" |
||||
|
||||
#include <sys/time.h> |
||||
#include <sys/resource.h> |
||||
|
||||
#include <algorithm> |
||||
#include <cmath> |
||||
#include <vector> |
||||
|
||||
using namespace EKFS; |
||||
using namespace Eigen; |
||||
|
||||
ExitHandler do_exit; |
||||
const double ACCEL_SANITY_CHECK = 100.0; // m/s^2
|
||||
const double ROTATION_SANITY_CHECK = 10.0; // rad/s
|
||||
const double TRANS_SANITY_CHECK = 200.0; // m/s
|
||||
const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
|
||||
const double ALTITUDE_SANITY_CHECK = 10000; // m
|
||||
const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
|
||||
const double SANE_GPS_UNCERTAINTY = 1500.0; // m
|
||||
const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker
|
||||
const double RESET_TRACKER_DECAY = 0.99995; |
||||
const double DECAY = 0.9993; // ~10 secs to resume after a bad input
|
||||
const double MAX_FILTER_REWIND_TIME = 0.8; // s
|
||||
const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30; |
||||
|
||||
// TODO: GPS sensor time offsets are empirically calculated
|
||||
// They should be replaced with synced time from a real clock
|
||||
const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
|
||||
const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
|
||||
const float GPS_POS_STD_THRESHOLD = 50.0; |
||||
const float GPS_VEL_STD_THRESHOLD = 5.0; |
||||
const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0; |
||||
const float GPS_POS_STD_RESET_THRESHOLD = 2.0; |
||||
const float GPS_VEL_STD_RESET_THRESHOLD = 0.5; |
||||
const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0; |
||||
const int GPS_ORIENTATION_ERROR_RESET_CNT = 3; |
||||
|
||||
const bool DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0"; |
||||
|
||||
template <typename ListType, typename Vector> |
||||
static Vector floatlist2vector(const ListType& floatlist) { |
||||
Vector res(floatlist.size()); |
||||
for (int i = 0; i < floatlist.size(); i++) { |
||||
res[i] = floatlist[i]; |
||||
} |
||||
return res; |
||||
} |
||||
|
||||
template <typename ListType> |
||||
static VectorXd float64list2vector(const ListType& floatlist) { |
||||
return floatlist2vector<ListType, VectorXd>(floatlist); |
||||
} |
||||
|
||||
template <typename ListType> |
||||
static VectorXf float32list2vector(const ListType& floatlist) { |
||||
return floatlist2vector<ListType, VectorXf>(floatlist); |
||||
} |
||||
|
||||
static Vector4d quat2vector(const Quaterniond& quat) { |
||||
return Vector4d(quat.w(), quat.x(), quat.y(), quat.z()); |
||||
} |
||||
|
||||
static Quaterniond vector2quat(const VectorXd& vec) { |
||||
return Quaterniond(vec(0), vec(1), vec(2), vec(3)); |
||||
} |
||||
|
||||
static void init_xyz_measurement(cereal::LivePose::XYZMeasurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) { |
||||
meas.setX(val[0]); meas.setY(val[1]); meas.setZ(val[2]); |
||||
meas.setXStd(std[0]); meas.setYStd(std[1]); meas.setZStd(std[2]); |
||||
meas.setValid(valid); |
||||
} |
||||
|
||||
static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) { |
||||
// To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix
|
||||
return ((rot_matrix * cov_in) * rot_matrix.transpose()); |
||||
} |
||||
|
||||
static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) { |
||||
// Stds cannot be rotated like values, only covariances can be rotated
|
||||
return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt(); |
||||
} |
||||
|
||||
Localizer::Localizer(LocalizerGnssSource gnss_source) { |
||||
this->kf = std::make_unique<LiveKalman>(); |
||||
this->reset_kalman(); |
||||
|
||||
this->calib = Vector3d(0.0, 0.0, 0.0); |
||||
this->device_from_calib = MatrixXdr::Identity(3, 3); |
||||
this->calib_from_device = MatrixXdr::Identity(3, 3); |
||||
|
||||
for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) { |
||||
this->posenet_stds.push_back(10.0); |
||||
} |
||||
|
||||
VectorXd ecef_pos = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START); |
||||
this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); |
||||
this->configure_gnss_source(gnss_source); |
||||
} |
||||
|
||||
void Localizer::build_live_pose(cereal::LivePose::Builder& fix) { |
||||
VectorXd predicted_state = this->kf->get_x(); |
||||
MatrixXdr predicted_cov = this->kf->get_P(); |
||||
VectorXd predicted_std = predicted_cov.diagonal().array().sqrt(); |
||||
|
||||
VectorXd fix_ecef = predicted_state.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START); |
||||
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) }; |
||||
VectorXd fix_ecef_std = predicted_std.segment<STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START); |
||||
VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START); |
||||
VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START); |
||||
VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))); |
||||
VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START); |
||||
MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); |
||||
MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose(); |
||||
|
||||
MatrixXdr vel_angular_cov = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START); |
||||
|
||||
VectorXd vel_device = device_from_ecef * vel_ecef; |
||||
VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose(); |
||||
MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN); |
||||
condensed_cov.topLeftCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() = |
||||
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); |
||||
condensed_cov.topRightCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() = |
||||
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START); |
||||
condensed_cov.bottomRightCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() = |
||||
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START); |
||||
condensed_cov.bottomLeftCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() = |
||||
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); |
||||
VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size()); |
||||
H_input << device_from_ecef_eul, vel_ecef; |
||||
MatrixXdr HH = this->kf->H(H_input); |
||||
MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose(); |
||||
VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt(); |
||||
|
||||
VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef); |
||||
VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt(); |
||||
VectorXd nextfix_ecef = fix_ecef + vel_ecef; |
||||
VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector(); |
||||
|
||||
VectorXd accDevice = predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START); |
||||
VectorXd accDeviceErr = predicted_std.segment<STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START); |
||||
|
||||
VectorXd angVelocityDevice = predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START); |
||||
VectorXd angVelocityDeviceErr = predicted_std.segment<STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START); |
||||
|
||||
// write measurements to msg
|
||||
init_xyz_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true); |
||||
init_xyz_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true); |
||||
init_xyz_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode); |
||||
init_xyz_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true); |
||||
if (DEBUG) { |
||||
cereal::LivePose::FilterState::Builder filter_state_builder = fix.initFilterState(); |
||||
filter_state_builder.setValue(kj::arrayPtr(predicted_state.data(), predicted_state.size())); |
||||
filter_state_builder.setStd(kj::arrayPtr(predicted_std.data(), predicted_std.size())); |
||||
filter_state_builder.setValid(true); |
||||
} |
||||
|
||||
double old_mean = 0.0, new_mean = 0.0; |
||||
int i = 0; |
||||
for (double x : this->posenet_stds) { |
||||
if (i < POSENET_STD_HIST_HALF) { |
||||
old_mean += x; |
||||
} else { |
||||
new_mean += x; |
||||
} |
||||
i++; |
||||
} |
||||
old_mean /= POSENET_STD_HIST_HALF; |
||||
new_mean /= POSENET_STD_HIST_HALF; |
||||
// experimentally found these values, no false positives in 20k minutes of driving
|
||||
bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0); |
||||
fix.setPosenetOK(!(std_spike && this->car_speed > 5.0)); |
||||
} |
||||
|
||||
VectorXd Localizer::get_position_geodetic() { |
||||
VectorXd fix_ecef = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START); |
||||
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) }; |
||||
Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef); |
||||
return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt); |
||||
} |
||||
|
||||
VectorXd Localizer::get_state() { |
||||
return this->kf->get_x(); |
||||
} |
||||
|
||||
VectorXd Localizer::get_stdev() { |
||||
return this->kf->get_P().diagonal().array().sqrt(); |
||||
} |
||||
|
||||
bool Localizer::are_inputs_ok() { |
||||
return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid; |
||||
} |
||||
|
||||
void Localizer::observation_timings_invalid_reset(){ |
||||
this->observation_timings_invalid = false; |
||||
} |
||||
|
||||
void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) { |
||||
// TODO does not yet account for double sensor readings in the log
|
||||
|
||||
// Ignore empty readings (e.g. in case the magnetometer had no data ready)
|
||||
if (log.getTimestamp() == 0) { |
||||
return; |
||||
} |
||||
|
||||
double sensor_time = 1e-9 * log.getTimestamp(); |
||||
|
||||
// sensor time and log time should be close
|
||||
if (std::abs(current_time - sensor_time) > 0.1) { |
||||
LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time"); |
||||
this->observation_timings_invalid = true; |
||||
return; |
||||
} else if (!this->is_timestamp_valid(sensor_time)) { |
||||
this->observation_timings_invalid = true; |
||||
return; |
||||
} |
||||
|
||||
// TODO: handle messages from two IMUs at the same time
|
||||
if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) { |
||||
return; |
||||
} |
||||
|
||||
// Gyro Uncalibrated
|
||||
if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { |
||||
auto v = log.getGyroUncalibrated().getV(); |
||||
auto meas = Vector3d(-v[2], -v[1], -v[0]); |
||||
|
||||
VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START); |
||||
float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]); |
||||
float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1]; |
||||
bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold; |
||||
|
||||
if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) { |
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas }); |
||||
this->observation_values_invalid["gyroscope"] *= DECAY; |
||||
} else { |
||||
this->observation_values_invalid["gyroscope"] += 1.0; |
||||
} |
||||
} |
||||
|
||||
// Accelerometer
|
||||
if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) { |
||||
auto v = log.getAcceleration().getV(); |
||||
|
||||
// TODO: reduce false positives and re-enable this check
|
||||
// check if device fell, estimate 10 for g
|
||||
// 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
|
||||
// this->device_fell |= (float64list2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
|
||||
|
||||
auto meas = Vector3d(-v[2], -v[1], -v[0]); |
||||
if (meas.norm() < ACCEL_SANITY_CHECK) { |
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas }); |
||||
this->observation_values_invalid["accelerometer"] *= DECAY; |
||||
} else { |
||||
this->observation_values_invalid["accelerometer"] += 1.0; |
||||
} |
||||
} |
||||
} |
||||
|
||||
void Localizer::input_fake_gps_observations(double current_time) { |
||||
// This is done to make sure that the error estimate of the position does not blow up
|
||||
// when the filter is in no-gps mode
|
||||
// Steps : first predict -> observe current obs with reasonable STD
|
||||
this->kf->predict(current_time); |
||||
|
||||
VectorXd current_x = this->kf->get_x(); |
||||
VectorXd ecef_pos = current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START); |
||||
VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START); |
||||
const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov(); |
||||
const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov(); |
||||
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); |
||||
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); |
||||
} |
||||
|
||||
void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) { |
||||
bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY); |
||||
bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)); |
||||
bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)); |
||||
bool gps_vel_insane = (float64list2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK); |
||||
|
||||
if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) { |
||||
//this->gps_valid = false;
|
||||
this->determine_gps_mode(current_time); |
||||
return; |
||||
} |
||||
|
||||
double sensor_time = current_time - sensor_time_offset; |
||||
|
||||
// Process message
|
||||
//this->gps_valid = true;
|
||||
this->gps_mode = true; |
||||
Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; |
||||
this->converter = std::make_unique<LocalCoord>(geodetic); |
||||
|
||||
VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector(); |
||||
VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos; |
||||
float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2)); |
||||
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal(); |
||||
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal(); |
||||
|
||||
this->unix_timestamp_millis = log.getUnixTimestampMillis(); |
||||
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm(); |
||||
|
||||
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))); |
||||
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef); |
||||
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg())); |
||||
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI; |
||||
for (int i = 0; i < orientation_error.size(); i++) { |
||||
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI); |
||||
if (orientation_error(i) < 0.0) { |
||||
orientation_error(i) += 2.0 * M_PI; |
||||
} |
||||
orientation_error(i) -= M_PI; |
||||
} |
||||
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); |
||||
|
||||
if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) { |
||||
LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset"); |
||||
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); |
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); |
||||
} else if (gps_est_error > 100.0) { |
||||
LOGE("Locationd vs ubloxLocation position difference too large, kalman reset"); |
||||
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); |
||||
} |
||||
|
||||
this->last_gps_msg = sensor_time; |
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); |
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); |
||||
} |
||||
|
||||
void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) { |
||||
|
||||
if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) { |
||||
this->determine_gps_mode(current_time); |
||||
return; |
||||
} |
||||
|
||||
double sensor_time = log.getMeasTime() * 1e-9; |
||||
sensor_time -= this->gps_time_offset; |
||||
|
||||
auto ecef_pos_v = log.getPositionECEF().getValue(); |
||||
VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]); |
||||
|
||||
// indexed at 0 cause all std values are the same MAE
|
||||
auto ecef_pos_std = log.getPositionECEF().getStd()[0]; |
||||
MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal(); |
||||
|
||||
auto ecef_vel_v = log.getVelocityECEF().getValue(); |
||||
VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]); |
||||
|
||||
// indexed at 0 cause all std values are the same MAE
|
||||
auto ecef_vel_std = log.getVelocityECEF().getStd()[0]; |
||||
MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal(); |
||||
|
||||
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm(); |
||||
|
||||
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))); |
||||
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef); |
||||
|
||||
LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); |
||||
ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]}; |
||||
VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector(); |
||||
double bearing_rad = atan2(ned_vel[1], ned_vel[0]); |
||||
|
||||
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad); |
||||
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI; |
||||
for (int i = 0; i < orientation_error.size(); i++) { |
||||
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI); |
||||
if (orientation_error(i) < 0.0) { |
||||
orientation_error(i) += 2.0 * M_PI; |
||||
} |
||||
orientation_error(i) -= M_PI; |
||||
} |
||||
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); |
||||
|
||||
if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) { |
||||
this->determine_gps_mode(current_time); |
||||
return; |
||||
} |
||||
|
||||
// prevent jumping gnss measurements (covered lots, standstill...)
|
||||
bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD; |
||||
orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD; |
||||
orientation_reset &= !this->standstill; |
||||
if (orientation_reset) { |
||||
this->orientation_reset_count++; |
||||
} else { |
||||
this->orientation_reset_count = 0; |
||||
} |
||||
|
||||
if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) { |
||||
// always reset on first gps message and if the location is off but the accuracy is high
|
||||
LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset"); |
||||
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); |
||||
} else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) { |
||||
LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset"); |
||||
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); |
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); |
||||
this->orientation_reset_count = 0; |
||||
} |
||||
|
||||
this->gps_mode = true; |
||||
this->last_gps_msg = sensor_time; |
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); |
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); |
||||
} |
||||
|
||||
void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) { |
||||
this->car_speed = std::abs(log.getVEgo()); |
||||
this->standstill = log.getStandstill(); |
||||
if (this->standstill) { |
||||
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) }); |
||||
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) }); |
||||
} |
||||
} |
||||
|
||||
void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) { |
||||
VectorXd rot_device = this->device_from_calib * float64list2vector(log.getRot()); |
||||
VectorXd trans_device = this->device_from_calib * float64list2vector(log.getTrans()); |
||||
|
||||
if (!this->is_timestamp_valid(current_time)) { |
||||
this->observation_timings_invalid = true; |
||||
return; |
||||
} |
||||
|
||||
if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) { |
||||
this->observation_values_invalid["cameraOdometry"] += 1.0; |
||||
return; |
||||
} |
||||
|
||||
VectorXd rot_calib_std = float64list2vector(log.getRotStd()); |
||||
VectorXd trans_calib_std = float64list2vector(log.getTransStd()); |
||||
|
||||
if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) { |
||||
this->observation_values_invalid["cameraOdometry"] += 1.0; |
||||
return; |
||||
} |
||||
|
||||
if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) { |
||||
this->observation_values_invalid["cameraOdometry"] += 1.0; |
||||
return; |
||||
} |
||||
|
||||
this->posenet_stds.pop_front(); |
||||
this->posenet_stds.push_back(trans_calib_std[0]); |
||||
|
||||
// Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise
|
||||
trans_calib_std *= 10.0; |
||||
rot_calib_std *= 10.0; |
||||
MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal(); |
||||
MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal(); |
||||
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION, |
||||
{ rot_device }, { rot_device_cov }); |
||||
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION, |
||||
{ trans_device }, { trans_device_cov }); |
||||
this->observation_values_invalid["cameraOdometry"] *= DECAY; |
||||
this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]); |
||||
} |
||||
|
||||
void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { |
||||
if (!this->is_timestamp_valid(current_time)) { |
||||
this->observation_timings_invalid = true; |
||||
return; |
||||
} |
||||
|
||||
if (log.getRpyCalib().size() > 0) { |
||||
auto live_calib = float64list2vector(log.getRpyCalib()); |
||||
if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) { |
||||
this->observation_values_invalid["liveCalibration"] += 1.0; |
||||
return; |
||||
} |
||||
|
||||
this->calib = live_calib; |
||||
this->device_from_calib = euler2rot(this->calib); |
||||
this->calib_from_device = this->device_from_calib.transpose(); |
||||
this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED; |
||||
this->observation_values_invalid["liveCalibration"] *= DECAY; |
||||
} |
||||
} |
||||
|
||||
void Localizer::reset_kalman(double current_time) { |
||||
const VectorXd &init_x = this->kf->get_initial_x(); |
||||
const MatrixXdr &init_P = this->kf->get_initial_P(); |
||||
this->reset_kalman(current_time, init_x, init_P); |
||||
} |
||||
|
||||
void Localizer::finite_check(double current_time) { |
||||
bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all(); |
||||
if (!all_finite) { |
||||
LOGE("Non-finite values detected, kalman reset"); |
||||
this->reset_kalman(current_time); |
||||
} |
||||
} |
||||
|
||||
void Localizer::time_check(double current_time) { |
||||
if (std::isnan(this->last_reset_time)) { |
||||
this->last_reset_time = current_time; |
||||
} |
||||
if (std::isnan(this->first_valid_log_time)) { |
||||
this->first_valid_log_time = current_time; |
||||
} |
||||
double filter_time = this->kf->get_filter_time(); |
||||
bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10); |
||||
if (big_time_gap) { |
||||
LOGE("Time gap of over 10s detected, kalman reset"); |
||||
this->reset_kalman(current_time); |
||||
} |
||||
} |
||||
|
||||
void Localizer::update_reset_tracker() { |
||||
// reset tracker is tuned to trigger when over 1reset/10s over 2min period
|
||||
if (this->is_gps_ok()) { |
||||
this->reset_tracker *= RESET_TRACKER_DECAY; |
||||
} else { |
||||
this->reset_tracker = 0.0; |
||||
} |
||||
} |
||||
|
||||
void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) { |
||||
// too nonlinear to init on completely wrong
|
||||
VectorXd current_x = this->kf->get_x(); |
||||
MatrixXdr current_P = this->kf->get_P(); |
||||
MatrixXdr init_P = this->kf->get_initial_P(); |
||||
const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P(); |
||||
int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN); |
||||
|
||||
current_x.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient; |
||||
current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START) = init_vel; |
||||
current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) = init_pos; |
||||
|
||||
init_P.block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal(); |
||||
init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal(); |
||||
init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal(); |
||||
init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START, |
||||
STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal(); |
||||
|
||||
this->reset_kalman(current_time, current_x, init_P); |
||||
} |
||||
|
||||
void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) { |
||||
this->kf->init_state(init_x, init_P, current_time); |
||||
this->last_reset_time = current_time; |
||||
this->reset_tracker += 1.0; |
||||
} |
||||
|
||||
void Localizer::handle_msg_bytes(const char *data, const size_t size) { |
||||
AlignedBuffer aligned_buf; |
||||
|
||||
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size)); |
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>(); |
||||
|
||||
this->handle_msg(event); |
||||
} |
||||
|
||||
void Localizer::handle_msg(const cereal::Event::Reader& log) { |
||||
double t = log.getLogMonoTime() * 1e-9; |
||||
this->time_check(t); |
||||
if (log.isAccelerometer()) { |
||||
this->handle_sensor(t, log.getAccelerometer()); |
||||
} else if (log.isGyroscope()) { |
||||
this->handle_sensor(t, log.getGyroscope()); |
||||
} else if (log.isGpsLocation()) { |
||||
this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); |
||||
} else if (log.isGpsLocationExternal()) { |
||||
this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); |
||||
//} else if (log.isGnssMeasurements()) {
|
||||
// this->handle_gnss(t, log.getGnssMeasurements());
|
||||
} else if (log.isCarState()) { |
||||
this->handle_car_state(t, log.getCarState()); |
||||
} else if (log.isCameraOdometry()) { |
||||
this->handle_cam_odo(t, log.getCameraOdometry()); |
||||
} else if (log.isLiveCalibration()) { |
||||
this->handle_live_calib(t, log.getLiveCalibration()); |
||||
} |
||||
this->finite_check(); |
||||
this->update_reset_tracker(); |
||||
} |
||||
|
||||
void Localizer::build_pose_message( |
||||
MessageBuilder& msg_builder, bool inputsOK, bool sensorsOK, bool msgValid) { |
||||
cereal::Event::Builder evt = msg_builder.initEvent(); |
||||
evt.setValid(msgValid); |
||||
cereal::LivePose::Builder livePose = evt.initLivePose(); |
||||
this->build_live_pose(livePose); |
||||
livePose.setSensorsOK(sensorsOK); |
||||
livePose.setInputsOK(inputsOK); |
||||
} |
||||
|
||||
bool Localizer::is_gps_ok() { |
||||
return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0; |
||||
} |
||||
|
||||
bool Localizer::critical_services_valid(const std::map<std::string, double> &critical_services) { |
||||
for (auto &kv : critical_services){ |
||||
if (kv.second >= INPUT_INVALID_THRESHOLD){ |
||||
return false; |
||||
} |
||||
} |
||||
return true; |
||||
} |
||||
|
||||
bool Localizer::is_timestamp_valid(double current_time) { |
||||
double filter_time = this->kf->get_filter_time(); |
||||
if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) { |
||||
LOGE("Observation timestamp is older than the max rewind threshold of the filter"); |
||||
return false; |
||||
} |
||||
return true; |
||||
} |
||||
|
||||
void Localizer::determine_gps_mode(double current_time) { |
||||
// 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode
|
||||
// 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs
|
||||
// 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is
|
||||
VectorXd current_pos_std = this->kf->get_P().block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt(); |
||||
if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){ |
||||
if (this->gps_mode){ |
||||
this->gps_mode = false; |
||||
this->reset_kalman(current_time); |
||||
} else { |
||||
this->input_fake_gps_observations(current_time); |
||||
} |
||||
} |
||||
} |
||||
|
||||
void Localizer::configure_gnss_source(const LocalizerGnssSource &source) { |
||||
this->gnss_source = source; |
||||
if (source == LocalizerGnssSource::UBLOX) { |
||||
this->gps_std_factor = 10.0; |
||||
this->gps_variance_factor = 1.0; |
||||
this->gps_vertical_variance_factor = 1.0; |
||||
this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET; |
||||
} else { |
||||
this->gps_std_factor = 2.0; |
||||
this->gps_variance_factor = 0.0; |
||||
this->gps_vertical_variance_factor = 3.0; |
||||
this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET; |
||||
} |
||||
} |
||||
|
||||
int Localizer::locationd_thread() { |
||||
Params params; |
||||
LocalizerGnssSource source; |
||||
const char* gps_location_socket; |
||||
if (params.getBool("UbloxAvailable")) { |
||||
source = LocalizerGnssSource::UBLOX; |
||||
gps_location_socket = "gpsLocationExternal"; |
||||
} else { |
||||
source = LocalizerGnssSource::QCOM; |
||||
gps_location_socket = "gpsLocation"; |
||||
} |
||||
|
||||
this->configure_gnss_source(source); |
||||
const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", |
||||
"carState", "accelerometer", "gyroscope"}; |
||||
|
||||
SubMaster sm(service_list, {}, nullptr, {gps_location_socket}); |
||||
PubMaster pm({"livePose"}); |
||||
|
||||
uint64_t cnt = 0; |
||||
bool filterInitialized = false; |
||||
const std::vector<std::string> critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"}; |
||||
for (std::string service : critical_input_services) { |
||||
this->observation_values_invalid.insert({service, 0.0}); |
||||
} |
||||
|
||||
while (!do_exit) { |
||||
sm.update(); |
||||
if (filterInitialized){ |
||||
this->observation_timings_invalid_reset(); |
||||
for (const char* service : service_list) { |
||||
if (sm.updated(service) && sm.valid(service)){ |
||||
const cereal::Event::Reader log = sm[service]; |
||||
this->handle_msg(log); |
||||
} |
||||
} |
||||
} else { |
||||
filterInitialized = sm.allAliveAndValid(); |
||||
} |
||||
|
||||
const char* trigger_msg = "cameraOdometry"; |
||||
if (sm.updated(trigger_msg)) { |
||||
bool inputsOK = sm.allValid() && this->are_inputs_ok(); |
||||
bool gpsOK = this->is_gps_ok(); |
||||
bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"}); |
||||
|
||||
// Log time to first fix
|
||||
if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) { |
||||
this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time); |
||||
} |
||||
|
||||
MessageBuilder pose_msg_builder; |
||||
this->build_pose_message(pose_msg_builder, inputsOK, sensorsOK, filterInitialized); |
||||
|
||||
kj::ArrayPtr<capnp::byte> pose_bytes = pose_msg_builder.toBytes(); |
||||
pm.send("livePose", pose_bytes.begin(), pose_bytes.size()); |
||||
|
||||
if (cnt % 1200 == 0 && gpsOK) { // once a minute
|
||||
VectorXd posGeo = this->get_position_geodetic(); |
||||
std::string lastGPSPosJSON = util::string_format( |
||||
"{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2)); |
||||
params.putNonBlocking("LastGPSPosition", lastGPSPosJSON); |
||||
} |
||||
cnt++; |
||||
} |
||||
} |
||||
return 0; |
||||
} |
||||
|
||||
int main() { |
||||
util::set_realtime_priority(5); |
||||
|
||||
Localizer localizer; |
||||
return localizer.locationd_thread(); |
||||
} |
||||
@ -0,0 +1,321 @@ |
||||
#!/usr/bin/env python3 |
||||
import os |
||||
import json |
||||
import time |
||||
import capnp |
||||
import numpy as np |
||||
from enum import Enum |
||||
from collections import defaultdict |
||||
|
||||
from cereal import log, messaging |
||||
from openpilot.common.transformations.orientation import rot_from_euler |
||||
from openpilot.common.realtime import config_realtime_process |
||||
from openpilot.common.params import Params |
||||
from openpilot.selfdrive.locationd.helpers import rotate_std |
||||
from openpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States |
||||
from openpilot.selfdrive.locationd.models.constants import ObservationKind, GENERATED_DIR |
||||
|
||||
ACCEL_SANITY_CHECK = 100.0 # m/s^2 |
||||
ROTATION_SANITY_CHECK = 10.0 # rad/s |
||||
TRANS_SANITY_CHECK = 200.0 # m/s |
||||
CALIB_RPY_SANITY_CHECK = 0.5 # rad (+- 30 deg) |
||||
MIN_STD_SANITY_CHECK = 1e-5 # m or rad |
||||
MAX_FILTER_REWIND_TIME = 0.8 # s |
||||
MAX_SENSOR_TIME_DIFF = 0.1 # s |
||||
YAWRATE_CROSS_ERR_CHECK_FACTOR = 30 |
||||
INPUT_INVALID_THRESHOLD = 0.5 |
||||
INPUT_INVALID_DECAY = 0.9993 # ~10 secs to resume after a bad input |
||||
POSENET_STD_INITIAL_VALUE = 10.0 |
||||
POSENET_STD_HIST_HALF = 20 |
||||
|
||||
|
||||
def init_xyz_measurement(measurement: capnp._DynamicStructBuilder, values: np.ndarray, stds: np.ndarray, valid: bool): |
||||
assert len(values) == len(stds) == 3 |
||||
for i, d in enumerate(("x", "y", "z")): |
||||
setattr(measurement, d, float(values[i])) |
||||
setattr(measurement, f"{d}Std", float(stds[i])) |
||||
measurement.valid = valid |
||||
|
||||
|
||||
class HandleLogResult(Enum): |
||||
SUCCESS = 0 |
||||
TIMING_INVALID = 1 |
||||
INPUT_INVALID = 2 |
||||
SENSOR_SOURCE_INVALID = 3 |
||||
|
||||
|
||||
class LocationEstimator: |
||||
def __init__(self, debug: bool): |
||||
self.kf = PoseKalman(GENERATED_DIR, MAX_FILTER_REWIND_TIME) |
||||
|
||||
self.debug = debug |
||||
|
||||
self.posenet_stds = [POSENET_STD_INITIAL_VALUE] * (POSENET_STD_HIST_HALF * 2) |
||||
self.car_speed = 0.0 |
||||
self.camodo_yawrate_distribution = np.array([0.0, 10.0]) # mean, std |
||||
self.device_from_calib = np.eye(3) |
||||
|
||||
obs_kinds = [ObservationKind.PHONE_ACCEL, ObservationKind.PHONE_GYRO, ObservationKind.CAMERA_ODO_ROTATION, ObservationKind.CAMERA_ODO_TRANSLATION] |
||||
self.observations = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds} |
||||
self.observation_errors = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds} |
||||
|
||||
def reset(self, t: float, x_initial: np.ndarray = PoseKalman.initial_x, P_initial: np.ndarray = PoseKalman.initial_P): |
||||
self.kf.reset(t, x_initial, P_initial) |
||||
|
||||
def _validate_sensor_source(self, source: log.SensorEventData.SensorSource): |
||||
# some segments have two IMUs, ignore the second one |
||||
return source != log.SensorEventData.SensorSource.bmx055 |
||||
|
||||
def _validate_sensor_time(self, sensor_time: float, t: float): |
||||
# ignore empty readings |
||||
if sensor_time == 0: |
||||
return False |
||||
|
||||
# sensor time and log time should be close |
||||
sensor_time_invalid = abs(sensor_time - t) > MAX_SENSOR_TIME_DIFF |
||||
if sensor_time_invalid: |
||||
print("Sensor reading ignored, sensor timestamp more than 100ms off from log time") |
||||
return not sensor_time_invalid |
||||
|
||||
def _validate_timestamp(self, t: float): |
||||
kf_t = self.kf.t |
||||
invalid = not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME |
||||
if invalid: |
||||
print("Observation timestamp is older than the max rewind threshold of the filter") |
||||
return not invalid |
||||
|
||||
def _finite_check(self, t: float, new_x: np.ndarray, new_P: np.ndarray): |
||||
all_finite = np.isfinite(new_x).all() and np.isfinite(new_P).all() |
||||
if not all_finite: |
||||
print("Non-finite values detected, kalman reset") |
||||
self.reset(t) |
||||
|
||||
def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> HandleLogResult: |
||||
new_x, new_P = None, None |
||||
if which == "accelerometer" and msg.which() == "acceleration": |
||||
sensor_time = msg.timestamp * 1e-9 |
||||
|
||||
if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time): |
||||
return HandleLogResult.TIMING_INVALID |
||||
|
||||
if not self._validate_sensor_source(msg.source): |
||||
return HandleLogResult.SENSOR_SOURCE_INVALID |
||||
|
||||
v = msg.acceleration.v |
||||
meas = np.array([-v[2], -v[1], -v[0]]) |
||||
if np.linalg.norm(meas) >= ACCEL_SANITY_CHECK: |
||||
return HandleLogResult.INPUT_INVALID |
||||
|
||||
acc_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_ACCEL, meas) |
||||
if acc_res is not None: |
||||
_, new_x, _, new_P, _, _, (acc_err,), _, _ = acc_res |
||||
self.observation_errors[ObservationKind.PHONE_ACCEL] = np.array(acc_err) |
||||
self.observations[ObservationKind.PHONE_ACCEL] = meas |
||||
|
||||
elif which == "gyroscope" and msg.which() == "gyroUncalibrated": |
||||
sensor_time = msg.timestamp * 1e-9 |
||||
|
||||
if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time): |
||||
return HandleLogResult.TIMING_INVALID |
||||
|
||||
if not self._validate_sensor_source(msg.source): |
||||
return HandleLogResult.SENSOR_SOURCE_INVALID |
||||
|
||||
v = msg.gyroUncalibrated.v |
||||
meas = np.array([-v[2], -v[1], -v[0]]) |
||||
|
||||
gyro_bias = self.kf.x[States.GYRO_BIAS] |
||||
gyro_camodo_yawrate_err = np.abs((meas[2] - gyro_bias[2]) - self.camodo_yawrate_distribution[0]) |
||||
gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * self.camodo_yawrate_distribution[1] |
||||
gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold |
||||
|
||||
if np.linalg.norm(meas) >= ROTATION_SANITY_CHECK or not gyro_valid: |
||||
return HandleLogResult.INPUT_INVALID |
||||
|
||||
gyro_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_GYRO, meas) |
||||
if gyro_res is not None: |
||||
_, new_x, _, new_P, _, _, (gyro_err,), _, _ = gyro_res |
||||
self.observation_errors[ObservationKind.PHONE_GYRO] = np.array(gyro_err) |
||||
self.observations[ObservationKind.PHONE_GYRO] = meas |
||||
|
||||
elif which == "carState": |
||||
self.car_speed = abs(msg.vEgo) |
||||
|
||||
elif which == "liveCalibration": |
||||
if len(msg.rpyCalib) > 0: |
||||
calib = np.array(msg.rpyCalib) |
||||
if calib.min() < -CALIB_RPY_SANITY_CHECK or calib.max() > CALIB_RPY_SANITY_CHECK: |
||||
return HandleLogResult.INPUT_INVALID |
||||
|
||||
self.device_from_calib = rot_from_euler(calib) |
||||
self.calibrated = msg.calStatus == log.LiveCalibrationData.Status.calibrated |
||||
|
||||
elif which == "cameraOdometry": |
||||
if not self._validate_timestamp(t): |
||||
return HandleLogResult.TIMING_INVALID |
||||
|
||||
rot_device = np.matmul(self.device_from_calib, np.array(msg.rot)) |
||||
trans_device = np.matmul(self.device_from_calib, np.array(msg.trans)) |
||||
|
||||
if np.linalg.norm(rot_device) > ROTATION_SANITY_CHECK or np.linalg.norm(trans_device) > TRANS_SANITY_CHECK: |
||||
return HandleLogResult.INPUT_INVALID |
||||
|
||||
rot_calib_std = np.array(msg.rotStd) |
||||
trans_calib_std = np.array(msg.transStd) |
||||
|
||||
if rot_calib_std.min() <= MIN_STD_SANITY_CHECK or trans_calib_std.min() <= MIN_STD_SANITY_CHECK: |
||||
return HandleLogResult.INPUT_INVALID |
||||
|
||||
if np.linalg.norm(rot_calib_std) > 10 * ROTATION_SANITY_CHECK or np.linalg.norm(trans_calib_std) > 10 * TRANS_SANITY_CHECK: |
||||
return HandleLogResult.INPUT_INVALID |
||||
|
||||
self.posenet_stds.pop(0) |
||||
self.posenet_stds.append(trans_calib_std[0]) |
||||
|
||||
# Multiply by N to avoid to high certainty in kalman filter because of temporally correlated noise |
||||
rot_calib_std *= 10 |
||||
trans_calib_std *= 2 |
||||
|
||||
rot_device_std = rotate_std(self.device_from_calib, rot_calib_std) |
||||
trans_device_std = rotate_std(self.device_from_calib, trans_calib_std) |
||||
rot_device_noise = rot_device_std ** 2 |
||||
trans_device_noise = trans_device_std ** 2 |
||||
|
||||
cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, rot_device_noise) |
||||
cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, trans_device_noise) |
||||
self.camodo_yawrate_distribution = np.array([rot_device[2], rot_device_std[2]]) |
||||
if cam_odo_rot_res is not None: |
||||
_, new_x, _, new_P, _, _, (cam_odo_rot_err,), _, _ = cam_odo_rot_res |
||||
self.observation_errors[ObservationKind.CAMERA_ODO_ROTATION] = np.array(cam_odo_rot_err) |
||||
self.observations[ObservationKind.CAMERA_ODO_ROTATION] = rot_device |
||||
if cam_odo_trans_res is not None: |
||||
_, new_x, _, new_P, _, _, (cam_odo_trans_err,), _, _ = cam_odo_trans_res |
||||
self.observation_errors[ObservationKind.CAMERA_ODO_TRANSLATION] = np.array(cam_odo_trans_err) |
||||
self.observations[ObservationKind.CAMERA_ODO_TRANSLATION] = trans_device |
||||
|
||||
if new_x is not None and new_P is not None: |
||||
self._finite_check(t, new_x, new_P) |
||||
return HandleLogResult.SUCCESS |
||||
|
||||
def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_valid: bool): |
||||
state, cov = self.kf.x, self.kf.P |
||||
std = np.sqrt(np.diag(cov)) |
||||
|
||||
orientation_ned, orientation_ned_std = state[States.NED_ORIENTATION], std[States.NED_ORIENTATION] |
||||
velocity_device, velocity_device_std = state[States.DEVICE_VELOCITY], std[States.DEVICE_VELOCITY] |
||||
angular_velocity_device, angular_velocity_device_std = state[States.ANGULAR_VELOCITY], std[States.ANGULAR_VELOCITY] |
||||
acceleration_device, acceleration_device_std = state[States.ACCELERATION], std[States.ACCELERATION] |
||||
|
||||
msg = messaging.new_message("livePose") |
||||
msg.valid = filter_valid |
||||
|
||||
livePose = msg.livePose |
||||
init_xyz_measurement(livePose.orientationNED, orientation_ned, orientation_ned_std, filter_valid) |
||||
init_xyz_measurement(livePose.velocityDevice, velocity_device, velocity_device_std, filter_valid) |
||||
init_xyz_measurement(livePose.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid) |
||||
init_xyz_measurement(livePose.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid) |
||||
if self.debug: |
||||
livePose.debugFilterState.value = state.tolist() |
||||
livePose.debugFilterState.std = std.tolist() |
||||
livePose.debugFilterState.valid = filter_valid |
||||
livePose.debugFilterState.observations = [ |
||||
{'kind': k, 'value': self.observations[k].tolist(), 'error': self.observation_errors[k].tolist()} |
||||
for k in self.observations.keys() |
||||
] |
||||
|
||||
old_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST_HALF]) |
||||
new_mean = np.mean(self.posenet_stds[POSENET_STD_HIST_HALF:]) |
||||
std_spike = (new_mean / old_mean) > 4.0 and new_mean > 7.0 |
||||
|
||||
livePose.inputsOK = inputs_valid |
||||
livePose.posenetOK = not std_spike or self.car_speed <= 5.0 |
||||
livePose.sensorsOK = sensors_valid |
||||
|
||||
return msg |
||||
|
||||
|
||||
def sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, simulation): |
||||
cur_time = time.monotonic() |
||||
for which, msgs in [("accelerometer", acc_msgs), ("gyroscope", gyro_msgs)]: |
||||
if len(msgs) > 0: |
||||
sensor_valid[which] = msgs[-1].valid |
||||
sensor_recv_time[which] = cur_time |
||||
|
||||
if not simulation: |
||||
sensor_alive[which] = (cur_time - sensor_recv_time[which]) < 0.1 |
||||
else: |
||||
sensor_alive[which] = len(msgs) > 0 |
||||
|
||||
return all(sensor_alive.values()) and all(sensor_valid.values()) |
||||
|
||||
|
||||
def main(): |
||||
config_realtime_process([0, 1, 2, 3], 5) |
||||
|
||||
DEBUG = bool(int(os.getenv("DEBUG", "0"))) |
||||
SIMULATION = bool(int(os.getenv("SIMULATION", "0"))) |
||||
|
||||
pm = messaging.PubMaster(['livePose']) |
||||
sm = messaging.SubMaster(['carState', 'liveCalibration', 'cameraOdometry'], poll='cameraOdometry') |
||||
# separate sensor sockets for efficiency |
||||
sensor_sockets = [messaging.sub_sock(which, timeout=20) for which in ['accelerometer', 'gyroscope']] |
||||
sensor_alive, sensor_valid, sensor_recv_time = defaultdict(bool), defaultdict(bool), defaultdict(float) |
||||
|
||||
params = Params() |
||||
|
||||
estimator = LocationEstimator(DEBUG) |
||||
|
||||
filter_initialized = False |
||||
critcal_services = ["accelerometer", "gyroscope", "liveCalibration", "cameraOdometry"] |
||||
observation_timing_invalid = False |
||||
observation_input_invalid = defaultdict(int) |
||||
|
||||
initial_pose = params.get("LocationFilterInitialState") |
||||
if initial_pose is not None: |
||||
initial_pose = json.loads(initial_pose) |
||||
x_initial = np.array(initial_pose["x"], dtype=np.float64) |
||||
P_initial = np.diag(np.array(initial_pose["P"], dtype=np.float64)) |
||||
estimator.reset(None, x_initial, P_initial) |
||||
|
||||
while True: |
||||
sm.update() |
||||
|
||||
acc_msgs, gyro_msgs = (messaging.drain_sock(sock) for sock in sensor_sockets) |
||||
|
||||
if filter_initialized: |
||||
observation_timing_invalid = False |
||||
|
||||
msgs = [] |
||||
for msg in acc_msgs + gyro_msgs: |
||||
t, valid, which, data = msg.logMonoTime, msg.valid, msg.which(), getattr(msg, msg.which()) |
||||
msgs.append((t, valid, which, data)) |
||||
for which, updated in sm.updated.items(): |
||||
if not updated: |
||||
continue |
||||
t, valid, data = sm.logMonoTime[which], sm.valid[which], sm[which] |
||||
msgs.append((t, valid, which, data)) |
||||
|
||||
for log_mono_time, valid, which, msg in sorted(msgs, key=lambda x: x[0]): |
||||
if valid: |
||||
t = log_mono_time * 1e-9 |
||||
res = estimator.handle_log(t, which, msg) |
||||
if res == HandleLogResult.TIMING_INVALID: |
||||
observation_timing_invalid = True |
||||
elif res == HandleLogResult.INPUT_INVALID: |
||||
observation_input_invalid[which] += 1 |
||||
else: |
||||
observation_input_invalid[which] *= INPUT_INVALID_DECAY |
||||
else: |
||||
filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) |
||||
|
||||
if sm.updated["cameraOdometry"]: |
||||
critical_service_inputs_valid = all(observation_input_invalid[s] < INPUT_INVALID_THRESHOLD for s in critcal_services) |
||||
inputs_valid = sm.all_valid() and critical_service_inputs_valid and not observation_timing_invalid |
||||
sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) |
||||
|
||||
msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized) |
||||
pm.send("livePose", msg) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
main() |
||||
@ -1,122 +0,0 @@ |
||||
#include "selfdrive/locationd/models/live_kf.h" |
||||
|
||||
using namespace EKFS; |
||||
using namespace Eigen; |
||||
|
||||
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) { |
||||
return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols()); |
||||
} |
||||
|
||||
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) { |
||||
return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols()); |
||||
} |
||||
|
||||
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) { |
||||
std::vector<Eigen::Map<Eigen::VectorXd>> res; |
||||
for (const Eigen::VectorXd &vec : vec_vec) { |
||||
res.push_back(get_mapvec(vec)); |
||||
} |
||||
return res; |
||||
} |
||||
|
||||
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) { |
||||
std::vector<Eigen::Map<MatrixXdr>> res; |
||||
for (const MatrixXdr &mat : mat_vec) { |
||||
res.push_back(get_mapmat(mat)); |
||||
} |
||||
return res; |
||||
} |
||||
|
||||
LiveKalman::LiveKalman() { |
||||
this->dim_state = live_initial_x.rows(); |
||||
this->dim_state_err = live_initial_P_diag.rows(); |
||||
|
||||
this->initial_x = live_initial_x; |
||||
this->initial_P = live_initial_P_diag.asDiagonal(); |
||||
this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal(); |
||||
this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal(); |
||||
this->reset_orientation_P = live_reset_orientation_diag.asDiagonal(); |
||||
this->Q = live_Q_diag.asDiagonal(); |
||||
for (auto& pair : live_obs_noise_diag) { |
||||
this->obs_noise[pair.first] = pair.second.asDiagonal(); |
||||
} |
||||
|
||||
// init filter
|
||||
this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x), |
||||
get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(), |
||||
std::vector<int>{3}, std::vector<std::string>(), 0.8); |
||||
} |
||||
|
||||
void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) { |
||||
MatrixXdr covs = covs_diag.asDiagonal(); |
||||
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); |
||||
} |
||||
|
||||
void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) { |
||||
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); |
||||
} |
||||
|
||||
void LiveKalman::init_state(const VectorXd &state, double filter_time) { |
||||
MatrixXdr covs = this->filter->covs(); |
||||
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); |
||||
} |
||||
|
||||
VectorXd LiveKalman::get_x() { |
||||
return this->filter->state(); |
||||
} |
||||
|
||||
MatrixXdr LiveKalman::get_P() { |
||||
return this->filter->covs(); |
||||
} |
||||
|
||||
double LiveKalman::get_filter_time() { |
||||
return this->filter->get_filter_time(); |
||||
} |
||||
|
||||
std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) { |
||||
std::vector<MatrixXdr> R; |
||||
for (int i = 0; i < n; i++) { |
||||
R.push_back(this->obs_noise[kind]); |
||||
} |
||||
return R; |
||||
} |
||||
|
||||
std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) { |
||||
std::optional<Estimate> r; |
||||
if (R.size() == 0) { |
||||
R = this->get_R(kind, meas.size()); |
||||
} |
||||
r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R)); |
||||
return r; |
||||
} |
||||
|
||||
void LiveKalman::predict(double t) { |
||||
this->filter->predict(t); |
||||
} |
||||
|
||||
const Eigen::VectorXd &LiveKalman::get_initial_x() { |
||||
return this->initial_x; |
||||
} |
||||
|
||||
const MatrixXdr &LiveKalman::get_initial_P() { |
||||
return this->initial_P; |
||||
} |
||||
|
||||
const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() { |
||||
return this->fake_gps_pos_cov; |
||||
} |
||||
|
||||
const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() { |
||||
return this->fake_gps_vel_cov; |
||||
} |
||||
|
||||
const MatrixXdr &LiveKalman::get_reset_orientation_P() { |
||||
return this->reset_orientation_P; |
||||
} |
||||
|
||||
MatrixXdr LiveKalman::H(const VectorXd &in) { |
||||
assert(in.size() == 6); |
||||
Matrix<double, 3, 6, Eigen::RowMajor> res; |
||||
this->filter->get_extra_routine("H")((double*)in.data(), res.data()); |
||||
return res; |
||||
} |
||||
@ -1,66 +0,0 @@ |
||||
#pragma once |
||||
|
||||
#include <string> |
||||
#include <cmath> |
||||
#include <memory> |
||||
#include <unordered_map> |
||||
#include <vector> |
||||
|
||||
#include <eigen3/Eigen/Core> |
||||
#include <eigen3/Eigen/Dense> |
||||
|
||||
#include "generated/live_kf_constants.h" |
||||
#include "rednose/helpers/ekf_sym.h" |
||||
|
||||
#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth)
|
||||
|
||||
using namespace EKFS; |
||||
|
||||
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec); |
||||
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat); |
||||
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec); |
||||
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec); |
||||
|
||||
class LiveKalman { |
||||
public: |
||||
LiveKalman(); |
||||
|
||||
void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time); |
||||
void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time); |
||||
void init_state(const Eigen::VectorXd &state, double filter_time); |
||||
|
||||
Eigen::VectorXd get_x(); |
||||
MatrixXdr get_P(); |
||||
double get_filter_time(); |
||||
std::vector<MatrixXdr> get_R(int kind, int n); |
||||
|
||||
std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {}); |
||||
std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind); |
||||
std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind); |
||||
std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind); |
||||
void predict(double t); |
||||
|
||||
const Eigen::VectorXd &get_initial_x(); |
||||
const MatrixXdr &get_initial_P(); |
||||
const MatrixXdr &get_fake_gps_pos_cov(); |
||||
const MatrixXdr &get_fake_gps_vel_cov(); |
||||
const MatrixXdr &get_reset_orientation_P(); |
||||
|
||||
MatrixXdr H(const Eigen::VectorXd &in); |
||||
|
||||
private: |
||||
std::string name = "live"; |
||||
|
||||
std::shared_ptr<EKFSym> filter; |
||||
|
||||
int dim_state; |
||||
int dim_state_err; |
||||
|
||||
Eigen::VectorXd initial_x; |
||||
MatrixXdr initial_P; |
||||
MatrixXdr fake_gps_pos_cov; |
||||
MatrixXdr fake_gps_vel_cov; |
||||
MatrixXdr reset_orientation_P; |
||||
MatrixXdr Q; // process noise
|
||||
std::unordered_map<int, MatrixXdr> obs_noise; |
||||
}; |
||||
@ -1,242 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
|
||||
import sys |
||||
import os |
||||
import numpy as np |
||||
|
||||
from openpilot.selfdrive.locationd.models.constants import ObservationKind |
||||
|
||||
import sympy as sp |
||||
import inspect |
||||
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate |
||||
from rednose.helpers.ekf_sym import gen_code |
||||
|
||||
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth) |
||||
|
||||
|
||||
def numpy2eigenstring(arr): |
||||
assert(len(arr.shape) == 1) |
||||
arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '') |
||||
return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()" |
||||
|
||||
|
||||
class States: |
||||
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters |
||||
ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef |
||||
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s |
||||
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s |
||||
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases |
||||
ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2 |
||||
ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2 |
||||
|
||||
# Error-state has different slices because it is an ESKF |
||||
ECEF_POS_ERR = slice(0, 3) |
||||
ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error |
||||
ECEF_VELOCITY_ERR = slice(6, 9) |
||||
ANGULAR_VELOCITY_ERR = slice(9, 12) |
||||
GYRO_BIAS_ERR = slice(12, 15) |
||||
ACCELERATION_ERR = slice(15, 18) |
||||
ACC_BIAS_ERR = slice(18, 21) |
||||
|
||||
|
||||
class LiveKalman: |
||||
name = 'live' |
||||
|
||||
initial_x = np.array([3.88e6, -3.37e6, 3.76e6, |
||||
0.42254641, -0.31238054, -0.83602975, -0.15788347, # NED [0,0,0] -> ECEF Quat |
||||
0, 0, 0, |
||||
0, 0, 0, |
||||
0, 0, 0, |
||||
0, 0, 0, |
||||
0, 0, 0]) |
||||
|
||||
# state covariance |
||||
initial_P_diag = np.array([10**2, 10**2, 10**2, |
||||
0.01**2, 0.01**2, 0.01**2, |
||||
10**2, 10**2, 10**2, |
||||
1**2, 1**2, 1**2, |
||||
1**2, 1**2, 1**2, |
||||
100**2, 100**2, 100**2, |
||||
0.01**2, 0.01**2, 0.01**2]) |
||||
|
||||
# state covariance when resetting midway in a segment |
||||
reset_orientation_diag = np.array([1**2, 1**2, 1**2]) |
||||
|
||||
# fake observation covariance, to ensure the uncertainty estimate of the filter is under control |
||||
fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2]) |
||||
fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2]) |
||||
|
||||
# process noise |
||||
Q_diag = np.array([0.03**2, 0.03**2, 0.03**2, |
||||
0.001**2, 0.001**2, 0.001**2, |
||||
0.01**2, 0.01**2, 0.01**2, |
||||
0.1**2, 0.1**2, 0.1**2, |
||||
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, |
||||
3**2, 3**2, 3**2, |
||||
0.005**2, 0.005**2, 0.005**2]) |
||||
|
||||
obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]), |
||||
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]), |
||||
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]), |
||||
ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]), |
||||
ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]), |
||||
ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]), |
||||
ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]), |
||||
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])} |
||||
|
||||
@staticmethod |
||||
def generate_code(generated_dir): |
||||
name = LiveKalman.name |
||||
dim_state = LiveKalman.initial_x.shape[0] |
||||
dim_state_err = LiveKalman.initial_P_diag.shape[0] |
||||
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1) |
||||
state = sp.Matrix(state_sym) |
||||
x, y, z = state[States.ECEF_POS, :] |
||||
q = state[States.ECEF_ORIENTATION, :] |
||||
v = state[States.ECEF_VELOCITY, :] |
||||
vx, vy, vz = v |
||||
omega = state[States.ANGULAR_VELOCITY, :] |
||||
vroll, vpitch, vyaw = omega |
||||
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :] |
||||
acceleration = state[States.ACCELERATION, :] |
||||
acc_bias = state[States.ACC_BIAS, :] |
||||
|
||||
dt = sp.Symbol('dt') |
||||
|
||||
# calibration and attitude rotation matrices |
||||
quat_rot = quat_rotate(*q) |
||||
|
||||
# Got the quat predict equations from here |
||||
# A New Quaternion-Based Kalman Filter for |
||||
# Real-Time Attitude Estimation Using the Two-Step |
||||
# Geometrically-Intuitive Correction Algorithm |
||||
A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw], |
||||
[vroll, 0, vyaw, -vpitch], |
||||
[vpitch, -vyaw, 0, vroll], |
||||
[vyaw, vpitch, -vroll, 0]]) |
||||
q_dot = A * q |
||||
|
||||
# Time derivative of the state as a function of state |
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1))) |
||||
state_dot[States.ECEF_POS, :] = v |
||||
state_dot[States.ECEF_ORIENTATION, :] = q_dot |
||||
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration |
||||
|
||||
# Basic descretization, 1st order intergrator |
||||
# Can be pretty bad if dt is big |
||||
f_sym = state + dt * state_dot |
||||
|
||||
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1) |
||||
state_err = sp.Matrix(state_err_sym) |
||||
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :] |
||||
v_err = state_err[States.ECEF_VELOCITY_ERR, :] |
||||
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :] |
||||
acceleration_err = state_err[States.ACCELERATION_ERR, :] |
||||
|
||||
# Time derivative of the state error as a function of state error and state |
||||
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2]) |
||||
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err) |
||||
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1))) |
||||
state_err_dot[States.ECEF_POS_ERR, :] = v_err |
||||
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot |
||||
state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err) |
||||
f_err_sym = state_err + dt * state_err_dot |
||||
|
||||
# Observation matrix modifier |
||||
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) |
||||
H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start) |
||||
H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:] |
||||
H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop) |
||||
|
||||
# these error functions are defined so that say there |
||||
# is a nominal x and true x: |
||||
# true x = err_function(nominal x, delta x) |
||||
# delta x = inv_err_function(nominal x, true x) |
||||
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1) |
||||
true_x = sp.MatrixSymbol('true_x', dim_state, 1) |
||||
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1) |
||||
|
||||
err_function_sym = sp.Matrix(np.zeros((dim_state, 1))) |
||||
delta_quat = sp.Matrix(np.ones(4)) |
||||
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :]) |
||||
err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :]) |
||||
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat |
||||
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :]) |
||||
|
||||
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1))) |
||||
inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0]) |
||||
delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0] |
||||
inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:]) |
||||
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0]) |
||||
|
||||
eskf_params = [[err_function_sym, nom_x, delta_x], |
||||
[inv_err_function_sym, nom_x, true_x], |
||||
H_mod_sym, f_err_sym, state_err_sym] |
||||
# |
||||
# Observation functions |
||||
# |
||||
h_gyro_sym = sp.Matrix([ |
||||
vroll + roll_bias, |
||||
vpitch + pitch_bias, |
||||
vyaw + yaw_bias]) |
||||
|
||||
pos = sp.Matrix([x, y, z]) |
||||
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos) |
||||
h_acc_sym = (gravity + acceleration + acc_bias) |
||||
h_acc_stationary_sym = acceleration |
||||
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw]) |
||||
h_pos_sym = sp.Matrix([x, y, z]) |
||||
h_vel_sym = sp.Matrix([vx, vy, vz]) |
||||
h_orientation_sym = q |
||||
h_relative_motion = sp.Matrix(quat_rot.T * v) |
||||
|
||||
obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None], |
||||
[h_phone_rot_sym, ObservationKind.NO_ROT, None], |
||||
[h_acc_sym, ObservationKind.PHONE_ACCEL, None], |
||||
[h_pos_sym, ObservationKind.ECEF_POS, None], |
||||
[h_vel_sym, ObservationKind.ECEF_VEL, None], |
||||
[h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None], |
||||
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], |
||||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], |
||||
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]] |
||||
|
||||
# this returns a sympy routine for the jacobian of the observation function of the local vel |
||||
in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz |
||||
h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]])) |
||||
extra_routines = [('H', h.jacobian(in_vec), [in_vec])] |
||||
|
||||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines) |
||||
|
||||
# write constants to extra header file for use in cpp |
||||
live_kf_header = "#pragma once\n\n" |
||||
live_kf_header += "#include <unordered_map>\n" |
||||
live_kf_header += "#include <eigen3/Eigen/Dense>\n\n" |
||||
for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)): |
||||
assert(slc.step is None) # unsupported |
||||
live_kf_header += f'#define STATE_{state}_START {slc.start}\n' |
||||
live_kf_header += f'#define STATE_{state}_END {slc.stop}\n' |
||||
live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n' |
||||
live_kf_header += "\n" |
||||
|
||||
for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)): |
||||
live_kf_header += f'#define OBSERVATION_{kind} {val}\n' |
||||
live_kf_header += "\n" |
||||
|
||||
live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n" |
||||
live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n" |
||||
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n" |
||||
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n" |
||||
live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n" |
||||
live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n" |
||||
live_kf_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {\n" |
||||
for kind, noise in LiveKalman.obs_noise_diag.items(): |
||||
live_kf_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n" |
||||
live_kf_header += "};\n\n" |
||||
|
||||
open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
generated_dir = sys.argv[2] |
||||
LiveKalman.generate_code(generated_dir) |
||||
@ -0,0 +1,138 @@ |
||||
#!/usr/bin/env python3 |
||||
|
||||
import sys |
||||
import numpy as np |
||||
|
||||
from openpilot.selfdrive.locationd.models.constants import ObservationKind |
||||
|
||||
if __name__=="__main__": |
||||
import sympy as sp |
||||
from rednose.helpers.ekf_sym import gen_code |
||||
from rednose.helpers.sympy_helpers import euler_rotate, rot_to_euler |
||||
else: |
||||
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx |
||||
|
||||
EARTH_G = 9.81 |
||||
|
||||
|
||||
class States: |
||||
NED_ORIENTATION = slice(0, 3) # roll, pitch, yaw in rad |
||||
DEVICE_VELOCITY = slice(3, 6) # ned velocity in m/s |
||||
ANGULAR_VELOCITY = slice(6, 9) # roll, pitch and yaw rates in rad/s |
||||
GYRO_BIAS = slice(9, 12) # roll, pitch and yaw gyroscope biases in rad/s |
||||
ACCELERATION = slice(12, 15) # acceleration in device frame in m/s**2 |
||||
ACCEL_BIAS = slice(15, 18) # Acceletometer bias in m/s**2 |
||||
|
||||
|
||||
class PoseKalman: |
||||
name = "pose" |
||||
|
||||
# state |
||||
initial_x = np.array([0.0, 0.0, 0.0, |
||||
0.0, 0.0, 0.0, |
||||
0.0, 0.0, 0.0, |
||||
0.0, 0.0, 0.0, |
||||
0.0, 0.0, 0.0, |
||||
0.0, 0.0, 0.0]) |
||||
# state covariance |
||||
initial_P = np.diag([0.01**2, 0.01**2, 0.01**2, |
||||
10**2, 10**2, 10**2, |
||||
1**2, 1**2, 1**2, |
||||
1**2, 1**2, 1**2, |
||||
100**2, 100**2, 100**2, |
||||
0.01**2, 0.01**2, 0.01**2]) |
||||
|
||||
# process noise |
||||
Q = np.diag([0.001**2, 0.001**2, 0.001**2, |
||||
0.01**2, 0.01**2, 0.01**2, |
||||
0.1**2, 0.1**2, 0.1**2, |
||||
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, |
||||
3**2, 3**2, 3**2, |
||||
0.005**2, 0.005**2, 0.005**2]) |
||||
|
||||
obs_noise = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]), |
||||
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]), |
||||
ObservationKind.CAMERA_ODO_TRANSLATION: np.array([0.5**2, 0.5**2, 0.5**2]), |
||||
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2])} |
||||
|
||||
@staticmethod |
||||
def generate_code(generated_dir): |
||||
name = PoseKalman.name |
||||
dim_state = PoseKalman.initial_x.shape[0] |
||||
dim_state_err = PoseKalman.initial_P.shape[0] |
||||
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1) |
||||
state = sp.Matrix(state_sym) |
||||
roll, pitch, yaw = state[States.NED_ORIENTATION, :] |
||||
velocity = state[States.DEVICE_VELOCITY, :] |
||||
angular_velocity = state[States.ANGULAR_VELOCITY, :] |
||||
vroll, vpitch, vyaw = angular_velocity |
||||
gyro_bias = state[States.GYRO_BIAS, :] |
||||
acceleration = state[States.ACCELERATION, :] |
||||
acc_bias = state[States.ACCEL_BIAS, :] |
||||
|
||||
dt = sp.Symbol('dt') |
||||
|
||||
ned_from_device = euler_rotate(roll, pitch, yaw) |
||||
device_from_ned = ned_from_device.T |
||||
|
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1))) |
||||
state_dot[States.DEVICE_VELOCITY, :] = acceleration |
||||
|
||||
f_sym = state + dt * state_dot |
||||
device_from_device_t1 = euler_rotate(dt*vroll, dt*vpitch, dt*vyaw) |
||||
ned_from_device_t1 = ned_from_device * device_from_device_t1 |
||||
f_sym[States.NED_ORIENTATION, :] = rot_to_euler(ned_from_device_t1) |
||||
|
||||
centripetal_acceleration = angular_velocity.cross(velocity) |
||||
gravity = sp.Matrix([0, 0, -EARTH_G]) |
||||
h_gyro_sym = angular_velocity + gyro_bias |
||||
h_acc_sym = device_from_ned * gravity + acceleration + centripetal_acceleration + acc_bias |
||||
h_phone_rot_sym = angular_velocity |
||||
h_relative_motion_sym = velocity |
||||
obs_eqs = [ |
||||
[h_gyro_sym, ObservationKind.PHONE_GYRO, None], |
||||
[h_acc_sym, ObservationKind.PHONE_ACCEL, None], |
||||
[h_relative_motion_sym, ObservationKind.CAMERA_ODO_TRANSLATION, None], |
||||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], |
||||
] |
||||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err) |
||||
|
||||
def __init__(self, generated_dir, max_rewind_age): |
||||
dim_state, dim_state_err = PoseKalman.initial_x.shape[0], PoseKalman.initial_P.shape[0] |
||||
self.filter = EKF_sym_pyx(generated_dir, self.name, PoseKalman.Q, PoseKalman.initial_x, PoseKalman.initial_P, |
||||
dim_state, dim_state_err, max_rewind_age=max_rewind_age) |
||||
|
||||
@property |
||||
def x(self): |
||||
return self.filter.state() |
||||
|
||||
@property |
||||
def P(self): |
||||
return self.filter.covs() |
||||
|
||||
@property |
||||
def t(self): |
||||
return self.filter.get_filter_time() |
||||
|
||||
def predict_and_observe(self, t, kind, data, obs_noise=None): |
||||
data = np.atleast_2d(data) |
||||
if obs_noise is None: |
||||
obs_noise = self.obs_noise[kind] |
||||
R = self._get_R(len(data), obs_noise) |
||||
return self.filter.predict_and_update_batch(t, kind, data, R) |
||||
|
||||
def reset(self, t, x_init, P_init): |
||||
self.filter.init_state(x_init, P_init, t) |
||||
|
||||
def _get_R(self, n, obs_noise): |
||||
dim = obs_noise.shape[0] |
||||
R = np.zeros((n, dim, dim)) |
||||
for i in range(n): |
||||
R[i, :, :] = np.diag(obs_noise) |
||||
return R |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
generated_dir = sys.argv[2] |
||||
PoseKalman.generate_code(generated_dir) |
||||
@ -1 +1 @@ |
||||
bf2bc833e238ec36c0519162edcaad743fad6dc5 |
||||
c2022c388da6eb2e26bb23ad6009be9d5314c0be |
||||
|
||||
Loading…
Reference in new issue