brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Your Name
2024-04-27 03:26:25 -05:00
parent 90b100e98a
commit 9bb33c11ac
20 changed files with 1541 additions and 2670 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
selfdrive/locationd/SConscript Normal file
View File

@@ -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)

18
selfdrive/locationd/calibrationd.py
View File

@@ -10,7 +10,7 @@ import gc
import os import os
import capnp import capnp
import numpy as np import numpy as np
from typing import List, NoReturn, Optional from typing import NoReturn
from cereal import log from cereal import log
import cereal.messaging as messaging import cereal.messaging as messaging
@@ -89,7 +89,7 @@ class Calibrator:
valid_blocks: int = 0, valid_blocks: int = 0,
wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT, wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT,
height_init: np.ndarray = HEIGHT_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(): if not np.isfinite(rpy_init).all():
self.rpy = RPY_INIT.copy() self.rpy = RPY_INIT.copy()
else: else:
@@ -125,7 +125,7 @@ class Calibrator:
self.old_rpy = smooth_from self.old_rpy = smooth_from
self.old_rpy_weight = 1.0 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 # exclude current block_idx from validity window
before_current = list(range(self.block_idx)) before_current = list(range(self.block_idx))
after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks)) after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks))
@@ -175,12 +175,12 @@ class Calibrator:
else: else:
return self.rpy return self.rpy
def handle_cam_odom(self, trans: List[float], def handle_cam_odom(self, trans: list[float],
rot: List[float], rot: list[float],
wide_from_device_euler: List[float], wide_from_device_euler: list[float],
trans_std: List[float], trans_std: list[float],
road_transform_trans: List[float], road_transform_trans: list[float],
road_transform_trans_std: List[float]) -> Optional[np.ndarray]: road_transform_trans_std: list[float]) -> np.ndarray | None:
self.old_rpy_weight = max(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES) 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)) straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))

10
selfdrive/locationd/helpers.py
View File

@@ -1,5 +1,5 @@
import numpy as np import numpy as np
from typing import List, Optional, Tuple, Any from typing import Any
from cereal import log from cereal import log
@@ -12,7 +12,7 @@ class NPQueue:
def __len__(self) -> int: def __len__(self) -> int:
return len(self.arr) return len(self.arr)
def append(self, pt: List[float]) -> None: def append(self, pt: list[float]) -> None:
if len(self.arr) < self.maxlen: if len(self.arr) < self.maxlen:
self.arr = np.append(self.arr, [pt], axis=0) self.arr = np.append(self.arr, [pt], axis=0)
else: else:
@@ -21,7 +21,7 @@ class NPQueue:
class PointBuckets: 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.x_bounds = x_bounds
self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in 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)) 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: def add_point(self, x: float, y: float, bucket_val: float) -> None:
raise NotImplementedError 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()]) points = np.vstack([x.arr for x in self.buckets.values()])
if num_points is None: if num_points is None:
return points return points
return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)] 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: for point in points:
self.add_point(*point) self.add_point(*point)

BIN
selfdrive/locationd/locationd
View File

Binary file not shown.

750
selfdrive/locationd/locationd.cc Normal file
View File

@@ -0,0 +1,750 @@
#include "selfdrive/locationd/locationd.h"
#include <sys/time.h>
#include <sys/resource.h>
#include <algorithm>
#include <cmath>
#include <vector>
using namespace EKFS;
using namespace Eigen;
ExitHandler do_exit;
const double ACCEL_SANITY_CHECK = 100.0; // m/s^2
const double ROTATION_SANITY_CHECK = 10.0; // rad/s
const double TRANS_SANITY_CHECK = 200.0; // m/s
const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
const double ALTITUDE_SANITY_CHECK = 10000; // m
const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
const double VALID_TIME_SINCE_RESET = 1.0; // s
const double VALID_POS_STD = 50.0; // m
const double MAX_RESET_TRACKER = 5.0;
const double SANE_GPS_UNCERTAINTY = 1500.0; // m
const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker
const double RESET_TRACKER_DECAY = 0.99995;
const double DECAY = 0.9993; // ~10 secs to resume after a bad input
const double MAX_FILTER_REWIND_TIME = 0.8; // s
const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30;
// TODO: GPS sensor time offsets are empirically calculated
// They should be replaced with synced time from a real clock
const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
const float GPS_POS_STD_THRESHOLD = 50.0;
const float GPS_VEL_STD_THRESHOLD = 5.0;
const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
const float GPS_POS_STD_RESET_THRESHOLD = 2.0;
const float GPS_VEL_STD_RESET_THRESHOLD = 0.5;
const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
const int GPS_ORIENTATION_ERROR_RESET_CNT = 3;
const bool DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0";
static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
VectorXd res(floatlist.size());
for (int i = 0; i < floatlist.size(); i++) {
res[i] = floatlist[i];
}
return res;
}
static Vector4d quat2vector(const Quaterniond& quat) {
return Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
}
static Quaterniond vector2quat(const VectorXd& vec) {
return Quaterniond(vec(0), vec(1), vec(2), vec(3));
}
static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) {
meas.setValue(kj::arrayPtr(val.data(), val.size()));
meas.setStd(kj::arrayPtr(std.data(), std.size()));
meas.setValid(valid);
}
static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) {
// To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix
return ((rot_matrix * cov_in) * rot_matrix.transpose());
}
static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) {
// Stds cannot be rotated like values, only covariances can be rotated
return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt();
}
Localizer::Localizer(LocalizerGnssSource gnss_source) {
this->kf = std::make_unique<LiveKalman>();
this->reset_kalman();
this->calib = Vector3d(0.0, 0.0, 0.0);
this->device_from_calib = MatrixXdr::Identity(3, 3);
this->calib_from_device = MatrixXdr::Identity(3, 3);
for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) {
this->posenet_stds.push_back(10.0);
}
VectorXd ecef_pos = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
this->configure_gnss_source(gnss_source);
}
void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
VectorXd predicted_state = this->kf->get_x();
MatrixXdr predicted_cov = this->kf->get_P();
VectorXd predicted_std = predicted_cov.diagonal().array().sqrt();
VectorXd fix_ecef = predicted_state.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
VectorXd fix_ecef_std = predicted_std.segment<STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START);
VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START);
VectorXd fix_pos_geo_vec = this->get_position_geodetic();
VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt();
VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
MatrixXdr vel_angular_cov = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START);
VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt();
VectorXd vel_device = device_from_ecef * vel_ecef;
VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
condensed_cov.topLeftCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
condensed_cov.topRightCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
condensed_cov.bottomRightCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
condensed_cov.bottomLeftCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size());
H_input << device_from_ecef_eul, vel_ecef;
MatrixXdr HH = this->kf->H(H_input);
MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose();
VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt();
VectorXd vel_calib = this->calib_from_device * vel_device;
VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt();
VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt();
VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
VectorXd nextfix_ecef = fix_ecef + vel_ecef;
VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
VectorXd accDevice = predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
VectorXd accDeviceErr = predicted_std.segment<STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START);
VectorXd angVelocityDevice = predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
VectorXd angVelocityDeviceErr = predicted_std.segment<STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START);
Vector3d nans = Vector3d(NAN, NAN, NAN);
// TODO fill in NED and Calibrated stds
// write measurements to msg
init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode);
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode);
init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated);
if (DEBUG) {
init_measurement(fix.initFilterState(), predicted_state, predicted_std, true);
}
double old_mean = 0.0, new_mean = 0.0;
int i = 0;
for (double x : this->posenet_stds) {
if (i < POSENET_STD_HIST_HALF) {
old_mean += x;
} else {
new_mean += x;
}
i++;
}
old_mean /= POSENET_STD_HIST_HALF;
new_mean /= POSENET_STD_HIST_HALF;
// experimentally found these values, no false positives in 20k minutes of driving
bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0);
fix.setPosenetOK(!(std_spike && this->car_speed > 5.0));
fix.setDeviceStable(!this->device_fell);
fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER);
fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff);
this->device_fell = false;
//fix.setGpsWeek(this->time.week);
//fix.setGpsTimeOfWeek(this->time.tow);
fix.setUnixTimestampMillis(this->unix_timestamp_millis);
double time_since_reset = this->kf->get_filter_time() - this->last_reset_time;
fix.setTimeSinceReset(time_since_reset);
if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) {
fix.setStatus(cereal::LiveLocationKalman::Status::VALID);
} else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) {
fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED);
} else {
fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED);
}
}
VectorXd Localizer::get_position_geodetic() {
VectorXd fix_ecef = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef);
return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt);
}
VectorXd Localizer::get_state() {
return this->kf->get_x();
}
VectorXd Localizer::get_stdev() {
return this->kf->get_P().diagonal().array().sqrt();
}
bool Localizer::are_inputs_ok() {
return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid;
}
void Localizer::observation_timings_invalid_reset(){
this->observation_timings_invalid = false;
}
void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) {
// TODO does not yet account for double sensor readings in the log
// Ignore empty readings (e.g. in case the magnetometer had no data ready)
if (log.getTimestamp() == 0) {
return;
}
double sensor_time = 1e-9 * log.getTimestamp();
// sensor time and log time should be close
if (std::abs(current_time - sensor_time) > 0.1) {
LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
this->observation_timings_invalid = true;
return;
} else if (!this->is_timestamp_valid(sensor_time)) {
this->observation_timings_invalid = true;
return;
}
// TODO: handle messages from two IMUs at the same time
if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
return;
}
// Gyro Uncalibrated
if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
auto v = log.getGyroUncalibrated().getV();
auto meas = Vector3d(-v[2], -v[1], -v[0]);
VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]);
float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1];
bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold;
if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
this->observation_values_invalid["gyroscope"] *= DECAY;
} else {
this->observation_values_invalid["gyroscope"] += 1.0;
}
}
// Accelerometer
if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) {
auto v = log.getAcceleration().getV();
// TODO: reduce false positives and re-enable this check
// check if device fell, estimate 10 for g
// 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
// this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
auto meas = Vector3d(-v[2], -v[1], -v[0]);
if (meas.norm() < ACCEL_SANITY_CHECK) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
this->observation_values_invalid["accelerometer"] *= DECAY;
} else {
this->observation_values_invalid["accelerometer"] += 1.0;
}
}
}
void Localizer::input_fake_gps_observations(double current_time) {
// This is done to make sure that the error estimate of the position does not blow up
// when the filter is in no-gps mode
// Steps : first predict -> observe current obs with reasonable STD
this->kf->predict(current_time);
VectorXd current_x = this->kf->get_x();
VectorXd ecef_pos = current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov();
const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov();
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
//this->gps_valid = false;
this->determine_gps_mode(current_time);
return;
}
double sensor_time = current_time - sensor_time_offset;
// Process message
//this->gps_valid = true;
this->gps_mode = true;
Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
this->converter = std::make_unique<LocalCoord>(geodetic);
VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
this->unix_timestamp_millis = log.getUnixTimestampMillis();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef);
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg()));
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
for (int i = 0; i < orientation_error.size(); i++) {
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
if (orientation_error(i) < 0.0) {
orientation_error(i) += 2.0 * M_PI;
}
orientation_error(i) -= M_PI;
}
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
} else if (gps_est_error > 100.0) {
LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
}
this->last_gps_msg = sensor_time;
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
this->determine_gps_mode(current_time);
return;
}
double sensor_time = log.getMeasTime() * 1e-9;
sensor_time -= this->gps_time_offset;
auto ecef_pos_v = log.getPositionECEF().getValue();
VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]);
// indexed at 0 cause all std values are the same MAE
auto ecef_pos_std = log.getPositionECEF().getStd()[0];
MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal();
auto ecef_vel_v = log.getVelocityECEF().getValue();
VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
// indexed at 0 cause all std values are the same MAE
auto ecef_vel_std = log.getVelocityECEF().getStd()[0];
MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef);
LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]};
VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector();
double bearing_rad = atan2(ned_vel[1], ned_vel[0]);
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad);
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
for (int i = 0; i < orientation_error.size(); i++) {
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
if (orientation_error(i) < 0.0) {
orientation_error(i) += 2.0 * M_PI;
}
orientation_error(i) -= M_PI;
}
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
this->determine_gps_mode(current_time);
return;
}
// prevent jumping gnss measurements (covered lots, standstill...)
bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD;
orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD;
orientation_reset &= !this->standstill;
if (orientation_reset) {
this->orientation_reset_count++;
} else {
this->orientation_reset_count = 0;
}
if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
// always reset on first gps message and if the location is off but the accuracy is high
LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
} else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) {
LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
this->orientation_reset_count = 0;
}
this->gps_mode = true;
this->last_gps_msg = sensor_time;
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) {
this->car_speed = std::abs(log.getVEgo());
this->standstill = log.getStandstill();
if (this->standstill) {
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) });
}
}
void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) {
VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
if (!this->is_timestamp_valid(current_time)) {
this->observation_timings_invalid = true;
return;
}
if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
this->posenet_stds.pop_front();
this->posenet_stds.push_back(trans_calib_std[0]);
// Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise
trans_calib_std *= 10.0;
rot_calib_std *= 10.0;
MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal();
MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal();
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
{ rot_device }, { rot_device_cov });
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
{ trans_device }, { trans_device_cov });
this->observation_values_invalid["cameraOdometry"] *= DECAY;
this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]);
}
void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
if (!this->is_timestamp_valid(current_time)) {
this->observation_timings_invalid = true;
return;
}
if (log.getRpyCalib().size() > 0) {
auto live_calib = floatlist2vector(log.getRpyCalib());
if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
this->observation_values_invalid["liveCalibration"] += 1.0;
return;
}
this->calib = live_calib;
this->device_from_calib = euler2rot(this->calib);
this->calib_from_device = this->device_from_calib.transpose();
this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED;
this->observation_values_invalid["liveCalibration"] *= DECAY;
}
}
void Localizer::reset_kalman(double current_time) {
const VectorXd &init_x = this->kf->get_initial_x();
const MatrixXdr &init_P = this->kf->get_initial_P();
this->reset_kalman(current_time, init_x, init_P);
}
void Localizer::finite_check(double current_time) {
bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all();
if (!all_finite) {
LOGE("Non-finite values detected, kalman reset");
this->reset_kalman(current_time);
}
}
void Localizer::time_check(double current_time) {
if (std::isnan(this->last_reset_time)) {
this->last_reset_time = current_time;
}
if (std::isnan(this->first_valid_log_time)) {
this->first_valid_log_time = current_time;
}
double filter_time = this->kf->get_filter_time();
bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10);
if (big_time_gap) {
LOGE("Time gap of over 10s detected, kalman reset");
this->reset_kalman(current_time);
}
}
void Localizer::update_reset_tracker() {
// reset tracker is tuned to trigger when over 1reset/10s over 2min period
if (this->is_gps_ok()) {
this->reset_tracker *= RESET_TRACKER_DECAY;
} else {
this->reset_tracker = 0.0;
}
}
void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) {
// too nonlinear to init on completely wrong
VectorXd current_x = this->kf->get_x();
MatrixXdr current_P = this->kf->get_P();
MatrixXdr init_P = this->kf->get_initial_P();
const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P();
int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
current_x.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START) = init_vel;
current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) = init_pos;
init_P.block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal();
init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START,
STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
this->reset_kalman(current_time, current_x, init_P);
}
void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) {
this->kf->init_state(init_x, init_P, current_time);
this->last_reset_time = current_time;
this->reset_tracker += 1.0;
}
void Localizer::handle_msg_bytes(const char *data, const size_t size) {
AlignedBuffer aligned_buf;
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size));
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
this->handle_msg(event);
}
void Localizer::handle_msg(const cereal::Event::Reader& log) {
double t = log.getLogMonoTime() * 1e-9;
this->time_check(t);
if (log.isAccelerometer()) {
this->handle_sensor(t, log.getAccelerometer());
} else if (log.isGyroscope()) {
this->handle_sensor(t, log.getGyroscope());
} else if (log.isGpsLocation()) {
this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
} else if (log.isGpsLocationExternal()) {
this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
//} else if (log.isGnssMeasurements()) {
// this->handle_gnss(t, log.getGnssMeasurements());
} else if (log.isCarState()) {
this->handle_car_state(t, log.getCarState());
} else if (log.isCameraOdometry()) {
this->handle_cam_odo(t, log.getCameraOdometry());
} else if (log.isLiveCalibration()) {
this->handle_live_calib(t, log.getLiveCalibration());
}
this->finite_check();
this->update_reset_tracker();
}
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK,
bool sensorsOK, bool gpsOK, bool msgValid) {
cereal::Event::Builder evt = msg_builder.initEvent();
evt.setValid(msgValid);
cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
this->build_live_location(liveLoc);
liveLoc.setSensorsOK(sensorsOK);
liveLoc.setGpsOK(gpsOK);
liveLoc.setInputsOK(inputsOK);
return msg_builder.toBytes();
}
bool Localizer::is_gps_ok() {
return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0;
}
bool Localizer::critical_services_valid(const std::map<std::string, double> &critical_services) {
for (auto &kv : critical_services){
if (kv.second >= INPUT_INVALID_THRESHOLD){
return false;
}
}
return true;
}
bool Localizer::is_timestamp_valid(double current_time) {
double filter_time = this->kf->get_filter_time();
if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) {
LOGE("Observation timestamp is older than the max rewind threshold of the filter");
return false;
}
return true;
}
void Localizer::determine_gps_mode(double current_time) {
// 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode
// 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs
// 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is
VectorXd current_pos_std = this->kf->get_P().block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt();
if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){
if (this->gps_mode){
this->gps_mode = false;
this->reset_kalman(current_time);
} else {
this->input_fake_gps_observations(current_time);
}
}
}
void Localizer::configure_gnss_source(const LocalizerGnssSource &source) {
this->gnss_source = source;
if (source == LocalizerGnssSource::UBLOX) {
this->gps_std_factor = 10.0;
this->gps_variance_factor = 1.0;
this->gps_vertical_variance_factor = 1.0;
this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET;
} else {
this->gps_std_factor = 2.0;
this->gps_variance_factor = 0.0;
this->gps_vertical_variance_factor = 3.0;
this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET;
}
}
int Localizer::locationd_thread() {
Params params;
LocalizerGnssSource source;
const char* gps_location_socket;
if (params.getBool("UbloxAvailable")) {
source = LocalizerGnssSource::UBLOX;
gps_location_socket = "gpsLocationExternal";
} else {
source = LocalizerGnssSource::QCOM;
gps_location_socket = "gpsLocation";
}
this->configure_gnss_source(source);
const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
"carState", "accelerometer", "gyroscope"};
SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
PubMaster pm({"liveLocationKalman"});
uint64_t cnt = 0;
bool filterInitialized = false;
const std::vector<std::string> critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"};
for (std::string service : critical_input_services) {
this->observation_values_invalid.insert({service, 0.0});
}
while (!do_exit) {
sm.update();
if (filterInitialized){
this->observation_timings_invalid_reset();
for (const char* service : service_list) {
if (sm.updated(service) && sm.valid(service)){
const cereal::Event::Reader log = sm[service];
this->handle_msg(log);
}
}
} else {
filterInitialized = sm.allAliveAndValid();
}
const char* trigger_msg = "cameraOdometry";
if (sm.updated(trigger_msg)) {
bool inputsOK = sm.allValid() && this->are_inputs_ok();
bool gpsOK = this->is_gps_ok();
bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
// Log time to first fix
if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) {
this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time);
}
MessageBuilder msg_builder;
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
pm.send("liveLocationKalman", bytes.begin(), bytes.size());
if (cnt % 1200 == 0 && gpsOK) { // once a minute
VectorXd posGeo = this->get_position_geodetic();
std::string lastGPSPosJSON = util::string_format(
"{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
params.putNonBlocking("LastGPSPosition", lastGPSPosJSON);
}
cnt++;
}
}
return 0;
}
int main() {
util::set_realtime_priority(5);
Localizer localizer;
return localizer.locationd_thread();
}

100
selfdrive/locationd/locationd.h Normal file
View File

@@ -0,0 +1,100 @@
#pragma once
#include <eigen3/Eigen/Dense>
#include <deque>
#include <fstream>
#include <memory>
#include <map>
#include <string>
#include "cereal/messaging/messaging.h"
#include "common/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<std::string, double> &critical_services);
bool is_timestamp_valid(double current_time);
void determine_gps_mode(double current_time);
bool are_inputs_ok();
void observation_timings_invalid_reset();
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
void build_live_location(cereal::LiveLocationKalman::Builder& fix);
Eigen::VectorXd get_position_geodetic();
Eigen::VectorXd get_state();
Eigen::VectorXd get_stdev();
void handle_msg_bytes(const char *data, const size_t size);
void handle_msg(const cereal::Event::Reader& log);
void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log);
void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset);
void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log);
void handle_car_state(double current_time, const cereal::CarState::Reader& log);
void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log);
void input_fake_gps_observations(double current_time);
private:
std::unique_ptr<LiveKalman> kf;
Eigen::VectorXd calib;
MatrixXdr device_from_calib;
MatrixXdr calib_from_device;
bool calibrated = false;
double car_speed = 0.0;
double last_reset_time = NAN;
std::deque<double> posenet_stds;
std::unique_ptr<LocalCoord> converter;
int64_t unix_timestamp_millis = 0;
double reset_tracker = 0.0;
bool device_fell = false;
bool gps_mode = false;
double first_valid_log_time = NAN;
double ttff = NAN;
double last_gps_msg = 0;
LocalizerGnssSource gnss_source;
bool observation_timings_invalid = false;
std::map<std::string, double> observation_values_invalid;
bool standstill = true;
int32_t orientation_reset_count = 0;
float gps_std_factor;
float gps_variance_factor;
float gps_vertical_variance_factor;
double gps_time_offset;
Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std
void configure_gnss_source(const LocalizerGnssSource &source);
};

4
selfdrive/locationd/models/car_kf.py
View File

@@ -1,7 +1,7 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
import math import math
import sys import sys
from typing import Any, Dict from typing import Any
import numpy as np import numpy as np
@@ -70,7 +70,7 @@ class CarKalman(KalmanFilter):
]) ])
P_initial = Q.copy() 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.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2), ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2), ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),

661
selfdrive/locationd/models/generated/car.cpp
View File

@@ -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 <eigen3/Eigen/Dense>
#include <iostream>
typedef Eigen::Matrix<double, DIM, DIM, Eigen::RowMajor> DDM;
typedef Eigen::Matrix<double, EDIM, EDIM, Eigen::RowMajor> EEM;
typedef Eigen::Matrix<double, DIM, EDIM, Eigen::RowMajor> DEM;
void predict(double *in_x, double *in_P, double *in_Q, double dt) {
typedef Eigen::Matrix<double, MEDIM, MEDIM, Eigen::RowMajor> 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 <int ZDIM, int EADIM, bool MAHA_TEST>
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<double, ZDIM, ZDIM, Eigen::RowMajor> ZZM;
typedef Eigen::Matrix<double, ZDIM, DIM, Eigen::RowMajor> ZDM;
typedef Eigen::Matrix<double, Eigen::Dynamic, EDIM, Eigen::RowMajor> XEM;
//typedef Eigen::Matrix<double, EDIM, ZDIM, Eigen::RowMajor> EZM;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> X1M;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> 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<double, ZDIM, 1> 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<double, ZDIM, 1> pre_y(in_hx); pre_y = z - pre_y;
X1M y; XXM H; XXM R;
if (Hea_fun){
typedef Eigen::Matrix<double, ZDIM, EADIM, Eigen::RowMajor> 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<double, EDIM, EDIM>::Identity() - (KT.transpose() * H_err);
// update state by injecting dx
Eigen::Matrix<double, EDIM, 1> 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<double, DIM, 1> 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)

BIN
selfdrive/locationd/models/generated/libcar.so
View File

Binary file not shown.

BIN
selfdrive/locationd/models/generated/liblive.so
View File

Binary file not shown.

1987
selfdrive/locationd/models/generated/live.cpp
View File

File diff suppressed because it is too large Load Diff

122
selfdrive/locationd/models/live_kf.cc Normal file
View File

@@ -0,0 +1,122 @@
#include "selfdrive/locationd/models/live_kf.h"
using namespace EKFS;
using namespace Eigen;
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
}
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
}
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
std::vector<Eigen::Map<Eigen::VectorXd>> res;
for (const Eigen::VectorXd &vec : vec_vec) {
res.push_back(get_mapvec(vec));
}
return res;
}
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
std::vector<Eigen::Map<MatrixXdr>> res;
for (const MatrixXdr &mat : mat_vec) {
res.push_back(get_mapmat(mat));
}
return res;
}
LiveKalman::LiveKalman() {
this->dim_state = live_initial_x.rows();
this->dim_state_err = live_initial_P_diag.rows();
this->initial_x = live_initial_x;
this->initial_P = live_initial_P_diag.asDiagonal();
this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal();
this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal();
this->reset_orientation_P = live_reset_orientation_diag.asDiagonal();
this->Q = live_Q_diag.asDiagonal();
for (auto& pair : live_obs_noise_diag) {
this->obs_noise[pair.first] = pair.second.asDiagonal();
}
// init filter
this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
std::vector<int>{3}, std::vector<std::string>(), 0.8);
}
void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
MatrixXdr covs = covs_diag.asDiagonal();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
void LiveKalman::init_state(const VectorXd &state, double filter_time) {
MatrixXdr covs = this->filter->covs();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
VectorXd LiveKalman::get_x() {
return this->filter->state();
}
MatrixXdr LiveKalman::get_P() {
return this->filter->covs();
}
double LiveKalman::get_filter_time() {
return this->filter->get_filter_time();
}
std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
std::vector<MatrixXdr> R;
for (int i = 0; i < n; i++) {
R.push_back(this->obs_noise[kind]);
}
return R;
}
std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
std::optional<Estimate> r;
if (R.size() == 0) {
R = this->get_R(kind, meas.size());
}
r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
return r;
}
void LiveKalman::predict(double t) {
this->filter->predict(t);
}
const Eigen::VectorXd &LiveKalman::get_initial_x() {
return this->initial_x;
}
const MatrixXdr &LiveKalman::get_initial_P() {
return this->initial_P;
}
const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() {
return this->fake_gps_pos_cov;
}
const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() {
return this->fake_gps_vel_cov;
}
const MatrixXdr &LiveKalman::get_reset_orientation_P() {
return this->reset_orientation_P;
}
MatrixXdr LiveKalman::H(const VectorXd &in) {
assert(in.size() == 6);
Matrix<double, 3, 6, Eigen::RowMajor> res;
this->filter->get_extra_routine("H")((double*)in.data(), res.data());
return res;
}

66
selfdrive/locationd/models/live_kf.h Normal file
View File

@@ -0,0 +1,66 @@
#pragma once
#include <string>
#include <cmath>
#include <memory>
#include <unordered_map>
#include <vector>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include "generated/live_kf_constants.h"
#include "rednose/helpers/ekf_sym.h"
#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth)
using namespace EKFS;
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
class LiveKalman {
public:
LiveKalman();
void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
void init_state(const Eigen::VectorXd &state, double filter_time);
Eigen::VectorXd get_x();
MatrixXdr get_P();
double get_filter_time();
std::vector<MatrixXdr> get_R(int kind, int n);
std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
void predict(double t);
const Eigen::VectorXd &get_initial_x();
const MatrixXdr &get_initial_P();
const MatrixXdr &get_fake_gps_pos_cov();
const MatrixXdr &get_fake_gps_vel_cov();
const MatrixXdr &get_reset_orientation_P();
MatrixXdr H(const Eigen::VectorXd &in);
private:
std::string name = "live";
std::shared_ptr<EKFSym> filter;
int dim_state;
int dim_state_err;
Eigen::VectorXd initial_x;
MatrixXdr initial_P;
MatrixXdr fake_gps_pos_cov;
MatrixXdr fake_gps_vel_cov;
MatrixXdr reset_orientation_P;
MatrixXdr Q; // process noise
std::unordered_map<int, MatrixXdr> obs_noise;
};

6
selfdrive/locationd/paramsd.py
View File

@@ -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.selfdrive.locationd.models.constants import GENERATED_DIR
from openpilot.common.swaglog import cloudlog from openpilot.common.swaglog import cloudlog
params_memory = Params("/dev/shm/params")
MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s 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 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 min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
params = params_reader.get("LiveParameters") params = params_reader.get("LiveParameters")
params_memory = Params("/dev/shm/params")
# Check if car model matches # Check if car model matches
if params is not None: if params is not None:
@@ -202,7 +202,7 @@ def main():
bearing = math.degrees(location.calibratedOrientationNED.value[2]) bearing = math.degrees(location.calibratedOrientationNED.value[2])
lat = location.positionGeodetic.value[0] lat = location.positionGeodetic.value[0]
lon = location.positionGeodetic.value[1] 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 x = learner.kf.x
P = np.sqrt(learner.kf.P.diagonal()) P = np.sqrt(learner.kf.P.diagonal())
if not all(map(math.isfinite, x)): if not all(map(math.isfinite, x)):
@@ -244,7 +244,7 @@ def main():
0.2 <= liveParameters.stiffnessFactor <= 5.0, 0.2 <= liveParameters.stiffnessFactor <= 5.0,
min_sr <= liveParameters.steerRatio <= max_sr, 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.valid = True
liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item()) liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item()) liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())

1
selfdrive/locationd/test/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
out/

0
selfdrive/locationd/test/__init__.py Normal file
View File

117
selfdrive/locationd/test/test_calibrationd.py Normal file
View File

@@ -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()

95
selfdrive/locationd/test/test_locationd.py Normal file
View File

@@ -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()

229
selfdrive/locationd/test/test_locationd_scenarios.py Normal file
View File

@@ -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()

8
selfdrive/locationd/torqued.py
View File

@@ -159,8 +159,10 @@ class TorqueEstimator(ParameterEstimator):
def handle_log(self, t, which, msg): def handle_log(self, t, which, msg):
if which == "carControl": if which == "carControl":
self.raw_points["carControl_t"].append(t + self.lag) 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) 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": elif which == "carState":
self.raw_points["carState_t"].append(t + self.lag) self.raw_points["carState_t"].append(t + self.lag)
self.raw_points["vego"].append(msg.vEgo) 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) 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) 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']) 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) 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): 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)) 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) config_realtime_process([0, 1, 2, 3], 5)
pm = messaging.PubMaster(['liveTorqueParameters']) pm = messaging.PubMaster(['liveTorqueParameters'])
sm = messaging.SubMaster(['carControl', 'carState', 'liveLocationKalman'], poll='liveLocationKalman') sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman'], poll='liveLocationKalman')
params = Params() params = Params()
with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP: with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP: