diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript new file mode 100644 index 0000000..07555a6 --- /dev/null +++ b/selfdrive/locationd/SConscript @@ -0,0 +1,37 @@ +Import('env', 'arch', 'common', 'cereal', 'messaging', 'rednose', 'transformations') + +loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread', 'dl'] + +# build ekf models +rednose_gen_dir = 'models/generated' +rednose_gen_deps = [ + "models/constants.py", +] +live_ekf = env.RednoseCompileFilter( + target='live', + filter_gen_script='models/live_kf.py', + output_dir=rednose_gen_dir, + extra_gen_artifacts=['live_kf_constants.h'], + gen_script_deps=rednose_gen_deps, +) +car_ekf = env.RednoseCompileFilter( + target='car', + filter_gen_script='models/car_kf.py', + output_dir=rednose_gen_dir, + extra_gen_artifacts=[], + gen_script_deps=rednose_gen_deps, +) + +# locationd build +locationd_sources = ["locationd.cc", "models/live_kf.cc"] + +lenv = env.Clone() +# ekf filter libraries need to be linked, even if no symbols are used +if arch != "Darwin": + lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"] + +lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath) +lenv["RPATH"].append(Dir(rednose_gen_dir).abspath) +locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations) +lenv.Depends(locationd, rednose) +lenv.Depends(locationd, live_ekf) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index da1c4ec..6e154bf 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -10,7 +10,7 @@ import gc import os import capnp import numpy as np -from typing import List, NoReturn, Optional +from typing import NoReturn from cereal import log import cereal.messaging as messaging @@ -89,7 +89,7 @@ class Calibrator: valid_blocks: int = 0, wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT, height_init: np.ndarray = HEIGHT_INIT, - smooth_from: Optional[np.ndarray] = None) -> None: + smooth_from: np.ndarray = None) -> None: if not np.isfinite(rpy_init).all(): self.rpy = RPY_INIT.copy() else: @@ -125,7 +125,7 @@ class Calibrator: self.old_rpy = smooth_from self.old_rpy_weight = 1.0 - def get_valid_idxs(self) -> List[int]: + def get_valid_idxs(self) -> list[int]: # exclude current block_idx from validity window before_current = list(range(self.block_idx)) after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks)) @@ -175,12 +175,12 @@ class Calibrator: else: return self.rpy - def handle_cam_odom(self, trans: List[float], - rot: List[float], - wide_from_device_euler: List[float], - trans_std: List[float], - road_transform_trans: List[float], - road_transform_trans_std: List[float]) -> Optional[np.ndarray]: + def handle_cam_odom(self, trans: list[float], + rot: list[float], + wide_from_device_euler: list[float], + trans_std: list[float], + road_transform_trans: list[float], + road_transform_trans_std: list[float]) -> np.ndarray | None: self.old_rpy_weight = max(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES) straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) diff --git a/selfdrive/locationd/helpers.py b/selfdrive/locationd/helpers.py index c292e98..786bdbb 100644 --- a/selfdrive/locationd/helpers.py +++ b/selfdrive/locationd/helpers.py @@ -1,5 +1,5 @@ import numpy as np -from typing import List, Optional, Tuple, Any +from typing import Any from cereal import log @@ -12,7 +12,7 @@ class NPQueue: def __len__(self) -> int: return len(self.arr) - def append(self, pt: List[float]) -> None: + def append(self, pt: list[float]) -> None: if len(self.arr) < self.maxlen: self.arr = np.append(self.arr, [pt], axis=0) else: @@ -21,7 +21,7 @@ class NPQueue: class PointBuckets: - def __init__(self, x_bounds: List[Tuple[float, float]], min_points: List[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None: + def __init__(self, x_bounds: list[tuple[float, float]], min_points: list[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None: self.x_bounds = x_bounds self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in x_bounds} self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True)) @@ -41,13 +41,13 @@ class PointBuckets: def add_point(self, x: float, y: float, bucket_val: float) -> None: raise NotImplementedError - def get_points(self, num_points: Optional[int] = None) -> Any: + def get_points(self, num_points: int = None) -> Any: points = np.vstack([x.arr for x in self.buckets.values()]) if num_points is None: return points return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)] - def load_points(self, points: List[List[float]]) -> None: + def load_points(self, points: list[list[float]]) -> None: for point in points: self.add_point(*point) diff --git a/selfdrive/locationd/locationd b/selfdrive/locationd/locationd deleted file mode 100755 index b4ab23a..0000000 Binary files a/selfdrive/locationd/locationd and /dev/null differ diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc new file mode 100644 index 0000000..2ac392a --- /dev/null +++ b/selfdrive/locationd/locationd.cc @@ -0,0 +1,750 @@ +#include "selfdrive/locationd/locationd.h" + +#include +#include + +#include +#include +#include + +using namespace EKFS; +using namespace Eigen; + +ExitHandler do_exit; +const double ACCEL_SANITY_CHECK = 100.0; // m/s^2 +const double ROTATION_SANITY_CHECK = 10.0; // rad/s +const double TRANS_SANITY_CHECK = 200.0; // m/s +const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg) +const double ALTITUDE_SANITY_CHECK = 10000; // m +const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad +const double VALID_TIME_SINCE_RESET = 1.0; // s +const double VALID_POS_STD = 50.0; // m +const double MAX_RESET_TRACKER = 5.0; +const double SANE_GPS_UNCERTAINTY = 1500.0; // m +const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker +const double RESET_TRACKER_DECAY = 0.99995; +const double DECAY = 0.9993; // ~10 secs to resume after a bad input +const double MAX_FILTER_REWIND_TIME = 0.8; // s +const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30; + +// TODO: GPS sensor time offsets are empirically calculated +// They should be replaced with synced time from a real clock +const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s +const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s +const float GPS_POS_STD_THRESHOLD = 50.0; +const float GPS_VEL_STD_THRESHOLD = 5.0; +const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0; +const float GPS_POS_STD_RESET_THRESHOLD = 2.0; +const float GPS_VEL_STD_RESET_THRESHOLD = 0.5; +const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0; +const int GPS_ORIENTATION_ERROR_RESET_CNT = 3; + +const bool DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0"; + +static VectorXd floatlist2vector(const capnp::List::Reader& floatlist) { + VectorXd res(floatlist.size()); + for (int i = 0; i < floatlist.size(); i++) { + res[i] = floatlist[i]; + } + return res; +} + +static Vector4d quat2vector(const Quaterniond& quat) { + return Vector4d(quat.w(), quat.x(), quat.y(), quat.z()); +} + +static Quaterniond vector2quat(const VectorXd& vec) { + return Quaterniond(vec(0), vec(1), vec(2), vec(3)); +} + +static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) { + meas.setValue(kj::arrayPtr(val.data(), val.size())); + meas.setStd(kj::arrayPtr(std.data(), std.size())); + meas.setValid(valid); +} + + +static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) { + // To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix + return ((rot_matrix * cov_in) * rot_matrix.transpose()); +} + +static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) { + // Stds cannot be rotated like values, only covariances can be rotated + return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt(); +} + +Localizer::Localizer(LocalizerGnssSource gnss_source) { + this->kf = std::make_unique(); + 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_START); + this->converter = std::make_unique((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); + this->configure_gnss_source(gnss_source); +} + +void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { + VectorXd predicted_state = this->kf->get_x(); + MatrixXdr predicted_cov = this->kf->get_P(); + VectorXd predicted_std = predicted_cov.diagonal().array().sqrt(); + + VectorXd fix_ecef = predicted_state.segment(STATE_ECEF_POS_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_START); + VectorXd vel_ecef = predicted_state.segment(STATE_ECEF_VELOCITY_START); + VectorXd vel_ecef_std = predicted_std.segment(STATE_ECEF_VELOCITY_ERR_START); + VectorXd fix_pos_geo_vec = this->get_position_geodetic(); + VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment(STATE_ECEF_ORIENTATION_START))); + VectorXd orientation_ecef_std = predicted_std.segment(STATE_ECEF_ORIENTATION_ERR_START); + MatrixXdr orientation_ecef_cov = predicted_cov.block(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_START); + MatrixXdr acc_calib_cov = predicted_cov.block(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_START); + + MatrixXdr vel_angular_cov = predicted_cov.block(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_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() = + predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START); + condensed_cov.topRightCorner() = + predicted_cov.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START); + condensed_cov.bottomRightCorner() = + predicted_cov.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START); + condensed_cov.bottomLeftCorner() = + predicted_cov.block(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_START); + VectorXd accDeviceErr = predicted_std.segment(STATE_ACCELERATION_ERR_START); + + VectorXd angVelocityDevice = predicted_state.segment(STATE_ANGULAR_VELOCITY_START); + VectorXd angVelocityDeviceErr = predicted_std.segment(STATE_ANGULAR_VELOCITY_ERR_START); + + Vector3d nans = Vector3d(NAN, NAN, NAN); + + // TODO fill in NED and Calibrated stds + // write measurements to msg + init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode); + init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode); + init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode); + init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode); + init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true); + init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true); + init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode); + init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode); + init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode); + init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode); + init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true); + init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated); + init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated); + init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated); + if (DEBUG) { + init_measurement(fix.initFilterState(), predicted_state, predicted_std, true); + } + + double old_mean = 0.0, new_mean = 0.0; + int i = 0; + for (double x : this->posenet_stds) { + if (i < POSENET_STD_HIST_HALF) { + old_mean += x; + } else { + new_mean += x; + } + i++; + } + old_mean /= POSENET_STD_HIST_HALF; + new_mean /= POSENET_STD_HIST_HALF; + // experimentally found these values, no false positives in 20k minutes of driving + bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0); + + fix.setPosenetOK(!(std_spike && this->car_speed > 5.0)); + fix.setDeviceStable(!this->device_fell); + fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER); + fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff); + this->device_fell = false; + + //fix.setGpsWeek(this->time.week); + //fix.setGpsTimeOfWeek(this->time.tow); + fix.setUnixTimestampMillis(this->unix_timestamp_millis); + + double time_since_reset = this->kf->get_filter_time() - this->last_reset_time; + fix.setTimeSinceReset(time_since_reset); + if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) { + fix.setStatus(cereal::LiveLocationKalman::Status::VALID); + } else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) { + fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED); + } else { + fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED); + } +} + +VectorXd Localizer::get_position_geodetic() { + VectorXd fix_ecef = this->kf->get_x().segment(STATE_ECEF_POS_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_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 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_START); + VectorXd ecef_vel = current_x.segment(STATE_ECEF_VELOCITY_START); + const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov(); + const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov(); + + this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); + this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); +} + +void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) { + bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY); + bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)); + bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)); + bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK); + + if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) { + //this->gps_valid = false; + this->determine_gps_mode(current_time); + return; + } + + double sensor_time = current_time - sensor_time_offset; + + // Process message + //this->gps_valid = true; + this->gps_mode = true; + Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; + this->converter = std::make_unique(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_START) - ecef_pos).norm(); + + VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(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_START) - ecef_pos).norm(); + + VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(STATE_ECEF_ORIENTATION_START))); + VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef); + + LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); + ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]}; + VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector(); + double bearing_rad = atan2(ned_vel[1], ned_vel[0]); + + VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad); + VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI; + for (int i = 0; i < orientation_error.size(); i++) { + orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI); + if (orientation_error(i) < 0.0) { + orientation_error(i) += 2.0 * M_PI; + } + orientation_error(i) -= M_PI; + } + VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); + + if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) { + this->determine_gps_mode(current_time); + return; + } + + // prevent jumping gnss measurements (covered lots, standstill...) + bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD; + orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD; + orientation_reset &= !this->standstill; + if (orientation_reset) { + this->orientation_reset_count++; + } else { + this->orientation_reset_count = 0; + } + + if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) { + // always reset on first gps message and if the location is off but the accuracy is high + LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); + } else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) { + LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset"); + this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); + this->orientation_reset_count = 0; + } + + this->gps_mode = true; + this->last_gps_msg = sensor_time; + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); + this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); +} + +void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) { + this->car_speed = std::abs(log.getVEgo()); + this->standstill = log.getStandstill(); + if (this->standstill) { + this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) }); + this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) }); + } +} + +void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) { + VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); + VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); + + if (!this->is_timestamp_valid(current_time)) { + this->observation_timings_invalid = true; + return; + } + + if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; + return; + } + + VectorXd rot_calib_std = floatlist2vector(log.getRotStd()); + VectorXd trans_calib_std = floatlist2vector(log.getTransStd()); + + if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; + return; + } + + if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) { + this->observation_values_invalid["cameraOdometry"] += 1.0; + return; + } + + this->posenet_stds.pop_front(); + this->posenet_stds.push_back(trans_calib_std[0]); + + // Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise + trans_calib_std *= 10.0; + rot_calib_std *= 10.0; + MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal(); + MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal(); + this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION, + { rot_device }, { rot_device_cov }); + this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION, + { trans_device }, { trans_device_cov }); + this->observation_values_invalid["cameraOdometry"] *= DECAY; + this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]); +} + +void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { + if (!this->is_timestamp_valid(current_time)) { + this->observation_timings_invalid = true; + return; + } + + if (log.getRpyCalib().size() > 0) { + auto live_calib = floatlist2vector(log.getRpyCalib()); + if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) { + this->observation_values_invalid["liveCalibration"] += 1.0; + return; + } + + this->calib = live_calib; + this->device_from_calib = euler2rot(this->calib); + this->calib_from_device = this->device_from_calib.transpose(); + this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED; + this->observation_values_invalid["liveCalibration"] *= DECAY; + } +} + +void Localizer::reset_kalman(double current_time) { + const VectorXd &init_x = this->kf->get_initial_x(); + const MatrixXdr &init_P = this->kf->get_initial_P(); + this->reset_kalman(current_time, init_x, init_P); +} + +void Localizer::finite_check(double current_time) { + bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all(); + if (!all_finite) { + LOGE("Non-finite values detected, kalman reset"); + this->reset_kalman(current_time); + } +} + +void Localizer::time_check(double current_time) { + if (std::isnan(this->last_reset_time)) { + this->last_reset_time = current_time; + } + if (std::isnan(this->first_valid_log_time)) { + this->first_valid_log_time = current_time; + } + double filter_time = this->kf->get_filter_time(); + bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10); + if (big_time_gap) { + LOGE("Time gap of over 10s detected, kalman reset"); + this->reset_kalman(current_time); + } +} + +void Localizer::update_reset_tracker() { + // reset tracker is tuned to trigger when over 1reset/10s over 2min period + if (this->is_gps_ok()) { + this->reset_tracker *= RESET_TRACKER_DECAY; + } else { + this->reset_tracker = 0.0; + } +} + +void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) { + // too nonlinear to init on completely wrong + VectorXd current_x = this->kf->get_x(); + MatrixXdr current_P = this->kf->get_P(); + MatrixXdr init_P = this->kf->get_initial_P(); + const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P(); + int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN); + + current_x.segment(STATE_ECEF_ORIENTATION_START) = init_orient; + current_x.segment(STATE_ECEF_VELOCITY_START) = init_vel; + current_x.segment(STATE_ECEF_POS_START) = init_pos; + + init_P.block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal(); + init_P.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal(); + init_P.block(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(); + + this->handle_msg(event); +} + +void Localizer::handle_msg(const cereal::Event::Reader& log) { + double t = log.getLogMonoTime() * 1e-9; + this->time_check(t); + if (log.isAccelerometer()) { + this->handle_sensor(t, log.getAccelerometer()); + } else if (log.isGyroscope()) { + this->handle_sensor(t, log.getGyroscope()); + } else if (log.isGpsLocation()) { + this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); + } else if (log.isGpsLocationExternal()) { + this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); + //} else if (log.isGnssMeasurements()) { + // this->handle_gnss(t, log.getGnssMeasurements()); + } else if (log.isCarState()) { + this->handle_car_state(t, log.getCarState()); + } else if (log.isCameraOdometry()) { + this->handle_cam_odo(t, log.getCameraOdometry()); + } else if (log.isLiveCalibration()) { + this->handle_live_calib(t, log.getLiveCalibration()); + } + this->finite_check(); + this->update_reset_tracker(); +} + +kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK, + bool sensorsOK, bool gpsOK, bool msgValid) { + cereal::Event::Builder evt = msg_builder.initEvent(); + evt.setValid(msgValid); + cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman(); + this->build_live_location(liveLoc); + liveLoc.setSensorsOK(sensorsOK); + liveLoc.setGpsOK(gpsOK); + liveLoc.setInputsOK(inputsOK); + return msg_builder.toBytes(); +} + +bool Localizer::is_gps_ok() { + return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0; +} + +bool Localizer::critical_services_valid(const std::map &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_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 service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", + "carState", "accelerometer", "gyroscope"}; + + SubMaster sm(service_list, {}, nullptr, {gps_location_socket}); + PubMaster pm({"liveLocationKalman"}); + + uint64_t cnt = 0; + bool filterInitialized = false; + const std::vector critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"}; + for (std::string service : critical_input_services) { + this->observation_values_invalid.insert({service, 0.0}); + } + + while (!do_exit) { + sm.update(); + if (filterInitialized){ + this->observation_timings_invalid_reset(); + for (const char* service : service_list) { + if (sm.updated(service) && sm.valid(service)){ + const cereal::Event::Reader log = sm[service]; + this->handle_msg(log); + } + } + } else { + filterInitialized = sm.allAliveAndValid(); + } + + const char* trigger_msg = "cameraOdometry"; + if (sm.updated(trigger_msg)) { + bool inputsOK = sm.allValid() && this->are_inputs_ok(); + bool gpsOK = this->is_gps_ok(); + bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"}); + + // Log time to first fix + if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) { + this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time); + } + + MessageBuilder msg_builder; + kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized); + pm.send("liveLocationKalman", bytes.begin(), bytes.size()); + + if (cnt % 1200 == 0 && gpsOK) { // once a minute + VectorXd posGeo = this->get_position_geodetic(); + std::string lastGPSPosJSON = util::string_format( + "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2)); + params.putNonBlocking("LastGPSPosition", lastGPSPosJSON); + } + cnt++; + } + } + return 0; +} + +int main() { + util::set_realtime_priority(5); + + Localizer localizer; + return localizer.locationd_thread(); +} diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h new file mode 100644 index 0000000..47c8bf5 --- /dev/null +++ b/selfdrive/locationd/locationd.h @@ -0,0 +1,100 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "common/transformations/coordinates.hpp" +#include "common/transformations/orientation.hpp" +#include "common/params.h" +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" + +#include "system/sensord/sensors/constants.h" +#define VISION_DECIMATION 2 +#define SENSOR_DECIMATION 10 +#include "selfdrive/locationd/models/live_kf.h" + +#define POSENET_STD_HIST_HALF 20 + +enum LocalizerGnssSource { + UBLOX, QCOM +}; + +class Localizer { +public: + Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX); + + int locationd_thread(); + + void reset_kalman(double current_time = NAN); + void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R); + void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P); + void finite_check(double current_time = NAN); + void time_check(double current_time = NAN); + void update_reset_tracker(); + bool is_gps_ok(); + bool critical_services_valid(const std::map &critical_services); + bool is_timestamp_valid(double current_time); + void determine_gps_mode(double current_time); + bool are_inputs_ok(); + void observation_timings_invalid_reset(); + + kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, + bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid); + void build_live_location(cereal::LiveLocationKalman::Builder& fix); + + Eigen::VectorXd get_position_geodetic(); + Eigen::VectorXd get_state(); + Eigen::VectorXd get_stdev(); + + void handle_msg_bytes(const char *data, const size_t size); + void handle_msg(const cereal::Event::Reader& log); + void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log); + void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset); + void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log); + void handle_car_state(double current_time, const cereal::CarState::Reader& log); + void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log); + void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log); + + void input_fake_gps_observations(double current_time); + +private: + std::unique_ptr 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 posenet_stds; + + std::unique_ptr converter; + + int64_t unix_timestamp_millis = 0; + double reset_tracker = 0.0; + bool device_fell = false; + bool gps_mode = false; + double first_valid_log_time = NAN; + double ttff = NAN; + double last_gps_msg = 0; + LocalizerGnssSource gnss_source; + bool observation_timings_invalid = false; + std::map observation_values_invalid; + bool standstill = true; + int32_t orientation_reset_count = 0; + float gps_std_factor; + float gps_variance_factor; + float gps_vertical_variance_factor; + double gps_time_offset; + Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std + + void configure_gnss_source(const LocalizerGnssSource &source); +}; diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 9230cb4..1f3e447 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import math import sys -from typing import Any, Dict +from typing import Any import numpy as np @@ -70,7 +70,7 @@ class CarKalman(KalmanFilter): ]) P_initial = Q.copy() - obs_noise: Dict[int, Any] = { + 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), diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp deleted file mode 100644 index 68b6df3..0000000 --- a/selfdrive/locationd/models/generated/car.cpp +++ /dev/null @@ -1,661 +0,0 @@ -#include "car.h" - -namespace { -#define DIM 9 -#define EDIM 9 -#define MEDIM 9 -typedef void (*Hfun)(double *, double *, double *); - -double mass; - -void set_mass(double x){ mass = x;} - -double rotational_inertia; - -void set_rotational_inertia(double x){ rotational_inertia = x;} - -double center_to_front; - -void set_center_to_front(double x){ center_to_front = x;} - -double center_to_rear; - -void set_center_to_rear(double x){ center_to_rear = x;} - -double stiffness_front; - -void set_stiffness_front(double x){ stiffness_front = x;} - -double stiffness_rear; - -void set_stiffness_rear(double x){ stiffness_rear = x;} -const static double MAHA_THRESH_25 = 3.8414588206941227; -const static double MAHA_THRESH_24 = 5.991464547107981; -const static double MAHA_THRESH_30 = 3.8414588206941227; -const static double MAHA_THRESH_26 = 3.8414588206941227; -const static double MAHA_THRESH_27 = 3.8414588206941227; -const static double MAHA_THRESH_29 = 3.8414588206941227; -const static double MAHA_THRESH_28 = 3.8414588206941227; -const static double MAHA_THRESH_31 = 3.8414588206941227; - -/****************************************************************************** - * Code generated with SymPy 1.12 * - * * - * See http://www.sympy.org/ for more information. * - * * - * This file is part of 'ekf' * - ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_7718808222430479447) { - out_7718808222430479447[0] = delta_x[0] + nom_x[0]; - out_7718808222430479447[1] = delta_x[1] + nom_x[1]; - out_7718808222430479447[2] = delta_x[2] + nom_x[2]; - out_7718808222430479447[3] = delta_x[3] + nom_x[3]; - out_7718808222430479447[4] = delta_x[4] + nom_x[4]; - out_7718808222430479447[5] = delta_x[5] + nom_x[5]; - out_7718808222430479447[6] = delta_x[6] + nom_x[6]; - out_7718808222430479447[7] = delta_x[7] + nom_x[7]; - out_7718808222430479447[8] = delta_x[8] + nom_x[8]; -} -void inv_err_fun(double *nom_x, double *true_x, double *out_1034114423937537934) { - out_1034114423937537934[0] = -nom_x[0] + true_x[0]; - out_1034114423937537934[1] = -nom_x[1] + true_x[1]; - out_1034114423937537934[2] = -nom_x[2] + true_x[2]; - out_1034114423937537934[3] = -nom_x[3] + true_x[3]; - out_1034114423937537934[4] = -nom_x[4] + true_x[4]; - out_1034114423937537934[5] = -nom_x[5] + true_x[5]; - out_1034114423937537934[6] = -nom_x[6] + true_x[6]; - out_1034114423937537934[7] = -nom_x[7] + true_x[7]; - out_1034114423937537934[8] = -nom_x[8] + true_x[8]; -} -void H_mod_fun(double *state, double *out_7225960904852606817) { - out_7225960904852606817[0] = 1.0; - out_7225960904852606817[1] = 0; - out_7225960904852606817[2] = 0; - out_7225960904852606817[3] = 0; - out_7225960904852606817[4] = 0; - out_7225960904852606817[5] = 0; - out_7225960904852606817[6] = 0; - out_7225960904852606817[7] = 0; - out_7225960904852606817[8] = 0; - out_7225960904852606817[9] = 0; - out_7225960904852606817[10] = 1.0; - out_7225960904852606817[11] = 0; - out_7225960904852606817[12] = 0; - out_7225960904852606817[13] = 0; - out_7225960904852606817[14] = 0; - out_7225960904852606817[15] = 0; - out_7225960904852606817[16] = 0; - out_7225960904852606817[17] = 0; - out_7225960904852606817[18] = 0; - out_7225960904852606817[19] = 0; - out_7225960904852606817[20] = 1.0; - out_7225960904852606817[21] = 0; - out_7225960904852606817[22] = 0; - out_7225960904852606817[23] = 0; - out_7225960904852606817[24] = 0; - out_7225960904852606817[25] = 0; - out_7225960904852606817[26] = 0; - out_7225960904852606817[27] = 0; - out_7225960904852606817[28] = 0; - out_7225960904852606817[29] = 0; - out_7225960904852606817[30] = 1.0; - out_7225960904852606817[31] = 0; - out_7225960904852606817[32] = 0; - out_7225960904852606817[33] = 0; - out_7225960904852606817[34] = 0; - out_7225960904852606817[35] = 0; - out_7225960904852606817[36] = 0; - out_7225960904852606817[37] = 0; - out_7225960904852606817[38] = 0; - out_7225960904852606817[39] = 0; - out_7225960904852606817[40] = 1.0; - out_7225960904852606817[41] = 0; - out_7225960904852606817[42] = 0; - out_7225960904852606817[43] = 0; - out_7225960904852606817[44] = 0; - out_7225960904852606817[45] = 0; - out_7225960904852606817[46] = 0; - out_7225960904852606817[47] = 0; - out_7225960904852606817[48] = 0; - out_7225960904852606817[49] = 0; - out_7225960904852606817[50] = 1.0; - out_7225960904852606817[51] = 0; - out_7225960904852606817[52] = 0; - out_7225960904852606817[53] = 0; - out_7225960904852606817[54] = 0; - out_7225960904852606817[55] = 0; - out_7225960904852606817[56] = 0; - out_7225960904852606817[57] = 0; - out_7225960904852606817[58] = 0; - out_7225960904852606817[59] = 0; - out_7225960904852606817[60] = 1.0; - out_7225960904852606817[61] = 0; - out_7225960904852606817[62] = 0; - out_7225960904852606817[63] = 0; - out_7225960904852606817[64] = 0; - out_7225960904852606817[65] = 0; - out_7225960904852606817[66] = 0; - out_7225960904852606817[67] = 0; - out_7225960904852606817[68] = 0; - out_7225960904852606817[69] = 0; - out_7225960904852606817[70] = 1.0; - out_7225960904852606817[71] = 0; - out_7225960904852606817[72] = 0; - out_7225960904852606817[73] = 0; - out_7225960904852606817[74] = 0; - out_7225960904852606817[75] = 0; - out_7225960904852606817[76] = 0; - out_7225960904852606817[77] = 0; - out_7225960904852606817[78] = 0; - out_7225960904852606817[79] = 0; - out_7225960904852606817[80] = 1.0; -} -void f_fun(double *state, double dt, double *out_5925834804962007644) { - out_5925834804962007644[0] = state[0]; - out_5925834804962007644[1] = state[1]; - out_5925834804962007644[2] = state[2]; - out_5925834804962007644[3] = state[3]; - out_5925834804962007644[4] = state[4]; - out_5925834804962007644[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5]; - out_5925834804962007644[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6]; - out_5925834804962007644[7] = state[7]; - out_5925834804962007644[8] = state[8]; -} -void F_fun(double *state, double dt, double *out_1594563891292900416) { - out_1594563891292900416[0] = 1; - out_1594563891292900416[1] = 0; - out_1594563891292900416[2] = 0; - out_1594563891292900416[3] = 0; - out_1594563891292900416[4] = 0; - out_1594563891292900416[5] = 0; - out_1594563891292900416[6] = 0; - out_1594563891292900416[7] = 0; - out_1594563891292900416[8] = 0; - out_1594563891292900416[9] = 0; - out_1594563891292900416[10] = 1; - out_1594563891292900416[11] = 0; - out_1594563891292900416[12] = 0; - out_1594563891292900416[13] = 0; - out_1594563891292900416[14] = 0; - out_1594563891292900416[15] = 0; - out_1594563891292900416[16] = 0; - out_1594563891292900416[17] = 0; - out_1594563891292900416[18] = 0; - out_1594563891292900416[19] = 0; - out_1594563891292900416[20] = 1; - out_1594563891292900416[21] = 0; - out_1594563891292900416[22] = 0; - out_1594563891292900416[23] = 0; - out_1594563891292900416[24] = 0; - out_1594563891292900416[25] = 0; - out_1594563891292900416[26] = 0; - out_1594563891292900416[27] = 0; - out_1594563891292900416[28] = 0; - out_1594563891292900416[29] = 0; - out_1594563891292900416[30] = 1; - out_1594563891292900416[31] = 0; - out_1594563891292900416[32] = 0; - out_1594563891292900416[33] = 0; - out_1594563891292900416[34] = 0; - out_1594563891292900416[35] = 0; - out_1594563891292900416[36] = 0; - out_1594563891292900416[37] = 0; - out_1594563891292900416[38] = 0; - out_1594563891292900416[39] = 0; - out_1594563891292900416[40] = 1; - out_1594563891292900416[41] = 0; - out_1594563891292900416[42] = 0; - out_1594563891292900416[43] = 0; - out_1594563891292900416[44] = 0; - out_1594563891292900416[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4])); - out_1594563891292900416[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_1594563891292900416[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_1594563891292900416[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_1594563891292900416[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2))); - out_1594563891292900416[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_1594563891292900416[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_1594563891292900416[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_1594563891292900416[53] = -9.8000000000000007*dt; - out_1594563891292900416[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4])); - out_1594563891292900416[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_1594563891292900416[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_1594563891292900416[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_1594563891292900416[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2))); - out_1594563891292900416[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_1594563891292900416[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1; - out_1594563891292900416[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_1594563891292900416[62] = 0; - out_1594563891292900416[63] = 0; - out_1594563891292900416[64] = 0; - out_1594563891292900416[65] = 0; - out_1594563891292900416[66] = 0; - out_1594563891292900416[67] = 0; - out_1594563891292900416[68] = 0; - out_1594563891292900416[69] = 0; - out_1594563891292900416[70] = 1; - out_1594563891292900416[71] = 0; - out_1594563891292900416[72] = 0; - out_1594563891292900416[73] = 0; - out_1594563891292900416[74] = 0; - out_1594563891292900416[75] = 0; - out_1594563891292900416[76] = 0; - out_1594563891292900416[77] = 0; - out_1594563891292900416[78] = 0; - out_1594563891292900416[79] = 0; - out_1594563891292900416[80] = 1; -} -void h_25(double *state, double *unused, double *out_4446559038823907444) { - out_4446559038823907444[0] = state[6]; -} -void H_25(double *state, double *unused, double *out_2291888026643029333) { - out_2291888026643029333[0] = 0; - out_2291888026643029333[1] = 0; - out_2291888026643029333[2] = 0; - out_2291888026643029333[3] = 0; - out_2291888026643029333[4] = 0; - out_2291888026643029333[5] = 0; - out_2291888026643029333[6] = 1; - out_2291888026643029333[7] = 0; - out_2291888026643029333[8] = 0; -} -void h_24(double *state, double *unused, double *out_5489680431960231161) { - out_5489680431960231161[0] = state[4]; - out_5489680431960231161[1] = state[5]; -} -void H_24(double *state, double *unused, double *out_1895242722289169838) { - out_1895242722289169838[0] = 0; - out_1895242722289169838[1] = 0; - out_1895242722289169838[2] = 0; - out_1895242722289169838[3] = 0; - out_1895242722289169838[4] = 1; - out_1895242722289169838[5] = 0; - out_1895242722289169838[6] = 0; - out_1895242722289169838[7] = 0; - out_1895242722289169838[8] = 0; - out_1895242722289169838[9] = 0; - out_1895242722289169838[10] = 0; - out_1895242722289169838[11] = 0; - out_1895242722289169838[12] = 0; - out_1895242722289169838[13] = 0; - out_1895242722289169838[14] = 1; - out_1895242722289169838[15] = 0; - out_1895242722289169838[16] = 0; - out_1895242722289169838[17] = 0; -} -void h_30(double *state, double *unused, double *out_8308242515252409096) { - out_8308242515252409096[0] = state[4]; -} -void H_30(double *state, double *unused, double *out_9208578368134646088) { - out_9208578368134646088[0] = 0; - out_9208578368134646088[1] = 0; - out_9208578368134646088[2] = 0; - out_9208578368134646088[3] = 0; - out_9208578368134646088[4] = 1; - out_9208578368134646088[5] = 0; - out_9208578368134646088[6] = 0; - out_9208578368134646088[7] = 0; - out_9208578368134646088[8] = 0; -} -void h_26(double *state, double *unused, double *out_2392611761518546760) { - out_2392611761518546760[0] = state[7]; -} -void H_26(double *state, double *unused, double *out_1449615292231026891) { - out_1449615292231026891[0] = 0; - out_1449615292231026891[1] = 0; - out_1449615292231026891[2] = 0; - out_1449615292231026891[3] = 0; - out_1449615292231026891[4] = 0; - out_1449615292231026891[5] = 0; - out_1449615292231026891[6] = 0; - out_1449615292231026891[7] = 1; - out_1449615292231026891[8] = 0; -} -void h_27(double *state, double *unused, double *out_5428643132159205556) { - out_5428643132159205556[0] = state[3]; -} -void H_27(double *state, double *unused, double *out_7033815056334221177) { - out_7033815056334221177[0] = 0; - out_7033815056334221177[1] = 0; - out_7033815056334221177[2] = 0; - out_7033815056334221177[3] = 1; - out_7033815056334221177[4] = 0; - out_7033815056334221177[5] = 0; - out_7033815056334221177[6] = 0; - out_7033815056334221177[7] = 0; - out_7033815056334221177[8] = 0; -} -void h_29(double *state, double *unused, double *out_5703837194443711445) { - out_5703837194443711445[0] = state[1]; -} -void H_29(double *state, double *unused, double *out_5320452329464670144) { - out_5320452329464670144[0] = 0; - out_5320452329464670144[1] = 1; - out_5320452329464670144[2] = 0; - out_5320452329464670144[3] = 0; - out_5320452329464670144[4] = 0; - out_5320452329464670144[5] = 0; - out_5320452329464670144[6] = 0; - out_5320452329464670144[7] = 0; - out_5320452329464670144[8] = 0; -} -void h_28(double *state, double *unused, double *out_6908571236733552516) { - out_6908571236733552516[0] = state[0]; -} -void H_28(double *state, double *unused, double *out_238053312395139570) { - out_238053312395139570[0] = 1; - out_238053312395139570[1] = 0; - out_238053312395139570[2] = 0; - out_238053312395139570[3] = 0; - out_238053312395139570[4] = 0; - out_238053312395139570[5] = 0; - out_238053312395139570[6] = 0; - out_238053312395139570[7] = 0; - out_238053312395139570[8] = 0; -} -void h_31(double *state, double *unused, double *out_3210479663755662439) { - out_3210479663755662439[0] = state[8]; -} -void H_31(double *state, double *unused, double *out_2322533988519989761) { - out_2322533988519989761[0] = 0; - out_2322533988519989761[1] = 0; - out_2322533988519989761[2] = 0; - out_2322533988519989761[3] = 0; - out_2322533988519989761[4] = 0; - out_2322533988519989761[5] = 0; - out_2322533988519989761[6] = 0; - out_2322533988519989761[7] = 0; - out_2322533988519989761[8] = 1; -} -#include -#include - -typedef Eigen::Matrix DDM; -typedef Eigen::Matrix EEM; -typedef Eigen::Matrix DEM; - -void predict(double *in_x, double *in_P, double *in_Q, double dt) { - typedef Eigen::Matrix RRM; - - double nx[DIM] = {0}; - double in_F[EDIM*EDIM] = {0}; - - // functions from sympy - f_fun(in_x, dt, nx); - F_fun(in_x, dt, in_F); - - - EEM F(in_F); - EEM P(in_P); - EEM Q(in_Q); - - RRM F_main = F.topLeftCorner(MEDIM, MEDIM); - P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose(); - P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM); - P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose(); - - P = P + dt*Q; - - // copy out state - memcpy(in_x, nx, DIM * sizeof(double)); - memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double)); -} - -// note: extra_args dim only correct when null space projecting -// otherwise 1 -template -void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) { - typedef Eigen::Matrix ZZM; - typedef Eigen::Matrix ZDM; - typedef Eigen::Matrix XEM; - //typedef Eigen::Matrix EZM; - typedef Eigen::Matrix X1M; - typedef Eigen::Matrix XXM; - - double in_hx[ZDIM] = {0}; - double in_H[ZDIM * DIM] = {0}; - double in_H_mod[EDIM * DIM] = {0}; - double delta_x[EDIM] = {0}; - double x_new[DIM] = {0}; - - - // state x, P - Eigen::Matrix z(in_z); - EEM P(in_P); - ZZM pre_R(in_R); - - // functions from sympy - h_fun(in_x, in_ea, in_hx); - H_fun(in_x, in_ea, in_H); - ZDM pre_H(in_H); - - // get y (y = z - hx) - Eigen::Matrix pre_y(in_hx); pre_y = z - pre_y; - X1M y; XXM H; XXM R; - if (Hea_fun){ - typedef Eigen::Matrix ZAM; - double in_Hea[ZDIM * EADIM] = {0}; - Hea_fun(in_x, in_ea, in_Hea); - ZAM Hea(in_Hea); - XXM A = Hea.transpose().fullPivLu().kernel(); - - - y = A.transpose() * pre_y; - H = A.transpose() * pre_H; - R = A.transpose() * pre_R * A; - } else { - y = pre_y; - H = pre_H; - R = pre_R; - } - // get modified H - H_mod_fun(in_x, in_H_mod); - DEM H_mod(in_H_mod); - XEM H_err = H * H_mod; - - // Do mahalobis distance test - if (MAHA_TEST){ - XXM a = (H_err * P * H_err.transpose() + R).inverse(); - double maha_dist = y.transpose() * a * y; - if (maha_dist > MAHA_THRESHOLD){ - R = 1.0e16 * R; - } - } - - // Outlier resilient weighting - double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum()); - - // kalman gains and I_KH - XXM S = ((H_err * P) * H_err.transpose()) + R/weight; - XEM KT = S.fullPivLu().solve(H_err * P.transpose()); - //EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE? - //EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose(); - //std::cout << "Here is the matrix rot:\n" << K << std::endl; - EEM I_KH = Eigen::Matrix::Identity() - (KT.transpose() * H_err); - - // update state by injecting dx - Eigen::Matrix dx(delta_x); - dx = (KT.transpose() * y); - memcpy(delta_x, dx.data(), EDIM * sizeof(double)); - err_fun(in_x, delta_x, x_new); - Eigen::Matrix x(x_new); - - // update cov - P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT); - - // copy out state - memcpy(in_x, x.data(), DIM * sizeof(double)); - memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double)); - memcpy(in_z, y.data(), y.rows() * sizeof(double)); -} - - - - -} -extern "C" { - -void car_update_25(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<1, 3, 0>(in_x, in_P, h_25, H_25, NULL, in_z, in_R, in_ea, MAHA_THRESH_25); -} -void car_update_24(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<2, 3, 0>(in_x, in_P, h_24, H_24, NULL, in_z, in_R, in_ea, MAHA_THRESH_24); -} -void car_update_30(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<1, 3, 0>(in_x, in_P, h_30, H_30, NULL, in_z, in_R, in_ea, MAHA_THRESH_30); -} -void car_update_26(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<1, 3, 0>(in_x, in_P, h_26, H_26, NULL, in_z, in_R, in_ea, MAHA_THRESH_26); -} -void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<1, 3, 0>(in_x, in_P, h_27, H_27, NULL, in_z, in_R, in_ea, MAHA_THRESH_27); -} -void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<1, 3, 0>(in_x, in_P, h_29, H_29, NULL, in_z, in_R, in_ea, MAHA_THRESH_29); -} -void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<1, 3, 0>(in_x, in_P, h_28, H_28, NULL, in_z, in_R, in_ea, MAHA_THRESH_28); -} -void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31); -} -void car_err_fun(double *nom_x, double *delta_x, double *out_7718808222430479447) { - err_fun(nom_x, delta_x, out_7718808222430479447); -} -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1034114423937537934) { - inv_err_fun(nom_x, true_x, out_1034114423937537934); -} -void car_H_mod_fun(double *state, double *out_7225960904852606817) { - H_mod_fun(state, out_7225960904852606817); -} -void car_f_fun(double *state, double dt, double *out_5925834804962007644) { - f_fun(state, dt, out_5925834804962007644); -} -void car_F_fun(double *state, double dt, double *out_1594563891292900416) { - F_fun(state, dt, out_1594563891292900416); -} -void car_h_25(double *state, double *unused, double *out_4446559038823907444) { - h_25(state, unused, out_4446559038823907444); -} -void car_H_25(double *state, double *unused, double *out_2291888026643029333) { - H_25(state, unused, out_2291888026643029333); -} -void car_h_24(double *state, double *unused, double *out_5489680431960231161) { - h_24(state, unused, out_5489680431960231161); -} -void car_H_24(double *state, double *unused, double *out_1895242722289169838) { - H_24(state, unused, out_1895242722289169838); -} -void car_h_30(double *state, double *unused, double *out_8308242515252409096) { - h_30(state, unused, out_8308242515252409096); -} -void car_H_30(double *state, double *unused, double *out_9208578368134646088) { - H_30(state, unused, out_9208578368134646088); -} -void car_h_26(double *state, double *unused, double *out_2392611761518546760) { - h_26(state, unused, out_2392611761518546760); -} -void car_H_26(double *state, double *unused, double *out_1449615292231026891) { - H_26(state, unused, out_1449615292231026891); -} -void car_h_27(double *state, double *unused, double *out_5428643132159205556) { - h_27(state, unused, out_5428643132159205556); -} -void car_H_27(double *state, double *unused, double *out_7033815056334221177) { - H_27(state, unused, out_7033815056334221177); -} -void car_h_29(double *state, double *unused, double *out_5703837194443711445) { - h_29(state, unused, out_5703837194443711445); -} -void car_H_29(double *state, double *unused, double *out_5320452329464670144) { - H_29(state, unused, out_5320452329464670144); -} -void car_h_28(double *state, double *unused, double *out_6908571236733552516) { - h_28(state, unused, out_6908571236733552516); -} -void car_H_28(double *state, double *unused, double *out_238053312395139570) { - H_28(state, unused, out_238053312395139570); -} -void car_h_31(double *state, double *unused, double *out_3210479663755662439) { - h_31(state, unused, out_3210479663755662439); -} -void car_H_31(double *state, double *unused, double *out_2322533988519989761) { - H_31(state, unused, out_2322533988519989761); -} -void car_predict(double *in_x, double *in_P, double *in_Q, double dt) { - predict(in_x, in_P, in_Q, dt); -} -void car_set_mass(double x) { - set_mass(x); -} -void car_set_rotational_inertia(double x) { - set_rotational_inertia(x); -} -void car_set_center_to_front(double x) { - set_center_to_front(x); -} -void car_set_center_to_rear(double x) { - set_center_to_rear(x); -} -void car_set_stiffness_front(double x) { - set_stiffness_front(x); -} -void car_set_stiffness_rear(double x) { - set_stiffness_rear(x); -} -} - -const EKF car = { - .name = "car", - .kinds = { 25, 24, 30, 26, 27, 29, 28, 31 }, - .feature_kinds = { }, - .f_fun = car_f_fun, - .F_fun = car_F_fun, - .err_fun = car_err_fun, - .inv_err_fun = car_inv_err_fun, - .H_mod_fun = car_H_mod_fun, - .predict = car_predict, - .hs = { - { 25, car_h_25 }, - { 24, car_h_24 }, - { 30, car_h_30 }, - { 26, car_h_26 }, - { 27, car_h_27 }, - { 29, car_h_29 }, - { 28, car_h_28 }, - { 31, car_h_31 }, - }, - .Hs = { - { 25, car_H_25 }, - { 24, car_H_24 }, - { 30, car_H_30 }, - { 26, car_H_26 }, - { 27, car_H_27 }, - { 29, car_H_29 }, - { 28, car_H_28 }, - { 31, car_H_31 }, - }, - .updates = { - { 25, car_update_25 }, - { 24, car_update_24 }, - { 30, car_update_30 }, - { 26, car_update_26 }, - { 27, car_update_27 }, - { 29, car_update_29 }, - { 28, car_update_28 }, - { 31, car_update_31 }, - }, - .Hes = { - }, - .sets = { - { "mass", car_set_mass }, - { "rotational_inertia", car_set_rotational_inertia }, - { "center_to_front", car_set_center_to_front }, - { "center_to_rear", car_set_center_to_rear }, - { "stiffness_front", car_set_stiffness_front }, - { "stiffness_rear", car_set_stiffness_rear }, - }, - .extra_routines = { - }, -}; - -ekf_lib_init(car) diff --git a/selfdrive/locationd/models/generated/libcar.so b/selfdrive/locationd/models/generated/libcar.so deleted file mode 100755 index 0f65107..0000000 Binary files a/selfdrive/locationd/models/generated/libcar.so and /dev/null differ diff --git a/selfdrive/locationd/models/generated/liblive.so b/selfdrive/locationd/models/generated/liblive.so deleted file mode 100755 index 1d02431..0000000 Binary files a/selfdrive/locationd/models/generated/liblive.so and /dev/null differ diff --git a/selfdrive/locationd/models/generated/live.cpp b/selfdrive/locationd/models/generated/live.cpp deleted file mode 100644 index 06ba1bb..0000000 --- a/selfdrive/locationd/models/generated/live.cpp +++ /dev/null @@ -1,1987 +0,0 @@ -#include "live.h" - -namespace { -#define DIM 22 -#define EDIM 21 -#define MEDIM 21 -typedef void (*Hfun)(double *, double *, double *); -const static double MAHA_THRESH_4 = 7.814727903251177; -const static double MAHA_THRESH_9 = 7.814727903251177; -const static double MAHA_THRESH_10 = 7.814727903251177; -const static double MAHA_THRESH_12 = 7.814727903251177; -const static double MAHA_THRESH_35 = 7.814727903251177; -const static double MAHA_THRESH_32 = 9.487729036781154; -const static double MAHA_THRESH_13 = 7.814727903251177; -const static double MAHA_THRESH_14 = 7.814727903251177; -const static double MAHA_THRESH_33 = 7.814727903251177; - -/****************************************************************************** - * Code generated with SymPy 1.12 * - * * - * See http://www.sympy.org/ for more information. * - * * - * This file is part of 'ekf' * - ******************************************************************************/ -void H(double *in_vec, double *out_3655759921532451903) { - out_3655759921532451903[0] = 0; - out_3655759921532451903[1] = -sin(in_vec[1])*sin(in_vec[2])*in_vec[4] - sin(in_vec[1])*cos(in_vec[2])*in_vec[3] - cos(in_vec[1])*in_vec[5]; - out_3655759921532451903[2] = -sin(in_vec[2])*cos(in_vec[1])*in_vec[3] + cos(in_vec[1])*cos(in_vec[2])*in_vec[4]; - out_3655759921532451903[3] = cos(in_vec[1])*cos(in_vec[2]); - out_3655759921532451903[4] = sin(in_vec[2])*cos(in_vec[1]); - out_3655759921532451903[5] = -sin(in_vec[1]); - out_3655759921532451903[6] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (-sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*in_vec[5]; - out_3655759921532451903[7] = -sin(in_vec[0])*sin(in_vec[1])*in_vec[5] + sin(in_vec[0])*sin(in_vec[2])*cos(in_vec[1])*in_vec[4] + sin(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3]; - out_3655759921532451903[8] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]))*in_vec[4]; - out_3655759921532451903[9] = sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]); - out_3655759921532451903[10] = sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) + cos(in_vec[0])*cos(in_vec[2]); - out_3655759921532451903[11] = sin(in_vec[0])*cos(in_vec[1]); - out_3655759921532451903[12] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (-sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) + sin(in_vec[2])*cos(in_vec[0]))*in_vec[3] - sin(in_vec[0])*cos(in_vec[1])*in_vec[5]; - out_3655759921532451903[13] = -sin(in_vec[1])*cos(in_vec[0])*in_vec[5] + sin(in_vec[2])*cos(in_vec[0])*cos(in_vec[1])*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3]; - out_3655759921532451903[14] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (sin(in_vec[0])*cos(in_vec[2]) - sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[3]; - out_3655759921532451903[15] = sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]); - out_3655759921532451903[16] = -sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]); - out_3655759921532451903[17] = cos(in_vec[0])*cos(in_vec[1]); -} -void err_fun(double *nom_x, double *delta_x, double *out_6606438496319011893) { - out_6606438496319011893[0] = delta_x[0] + nom_x[0]; - out_6606438496319011893[1] = delta_x[1] + nom_x[1]; - out_6606438496319011893[2] = delta_x[2] + nom_x[2]; - out_6606438496319011893[3] = -0.5*delta_x[3]*nom_x[4] - 0.5*delta_x[4]*nom_x[5] - 0.5*delta_x[5]*nom_x[6] + 1.0*nom_x[3]; - out_6606438496319011893[4] = 0.5*delta_x[3]*nom_x[3] + 0.5*delta_x[4]*nom_x[6] - 0.5*delta_x[5]*nom_x[5] + 1.0*nom_x[4]; - out_6606438496319011893[5] = -0.5*delta_x[3]*nom_x[6] + 0.5*delta_x[4]*nom_x[3] + 0.5*delta_x[5]*nom_x[4] + 1.0*nom_x[5]; - out_6606438496319011893[6] = 0.5*delta_x[3]*nom_x[5] - 0.5*delta_x[4]*nom_x[4] + 0.5*delta_x[5]*nom_x[3] + 1.0*nom_x[6]; - out_6606438496319011893[7] = delta_x[6] + nom_x[7]; - out_6606438496319011893[8] = delta_x[7] + nom_x[8]; - out_6606438496319011893[9] = delta_x[8] + nom_x[9]; - out_6606438496319011893[10] = delta_x[9] + nom_x[10]; - out_6606438496319011893[11] = delta_x[10] + nom_x[11]; - out_6606438496319011893[12] = delta_x[11] + nom_x[12]; - out_6606438496319011893[13] = delta_x[12] + nom_x[13]; - out_6606438496319011893[14] = delta_x[13] + nom_x[14]; - out_6606438496319011893[15] = delta_x[14] + nom_x[15]; - out_6606438496319011893[16] = delta_x[15] + nom_x[16]; - out_6606438496319011893[17] = delta_x[16] + nom_x[17]; - out_6606438496319011893[18] = delta_x[17] + nom_x[18]; - out_6606438496319011893[19] = delta_x[18] + nom_x[19]; - out_6606438496319011893[20] = delta_x[19] + nom_x[20]; - out_6606438496319011893[21] = delta_x[20] + nom_x[21]; -} -void inv_err_fun(double *nom_x, double *true_x, double *out_1892773414432101423) { - out_1892773414432101423[0] = -nom_x[0] + true_x[0]; - out_1892773414432101423[1] = -nom_x[1] + true_x[1]; - out_1892773414432101423[2] = -nom_x[2] + true_x[2]; - out_1892773414432101423[3] = 2*nom_x[3]*true_x[4] - 2*nom_x[4]*true_x[3] + 2*nom_x[5]*true_x[6] - 2*nom_x[6]*true_x[5]; - out_1892773414432101423[4] = 2*nom_x[3]*true_x[5] - 2*nom_x[4]*true_x[6] - 2*nom_x[5]*true_x[3] + 2*nom_x[6]*true_x[4]; - out_1892773414432101423[5] = 2*nom_x[3]*true_x[6] + 2*nom_x[4]*true_x[5] - 2*nom_x[5]*true_x[4] - 2*nom_x[6]*true_x[3]; - out_1892773414432101423[6] = -nom_x[7] + true_x[7]; - out_1892773414432101423[7] = -nom_x[8] + true_x[8]; - out_1892773414432101423[8] = -nom_x[9] + true_x[9]; - out_1892773414432101423[9] = -nom_x[10] + true_x[10]; - out_1892773414432101423[10] = -nom_x[11] + true_x[11]; - out_1892773414432101423[11] = -nom_x[12] + true_x[12]; - out_1892773414432101423[12] = -nom_x[13] + true_x[13]; - out_1892773414432101423[13] = -nom_x[14] + true_x[14]; - out_1892773414432101423[14] = -nom_x[15] + true_x[15]; - out_1892773414432101423[15] = -nom_x[16] + true_x[16]; - out_1892773414432101423[16] = -nom_x[17] + true_x[17]; - out_1892773414432101423[17] = -nom_x[18] + true_x[18]; - out_1892773414432101423[18] = -nom_x[19] + true_x[19]; - out_1892773414432101423[19] = -nom_x[20] + true_x[20]; - out_1892773414432101423[20] = -nom_x[21] + true_x[21]; -} -void H_mod_fun(double *state, double *out_31505903139368806) { - out_31505903139368806[0] = 1.0; - out_31505903139368806[1] = 0; - out_31505903139368806[2] = 0; - out_31505903139368806[3] = 0; - out_31505903139368806[4] = 0; - out_31505903139368806[5] = 0; - out_31505903139368806[6] = 0; - out_31505903139368806[7] = 0; - out_31505903139368806[8] = 0; - out_31505903139368806[9] = 0; - out_31505903139368806[10] = 0; - out_31505903139368806[11] = 0; - out_31505903139368806[12] = 0; - out_31505903139368806[13] = 0; - out_31505903139368806[14] = 0; - out_31505903139368806[15] = 0; - out_31505903139368806[16] = 0; - out_31505903139368806[17] = 0; - out_31505903139368806[18] = 0; - out_31505903139368806[19] = 0; - out_31505903139368806[20] = 0; - out_31505903139368806[21] = 0; - out_31505903139368806[22] = 1.0; - out_31505903139368806[23] = 0; - out_31505903139368806[24] = 0; - out_31505903139368806[25] = 0; - out_31505903139368806[26] = 0; - out_31505903139368806[27] = 0; - out_31505903139368806[28] = 0; - out_31505903139368806[29] = 0; - out_31505903139368806[30] = 0; - out_31505903139368806[31] = 0; - out_31505903139368806[32] = 0; - out_31505903139368806[33] = 0; - out_31505903139368806[34] = 0; - out_31505903139368806[35] = 0; - out_31505903139368806[36] = 0; - out_31505903139368806[37] = 0; - out_31505903139368806[38] = 0; - out_31505903139368806[39] = 0; - out_31505903139368806[40] = 0; - out_31505903139368806[41] = 0; - out_31505903139368806[42] = 0; - out_31505903139368806[43] = 0; - out_31505903139368806[44] = 1.0; - out_31505903139368806[45] = 0; - out_31505903139368806[46] = 0; - out_31505903139368806[47] = 0; - out_31505903139368806[48] = 0; - out_31505903139368806[49] = 0; - out_31505903139368806[50] = 0; - out_31505903139368806[51] = 0; - out_31505903139368806[52] = 0; - out_31505903139368806[53] = 0; - out_31505903139368806[54] = 0; - out_31505903139368806[55] = 0; - out_31505903139368806[56] = 0; - out_31505903139368806[57] = 0; - out_31505903139368806[58] = 0; - out_31505903139368806[59] = 0; - out_31505903139368806[60] = 0; - out_31505903139368806[61] = 0; - out_31505903139368806[62] = 0; - out_31505903139368806[63] = 0; - out_31505903139368806[64] = 0; - out_31505903139368806[65] = 0; - out_31505903139368806[66] = -0.5*state[4]; - out_31505903139368806[67] = -0.5*state[5]; - out_31505903139368806[68] = -0.5*state[6]; - out_31505903139368806[69] = 0; - out_31505903139368806[70] = 0; - out_31505903139368806[71] = 0; - out_31505903139368806[72] = 0; - out_31505903139368806[73] = 0; - out_31505903139368806[74] = 0; - out_31505903139368806[75] = 0; - out_31505903139368806[76] = 0; - out_31505903139368806[77] = 0; - out_31505903139368806[78] = 0; - out_31505903139368806[79] = 0; - out_31505903139368806[80] = 0; - out_31505903139368806[81] = 0; - out_31505903139368806[82] = 0; - out_31505903139368806[83] = 0; - out_31505903139368806[84] = 0; - out_31505903139368806[85] = 0; - out_31505903139368806[86] = 0; - out_31505903139368806[87] = 0.5*state[3]; - out_31505903139368806[88] = 0.5*state[6]; - out_31505903139368806[89] = -0.5*state[5]; - out_31505903139368806[90] = 0; - out_31505903139368806[91] = 0; - out_31505903139368806[92] = 0; - out_31505903139368806[93] = 0; - out_31505903139368806[94] = 0; - out_31505903139368806[95] = 0; - out_31505903139368806[96] = 0; - out_31505903139368806[97] = 0; - out_31505903139368806[98] = 0; - out_31505903139368806[99] = 0; - out_31505903139368806[100] = 0; - out_31505903139368806[101] = 0; - out_31505903139368806[102] = 0; - out_31505903139368806[103] = 0; - out_31505903139368806[104] = 0; - out_31505903139368806[105] = 0; - out_31505903139368806[106] = 0; - out_31505903139368806[107] = 0; - out_31505903139368806[108] = -0.5*state[6]; - out_31505903139368806[109] = 0.5*state[3]; - out_31505903139368806[110] = 0.5*state[4]; - out_31505903139368806[111] = 0; - out_31505903139368806[112] = 0; - out_31505903139368806[113] = 0; - out_31505903139368806[114] = 0; - out_31505903139368806[115] = 0; - out_31505903139368806[116] = 0; - out_31505903139368806[117] = 0; - out_31505903139368806[118] = 0; - out_31505903139368806[119] = 0; - out_31505903139368806[120] = 0; - out_31505903139368806[121] = 0; - out_31505903139368806[122] = 0; - out_31505903139368806[123] = 0; - out_31505903139368806[124] = 0; - out_31505903139368806[125] = 0; - out_31505903139368806[126] = 0; - out_31505903139368806[127] = 0; - out_31505903139368806[128] = 0; - out_31505903139368806[129] = 0.5*state[5]; - out_31505903139368806[130] = -0.5*state[4]; - out_31505903139368806[131] = 0.5*state[3]; - out_31505903139368806[132] = 0; - out_31505903139368806[133] = 0; - out_31505903139368806[134] = 0; - out_31505903139368806[135] = 0; - out_31505903139368806[136] = 0; - out_31505903139368806[137] = 0; - out_31505903139368806[138] = 0; - out_31505903139368806[139] = 0; - out_31505903139368806[140] = 0; - out_31505903139368806[141] = 0; - out_31505903139368806[142] = 0; - out_31505903139368806[143] = 0; - out_31505903139368806[144] = 0; - out_31505903139368806[145] = 0; - out_31505903139368806[146] = 0; - out_31505903139368806[147] = 0; - out_31505903139368806[148] = 0; - out_31505903139368806[149] = 0; - out_31505903139368806[150] = 0; - out_31505903139368806[151] = 0; - out_31505903139368806[152] = 0; - out_31505903139368806[153] = 1.0; - out_31505903139368806[154] = 0; - out_31505903139368806[155] = 0; - out_31505903139368806[156] = 0; - out_31505903139368806[157] = 0; - out_31505903139368806[158] = 0; - out_31505903139368806[159] = 0; - out_31505903139368806[160] = 0; - out_31505903139368806[161] = 0; - out_31505903139368806[162] = 0; - out_31505903139368806[163] = 0; - out_31505903139368806[164] = 0; - out_31505903139368806[165] = 0; - out_31505903139368806[166] = 0; - out_31505903139368806[167] = 0; - out_31505903139368806[168] = 0; - out_31505903139368806[169] = 0; - out_31505903139368806[170] = 0; - out_31505903139368806[171] = 0; - out_31505903139368806[172] = 0; - out_31505903139368806[173] = 0; - out_31505903139368806[174] = 0; - out_31505903139368806[175] = 1.0; - out_31505903139368806[176] = 0; - out_31505903139368806[177] = 0; - out_31505903139368806[178] = 0; - out_31505903139368806[179] = 0; - out_31505903139368806[180] = 0; - out_31505903139368806[181] = 0; - out_31505903139368806[182] = 0; - out_31505903139368806[183] = 0; - out_31505903139368806[184] = 0; - out_31505903139368806[185] = 0; - out_31505903139368806[186] = 0; - out_31505903139368806[187] = 0; - out_31505903139368806[188] = 0; - out_31505903139368806[189] = 0; - out_31505903139368806[190] = 0; - out_31505903139368806[191] = 0; - out_31505903139368806[192] = 0; - out_31505903139368806[193] = 0; - out_31505903139368806[194] = 0; - out_31505903139368806[195] = 0; - out_31505903139368806[196] = 0; - out_31505903139368806[197] = 1.0; - out_31505903139368806[198] = 0; - out_31505903139368806[199] = 0; - out_31505903139368806[200] = 0; - out_31505903139368806[201] = 0; - out_31505903139368806[202] = 0; - out_31505903139368806[203] = 0; - out_31505903139368806[204] = 0; - out_31505903139368806[205] = 0; - out_31505903139368806[206] = 0; - out_31505903139368806[207] = 0; - out_31505903139368806[208] = 0; - out_31505903139368806[209] = 0; - out_31505903139368806[210] = 0; - out_31505903139368806[211] = 0; - out_31505903139368806[212] = 0; - out_31505903139368806[213] = 0; - out_31505903139368806[214] = 0; - out_31505903139368806[215] = 0; - out_31505903139368806[216] = 0; - out_31505903139368806[217] = 0; - out_31505903139368806[218] = 0; - out_31505903139368806[219] = 1.0; - out_31505903139368806[220] = 0; - out_31505903139368806[221] = 0; - out_31505903139368806[222] = 0; - out_31505903139368806[223] = 0; - out_31505903139368806[224] = 0; - out_31505903139368806[225] = 0; - out_31505903139368806[226] = 0; - out_31505903139368806[227] = 0; - out_31505903139368806[228] = 0; - out_31505903139368806[229] = 0; - out_31505903139368806[230] = 0; - out_31505903139368806[231] = 0; - out_31505903139368806[232] = 0; - out_31505903139368806[233] = 0; - out_31505903139368806[234] = 0; - out_31505903139368806[235] = 0; - out_31505903139368806[236] = 0; - out_31505903139368806[237] = 0; - out_31505903139368806[238] = 0; - out_31505903139368806[239] = 0; - out_31505903139368806[240] = 0; - out_31505903139368806[241] = 1.0; - out_31505903139368806[242] = 0; - out_31505903139368806[243] = 0; - out_31505903139368806[244] = 0; - out_31505903139368806[245] = 0; - out_31505903139368806[246] = 0; - out_31505903139368806[247] = 0; - out_31505903139368806[248] = 0; - out_31505903139368806[249] = 0; - out_31505903139368806[250] = 0; - out_31505903139368806[251] = 0; - out_31505903139368806[252] = 0; - out_31505903139368806[253] = 0; - out_31505903139368806[254] = 0; - out_31505903139368806[255] = 0; - out_31505903139368806[256] = 0; - out_31505903139368806[257] = 0; - out_31505903139368806[258] = 0; - out_31505903139368806[259] = 0; - out_31505903139368806[260] = 0; - out_31505903139368806[261] = 0; - out_31505903139368806[262] = 0; - out_31505903139368806[263] = 1.0; - out_31505903139368806[264] = 0; - out_31505903139368806[265] = 0; - out_31505903139368806[266] = 0; - out_31505903139368806[267] = 0; - out_31505903139368806[268] = 0; - out_31505903139368806[269] = 0; - out_31505903139368806[270] = 0; - out_31505903139368806[271] = 0; - out_31505903139368806[272] = 0; - out_31505903139368806[273] = 0; - out_31505903139368806[274] = 0; - out_31505903139368806[275] = 0; - out_31505903139368806[276] = 0; - out_31505903139368806[277] = 0; - out_31505903139368806[278] = 0; - out_31505903139368806[279] = 0; - out_31505903139368806[280] = 0; - out_31505903139368806[281] = 0; - out_31505903139368806[282] = 0; - out_31505903139368806[283] = 0; - out_31505903139368806[284] = 0; - out_31505903139368806[285] = 1.0; - out_31505903139368806[286] = 0; - out_31505903139368806[287] = 0; - out_31505903139368806[288] = 0; - out_31505903139368806[289] = 0; - out_31505903139368806[290] = 0; - out_31505903139368806[291] = 0; - out_31505903139368806[292] = 0; - out_31505903139368806[293] = 0; - out_31505903139368806[294] = 0; - out_31505903139368806[295] = 0; - out_31505903139368806[296] = 0; - out_31505903139368806[297] = 0; - out_31505903139368806[298] = 0; - out_31505903139368806[299] = 0; - out_31505903139368806[300] = 0; - out_31505903139368806[301] = 0; - out_31505903139368806[302] = 0; - out_31505903139368806[303] = 0; - out_31505903139368806[304] = 0; - out_31505903139368806[305] = 0; - out_31505903139368806[306] = 0; - out_31505903139368806[307] = 1.0; - out_31505903139368806[308] = 0; - out_31505903139368806[309] = 0; - out_31505903139368806[310] = 0; - out_31505903139368806[311] = 0; - out_31505903139368806[312] = 0; - out_31505903139368806[313] = 0; - out_31505903139368806[314] = 0; - out_31505903139368806[315] = 0; - out_31505903139368806[316] = 0; - out_31505903139368806[317] = 0; - out_31505903139368806[318] = 0; - out_31505903139368806[319] = 0; - out_31505903139368806[320] = 0; - out_31505903139368806[321] = 0; - out_31505903139368806[322] = 0; - out_31505903139368806[323] = 0; - out_31505903139368806[324] = 0; - out_31505903139368806[325] = 0; - out_31505903139368806[326] = 0; - out_31505903139368806[327] = 0; - out_31505903139368806[328] = 0; - out_31505903139368806[329] = 1.0; - out_31505903139368806[330] = 0; - out_31505903139368806[331] = 0; - out_31505903139368806[332] = 0; - out_31505903139368806[333] = 0; - out_31505903139368806[334] = 0; - out_31505903139368806[335] = 0; - out_31505903139368806[336] = 0; - out_31505903139368806[337] = 0; - out_31505903139368806[338] = 0; - out_31505903139368806[339] = 0; - out_31505903139368806[340] = 0; - out_31505903139368806[341] = 0; - out_31505903139368806[342] = 0; - out_31505903139368806[343] = 0; - out_31505903139368806[344] = 0; - out_31505903139368806[345] = 0; - out_31505903139368806[346] = 0; - out_31505903139368806[347] = 0; - out_31505903139368806[348] = 0; - out_31505903139368806[349] = 0; - out_31505903139368806[350] = 0; - out_31505903139368806[351] = 1.0; - out_31505903139368806[352] = 0; - out_31505903139368806[353] = 0; - out_31505903139368806[354] = 0; - out_31505903139368806[355] = 0; - out_31505903139368806[356] = 0; - out_31505903139368806[357] = 0; - out_31505903139368806[358] = 0; - out_31505903139368806[359] = 0; - out_31505903139368806[360] = 0; - out_31505903139368806[361] = 0; - out_31505903139368806[362] = 0; - out_31505903139368806[363] = 0; - out_31505903139368806[364] = 0; - out_31505903139368806[365] = 0; - out_31505903139368806[366] = 0; - out_31505903139368806[367] = 0; - out_31505903139368806[368] = 0; - out_31505903139368806[369] = 0; - out_31505903139368806[370] = 0; - out_31505903139368806[371] = 0; - out_31505903139368806[372] = 0; - out_31505903139368806[373] = 1.0; - out_31505903139368806[374] = 0; - out_31505903139368806[375] = 0; - out_31505903139368806[376] = 0; - out_31505903139368806[377] = 0; - out_31505903139368806[378] = 0; - out_31505903139368806[379] = 0; - out_31505903139368806[380] = 0; - out_31505903139368806[381] = 0; - out_31505903139368806[382] = 0; - out_31505903139368806[383] = 0; - out_31505903139368806[384] = 0; - out_31505903139368806[385] = 0; - out_31505903139368806[386] = 0; - out_31505903139368806[387] = 0; - out_31505903139368806[388] = 0; - out_31505903139368806[389] = 0; - out_31505903139368806[390] = 0; - out_31505903139368806[391] = 0; - out_31505903139368806[392] = 0; - out_31505903139368806[393] = 0; - out_31505903139368806[394] = 0; - out_31505903139368806[395] = 1.0; - out_31505903139368806[396] = 0; - out_31505903139368806[397] = 0; - out_31505903139368806[398] = 0; - out_31505903139368806[399] = 0; - out_31505903139368806[400] = 0; - out_31505903139368806[401] = 0; - out_31505903139368806[402] = 0; - out_31505903139368806[403] = 0; - out_31505903139368806[404] = 0; - out_31505903139368806[405] = 0; - out_31505903139368806[406] = 0; - out_31505903139368806[407] = 0; - out_31505903139368806[408] = 0; - out_31505903139368806[409] = 0; - out_31505903139368806[410] = 0; - out_31505903139368806[411] = 0; - out_31505903139368806[412] = 0; - out_31505903139368806[413] = 0; - out_31505903139368806[414] = 0; - out_31505903139368806[415] = 0; - out_31505903139368806[416] = 0; - out_31505903139368806[417] = 1.0; - out_31505903139368806[418] = 0; - out_31505903139368806[419] = 0; - out_31505903139368806[420] = 0; - out_31505903139368806[421] = 0; - out_31505903139368806[422] = 0; - out_31505903139368806[423] = 0; - out_31505903139368806[424] = 0; - out_31505903139368806[425] = 0; - out_31505903139368806[426] = 0; - out_31505903139368806[427] = 0; - out_31505903139368806[428] = 0; - out_31505903139368806[429] = 0; - out_31505903139368806[430] = 0; - out_31505903139368806[431] = 0; - out_31505903139368806[432] = 0; - out_31505903139368806[433] = 0; - out_31505903139368806[434] = 0; - out_31505903139368806[435] = 0; - out_31505903139368806[436] = 0; - out_31505903139368806[437] = 0; - out_31505903139368806[438] = 0; - out_31505903139368806[439] = 1.0; - out_31505903139368806[440] = 0; - out_31505903139368806[441] = 0; - out_31505903139368806[442] = 0; - out_31505903139368806[443] = 0; - out_31505903139368806[444] = 0; - out_31505903139368806[445] = 0; - out_31505903139368806[446] = 0; - out_31505903139368806[447] = 0; - out_31505903139368806[448] = 0; - out_31505903139368806[449] = 0; - out_31505903139368806[450] = 0; - out_31505903139368806[451] = 0; - out_31505903139368806[452] = 0; - out_31505903139368806[453] = 0; - out_31505903139368806[454] = 0; - out_31505903139368806[455] = 0; - out_31505903139368806[456] = 0; - out_31505903139368806[457] = 0; - out_31505903139368806[458] = 0; - out_31505903139368806[459] = 0; - out_31505903139368806[460] = 0; - out_31505903139368806[461] = 1.0; -} -void f_fun(double *state, double dt, double *out_4984514158829912596) { - out_4984514158829912596[0] = dt*state[7] + state[0]; - out_4984514158829912596[1] = dt*state[8] + state[1]; - out_4984514158829912596[2] = dt*state[9] + state[2]; - out_4984514158829912596[3] = dt*(-0.5*state[4]*state[10] - 0.5*state[5]*state[11] - 0.5*state[6]*state[12]) + state[3]; - out_4984514158829912596[4] = dt*(0.5*state[3]*state[10] + 0.5*state[5]*state[12] - 0.5*state[6]*state[11]) + state[4]; - out_4984514158829912596[5] = dt*(0.5*state[3]*state[11] - 0.5*state[4]*state[12] + 0.5*state[6]*state[10]) + state[5]; - out_4984514158829912596[6] = dt*(0.5*state[3]*state[12] + 0.5*state[4]*state[11] - 0.5*state[5]*state[10]) + state[6]; - out_4984514158829912596[7] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]) + state[7]; - out_4984514158829912596[8] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]) + state[8]; - out_4984514158829912596[9] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]) + state[9]; - out_4984514158829912596[10] = state[10]; - out_4984514158829912596[11] = state[11]; - out_4984514158829912596[12] = state[12]; - out_4984514158829912596[13] = state[13]; - out_4984514158829912596[14] = state[14]; - out_4984514158829912596[15] = state[15]; - out_4984514158829912596[16] = state[16]; - out_4984514158829912596[17] = state[17]; - out_4984514158829912596[18] = state[18]; - out_4984514158829912596[19] = state[19]; - out_4984514158829912596[20] = state[20]; - out_4984514158829912596[21] = state[21]; -} -void F_fun(double *state, double dt, double *out_495900107318952904) { - out_495900107318952904[0] = 1; - out_495900107318952904[1] = 0; - out_495900107318952904[2] = 0; - out_495900107318952904[3] = 0; - out_495900107318952904[4] = 0; - out_495900107318952904[5] = 0; - out_495900107318952904[6] = dt; - out_495900107318952904[7] = 0; - out_495900107318952904[8] = 0; - out_495900107318952904[9] = 0; - out_495900107318952904[10] = 0; - out_495900107318952904[11] = 0; - out_495900107318952904[12] = 0; - out_495900107318952904[13] = 0; - out_495900107318952904[14] = 0; - out_495900107318952904[15] = 0; - out_495900107318952904[16] = 0; - out_495900107318952904[17] = 0; - out_495900107318952904[18] = 0; - out_495900107318952904[19] = 0; - out_495900107318952904[20] = 0; - out_495900107318952904[21] = 0; - out_495900107318952904[22] = 1; - out_495900107318952904[23] = 0; - out_495900107318952904[24] = 0; - out_495900107318952904[25] = 0; - out_495900107318952904[26] = 0; - out_495900107318952904[27] = 0; - out_495900107318952904[28] = dt; - out_495900107318952904[29] = 0; - out_495900107318952904[30] = 0; - out_495900107318952904[31] = 0; - out_495900107318952904[32] = 0; - out_495900107318952904[33] = 0; - out_495900107318952904[34] = 0; - out_495900107318952904[35] = 0; - out_495900107318952904[36] = 0; - out_495900107318952904[37] = 0; - out_495900107318952904[38] = 0; - out_495900107318952904[39] = 0; - out_495900107318952904[40] = 0; - out_495900107318952904[41] = 0; - out_495900107318952904[42] = 0; - out_495900107318952904[43] = 0; - out_495900107318952904[44] = 1; - out_495900107318952904[45] = 0; - out_495900107318952904[46] = 0; - out_495900107318952904[47] = 0; - out_495900107318952904[48] = 0; - out_495900107318952904[49] = 0; - out_495900107318952904[50] = dt; - out_495900107318952904[51] = 0; - out_495900107318952904[52] = 0; - out_495900107318952904[53] = 0; - out_495900107318952904[54] = 0; - out_495900107318952904[55] = 0; - out_495900107318952904[56] = 0; - out_495900107318952904[57] = 0; - out_495900107318952904[58] = 0; - out_495900107318952904[59] = 0; - out_495900107318952904[60] = 0; - out_495900107318952904[61] = 0; - out_495900107318952904[62] = 0; - out_495900107318952904[63] = 0; - out_495900107318952904[64] = 0; - out_495900107318952904[65] = 0; - out_495900107318952904[66] = 1; - out_495900107318952904[67] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[11] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]); - out_495900107318952904[68] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[12] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[10] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[11]); - out_495900107318952904[69] = 0; - out_495900107318952904[70] = 0; - out_495900107318952904[71] = 0; - out_495900107318952904[72] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); - out_495900107318952904[73] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]); - out_495900107318952904[74] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]); - out_495900107318952904[75] = 0; - out_495900107318952904[76] = 0; - out_495900107318952904[77] = 0; - out_495900107318952904[78] = 0; - out_495900107318952904[79] = 0; - out_495900107318952904[80] = 0; - out_495900107318952904[81] = 0; - out_495900107318952904[82] = 0; - out_495900107318952904[83] = 0; - out_495900107318952904[84] = 0; - out_495900107318952904[85] = 0; - out_495900107318952904[86] = 0; - out_495900107318952904[87] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[11] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]); - out_495900107318952904[88] = 1; - out_495900107318952904[89] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[12] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[11] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[10]); - out_495900107318952904[90] = 0; - out_495900107318952904[91] = 0; - out_495900107318952904[92] = 0; - out_495900107318952904[93] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]); - out_495900107318952904[94] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); - out_495900107318952904[95] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]); - out_495900107318952904[96] = 0; - out_495900107318952904[97] = 0; - out_495900107318952904[98] = 0; - out_495900107318952904[99] = 0; - out_495900107318952904[100] = 0; - out_495900107318952904[101] = 0; - out_495900107318952904[102] = 0; - out_495900107318952904[103] = 0; - out_495900107318952904[104] = 0; - out_495900107318952904[105] = 0; - out_495900107318952904[106] = 0; - out_495900107318952904[107] = 0; - out_495900107318952904[108] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[12] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[10] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[11]); - out_495900107318952904[109] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[12] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[11] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[10]); - out_495900107318952904[110] = 1; - out_495900107318952904[111] = 0; - out_495900107318952904[112] = 0; - out_495900107318952904[113] = 0; - out_495900107318952904[114] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]); - out_495900107318952904[115] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]); - out_495900107318952904[116] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); - out_495900107318952904[117] = 0; - out_495900107318952904[118] = 0; - out_495900107318952904[119] = 0; - out_495900107318952904[120] = 0; - out_495900107318952904[121] = 0; - out_495900107318952904[122] = 0; - out_495900107318952904[123] = 0; - out_495900107318952904[124] = 0; - out_495900107318952904[125] = 0; - out_495900107318952904[126] = 0; - out_495900107318952904[127] = 0; - out_495900107318952904[128] = 0; - out_495900107318952904[129] = 0; - out_495900107318952904[130] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]); - out_495900107318952904[131] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[18] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[16] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[17]); - out_495900107318952904[132] = 1; - out_495900107318952904[133] = 0; - out_495900107318952904[134] = 0; - out_495900107318952904[135] = 0; - out_495900107318952904[136] = 0; - out_495900107318952904[137] = 0; - out_495900107318952904[138] = 0; - out_495900107318952904[139] = 0; - out_495900107318952904[140] = 0; - out_495900107318952904[141] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); - out_495900107318952904[142] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]); - out_495900107318952904[143] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]); - out_495900107318952904[144] = 0; - out_495900107318952904[145] = 0; - out_495900107318952904[146] = 0; - out_495900107318952904[147] = 0; - out_495900107318952904[148] = 0; - out_495900107318952904[149] = 0; - out_495900107318952904[150] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[17] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]); - out_495900107318952904[151] = 0; - out_495900107318952904[152] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]); - out_495900107318952904[153] = 0; - out_495900107318952904[154] = 1; - out_495900107318952904[155] = 0; - out_495900107318952904[156] = 0; - out_495900107318952904[157] = 0; - out_495900107318952904[158] = 0; - out_495900107318952904[159] = 0; - out_495900107318952904[160] = 0; - out_495900107318952904[161] = 0; - out_495900107318952904[162] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]); - out_495900107318952904[163] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); - out_495900107318952904[164] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]); - out_495900107318952904[165] = 0; - out_495900107318952904[166] = 0; - out_495900107318952904[167] = 0; - out_495900107318952904[168] = 0; - out_495900107318952904[169] = 0; - out_495900107318952904[170] = 0; - out_495900107318952904[171] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]); - out_495900107318952904[172] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[18] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[17] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[16]); - out_495900107318952904[173] = 0; - out_495900107318952904[174] = 0; - out_495900107318952904[175] = 0; - out_495900107318952904[176] = 1; - out_495900107318952904[177] = 0; - out_495900107318952904[178] = 0; - out_495900107318952904[179] = 0; - out_495900107318952904[180] = 0; - out_495900107318952904[181] = 0; - out_495900107318952904[182] = 0; - out_495900107318952904[183] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]); - out_495900107318952904[184] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]); - out_495900107318952904[185] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); - out_495900107318952904[186] = 0; - out_495900107318952904[187] = 0; - out_495900107318952904[188] = 0; - out_495900107318952904[189] = 0; - out_495900107318952904[190] = 0; - out_495900107318952904[191] = 0; - out_495900107318952904[192] = 0; - out_495900107318952904[193] = 0; - out_495900107318952904[194] = 0; - out_495900107318952904[195] = 0; - out_495900107318952904[196] = 0; - out_495900107318952904[197] = 0; - out_495900107318952904[198] = 1; - out_495900107318952904[199] = 0; - out_495900107318952904[200] = 0; - out_495900107318952904[201] = 0; - out_495900107318952904[202] = 0; - out_495900107318952904[203] = 0; - out_495900107318952904[204] = 0; - out_495900107318952904[205] = 0; - out_495900107318952904[206] = 0; - out_495900107318952904[207] = 0; - out_495900107318952904[208] = 0; - out_495900107318952904[209] = 0; - out_495900107318952904[210] = 0; - out_495900107318952904[211] = 0; - out_495900107318952904[212] = 0; - out_495900107318952904[213] = 0; - out_495900107318952904[214] = 0; - out_495900107318952904[215] = 0; - out_495900107318952904[216] = 0; - out_495900107318952904[217] = 0; - out_495900107318952904[218] = 0; - out_495900107318952904[219] = 0; - out_495900107318952904[220] = 1; - out_495900107318952904[221] = 0; - out_495900107318952904[222] = 0; - out_495900107318952904[223] = 0; - out_495900107318952904[224] = 0; - out_495900107318952904[225] = 0; - out_495900107318952904[226] = 0; - out_495900107318952904[227] = 0; - out_495900107318952904[228] = 0; - out_495900107318952904[229] = 0; - out_495900107318952904[230] = 0; - out_495900107318952904[231] = 0; - out_495900107318952904[232] = 0; - out_495900107318952904[233] = 0; - out_495900107318952904[234] = 0; - out_495900107318952904[235] = 0; - out_495900107318952904[236] = 0; - out_495900107318952904[237] = 0; - out_495900107318952904[238] = 0; - out_495900107318952904[239] = 0; - out_495900107318952904[240] = 0; - out_495900107318952904[241] = 0; - out_495900107318952904[242] = 1; - out_495900107318952904[243] = 0; - out_495900107318952904[244] = 0; - out_495900107318952904[245] = 0; - out_495900107318952904[246] = 0; - out_495900107318952904[247] = 0; - out_495900107318952904[248] = 0; - out_495900107318952904[249] = 0; - out_495900107318952904[250] = 0; - out_495900107318952904[251] = 0; - out_495900107318952904[252] = 0; - out_495900107318952904[253] = 0; - out_495900107318952904[254] = 0; - out_495900107318952904[255] = 0; - out_495900107318952904[256] = 0; - out_495900107318952904[257] = 0; - out_495900107318952904[258] = 0; - out_495900107318952904[259] = 0; - out_495900107318952904[260] = 0; - out_495900107318952904[261] = 0; - out_495900107318952904[262] = 0; - out_495900107318952904[263] = 0; - out_495900107318952904[264] = 1; - out_495900107318952904[265] = 0; - out_495900107318952904[266] = 0; - out_495900107318952904[267] = 0; - out_495900107318952904[268] = 0; - out_495900107318952904[269] = 0; - out_495900107318952904[270] = 0; - out_495900107318952904[271] = 0; - out_495900107318952904[272] = 0; - out_495900107318952904[273] = 0; - out_495900107318952904[274] = 0; - out_495900107318952904[275] = 0; - out_495900107318952904[276] = 0; - out_495900107318952904[277] = 0; - out_495900107318952904[278] = 0; - out_495900107318952904[279] = 0; - out_495900107318952904[280] = 0; - out_495900107318952904[281] = 0; - out_495900107318952904[282] = 0; - out_495900107318952904[283] = 0; - out_495900107318952904[284] = 0; - out_495900107318952904[285] = 0; - out_495900107318952904[286] = 1; - out_495900107318952904[287] = 0; - out_495900107318952904[288] = 0; - out_495900107318952904[289] = 0; - out_495900107318952904[290] = 0; - out_495900107318952904[291] = 0; - out_495900107318952904[292] = 0; - out_495900107318952904[293] = 0; - out_495900107318952904[294] = 0; - out_495900107318952904[295] = 0; - out_495900107318952904[296] = 0; - out_495900107318952904[297] = 0; - out_495900107318952904[298] = 0; - out_495900107318952904[299] = 0; - out_495900107318952904[300] = 0; - out_495900107318952904[301] = 0; - out_495900107318952904[302] = 0; - out_495900107318952904[303] = 0; - out_495900107318952904[304] = 0; - out_495900107318952904[305] = 0; - out_495900107318952904[306] = 0; - out_495900107318952904[307] = 0; - out_495900107318952904[308] = 1; - out_495900107318952904[309] = 0; - out_495900107318952904[310] = 0; - out_495900107318952904[311] = 0; - out_495900107318952904[312] = 0; - out_495900107318952904[313] = 0; - out_495900107318952904[314] = 0; - out_495900107318952904[315] = 0; - out_495900107318952904[316] = 0; - out_495900107318952904[317] = 0; - out_495900107318952904[318] = 0; - out_495900107318952904[319] = 0; - out_495900107318952904[320] = 0; - out_495900107318952904[321] = 0; - out_495900107318952904[322] = 0; - out_495900107318952904[323] = 0; - out_495900107318952904[324] = 0; - out_495900107318952904[325] = 0; - out_495900107318952904[326] = 0; - out_495900107318952904[327] = 0; - out_495900107318952904[328] = 0; - out_495900107318952904[329] = 0; - out_495900107318952904[330] = 1; - out_495900107318952904[331] = 0; - out_495900107318952904[332] = 0; - out_495900107318952904[333] = 0; - out_495900107318952904[334] = 0; - out_495900107318952904[335] = 0; - out_495900107318952904[336] = 0; - out_495900107318952904[337] = 0; - out_495900107318952904[338] = 0; - out_495900107318952904[339] = 0; - out_495900107318952904[340] = 0; - out_495900107318952904[341] = 0; - out_495900107318952904[342] = 0; - out_495900107318952904[343] = 0; - out_495900107318952904[344] = 0; - out_495900107318952904[345] = 0; - out_495900107318952904[346] = 0; - out_495900107318952904[347] = 0; - out_495900107318952904[348] = 0; - out_495900107318952904[349] = 0; - out_495900107318952904[350] = 0; - out_495900107318952904[351] = 0; - out_495900107318952904[352] = 1; - out_495900107318952904[353] = 0; - out_495900107318952904[354] = 0; - out_495900107318952904[355] = 0; - out_495900107318952904[356] = 0; - out_495900107318952904[357] = 0; - out_495900107318952904[358] = 0; - out_495900107318952904[359] = 0; - out_495900107318952904[360] = 0; - out_495900107318952904[361] = 0; - out_495900107318952904[362] = 0; - out_495900107318952904[363] = 0; - out_495900107318952904[364] = 0; - out_495900107318952904[365] = 0; - out_495900107318952904[366] = 0; - out_495900107318952904[367] = 0; - out_495900107318952904[368] = 0; - out_495900107318952904[369] = 0; - out_495900107318952904[370] = 0; - out_495900107318952904[371] = 0; - out_495900107318952904[372] = 0; - out_495900107318952904[373] = 0; - out_495900107318952904[374] = 1; - out_495900107318952904[375] = 0; - out_495900107318952904[376] = 0; - out_495900107318952904[377] = 0; - out_495900107318952904[378] = 0; - out_495900107318952904[379] = 0; - out_495900107318952904[380] = 0; - out_495900107318952904[381] = 0; - out_495900107318952904[382] = 0; - out_495900107318952904[383] = 0; - out_495900107318952904[384] = 0; - out_495900107318952904[385] = 0; - out_495900107318952904[386] = 0; - out_495900107318952904[387] = 0; - out_495900107318952904[388] = 0; - out_495900107318952904[389] = 0; - out_495900107318952904[390] = 0; - out_495900107318952904[391] = 0; - out_495900107318952904[392] = 0; - out_495900107318952904[393] = 0; - out_495900107318952904[394] = 0; - out_495900107318952904[395] = 0; - out_495900107318952904[396] = 1; - out_495900107318952904[397] = 0; - out_495900107318952904[398] = 0; - out_495900107318952904[399] = 0; - out_495900107318952904[400] = 0; - out_495900107318952904[401] = 0; - out_495900107318952904[402] = 0; - out_495900107318952904[403] = 0; - out_495900107318952904[404] = 0; - out_495900107318952904[405] = 0; - out_495900107318952904[406] = 0; - out_495900107318952904[407] = 0; - out_495900107318952904[408] = 0; - out_495900107318952904[409] = 0; - out_495900107318952904[410] = 0; - out_495900107318952904[411] = 0; - out_495900107318952904[412] = 0; - out_495900107318952904[413] = 0; - out_495900107318952904[414] = 0; - out_495900107318952904[415] = 0; - out_495900107318952904[416] = 0; - out_495900107318952904[417] = 0; - out_495900107318952904[418] = 1; - out_495900107318952904[419] = 0; - out_495900107318952904[420] = 0; - out_495900107318952904[421] = 0; - out_495900107318952904[422] = 0; - out_495900107318952904[423] = 0; - out_495900107318952904[424] = 0; - out_495900107318952904[425] = 0; - out_495900107318952904[426] = 0; - out_495900107318952904[427] = 0; - out_495900107318952904[428] = 0; - out_495900107318952904[429] = 0; - out_495900107318952904[430] = 0; - out_495900107318952904[431] = 0; - out_495900107318952904[432] = 0; - out_495900107318952904[433] = 0; - out_495900107318952904[434] = 0; - out_495900107318952904[435] = 0; - out_495900107318952904[436] = 0; - out_495900107318952904[437] = 0; - out_495900107318952904[438] = 0; - out_495900107318952904[439] = 0; - out_495900107318952904[440] = 1; -} -void h_4(double *state, double *unused, double *out_4218409302925889705) { - out_4218409302925889705[0] = state[10] + state[13]; - out_4218409302925889705[1] = state[11] + state[14]; - out_4218409302925889705[2] = state[12] + state[15]; -} -void H_4(double *state, double *unused, double *out_2064362458237902773) { - out_2064362458237902773[0] = 0; - out_2064362458237902773[1] = 0; - out_2064362458237902773[2] = 0; - out_2064362458237902773[3] = 0; - out_2064362458237902773[4] = 0; - out_2064362458237902773[5] = 0; - out_2064362458237902773[6] = 0; - out_2064362458237902773[7] = 0; - out_2064362458237902773[8] = 0; - out_2064362458237902773[9] = 0; - out_2064362458237902773[10] = 1; - out_2064362458237902773[11] = 0; - out_2064362458237902773[12] = 0; - out_2064362458237902773[13] = 1; - out_2064362458237902773[14] = 0; - out_2064362458237902773[15] = 0; - out_2064362458237902773[16] = 0; - out_2064362458237902773[17] = 0; - out_2064362458237902773[18] = 0; - out_2064362458237902773[19] = 0; - out_2064362458237902773[20] = 0; - out_2064362458237902773[21] = 0; - out_2064362458237902773[22] = 0; - out_2064362458237902773[23] = 0; - out_2064362458237902773[24] = 0; - out_2064362458237902773[25] = 0; - out_2064362458237902773[26] = 0; - out_2064362458237902773[27] = 0; - out_2064362458237902773[28] = 0; - out_2064362458237902773[29] = 0; - out_2064362458237902773[30] = 0; - out_2064362458237902773[31] = 0; - out_2064362458237902773[32] = 0; - out_2064362458237902773[33] = 1; - out_2064362458237902773[34] = 0; - out_2064362458237902773[35] = 0; - out_2064362458237902773[36] = 1; - out_2064362458237902773[37] = 0; - out_2064362458237902773[38] = 0; - out_2064362458237902773[39] = 0; - out_2064362458237902773[40] = 0; - out_2064362458237902773[41] = 0; - out_2064362458237902773[42] = 0; - out_2064362458237902773[43] = 0; - out_2064362458237902773[44] = 0; - out_2064362458237902773[45] = 0; - out_2064362458237902773[46] = 0; - out_2064362458237902773[47] = 0; - out_2064362458237902773[48] = 0; - out_2064362458237902773[49] = 0; - out_2064362458237902773[50] = 0; - out_2064362458237902773[51] = 0; - out_2064362458237902773[52] = 0; - out_2064362458237902773[53] = 0; - out_2064362458237902773[54] = 0; - out_2064362458237902773[55] = 0; - out_2064362458237902773[56] = 1; - out_2064362458237902773[57] = 0; - out_2064362458237902773[58] = 0; - out_2064362458237902773[59] = 1; - out_2064362458237902773[60] = 0; - out_2064362458237902773[61] = 0; - out_2064362458237902773[62] = 0; - out_2064362458237902773[63] = 0; - out_2064362458237902773[64] = 0; - out_2064362458237902773[65] = 0; -} -void h_9(double *state, double *unused, double *out_4201404494137813277) { - out_4201404494137813277[0] = state[10]; - out_4201404494137813277[1] = state[11]; - out_4201404494137813277[2] = state[12]; -} -void H_9(double *state, double *unused, double *out_1823172811608312128) { - out_1823172811608312128[0] = 0; - out_1823172811608312128[1] = 0; - out_1823172811608312128[2] = 0; - out_1823172811608312128[3] = 0; - out_1823172811608312128[4] = 0; - out_1823172811608312128[5] = 0; - out_1823172811608312128[6] = 0; - out_1823172811608312128[7] = 0; - out_1823172811608312128[8] = 0; - out_1823172811608312128[9] = 0; - out_1823172811608312128[10] = 1; - out_1823172811608312128[11] = 0; - out_1823172811608312128[12] = 0; - out_1823172811608312128[13] = 0; - out_1823172811608312128[14] = 0; - out_1823172811608312128[15] = 0; - out_1823172811608312128[16] = 0; - out_1823172811608312128[17] = 0; - out_1823172811608312128[18] = 0; - out_1823172811608312128[19] = 0; - out_1823172811608312128[20] = 0; - out_1823172811608312128[21] = 0; - out_1823172811608312128[22] = 0; - out_1823172811608312128[23] = 0; - out_1823172811608312128[24] = 0; - out_1823172811608312128[25] = 0; - out_1823172811608312128[26] = 0; - out_1823172811608312128[27] = 0; - out_1823172811608312128[28] = 0; - out_1823172811608312128[29] = 0; - out_1823172811608312128[30] = 0; - out_1823172811608312128[31] = 0; - out_1823172811608312128[32] = 0; - out_1823172811608312128[33] = 1; - out_1823172811608312128[34] = 0; - out_1823172811608312128[35] = 0; - out_1823172811608312128[36] = 0; - out_1823172811608312128[37] = 0; - out_1823172811608312128[38] = 0; - out_1823172811608312128[39] = 0; - out_1823172811608312128[40] = 0; - out_1823172811608312128[41] = 0; - out_1823172811608312128[42] = 0; - out_1823172811608312128[43] = 0; - out_1823172811608312128[44] = 0; - out_1823172811608312128[45] = 0; - out_1823172811608312128[46] = 0; - out_1823172811608312128[47] = 0; - out_1823172811608312128[48] = 0; - out_1823172811608312128[49] = 0; - out_1823172811608312128[50] = 0; - out_1823172811608312128[51] = 0; - out_1823172811608312128[52] = 0; - out_1823172811608312128[53] = 0; - out_1823172811608312128[54] = 0; - out_1823172811608312128[55] = 0; - out_1823172811608312128[56] = 1; - out_1823172811608312128[57] = 0; - out_1823172811608312128[58] = 0; - out_1823172811608312128[59] = 0; - out_1823172811608312128[60] = 0; - out_1823172811608312128[61] = 0; - out_1823172811608312128[62] = 0; - out_1823172811608312128[63] = 0; - out_1823172811608312128[64] = 0; - out_1823172811608312128[65] = 0; -} -void h_10(double *state, double *unused, double *out_4904878276509257913) { - out_4904878276509257913[0] = 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0] + state[16] + state[19]; - out_4904878276509257913[1] = 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1] + state[17] + state[20]; - out_4904878276509257913[2] = 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[2] + state[18] + state[21]; -} -void H_10(double *state, double *unused, double *out_7361770315712994123) { - out_7361770315712994123[0] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*pow(state[0], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); - out_7361770315712994123[1] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1]; - out_7361770315712994123[2] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[2]; - out_7361770315712994123[3] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; - out_7361770315712994123[4] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; - out_7361770315712994123[5] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; - out_7361770315712994123[6] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; - out_7361770315712994123[7] = 0; - out_7361770315712994123[8] = 0; - out_7361770315712994123[9] = 0; - out_7361770315712994123[10] = 0; - out_7361770315712994123[11] = 0; - out_7361770315712994123[12] = 0; - out_7361770315712994123[13] = 0; - out_7361770315712994123[14] = 0; - out_7361770315712994123[15] = 0; - out_7361770315712994123[16] = 1; - out_7361770315712994123[17] = 0; - out_7361770315712994123[18] = 0; - out_7361770315712994123[19] = 1; - out_7361770315712994123[20] = 0; - out_7361770315712994123[21] = 0; - out_7361770315712994123[22] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1]; - out_7361770315712994123[23] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*pow(state[1], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); - out_7361770315712994123[24] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1]*state[2]; - out_7361770315712994123[25] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; - out_7361770315712994123[26] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; - out_7361770315712994123[27] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; - out_7361770315712994123[28] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; - out_7361770315712994123[29] = 0; - out_7361770315712994123[30] = 0; - out_7361770315712994123[31] = 0; - out_7361770315712994123[32] = 0; - out_7361770315712994123[33] = 0; - out_7361770315712994123[34] = 0; - out_7361770315712994123[35] = 0; - out_7361770315712994123[36] = 0; - out_7361770315712994123[37] = 0; - out_7361770315712994123[38] = 0; - out_7361770315712994123[39] = 1; - out_7361770315712994123[40] = 0; - out_7361770315712994123[41] = 0; - out_7361770315712994123[42] = 1; - out_7361770315712994123[43] = 0; - out_7361770315712994123[44] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[0]*state[2]; - out_7361770315712994123[45] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[1]*state[2]; - out_7361770315712994123[46] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*pow(state[2], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); - out_7361770315712994123[47] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; - out_7361770315712994123[48] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; - out_7361770315712994123[49] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; - out_7361770315712994123[50] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; - out_7361770315712994123[51] = 0; - out_7361770315712994123[52] = 0; - out_7361770315712994123[53] = 0; - out_7361770315712994123[54] = 0; - out_7361770315712994123[55] = 0; - out_7361770315712994123[56] = 0; - out_7361770315712994123[57] = 0; - out_7361770315712994123[58] = 0; - out_7361770315712994123[59] = 0; - out_7361770315712994123[60] = 0; - out_7361770315712994123[61] = 0; - out_7361770315712994123[62] = 1; - out_7361770315712994123[63] = 0; - out_7361770315712994123[64] = 0; - out_7361770315712994123[65] = 1; -} -void h_12(double *state, double *unused, double *out_3237267128515768115) { - out_3237267128515768115[0] = state[0]; - out_3237267128515768115[1] = state[1]; - out_3237267128515768115[2] = state[2]; -} -void H_12(double *state, double *unused, double *out_2955093949794059022) { - out_2955093949794059022[0] = 1; - out_2955093949794059022[1] = 0; - out_2955093949794059022[2] = 0; - out_2955093949794059022[3] = 0; - out_2955093949794059022[4] = 0; - out_2955093949794059022[5] = 0; - out_2955093949794059022[6] = 0; - out_2955093949794059022[7] = 0; - out_2955093949794059022[8] = 0; - out_2955093949794059022[9] = 0; - out_2955093949794059022[10] = 0; - out_2955093949794059022[11] = 0; - out_2955093949794059022[12] = 0; - out_2955093949794059022[13] = 0; - out_2955093949794059022[14] = 0; - out_2955093949794059022[15] = 0; - out_2955093949794059022[16] = 0; - out_2955093949794059022[17] = 0; - out_2955093949794059022[18] = 0; - out_2955093949794059022[19] = 0; - out_2955093949794059022[20] = 0; - out_2955093949794059022[21] = 0; - out_2955093949794059022[22] = 0; - out_2955093949794059022[23] = 1; - out_2955093949794059022[24] = 0; - out_2955093949794059022[25] = 0; - out_2955093949794059022[26] = 0; - out_2955093949794059022[27] = 0; - out_2955093949794059022[28] = 0; - out_2955093949794059022[29] = 0; - out_2955093949794059022[30] = 0; - out_2955093949794059022[31] = 0; - out_2955093949794059022[32] = 0; - out_2955093949794059022[33] = 0; - out_2955093949794059022[34] = 0; - out_2955093949794059022[35] = 0; - out_2955093949794059022[36] = 0; - out_2955093949794059022[37] = 0; - out_2955093949794059022[38] = 0; - out_2955093949794059022[39] = 0; - out_2955093949794059022[40] = 0; - out_2955093949794059022[41] = 0; - out_2955093949794059022[42] = 0; - out_2955093949794059022[43] = 0; - out_2955093949794059022[44] = 0; - out_2955093949794059022[45] = 0; - out_2955093949794059022[46] = 1; - out_2955093949794059022[47] = 0; - out_2955093949794059022[48] = 0; - out_2955093949794059022[49] = 0; - out_2955093949794059022[50] = 0; - out_2955093949794059022[51] = 0; - out_2955093949794059022[52] = 0; - out_2955093949794059022[53] = 0; - out_2955093949794059022[54] = 0; - out_2955093949794059022[55] = 0; - out_2955093949794059022[56] = 0; - out_2955093949794059022[57] = 0; - out_2955093949794059022[58] = 0; - out_2955093949794059022[59] = 0; - out_2955093949794059022[60] = 0; - out_2955093949794059022[61] = 0; - out_2955093949794059022[62] = 0; - out_2955093949794059022[63] = 0; - out_2955093949794059022[64] = 0; - out_2955093949794059022[65] = 0; -} -void h_35(double *state, double *unused, double *out_9093746190498438204) { - out_9093746190498438204[0] = state[7]; - out_9093746190498438204[1] = state[8]; - out_9093746190498438204[2] = state[9]; -} -void H_35(double *state, double *unused, double *out_1302299599134704603) { - out_1302299599134704603[0] = 0; - out_1302299599134704603[1] = 0; - out_1302299599134704603[2] = 0; - out_1302299599134704603[3] = 0; - out_1302299599134704603[4] = 0; - out_1302299599134704603[5] = 0; - out_1302299599134704603[6] = 0; - out_1302299599134704603[7] = 1; - out_1302299599134704603[8] = 0; - out_1302299599134704603[9] = 0; - out_1302299599134704603[10] = 0; - out_1302299599134704603[11] = 0; - out_1302299599134704603[12] = 0; - out_1302299599134704603[13] = 0; - out_1302299599134704603[14] = 0; - out_1302299599134704603[15] = 0; - out_1302299599134704603[16] = 0; - out_1302299599134704603[17] = 0; - out_1302299599134704603[18] = 0; - out_1302299599134704603[19] = 0; - out_1302299599134704603[20] = 0; - out_1302299599134704603[21] = 0; - out_1302299599134704603[22] = 0; - out_1302299599134704603[23] = 0; - out_1302299599134704603[24] = 0; - out_1302299599134704603[25] = 0; - out_1302299599134704603[26] = 0; - out_1302299599134704603[27] = 0; - out_1302299599134704603[28] = 0; - out_1302299599134704603[29] = 0; - out_1302299599134704603[30] = 1; - out_1302299599134704603[31] = 0; - out_1302299599134704603[32] = 0; - out_1302299599134704603[33] = 0; - out_1302299599134704603[34] = 0; - out_1302299599134704603[35] = 0; - out_1302299599134704603[36] = 0; - out_1302299599134704603[37] = 0; - out_1302299599134704603[38] = 0; - out_1302299599134704603[39] = 0; - out_1302299599134704603[40] = 0; - out_1302299599134704603[41] = 0; - out_1302299599134704603[42] = 0; - out_1302299599134704603[43] = 0; - out_1302299599134704603[44] = 0; - out_1302299599134704603[45] = 0; - out_1302299599134704603[46] = 0; - out_1302299599134704603[47] = 0; - out_1302299599134704603[48] = 0; - out_1302299599134704603[49] = 0; - out_1302299599134704603[50] = 0; - out_1302299599134704603[51] = 0; - out_1302299599134704603[52] = 0; - out_1302299599134704603[53] = 1; - out_1302299599134704603[54] = 0; - out_1302299599134704603[55] = 0; - out_1302299599134704603[56] = 0; - out_1302299599134704603[57] = 0; - out_1302299599134704603[58] = 0; - out_1302299599134704603[59] = 0; - out_1302299599134704603[60] = 0; - out_1302299599134704603[61] = 0; - out_1302299599134704603[62] = 0; - out_1302299599134704603[63] = 0; - out_1302299599134704603[64] = 0; - out_1302299599134704603[65] = 0; -} -void h_32(double *state, double *unused, double *out_5484223878963608382) { - out_5484223878963608382[0] = state[3]; - out_5484223878963608382[1] = state[4]; - out_5484223878963608382[2] = state[5]; - out_5484223878963608382[3] = state[6]; -} -void H_32(double *state, double *unused, double *out_5612564904559411753) { - out_5612564904559411753[0] = 0; - out_5612564904559411753[1] = 0; - out_5612564904559411753[2] = 0; - out_5612564904559411753[3] = 1; - out_5612564904559411753[4] = 0; - out_5612564904559411753[5] = 0; - out_5612564904559411753[6] = 0; - out_5612564904559411753[7] = 0; - out_5612564904559411753[8] = 0; - out_5612564904559411753[9] = 0; - out_5612564904559411753[10] = 0; - out_5612564904559411753[11] = 0; - out_5612564904559411753[12] = 0; - out_5612564904559411753[13] = 0; - out_5612564904559411753[14] = 0; - out_5612564904559411753[15] = 0; - out_5612564904559411753[16] = 0; - out_5612564904559411753[17] = 0; - out_5612564904559411753[18] = 0; - out_5612564904559411753[19] = 0; - out_5612564904559411753[20] = 0; - out_5612564904559411753[21] = 0; - out_5612564904559411753[22] = 0; - out_5612564904559411753[23] = 0; - out_5612564904559411753[24] = 0; - out_5612564904559411753[25] = 0; - out_5612564904559411753[26] = 1; - out_5612564904559411753[27] = 0; - out_5612564904559411753[28] = 0; - out_5612564904559411753[29] = 0; - out_5612564904559411753[30] = 0; - out_5612564904559411753[31] = 0; - out_5612564904559411753[32] = 0; - out_5612564904559411753[33] = 0; - out_5612564904559411753[34] = 0; - out_5612564904559411753[35] = 0; - out_5612564904559411753[36] = 0; - out_5612564904559411753[37] = 0; - out_5612564904559411753[38] = 0; - out_5612564904559411753[39] = 0; - out_5612564904559411753[40] = 0; - out_5612564904559411753[41] = 0; - out_5612564904559411753[42] = 0; - out_5612564904559411753[43] = 0; - out_5612564904559411753[44] = 0; - out_5612564904559411753[45] = 0; - out_5612564904559411753[46] = 0; - out_5612564904559411753[47] = 0; - out_5612564904559411753[48] = 0; - out_5612564904559411753[49] = 1; - out_5612564904559411753[50] = 0; - out_5612564904559411753[51] = 0; - out_5612564904559411753[52] = 0; - out_5612564904559411753[53] = 0; - out_5612564904559411753[54] = 0; - out_5612564904559411753[55] = 0; - out_5612564904559411753[56] = 0; - out_5612564904559411753[57] = 0; - out_5612564904559411753[58] = 0; - out_5612564904559411753[59] = 0; - out_5612564904559411753[60] = 0; - out_5612564904559411753[61] = 0; - out_5612564904559411753[62] = 0; - out_5612564904559411753[63] = 0; - out_5612564904559411753[64] = 0; - out_5612564904559411753[65] = 0; - out_5612564904559411753[66] = 0; - out_5612564904559411753[67] = 0; - out_5612564904559411753[68] = 0; - out_5612564904559411753[69] = 0; - out_5612564904559411753[70] = 0; - out_5612564904559411753[71] = 0; - out_5612564904559411753[72] = 1; - out_5612564904559411753[73] = 0; - out_5612564904559411753[74] = 0; - out_5612564904559411753[75] = 0; - out_5612564904559411753[76] = 0; - out_5612564904559411753[77] = 0; - out_5612564904559411753[78] = 0; - out_5612564904559411753[79] = 0; - out_5612564904559411753[80] = 0; - out_5612564904559411753[81] = 0; - out_5612564904559411753[82] = 0; - out_5612564904559411753[83] = 0; - out_5612564904559411753[84] = 0; - out_5612564904559411753[85] = 0; - out_5612564904559411753[86] = 0; - out_5612564904559411753[87] = 0; -} -void h_13(double *state, double *unused, double *out_7023021544313133462) { - out_7023021544313133462[0] = (-2*state[3]*state[5] + 2*state[4]*state[6])*state[9] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[8] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[7]; - out_7023021544313133462[1] = (2*state[3]*state[4] + 2*state[5]*state[6])*state[9] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[7] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[8]; - out_7023021544313133462[2] = (-2*state[3]*state[4] + 2*state[5]*state[6])*state[8] + (2*state[3]*state[5] + 2*state[4]*state[6])*state[7] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[9]; -} -void H_13(double *state, double *unused, double *out_6405415510614721399) { - out_6405415510614721399[0] = 0; - out_6405415510614721399[1] = 0; - out_6405415510614721399[2] = 0; - out_6405415510614721399[3] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8]; - out_6405415510614721399[4] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; - out_6405415510614721399[5] = -2*state[3]*state[9] + 2*state[4]*state[8] - 2*state[5]*state[7]; - out_6405415510614721399[6] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7]; - out_6405415510614721399[7] = pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2); - out_6405415510614721399[8] = 2*state[3]*state[6] + 2*state[4]*state[5]; - out_6405415510614721399[9] = -2*state[3]*state[5] + 2*state[4]*state[6]; - out_6405415510614721399[10] = 0; - out_6405415510614721399[11] = 0; - out_6405415510614721399[12] = 0; - out_6405415510614721399[13] = 0; - out_6405415510614721399[14] = 0; - out_6405415510614721399[15] = 0; - out_6405415510614721399[16] = 0; - out_6405415510614721399[17] = 0; - out_6405415510614721399[18] = 0; - out_6405415510614721399[19] = 0; - out_6405415510614721399[20] = 0; - out_6405415510614721399[21] = 0; - out_6405415510614721399[22] = 0; - out_6405415510614721399[23] = 0; - out_6405415510614721399[24] = 0; - out_6405415510614721399[25] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7]; - out_6405415510614721399[26] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7]; - out_6405415510614721399[27] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; - out_6405415510614721399[28] = -2*state[3]*state[7] + 2*state[5]*state[9] - 2*state[6]*state[8]; - out_6405415510614721399[29] = -2*state[3]*state[6] + 2*state[4]*state[5]; - out_6405415510614721399[30] = pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2); - out_6405415510614721399[31] = 2*state[3]*state[4] + 2*state[5]*state[6]; - out_6405415510614721399[32] = 0; - out_6405415510614721399[33] = 0; - out_6405415510614721399[34] = 0; - out_6405415510614721399[35] = 0; - out_6405415510614721399[36] = 0; - out_6405415510614721399[37] = 0; - out_6405415510614721399[38] = 0; - out_6405415510614721399[39] = 0; - out_6405415510614721399[40] = 0; - out_6405415510614721399[41] = 0; - out_6405415510614721399[42] = 0; - out_6405415510614721399[43] = 0; - out_6405415510614721399[44] = 0; - out_6405415510614721399[45] = 0; - out_6405415510614721399[46] = 0; - out_6405415510614721399[47] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7]; - out_6405415510614721399[48] = -2*state[3]*state[8] - 2*state[4]*state[9] + 2*state[6]*state[7]; - out_6405415510614721399[49] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8]; - out_6405415510614721399[50] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; - out_6405415510614721399[51] = 2*state[3]*state[5] + 2*state[4]*state[6]; - out_6405415510614721399[52] = -2*state[3]*state[4] + 2*state[5]*state[6]; - out_6405415510614721399[53] = pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2); - out_6405415510614721399[54] = 0; - out_6405415510614721399[55] = 0; - out_6405415510614721399[56] = 0; - out_6405415510614721399[57] = 0; - out_6405415510614721399[58] = 0; - out_6405415510614721399[59] = 0; - out_6405415510614721399[60] = 0; - out_6405415510614721399[61] = 0; - out_6405415510614721399[62] = 0; - out_6405415510614721399[63] = 0; - out_6405415510614721399[64] = 0; - out_6405415510614721399[65] = 0; -} -void h_14(double *state, double *unused, double *out_4201404494137813277) { - out_4201404494137813277[0] = state[10]; - out_4201404494137813277[1] = state[11]; - out_4201404494137813277[2] = state[12]; -} -void H_14(double *state, double *unused, double *out_1823172811608312128) { - out_1823172811608312128[0] = 0; - out_1823172811608312128[1] = 0; - out_1823172811608312128[2] = 0; - out_1823172811608312128[3] = 0; - out_1823172811608312128[4] = 0; - out_1823172811608312128[5] = 0; - out_1823172811608312128[6] = 0; - out_1823172811608312128[7] = 0; - out_1823172811608312128[8] = 0; - out_1823172811608312128[9] = 0; - out_1823172811608312128[10] = 1; - out_1823172811608312128[11] = 0; - out_1823172811608312128[12] = 0; - out_1823172811608312128[13] = 0; - out_1823172811608312128[14] = 0; - out_1823172811608312128[15] = 0; - out_1823172811608312128[16] = 0; - out_1823172811608312128[17] = 0; - out_1823172811608312128[18] = 0; - out_1823172811608312128[19] = 0; - out_1823172811608312128[20] = 0; - out_1823172811608312128[21] = 0; - out_1823172811608312128[22] = 0; - out_1823172811608312128[23] = 0; - out_1823172811608312128[24] = 0; - out_1823172811608312128[25] = 0; - out_1823172811608312128[26] = 0; - out_1823172811608312128[27] = 0; - out_1823172811608312128[28] = 0; - out_1823172811608312128[29] = 0; - out_1823172811608312128[30] = 0; - out_1823172811608312128[31] = 0; - out_1823172811608312128[32] = 0; - out_1823172811608312128[33] = 1; - out_1823172811608312128[34] = 0; - out_1823172811608312128[35] = 0; - out_1823172811608312128[36] = 0; - out_1823172811608312128[37] = 0; - out_1823172811608312128[38] = 0; - out_1823172811608312128[39] = 0; - out_1823172811608312128[40] = 0; - out_1823172811608312128[41] = 0; - out_1823172811608312128[42] = 0; - out_1823172811608312128[43] = 0; - out_1823172811608312128[44] = 0; - out_1823172811608312128[45] = 0; - out_1823172811608312128[46] = 0; - out_1823172811608312128[47] = 0; - out_1823172811608312128[48] = 0; - out_1823172811608312128[49] = 0; - out_1823172811608312128[50] = 0; - out_1823172811608312128[51] = 0; - out_1823172811608312128[52] = 0; - out_1823172811608312128[53] = 0; - out_1823172811608312128[54] = 0; - out_1823172811608312128[55] = 0; - out_1823172811608312128[56] = 1; - out_1823172811608312128[57] = 0; - out_1823172811608312128[58] = 0; - out_1823172811608312128[59] = 0; - out_1823172811608312128[60] = 0; - out_1823172811608312128[61] = 0; - out_1823172811608312128[62] = 0; - out_1823172811608312128[63] = 0; - out_1823172811608312128[64] = 0; - out_1823172811608312128[65] = 0; -} -void h_33(double *state, double *unused, double *out_3622091043585692181) { - out_3622091043585692181[0] = state[16]; - out_3622091043585692181[1] = state[17]; - out_3622091043585692181[2] = state[18]; -} -void H_33(double *state, double *unused, double *out_4452856603773562207) { - out_4452856603773562207[0] = 0; - out_4452856603773562207[1] = 0; - out_4452856603773562207[2] = 0; - out_4452856603773562207[3] = 0; - out_4452856603773562207[4] = 0; - out_4452856603773562207[5] = 0; - out_4452856603773562207[6] = 0; - out_4452856603773562207[7] = 0; - out_4452856603773562207[8] = 0; - out_4452856603773562207[9] = 0; - out_4452856603773562207[10] = 0; - out_4452856603773562207[11] = 0; - out_4452856603773562207[12] = 0; - out_4452856603773562207[13] = 0; - out_4452856603773562207[14] = 0; - out_4452856603773562207[15] = 0; - out_4452856603773562207[16] = 1; - out_4452856603773562207[17] = 0; - out_4452856603773562207[18] = 0; - out_4452856603773562207[19] = 0; - out_4452856603773562207[20] = 0; - out_4452856603773562207[21] = 0; - out_4452856603773562207[22] = 0; - out_4452856603773562207[23] = 0; - out_4452856603773562207[24] = 0; - out_4452856603773562207[25] = 0; - out_4452856603773562207[26] = 0; - out_4452856603773562207[27] = 0; - out_4452856603773562207[28] = 0; - out_4452856603773562207[29] = 0; - out_4452856603773562207[30] = 0; - out_4452856603773562207[31] = 0; - out_4452856603773562207[32] = 0; - out_4452856603773562207[33] = 0; - out_4452856603773562207[34] = 0; - out_4452856603773562207[35] = 0; - out_4452856603773562207[36] = 0; - out_4452856603773562207[37] = 0; - out_4452856603773562207[38] = 0; - out_4452856603773562207[39] = 1; - out_4452856603773562207[40] = 0; - out_4452856603773562207[41] = 0; - out_4452856603773562207[42] = 0; - out_4452856603773562207[43] = 0; - out_4452856603773562207[44] = 0; - out_4452856603773562207[45] = 0; - out_4452856603773562207[46] = 0; - out_4452856603773562207[47] = 0; - out_4452856603773562207[48] = 0; - out_4452856603773562207[49] = 0; - out_4452856603773562207[50] = 0; - out_4452856603773562207[51] = 0; - out_4452856603773562207[52] = 0; - out_4452856603773562207[53] = 0; - out_4452856603773562207[54] = 0; - out_4452856603773562207[55] = 0; - out_4452856603773562207[56] = 0; - out_4452856603773562207[57] = 0; - out_4452856603773562207[58] = 0; - out_4452856603773562207[59] = 0; - out_4452856603773562207[60] = 0; - out_4452856603773562207[61] = 0; - out_4452856603773562207[62] = 1; - out_4452856603773562207[63] = 0; - out_4452856603773562207[64] = 0; - out_4452856603773562207[65] = 0; -} -#include -#include - -typedef Eigen::Matrix DDM; -typedef Eigen::Matrix EEM; -typedef Eigen::Matrix DEM; - -void predict(double *in_x, double *in_P, double *in_Q, double dt) { - typedef Eigen::Matrix RRM; - - double nx[DIM] = {0}; - double in_F[EDIM*EDIM] = {0}; - - // functions from sympy - f_fun(in_x, dt, nx); - F_fun(in_x, dt, in_F); - - - EEM F(in_F); - EEM P(in_P); - EEM Q(in_Q); - - RRM F_main = F.topLeftCorner(MEDIM, MEDIM); - P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose(); - P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM); - P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose(); - - P = P + dt*Q; - - // copy out state - memcpy(in_x, nx, DIM * sizeof(double)); - memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double)); -} - -// note: extra_args dim only correct when null space projecting -// otherwise 1 -template -void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) { - typedef Eigen::Matrix ZZM; - typedef Eigen::Matrix ZDM; - typedef Eigen::Matrix XEM; - //typedef Eigen::Matrix EZM; - typedef Eigen::Matrix X1M; - typedef Eigen::Matrix XXM; - - double in_hx[ZDIM] = {0}; - double in_H[ZDIM * DIM] = {0}; - double in_H_mod[EDIM * DIM] = {0}; - double delta_x[EDIM] = {0}; - double x_new[DIM] = {0}; - - - // state x, P - Eigen::Matrix z(in_z); - EEM P(in_P); - ZZM pre_R(in_R); - - // functions from sympy - h_fun(in_x, in_ea, in_hx); - H_fun(in_x, in_ea, in_H); - ZDM pre_H(in_H); - - // get y (y = z - hx) - Eigen::Matrix pre_y(in_hx); pre_y = z - pre_y; - X1M y; XXM H; XXM R; - if (Hea_fun){ - typedef Eigen::Matrix ZAM; - double in_Hea[ZDIM * EADIM] = {0}; - Hea_fun(in_x, in_ea, in_Hea); - ZAM Hea(in_Hea); - XXM A = Hea.transpose().fullPivLu().kernel(); - - - y = A.transpose() * pre_y; - H = A.transpose() * pre_H; - R = A.transpose() * pre_R * A; - } else { - y = pre_y; - H = pre_H; - R = pre_R; - } - // get modified H - H_mod_fun(in_x, in_H_mod); - DEM H_mod(in_H_mod); - XEM H_err = H * H_mod; - - // Do mahalobis distance test - if (MAHA_TEST){ - XXM a = (H_err * P * H_err.transpose() + R).inverse(); - double maha_dist = y.transpose() * a * y; - if (maha_dist > MAHA_THRESHOLD){ - R = 1.0e16 * R; - } - } - - // Outlier resilient weighting - double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum()); - - // kalman gains and I_KH - XXM S = ((H_err * P) * H_err.transpose()) + R/weight; - XEM KT = S.fullPivLu().solve(H_err * P.transpose()); - //EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE? - //EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose(); - //std::cout << "Here is the matrix rot:\n" << K << std::endl; - EEM I_KH = Eigen::Matrix::Identity() - (KT.transpose() * H_err); - - // update state by injecting dx - Eigen::Matrix dx(delta_x); - dx = (KT.transpose() * y); - memcpy(delta_x, dx.data(), EDIM * sizeof(double)); - err_fun(in_x, delta_x, x_new); - Eigen::Matrix x(x_new); - - // update cov - P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT); - - // copy out state - memcpy(in_x, x.data(), DIM * sizeof(double)); - memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double)); - memcpy(in_z, y.data(), y.rows() * sizeof(double)); -} - - - - -} -extern "C" { - -void live_update_4(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<3, 3, 0>(in_x, in_P, h_4, H_4, NULL, in_z, in_R, in_ea, MAHA_THRESH_4); -} -void live_update_9(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<3, 3, 0>(in_x, in_P, h_9, H_9, NULL, in_z, in_R, in_ea, MAHA_THRESH_9); -} -void live_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<3, 3, 0>(in_x, in_P, h_10, H_10, NULL, in_z, in_R, in_ea, MAHA_THRESH_10); -} -void live_update_12(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<3, 3, 0>(in_x, in_P, h_12, H_12, NULL, in_z, in_R, in_ea, MAHA_THRESH_12); -} -void live_update_35(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<3, 3, 0>(in_x, in_P, h_35, H_35, NULL, in_z, in_R, in_ea, MAHA_THRESH_35); -} -void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<4, 3, 0>(in_x, in_P, h_32, H_32, NULL, in_z, in_R, in_ea, MAHA_THRESH_32); -} -void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<3, 3, 0>(in_x, in_P, h_13, H_13, NULL, in_z, in_R, in_ea, MAHA_THRESH_13); -} -void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<3, 3, 0>(in_x, in_P, h_14, H_14, NULL, in_z, in_R, in_ea, MAHA_THRESH_14); -} -void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { - update<3, 3, 0>(in_x, in_P, h_33, H_33, NULL, in_z, in_R, in_ea, MAHA_THRESH_33); -} -void live_H(double *in_vec, double *out_3655759921532451903) { - H(in_vec, out_3655759921532451903); -} -void live_err_fun(double *nom_x, double *delta_x, double *out_6606438496319011893) { - err_fun(nom_x, delta_x, out_6606438496319011893); -} -void live_inv_err_fun(double *nom_x, double *true_x, double *out_1892773414432101423) { - inv_err_fun(nom_x, true_x, out_1892773414432101423); -} -void live_H_mod_fun(double *state, double *out_31505903139368806) { - H_mod_fun(state, out_31505903139368806); -} -void live_f_fun(double *state, double dt, double *out_4984514158829912596) { - f_fun(state, dt, out_4984514158829912596); -} -void live_F_fun(double *state, double dt, double *out_495900107318952904) { - F_fun(state, dt, out_495900107318952904); -} -void live_h_4(double *state, double *unused, double *out_4218409302925889705) { - h_4(state, unused, out_4218409302925889705); -} -void live_H_4(double *state, double *unused, double *out_2064362458237902773) { - H_4(state, unused, out_2064362458237902773); -} -void live_h_9(double *state, double *unused, double *out_4201404494137813277) { - h_9(state, unused, out_4201404494137813277); -} -void live_H_9(double *state, double *unused, double *out_1823172811608312128) { - H_9(state, unused, out_1823172811608312128); -} -void live_h_10(double *state, double *unused, double *out_4904878276509257913) { - h_10(state, unused, out_4904878276509257913); -} -void live_H_10(double *state, double *unused, double *out_7361770315712994123) { - H_10(state, unused, out_7361770315712994123); -} -void live_h_12(double *state, double *unused, double *out_3237267128515768115) { - h_12(state, unused, out_3237267128515768115); -} -void live_H_12(double *state, double *unused, double *out_2955093949794059022) { - H_12(state, unused, out_2955093949794059022); -} -void live_h_35(double *state, double *unused, double *out_9093746190498438204) { - h_35(state, unused, out_9093746190498438204); -} -void live_H_35(double *state, double *unused, double *out_1302299599134704603) { - H_35(state, unused, out_1302299599134704603); -} -void live_h_32(double *state, double *unused, double *out_5484223878963608382) { - h_32(state, unused, out_5484223878963608382); -} -void live_H_32(double *state, double *unused, double *out_5612564904559411753) { - H_32(state, unused, out_5612564904559411753); -} -void live_h_13(double *state, double *unused, double *out_7023021544313133462) { - h_13(state, unused, out_7023021544313133462); -} -void live_H_13(double *state, double *unused, double *out_6405415510614721399) { - H_13(state, unused, out_6405415510614721399); -} -void live_h_14(double *state, double *unused, double *out_4201404494137813277) { - h_14(state, unused, out_4201404494137813277); -} -void live_H_14(double *state, double *unused, double *out_1823172811608312128) { - H_14(state, unused, out_1823172811608312128); -} -void live_h_33(double *state, double *unused, double *out_3622091043585692181) { - h_33(state, unused, out_3622091043585692181); -} -void live_H_33(double *state, double *unused, double *out_4452856603773562207) { - H_33(state, unused, out_4452856603773562207); -} -void live_predict(double *in_x, double *in_P, double *in_Q, double dt) { - predict(in_x, in_P, in_Q, dt); -} -} - -const EKF live = { - .name = "live", - .kinds = { 4, 9, 10, 12, 35, 32, 13, 14, 33 }, - .feature_kinds = { }, - .f_fun = live_f_fun, - .F_fun = live_F_fun, - .err_fun = live_err_fun, - .inv_err_fun = live_inv_err_fun, - .H_mod_fun = live_H_mod_fun, - .predict = live_predict, - .hs = { - { 4, live_h_4 }, - { 9, live_h_9 }, - { 10, live_h_10 }, - { 12, live_h_12 }, - { 35, live_h_35 }, - { 32, live_h_32 }, - { 13, live_h_13 }, - { 14, live_h_14 }, - { 33, live_h_33 }, - }, - .Hs = { - { 4, live_H_4 }, - { 9, live_H_9 }, - { 10, live_H_10 }, - { 12, live_H_12 }, - { 35, live_H_35 }, - { 32, live_H_32 }, - { 13, live_H_13 }, - { 14, live_H_14 }, - { 33, live_H_33 }, - }, - .updates = { - { 4, live_update_4 }, - { 9, live_update_9 }, - { 10, live_update_10 }, - { 12, live_update_12 }, - { 35, live_update_35 }, - { 32, live_update_32 }, - { 13, live_update_13 }, - { 14, live_update_14 }, - { 33, live_update_33 }, - }, - .Hes = { - }, - .sets = { - }, - .extra_routines = { - { "H", live_H }, - }, -}; - -ekf_lib_init(live) diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc new file mode 100644 index 0000000..fc3bfb7 --- /dev/null +++ b/selfdrive/locationd/models/live_kf.cc @@ -0,0 +1,122 @@ +#include "selfdrive/locationd/models/live_kf.h" + +using namespace EKFS; +using namespace Eigen; + +Eigen::Map get_mapvec(const Eigen::VectorXd &vec) { + return Eigen::Map((double*)vec.data(), vec.rows(), vec.cols()); +} + +Eigen::Map get_mapmat(const MatrixXdr &mat) { + return Eigen::Map((double*)mat.data(), mat.rows(), mat.cols()); +} + +std::vector> get_vec_mapvec(const std::vector &vec_vec) { + std::vector> res; + for (const Eigen::VectorXd &vec : vec_vec) { + res.push_back(get_mapvec(vec)); + } + return res; +} + +std::vector> get_vec_mapmat(const std::vector &mat_vec) { + std::vector> 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(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(), + std::vector{3}, std::vector(), 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 LiveKalman::get_R(int kind, int n) { + std::vector R; + for (int i = 0; i < n; i++) { + R.push_back(this->obs_noise[kind]); + } + return R; +} + +std::optional LiveKalman::predict_and_observe(double t, int kind, const std::vector &meas, std::vector R) { + std::optional 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 res; + this->filter->get_extra_routine("H")((double*)in.data(), res.data()); + return res; +} diff --git a/selfdrive/locationd/models/live_kf.h b/selfdrive/locationd/models/live_kf.h new file mode 100644 index 0000000..e4b3e32 --- /dev/null +++ b/selfdrive/locationd/models/live_kf.h @@ -0,0 +1,66 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +#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 get_mapvec(const Eigen::VectorXd &vec); +Eigen::Map get_mapmat(const MatrixXdr &mat); +std::vector> get_vec_mapvec(const std::vector &vec_vec); +std::vector> get_vec_mapmat(const std::vector &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 get_R(int kind, int n); + + std::optional predict_and_observe(double t, int kind, const std::vector &meas, std::vector R = {}); + std::optional predict_and_update_odo_speed(std::vector speed, double t, int kind); + std::optional predict_and_update_odo_trans(std::vector trans, double t, int kind); + std::optional predict_and_update_odo_rot(std::vector 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 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 obs_noise; +}; diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index de2578f..de4bbcf 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -14,7 +14,6 @@ from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKi from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR from openpilot.common.swaglog import cloudlog -params_memory = Params("/dev/shm/params") MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits @@ -142,6 +141,7 @@ def main(): min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio params = params_reader.get("LiveParameters") + params_memory = Params("/dev/shm/params") # Check if car model matches if params is not None: @@ -202,7 +202,7 @@ def main(): bearing = math.degrees(location.calibratedOrientationNED.value[2]) lat = location.positionGeodetic.value[0] lon = location.positionGeodetic.value[1] - params_memory.put("LastGPSPosition", json.dumps({ "latitude": lat, "longitude": lon, "bearing": bearing })) + params_memory.put("LastGPSPosition", json.dumps({"latitude": lat, "longitude": lon, "bearing": bearing})) x = learner.kf.x P = np.sqrt(learner.kf.P.diagonal()) if not all(map(math.isfinite, x)): @@ -244,7 +244,7 @@ def main(): 0.2 <= liveParameters.stiffnessFactor <= 5.0, min_sr <= liveParameters.steerRatio <= max_sr, )) - if (CP.carName == 'subaru' and CP.lateralTuning.which() == 'torque'): + if (CP.carName == "subaru" and CP.lateralTuning.which() == "torque"): liveParameters.valid = True liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item()) liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item()) diff --git a/selfdrive/locationd/test/.gitignore b/selfdrive/locationd/test/.gitignore new file mode 100644 index 0000000..89f9ac0 --- /dev/null +++ b/selfdrive/locationd/test/.gitignore @@ -0,0 +1 @@ +out/ diff --git a/selfdrive/locationd/test/__init__.py b/selfdrive/locationd/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/selfdrive/locationd/test/test_calibrationd.py b/selfdrive/locationd/test/test_calibrationd.py new file mode 100644 index 0000000..e2db094 --- /dev/null +++ b/selfdrive/locationd/test/test_calibrationd.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 +import random +import unittest + +import numpy as np + +import cereal.messaging as messaging +from cereal import log +from openpilot.common.params import Params +from openpilot.selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, \ + MAX_YAW_RATE_FILTER, SMOOTH_CYCLES, HEIGHT_INIT, MAX_ALLOWED_PITCH_SPREAD, MAX_ALLOWED_YAW_SPREAD + + +def process_messages(c, cam_odo_calib, cycles, + cam_odo_speed=MIN_SPEED_FILTER + 1, + carstate_speed=MIN_SPEED_FILTER + 1, + cam_odo_yr=0.0, + cam_odo_speed_std=1e-3, + cam_odo_height_std=1e-3): + old_rpy_weight_prev = 0.0 + for _ in range(cycles): + assert (old_rpy_weight_prev - c.old_rpy_weight < 1/SMOOTH_CYCLES + 1e-3) + old_rpy_weight_prev = c.old_rpy_weight + c.handle_v_ego(carstate_speed) + c.handle_cam_odom([cam_odo_speed, + np.sin(cam_odo_calib[2]) * cam_odo_speed, + -np.sin(cam_odo_calib[1]) * cam_odo_speed], + [0.0, 0.0, cam_odo_yr], + [0.0, 0.0, 0.0], + [cam_odo_speed_std, cam_odo_speed_std, cam_odo_speed_std], + [0.0, 0.0, HEIGHT_INIT.item()], + [cam_odo_height_std, cam_odo_height_std, cam_odo_height_std]) + +class TestCalibrationd(unittest.TestCase): + + def test_read_saved_params(self): + msg = messaging.new_message('liveCalibration') + msg.liveCalibration.validBlocks = random.randint(1, 10) + msg.liveCalibration.rpyCalib = [random.random() for _ in range(3)] + msg.liveCalibration.height = [random.random() for _ in range(1)] + Params().put("CalibrationParams", msg.to_bytes()) + c = Calibrator(param_put=True) + + np.testing.assert_allclose(msg.liveCalibration.rpyCalib, c.rpy) + np.testing.assert_allclose(msg.liveCalibration.height, c.height) + self.assertEqual(msg.liveCalibration.validBlocks, c.valid_blocks) + + + def test_calibration_basics(self): + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED) + self.assertEqual(c.valid_blocks, INPUTS_WANTED) + np.testing.assert_allclose(c.rpy, np.zeros(3)) + np.testing.assert_allclose(c.height, HEIGHT_INIT) + c.reset() + + + def test_calibration_low_speed_reject(self): + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed=MIN_SPEED_FILTER - 1) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, carstate_speed=MIN_SPEED_FILTER - 1) + self.assertEqual(c.valid_blocks, 0) + np.testing.assert_allclose(c.rpy, np.zeros(3)) + np.testing.assert_allclose(c.height, HEIGHT_INIT) + + + def test_calibration_yaw_rate_reject(self): + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_yr=MAX_YAW_RATE_FILTER) + self.assertEqual(c.valid_blocks, 0) + np.testing.assert_allclose(c.rpy, np.zeros(3)) + np.testing.assert_allclose(c.height, HEIGHT_INIT) + + + def test_calibration_speed_std_reject(self): + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed_std=1e3) + self.assertEqual(c.valid_blocks, INPUTS_NEEDED) + np.testing.assert_allclose(c.rpy, np.zeros(3)) + + + def test_calibration_speed_std_height_reject(self): + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_height_std=1e3) + self.assertEqual(c.valid_blocks, INPUTS_NEEDED) + np.testing.assert_allclose(c.rpy, np.zeros(3)) + + + def test_calibration_auto_reset(self): + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED) + self.assertEqual(c.valid_blocks, INPUTS_NEEDED) + np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0], atol=1e-3) + process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*0.9, MAX_ALLOWED_YAW_SPREAD*0.9], BLOCK_SIZE + 10) + self.assertEqual(c.valid_blocks, INPUTS_NEEDED + 1) + self.assertEqual(c.cal_status, log.LiveCalibrationData.Status.calibrated) + + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED) + self.assertEqual(c.valid_blocks, INPUTS_NEEDED) + np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0]) + process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], BLOCK_SIZE + 10) + self.assertEqual(c.valid_blocks, 1) + self.assertEqual(c.cal_status, log.LiveCalibrationData.Status.recalibrating) + np.testing.assert_allclose(c.rpy, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], atol=1e-2) + + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED) + self.assertEqual(c.valid_blocks, INPUTS_NEEDED) + np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0]) + process_messages(c, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], BLOCK_SIZE + 10) + self.assertEqual(c.valid_blocks, 1) + self.assertEqual(c.cal_status, log.LiveCalibrationData.Status.recalibrating) + np.testing.assert_allclose(c.rpy, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], atol=1e-2) + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py new file mode 100644 index 0000000..cd032db --- /dev/null +++ b/selfdrive/locationd/test/test_locationd.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python3 +import json +import random +import unittest +import time +import capnp + +import cereal.messaging as messaging +from cereal.services import SERVICE_LIST +from openpilot.common.params import Params +from openpilot.common.transformations.coordinates import ecef2geodetic + +from openpilot.selfdrive.manager.process_config import managed_processes + + +class TestLocationdProc(unittest.TestCase): + LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', + 'accelerometer', 'gyroscope', 'magnetometer'] + + def setUp(self): + random.seed(123489234) + + self.pm = messaging.PubMaster(self.LLD_MSGS) + + self.params = Params() + self.params.put_bool("UbloxAvailable", True) + managed_processes['locationd'].prepare() + managed_processes['locationd'].start() + + def tearDown(self): + managed_processes['locationd'].stop() + + def get_msg(self, name, t): + try: + msg = messaging.new_message(name) + except capnp.lib.capnp.KjException: + msg = messaging.new_message(name, 0) + + if name == "gpsLocationExternal": + msg.gpsLocationExternal.flags = 1 + msg.gpsLocationExternal.hasFix = True + msg.gpsLocationExternal.verticalAccuracy = 1.0 + msg.gpsLocationExternal.speedAccuracy = 1.0 + msg.gpsLocationExternal.bearingAccuracyDeg = 1.0 + msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] + msg.gpsLocationExternal.latitude = float(self.lat) + msg.gpsLocationExternal.longitude = float(self.lon) + msg.gpsLocationExternal.unixTimestampMillis = t * 1e6 + msg.gpsLocationExternal.altitude = float(self.alt) + #if name == "gnssMeasurements": + # msg.gnssMeasurements.measTime = t + # msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z] + # msg.gnssMeasurements.positionECEF.std = [0,0,0] + # msg.gnssMeasurements.positionECEF.valid = True + # msg.gnssMeasurements.velocityECEF.value = [] + # msg.gnssMeasurements.velocityECEF.std = [0,0,0] + # msg.gnssMeasurements.velocityECEF.valid = True + elif name == 'cameraOdometry': + msg.cameraOdometry.rot = [0.0, 0.0, 0.0] + msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0] + msg.cameraOdometry.trans = [0.0, 0.0, 0.0] + msg.cameraOdometry.transStd = [0.0, 0.0, 0.0] + msg.logMonoTime = t + msg.valid = True + return msg + + def test_params_gps(self): + self.params.remove('LastGPSPosition') + + self.x = -2710700 + (random.random() * 1e5) + self.y = -4280600 + (random.random() * 1e5) + self.z = 3850300 + (random.random() * 1e5) + self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z]) + + # get fake messages at the correct frequency, listed in services.py + msgs = [] + for sec in range(65): + for name in self.LLD_MSGS: + for j in range(int(SERVICE_LIST[name].frequency)): + msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9))) + + for msg in sorted(msgs, key=lambda x: x.logMonoTime): + self.pm.send(msg.which(), msg) + if msg.which() == "cameraOdometry": + self.pm.wait_for_readers_to_update(msg.which(), 0.1, dt=0.005) + time.sleep(1) # wait for async params write + + lastGPS = json.loads(self.params.get('LastGPSPosition')) + self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=3) + self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=3) + self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=3) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/locationd/test/test_locationd_scenarios.py b/selfdrive/locationd/test/test_locationd_scenarios.py new file mode 100644 index 0000000..3fdd472 --- /dev/null +++ b/selfdrive/locationd/test/test_locationd_scenarios.py @@ -0,0 +1,229 @@ +#!/usr/bin/env python3 +import pytest +import unittest +import numpy as np +from collections import defaultdict +from enum import Enum + +from openpilot.tools.lib.logreader import LogReader +from openpilot.selfdrive.test.process_replay.migration import migrate_all +from openpilot.selfdrive.test.process_replay.process_replay import replay_process_with_name + +TEST_ROUTE = "ff2bd20623fcaeaa|2023-09-05--10-14-54/4" +GPS_MESSAGES = ['gpsLocationExternal', 'gpsLocation'] +SELECT_COMPARE_FIELDS = { + 'yaw_rate': ['angularVelocityCalibrated', 'value', 2], + 'roll': ['orientationNED', 'value', 0], + 'gps_flag': ['gpsOK'], + 'inputs_flag': ['inputsOK'], + 'sensors_flag': ['sensorsOK'], +} +JUNK_IDX = 100 + + +class Scenario(Enum): + BASE = 'base' + GPS_OFF = 'gps_off' + GPS_OFF_MIDWAY = 'gps_off_midway' + GPS_ON_MIDWAY = 'gps_on_midway' + GPS_TUNNEL = 'gps_tunnel' + GYRO_OFF = 'gyro_off' + GYRO_SPIKE_MIDWAY = 'gyro_spike_midway' + ACCEL_OFF = 'accel_off' + ACCEL_SPIKE_MIDWAY = 'accel_spike_midway' + + +def get_select_fields_data(logs): + def get_nested_keys(msg, keys): + val = None + for key in keys: + val = getattr(msg if val is None else val, key) if isinstance(key, str) else val[key] + return val + llk = [x.liveLocationKalman for x in logs if x.which() == 'liveLocationKalman'] + data = defaultdict(list) + for msg in llk: + for key, fields in SELECT_COMPARE_FIELDS.items(): + data[key].append(get_nested_keys(msg, fields)) + for key in data: + data[key] = np.array(data[key][JUNK_IDX:], dtype=float) + return data + + +def run_scenarios(scenario, logs): + if scenario == Scenario.BASE: + pass + + elif scenario == Scenario.GPS_OFF: + logs = sorted([x for x in logs if x.which() not in GPS_MESSAGES], key=lambda x: x.logMonoTime) + + elif scenario == Scenario.GPS_OFF_MIDWAY: + non_gps = [x for x in logs if x.which() not in GPS_MESSAGES] + gps = [x for x in logs if x.which() in GPS_MESSAGES] + logs = sorted(non_gps + gps[: len(gps) // 2], key=lambda x: x.logMonoTime) + + elif scenario == Scenario.GPS_ON_MIDWAY: + non_gps = [x for x in logs if x.which() not in GPS_MESSAGES] + gps = [x for x in logs if x.which() in GPS_MESSAGES] + logs = sorted(non_gps + gps[len(gps) // 2:], key=lambda x: x.logMonoTime) + + elif scenario == Scenario.GPS_TUNNEL: + non_gps = [x for x in logs if x.which() not in GPS_MESSAGES] + gps = [x for x in logs if x.which() in GPS_MESSAGES] + logs = sorted(non_gps + gps[:len(gps) // 4] + gps[-len(gps) // 4:], key=lambda x: x.logMonoTime) + + elif scenario == Scenario.GYRO_OFF: + logs = sorted([x for x in logs if x.which() != 'gyroscope'], key=lambda x: x.logMonoTime) + + elif scenario == Scenario.GYRO_SPIKE_MIDWAY: + non_gyro = [x for x in logs if x.which() not in 'gyroscope'] + gyro = [x for x in logs if x.which() in 'gyroscope'] + temp = gyro[len(gyro) // 2].as_builder() + temp.gyroscope.gyroUncalibrated.v[0] += 3.0 + gyro[len(gyro) // 2] = temp.as_reader() + logs = sorted(non_gyro + gyro, key=lambda x: x.logMonoTime) + + elif scenario == Scenario.ACCEL_OFF: + logs = sorted([x for x in logs if x.which() != 'accelerometer'], key=lambda x: x.logMonoTime) + + elif scenario == Scenario.ACCEL_SPIKE_MIDWAY: + non_accel = [x for x in logs if x.which() not in 'accelerometer'] + accel = [x for x in logs if x.which() in 'accelerometer'] + temp = accel[len(accel) // 2].as_builder() + temp.accelerometer.acceleration.v[0] += 10.0 + accel[len(accel) // 2] = temp.as_reader() + logs = sorted(non_accel + accel, key=lambda x: x.logMonoTime) + + replayed_logs = replay_process_with_name(name='locationd', lr=logs) + return get_select_fields_data(logs), get_select_fields_data(replayed_logs) + + +@pytest.mark.xdist_group("test_locationd_scenarios") +@pytest.mark.shared_download_cache +class TestLocationdScenarios(unittest.TestCase): + """ + Test locationd with different scenarios. In all these scenarios, we expect the following: + - locationd kalman filter should never go unstable (we care mostly about yaw_rate, roll, gpsOK, inputsOK, sensorsOK) + - faulty values should be ignored, with appropriate flags set + """ + + @classmethod + def setUpClass(cls): + cls.logs = migrate_all(LogReader(TEST_ROUTE)) + + def test_base(self): + """ + Test: unchanged log + Expected Result: + - yaw_rate: unchanged + - roll: unchanged + """ + orig_data, replayed_data = run_scenarios(Scenario.BASE, self.logs) + self.assertTrue(np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))) + self.assertTrue(np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))) + + def test_gps_off(self): + """ + Test: no GPS message for the entire segment + Expected Result: + - yaw_rate: unchanged + - roll: + - gpsOK: False + """ + orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF, self.logs) + self.assertTrue(np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))) + self.assertTrue(np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))) + self.assertTrue(np.all(replayed_data['gps_flag'] == 0.0)) + + def test_gps_off_midway(self): + """ + Test: no GPS message for the second half of the segment + Expected Result: + - yaw_rate: unchanged + - roll: + - gpsOK: True for the first half, False for the second half + """ + orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF_MIDWAY, self.logs) + self.assertTrue(np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))) + self.assertTrue(np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))) + self.assertTrue(np.diff(replayed_data['gps_flag'])[512] == -1.0) + + def test_gps_on_midway(self): + """ + Test: no GPS message for the first half of the segment + Expected Result: + - yaw_rate: unchanged + - roll: + - gpsOK: False for the first half, True for the second half + """ + orig_data, replayed_data = run_scenarios(Scenario.GPS_ON_MIDWAY, self.logs) + self.assertTrue(np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))) + self.assertTrue(np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(1.5))) + self.assertTrue(np.diff(replayed_data['gps_flag'])[505] == 1.0) + + def test_gps_tunnel(self): + """ + Test: no GPS message for the middle section of the segment + Expected Result: + - yaw_rate: unchanged + - roll: + - gpsOK: False for the middle section, True for the rest + """ + orig_data, replayed_data = run_scenarios(Scenario.GPS_TUNNEL, self.logs) + self.assertTrue(np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))) + self.assertTrue(np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))) + self.assertTrue(np.diff(replayed_data['gps_flag'])[213] == -1.0) + self.assertTrue(np.diff(replayed_data['gps_flag'])[805] == 1.0) + + def test_gyro_off(self): + """ + Test: no gyroscope message for the entire segment + Expected Result: + - yaw_rate: 0 + - roll: 0 + - sensorsOK: False + """ + _, replayed_data = run_scenarios(Scenario.GYRO_OFF, self.logs) + self.assertTrue(np.allclose(replayed_data['yaw_rate'], 0.0)) + self.assertTrue(np.allclose(replayed_data['roll'], 0.0)) + self.assertTrue(np.all(replayed_data['sensors_flag'] == 0.0)) + + def test_gyro_spikes(self): + """ + Test: a gyroscope spike in the middle of the segment + Expected Result: + - yaw_rate: unchanged + - roll: unchanged + - inputsOK: False for some time after the spike, True for the rest + """ + orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs) + self.assertTrue(np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))) + self.assertTrue(np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))) + self.assertTrue(np.diff(replayed_data['inputs_flag'])[500] == -1.0) + self.assertTrue(np.diff(replayed_data['inputs_flag'])[694] == 1.0) + + def test_accel_off(self): + """ + Test: no accelerometer message for the entire segment + Expected Result: + - yaw_rate: 0 + - roll: 0 + - sensorsOK: False + """ + _, replayed_data = run_scenarios(Scenario.ACCEL_OFF, self.logs) + self.assertTrue(np.allclose(replayed_data['yaw_rate'], 0.0)) + self.assertTrue(np.allclose(replayed_data['roll'], 0.0)) + self.assertTrue(np.all(replayed_data['sensors_flag'] == 0.0)) + + def test_accel_spikes(self): + """ + ToDo: + Test: an accelerometer spike in the middle of the segment + Expected Result: Right now, the kalman filter is not robust to small spikes like it is to gyroscope spikes. + """ + orig_data, replayed_data = run_scenarios(Scenario.ACCEL_SPIKE_MIDWAY, self.logs) + self.assertTrue(np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))) + self.assertTrue(np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 69bab8d..06f9044 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -159,8 +159,10 @@ class TorqueEstimator(ParameterEstimator): def handle_log(self, t, which, msg): if which == "carControl": self.raw_points["carControl_t"].append(t + self.lag) - self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer) self.raw_points["active"].append(msg.latActive) + elif which == "carOutput": + self.raw_points["carOutput_t"].append(t + self.lag) + self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer) elif which == "carState": self.raw_points["carState_t"].append(t + self.lag) self.raw_points["vego"].append(msg.vEgo) @@ -172,7 +174,7 @@ class TorqueEstimator(ParameterEstimator): active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carControl_t'], self.raw_points['active']).astype(bool) steer_override = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carState_t'], self.raw_points['steer_override']).astype(bool) vego = np.interp(t, self.raw_points['carState_t'], self.raw_points['vego']) - steer = np.interp(t, self.raw_points['carControl_t'], self.raw_points['steer_torque']) + steer = np.interp(t, self.raw_points['carOutput_t'], self.raw_points['steer_torque']) lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY) if all(active) and (not any(steer_override)) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD) and (abs(lateral_acc) <= LAT_ACC_THRESHOLD): self.filtered_points.add_point(float(steer), float(lateral_acc)) @@ -218,7 +220,7 @@ def main(demo=False): config_realtime_process([0, 1, 2, 3], 5) pm = messaging.PubMaster(['liveTorqueParameters']) - sm = messaging.SubMaster(['carControl', 'carState', 'liveLocationKalman'], poll='liveLocationKalman') + sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman'], poll='liveLocationKalman') params = Params() with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP: