convert locationd to c++ (#20622)
* live_kf to c++
* first locationd code
* Running in process_replay
* locationd handle cam_odo and live_calib
* log event handlers
* working message receiving
* compiling message sending
* correctly sending some messages
* correct receiving and sending
* update ref_commit with some all_alive_and_valid being false, minor fixes
* fix std abs
* linking on device fix
* fix cpu usage test
* generate kf constants and defines
* fix replay test
* replay without acks, cleanup
* operate on bytearray messages
* cleanup
* send msg fix
* small sleep, less flaky test
* remove python locationd
* review feedback
* bump rednose
old-commit-hash: 3420707ad5
commatwo_master
Joost Wooning
4 years ago
committed by
GitHub
parent
19f02898d8
commit
c0ac9bb63c
17 changed files with 735 additions and 478 deletions
@ -1 +1 @@ |
||||
Subproject commit a0eb4d249e4739cae7e4da4c38ab81c4cafa5df9 |
||||
Subproject commit f069de89f6ccd277a557fb14590f8cb0fb069a8a |
||||
@ -0,0 +1,378 @@ |
||||
#include "locationd.h" |
||||
|
||||
using namespace EKFS; |
||||
using namespace Eigen; |
||||
|
||||
ExitHandler do_exit; |
||||
|
||||
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); |
||||
} |
||||
|
||||
Localizer::Localizer() { |
||||
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] }); |
||||
} |
||||
|
||||
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(); |
||||
//fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo)
|
||||
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 device_from_ecef = quat2rot(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose(); |
||||
VectorXd calibrated_orientation_ecef = rot2euler(this->calib_from_device * device_from_ecef); |
||||
|
||||
VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START); |
||||
VectorXd acc_calib_std = ((this->calib_from_device * predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START)) * this->calib_from_device.transpose()).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_err = 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 = ((this->calib_from_device * vel_angular_err) * this->calib_from_device.transpose()).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 = ((this->calib_from_device * vel_device_cov) * this->calib_from_device.transpose()).diagonal().array().sqrt(); |
||||
|
||||
VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef); |
||||
//orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
|
||||
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(); |
||||
//ned_vel_std = self.converter->ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter->ecef2ned(fix_ecef + vel_ecef)
|
||||
|
||||
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); |
||||
|
||||
// write measurements to msg
|
||||
init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, true); |
||||
init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, true); |
||||
init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, true); |
||||
init_measurement(fix.initVelocityNED(), ned_vel, nans, true); |
||||
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, true); |
||||
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated); |
||||
init_measurement(fix.initOrientationNED(), orientation_ned, nans, true); |
||||
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); |
||||
|
||||
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); |
||||
this->device_fell = false; |
||||
|
||||
//fix.setGpsWeek(this->time.week);
|
||||
//fix.setGpsTimeOfWeek(this->time.tow);
|
||||
fix.setUnixTimestampMillis(this->unix_timestamp_millis); |
||||
|
||||
if (fix_ecef_std.norm() < 50.0 && this->calibrated) { |
||||
fix.setStatus(cereal::LiveLocationKalman::Status::VALID); |
||||
} else if (fix_ecef_std.norm() < 50.0) { |
||||
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); |
||||
} |
||||
|
||||
void Localizer::handle_sensors(double current_time, const capnp::List<cereal::SensorEventData, capnp::Kind::STRUCT>::Reader& log) { |
||||
// TODO does not yet account for double sensor readings in the log
|
||||
for (int i = 0; i < log.size(); i++) { |
||||
const cereal::SensorEventData::Reader& sensor_reading = log[i]; |
||||
double sensor_time = 1e-9 * sensor_reading.getTimestamp(); |
||||
// TODO: handle messages from two IMUs at the same time
|
||||
if (sensor_reading.getSource() == cereal::SensorEventData::SensorSource::LSM6DS3) { |
||||
continue; |
||||
} |
||||
|
||||
// Gyro Uncalibrated
|
||||
if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { |
||||
this->gyro_counter++; |
||||
if (this->gyro_counter % SENSOR_DECIMATION == 0) { |
||||
auto v = sensor_reading.getGyroUncalibrated().getV(); |
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) }); |
||||
} |
||||
} |
||||
|
||||
// Accelerometer
|
||||
if (sensor_reading.getSensor() == SENSOR_ACCELEROMETER && sensor_reading.getType() == SENSOR_TYPE_ACCELEROMETER) { |
||||
auto v = sensor_reading.getAcceleration().getV(); |
||||
|
||||
// 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; |
||||
|
||||
this->acc_counter++; |
||||
if (this->acc_counter % SENSOR_DECIMATION == 0) { |
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { Vector3d(-v[2], -v[1], -v[0]) }); |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) { |
||||
// ignore the message if the fix is invalid
|
||||
if (log.getFlags() % 2 == 0) { |
||||
return; |
||||
} |
||||
|
||||
this->last_gps_fix = current_time; |
||||
|
||||
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; |
||||
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(3.0 * log.getVerticalAccuracy(), 2)).asDiagonal(); |
||||
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy(), 2)).asDiagonal(); |
||||
|
||||
this->unix_timestamp_millis = log.getTimestamp(); |
||||
double gps_est_error = (this->kf->get_x().head(3) - 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); |
||||
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat }); |
||||
} else if (gps_est_error > 50.0) { |
||||
LOGE("Locationd vs ubloxLocation position difference too large, kalman reset"); |
||||
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos); |
||||
} |
||||
|
||||
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_car_state(double current_time, const cereal::CarState::Reader& log) { |
||||
this->speed_counter++; |
||||
|
||||
if (this->speed_counter % SENSOR_DECIMATION == 0) { |
||||
this->kf->predict_and_observe(current_time, OBSERVATION_ODOMETRIC_SPEED, { (VectorXd(1) << log.getVEgo()).finished() }); |
||||
this->car_speed = std::abs(log.getVEgo()); |
||||
if (log.getVEgo() == 0.0) { // TODO probably never really 0.0
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) }); |
||||
} |
||||
} |
||||
} |
||||
|
||||
void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) { |
||||
this->cam_counter++; |
||||
|
||||
if (this->cam_counter % VISION_DECIMATION == 0) { |
||||
VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); |
||||
VectorXd rot_device_std = (this->device_from_calib * floatlist2vector(log.getRotStd())) * 10.0; |
||||
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION, |
||||
{ (VectorXd(rot_device.rows() + rot_device_std.rows()) << rot_device, rot_device_std).finished() }); |
||||
|
||||
VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); |
||||
VectorXd trans_device_std = this->device_from_calib * floatlist2vector(log.getTransStd()); |
||||
|
||||
this->posenet_stds.pop_front(); |
||||
this->posenet_stds.push_back(trans_device_std[0]); |
||||
|
||||
trans_device_std *= 10.0; |
||||
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION, |
||||
{ (VectorXd(trans_device.rows() + trans_device_std.rows()) << trans_device, trans_device_std).finished() }); |
||||
} |
||||
} |
||||
|
||||
void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { |
||||
if (log.getRpyCalib().size() > 0) { |
||||
this->calib = floatlist2vector(log.getRpyCalib()); |
||||
this->device_from_calib = euler2rot(this->calib); |
||||
this->calib_from_device = this->device_from_calib.transpose(); |
||||
this->calibrated = log.getCalStatus() == 1; |
||||
} |
||||
} |
||||
|
||||
void Localizer::reset_kalman(double current_time) { |
||||
VectorXd init_x = this->kf->get_initial_x(); |
||||
this->reset_kalman(current_time, init_x.segment<4>(3), init_x.head(3)); |
||||
} |
||||
|
||||
void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos) { |
||||
// too nonlinear to init on completely wrong
|
||||
VectorXd init_x = this->kf->get_initial_x(); |
||||
MatrixXdr init_P = this->kf->get_initial_P(); |
||||
init_x.segment<4>(3) = init_orient; |
||||
init_x.head(3) = init_pos; |
||||
|
||||
this->kf->init_state(init_x, init_P, current_time); |
||||
|
||||
this->gyro_counter = 0; |
||||
this->acc_counter = 0; |
||||
this->speed_counter = 0; |
||||
this->cam_counter = 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; |
||||
if (log.isSensorEvents()) { |
||||
this->handle_sensors(t, log.getSensorEvents()); |
||||
} else if (log.isGpsLocationExternal()) { |
||||
this->handle_gps(t, log.getGpsLocationExternal()); |
||||
} 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()); |
||||
} |
||||
} |
||||
|
||||
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, |
||||
bool inputsOK, bool sensorsOK, bool gpsOK) |
||||
{ |
||||
cereal::Event::Builder evt = msg_builder.initEvent(); |
||||
evt.setLogMonoTime(logMonoTime); |
||||
cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman(); |
||||
this->build_live_location(liveLoc); |
||||
liveLoc.setInputsOK(inputsOK); |
||||
liveLoc.setSensorsOK(sensorsOK); |
||||
liveLoc.setGpsOK(gpsOK); |
||||
return msg_builder.toBytes(); |
||||
} |
||||
|
||||
int Localizer::locationd_thread() { |
||||
const std::initializer_list<const char *> service_list = |
||||
{ "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" }; |
||||
SubMaster sm(service_list, nullptr, { "gpsLocationExternal" }); |
||||
PubMaster pm({ "liveLocationKalman" }); |
||||
|
||||
Params params; |
||||
|
||||
while (!do_exit) { |
||||
sm.update(); |
||||
for (const char* service : service_list) { |
||||
if (sm.updated(service) && sm.valid(service)) { |
||||
const cereal::Event::Reader log = sm[service]; |
||||
this->handle_msg(log); |
||||
} |
||||
} |
||||
|
||||
if (sm.updated("cameraOdometry")) { |
||||
uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime(); |
||||
bool inputsOK = sm.allAliveAndValid(); |
||||
bool sensorsOK = sm.alive("sensorEvents") && sm.valid("sensorEvents"); |
||||
bool gpsOK = (logMonoTime / 1e9) - this->last_gps_fix < 1.0; |
||||
|
||||
MessageBuilder msg_builder; |
||||
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK); |
||||
pm.send("liveLocationKalman", bytes.begin(), bytes.size()); |
||||
|
||||
if (sm.frame % 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.put("LastGPSPosition", lastGPSPosJSON); // TODO write async
|
||||
} |
||||
} |
||||
} |
||||
return 0; |
||||
} |
||||
|
||||
int main() { |
||||
Localizer localizer; |
||||
return localizer.locationd_thread(); |
||||
} |
||||
@ -0,0 +1,69 @@ |
||||
#pragma once |
||||
|
||||
#include <fstream> |
||||
#include <string> |
||||
#include <memory> |
||||
|
||||
#include <eigen3/Eigen/Dense> |
||||
|
||||
#include "messaging.hpp" |
||||
#include "common/params.h" |
||||
#include "common/util.h" |
||||
#include "common/swaglog.h" |
||||
#include "common/timing.h" |
||||
#include "common/transformations/coordinates.hpp" |
||||
#include "common/transformations/orientation.hpp" |
||||
#include "selfdrive/sensord/sensors/constants.hpp" |
||||
|
||||
#include "models/live_kf.h" |
||||
|
||||
#define VISION_DECIMATION 2 |
||||
#define SENSOR_DECIMATION 10 |
||||
|
||||
#define POSENET_STD_HIST_HALF 20 |
||||
|
||||
class Localizer { |
||||
public: |
||||
Localizer(); |
||||
|
||||
int locationd_thread(); |
||||
|
||||
void reset_kalman(double current_time = NAN); |
||||
void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos); |
||||
|
||||
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, |
||||
bool inputsOK, bool sensorsOK, bool gpsOK); |
||||
void build_live_location(cereal::LiveLocationKalman::Builder& fix); |
||||
|
||||
Eigen::VectorXd get_position_geodetic(); |
||||
|
||||
void handle_msg_bytes(const char *data, const size_t size); |
||||
void handle_msg(const cereal::Event::Reader& log); |
||||
void handle_sensors(double current_time, const capnp::List<cereal::SensorEventData, capnp::Kind::STRUCT>::Reader& log); |
||||
void handle_gps(double current_time, const cereal::GpsLocationData::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); |
||||
|
||||
private: |
||||
std::unique_ptr<LiveKalman> kf; |
||||
|
||||
Eigen::VectorXd calib; |
||||
MatrixXdr device_from_calib; |
||||
MatrixXdr calib_from_device; |
||||
bool calibrated = false; |
||||
|
||||
int car_speed = 0; |
||||
std::deque<double> posenet_stds; |
||||
|
||||
std::unique_ptr<LocalCoord> converter; |
||||
|
||||
int64_t unix_timestamp_millis = 0; |
||||
double last_gps_fix = 0; |
||||
bool device_fell = false; |
||||
|
||||
int gyro_counter = 0; |
||||
int acc_counter = 0; |
||||
int speed_counter = 0; |
||||
int cam_counter = 0; |
||||
}; |
||||
@ -1,353 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
import json |
||||
import gc |
||||
import numpy as np |
||||
import sympy as sp |
||||
import cereal.messaging as messaging |
||||
from cereal import log |
||||
from common.params import Params |
||||
import common.transformations.coordinates as coord |
||||
from common.transformations.orientation import ecef_euler_from_ned, \ |
||||
euler_from_quat, \ |
||||
ned_euler_from_ecef, \ |
||||
quat_from_euler, euler_from_rot, \ |
||||
rot_from_quat, rot_from_euler |
||||
from rednose.helpers import KalmanError |
||||
from selfdrive.locationd.models.live_kf import LiveKalman, States, ObservationKind |
||||
from selfdrive.locationd.models.constants import GENERATED_DIR |
||||
from selfdrive.swaglog import cloudlog |
||||
|
||||
#from datetime import datetime |
||||
#from laika.gps_time import GPSTime |
||||
|
||||
from sympy.utilities.lambdify import lambdify |
||||
from rednose.helpers.sympy_helpers import euler_rotate |
||||
|
||||
SensorSource = log.SensorEventData.SensorSource |
||||
|
||||
|
||||
VISION_DECIMATION = 2 |
||||
SENSOR_DECIMATION = 10 |
||||
POSENET_STD_HIST = 40 |
||||
|
||||
|
||||
def to_float(arr): |
||||
return [float(arr[0]), float(arr[1]), float(arr[2])] |
||||
|
||||
|
||||
def get_H(): |
||||
# this returns a function to eval the jacobian |
||||
# of the observation function of the local vel |
||||
roll = sp.Symbol('roll') |
||||
pitch = sp.Symbol('pitch') |
||||
yaw = sp.Symbol('yaw') |
||||
vx = sp.Symbol('vx') |
||||
vy = sp.Symbol('vy') |
||||
vz = sp.Symbol('vz') |
||||
|
||||
h = euler_rotate(roll, pitch, yaw).T*(sp.Matrix([vx, vy, vz])) |
||||
H = h.jacobian(sp.Matrix([roll, pitch, yaw, vx, vy, vz])) |
||||
H_f = lambdify([roll, pitch, yaw, vx, vy, vz], H) |
||||
return H_f |
||||
|
||||
|
||||
class Localizer(): |
||||
def __init__(self, disabled_logs=None, dog=None): |
||||
if disabled_logs is None: |
||||
disabled_logs = [] |
||||
|
||||
self.kf = LiveKalman(GENERATED_DIR) |
||||
self.reset_kalman() |
||||
self.max_age = .1 # seconds |
||||
self.disabled_logs = disabled_logs |
||||
self.calib = np.zeros(3) |
||||
self.device_from_calib = np.eye(3) |
||||
self.calib_from_device = np.eye(3) |
||||
self.calibrated = False |
||||
self.H = get_H() |
||||
|
||||
self.posenet_invalid_count = 0 |
||||
self.posenet_speed = 0 |
||||
self.car_speed = 0 |
||||
self.posenet_stds = 10*np.ones((POSENET_STD_HIST)) |
||||
|
||||
self.converter = coord.LocalCoord.from_ecef(self.kf.x[States.ECEF_POS]) |
||||
|
||||
self.unix_timestamp_millis = 0 |
||||
self.last_gps_fix = 0 |
||||
self.device_fell = False |
||||
|
||||
@staticmethod |
||||
def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov, calibrated): |
||||
predicted_std = np.sqrt(np.diagonal(predicted_cov)) |
||||
|
||||
fix_ecef = predicted_state[States.ECEF_POS] |
||||
fix_ecef_std = predicted_std[States.ECEF_POS_ERR] |
||||
vel_ecef = predicted_state[States.ECEF_VELOCITY] |
||||
vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR] |
||||
fix_pos_geo = coord.ecef2geodetic(fix_ecef) |
||||
#fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo) |
||||
orientation_ecef = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]) |
||||
orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR] |
||||
device_from_ecef = rot_from_quat(predicted_state[States.ECEF_ORIENTATION]).T |
||||
calibrated_orientation_ecef = euler_from_rot(calib_from_device.dot(device_from_ecef)) |
||||
|
||||
acc_calib = calib_from_device.dot(predicted_state[States.ACCELERATION]) |
||||
acc_calib_std = np.sqrt(np.diagonal(calib_from_device.dot( |
||||
predicted_cov[States.ACCELERATION_ERR, States.ACCELERATION_ERR]).dot( |
||||
calib_from_device.T))) |
||||
ang_vel_calib = calib_from_device.dot(predicted_state[States.ANGULAR_VELOCITY]) |
||||
ang_vel_calib_std = np.sqrt(np.diagonal(calib_from_device.dot( |
||||
predicted_cov[States.ANGULAR_VELOCITY_ERR, States.ANGULAR_VELOCITY_ERR]).dot( |
||||
calib_from_device.T))) |
||||
|
||||
vel_device = device_from_ecef.dot(vel_ecef) |
||||
device_from_ecef_eul = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]).T |
||||
idxs = list(range(States.ECEF_ORIENTATION_ERR.start, States.ECEF_ORIENTATION_ERR.stop)) + \ |
||||
list(range(States.ECEF_VELOCITY_ERR.start, States.ECEF_VELOCITY_ERR.stop)) |
||||
condensed_cov = predicted_cov[idxs][:, idxs] |
||||
HH = H(*list(np.concatenate([device_from_ecef_eul, vel_ecef]))) |
||||
vel_device_cov = HH.dot(condensed_cov).dot(HH.T) |
||||
vel_device_std = np.sqrt(np.diagonal(vel_device_cov)) |
||||
|
||||
vel_calib = calib_from_device.dot(vel_device) |
||||
vel_calib_std = np.sqrt(np.diagonal(calib_from_device.dot( |
||||
vel_device_cov).dot(calib_from_device.T))) |
||||
|
||||
orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef) |
||||
#orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned |
||||
ned_vel = converter.ecef2ned(fix_ecef + vel_ecef) - converter.ecef2ned(fix_ecef) |
||||
#ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef) |
||||
|
||||
fix = messaging.log.LiveLocationKalman.new_message() |
||||
|
||||
# write measurements to msg |
||||
measurements = [ |
||||
# measurement field, value, std, valid |
||||
(fix.positionGeodetic, fix_pos_geo, np.nan*np.zeros(3), True), |
||||
(fix.positionECEF, fix_ecef, fix_ecef_std, True), |
||||
(fix.velocityECEF, vel_ecef, vel_ecef_std, True), |
||||
(fix.velocityNED, ned_vel, np.nan*np.zeros(3), True), |
||||
(fix.velocityDevice, vel_device, vel_device_std, True), |
||||
(fix.accelerationDevice, predicted_state[States.ACCELERATION], predicted_std[States.ACCELERATION_ERR], True), |
||||
(fix.orientationECEF, orientation_ecef, orientation_ecef_std, True), |
||||
(fix.calibratedOrientationECEF, calibrated_orientation_ecef, np.nan*np.zeros(3), calibrated), |
||||
(fix.orientationNED, orientation_ned, np.nan*np.zeros(3), True), |
||||
(fix.angularVelocityDevice, predicted_state[States.ANGULAR_VELOCITY], predicted_std[States.ANGULAR_VELOCITY_ERR], True), |
||||
(fix.velocityCalibrated, vel_calib, vel_calib_std, calibrated), |
||||
(fix.angularVelocityCalibrated, ang_vel_calib, ang_vel_calib_std, calibrated), |
||||
(fix.accelerationCalibrated, acc_calib, acc_calib_std, calibrated), |
||||
] |
||||
|
||||
for field, value, std, valid in measurements: |
||||
# TODO: can we write the lists faster? |
||||
field.value = to_float(value) |
||||
field.std = to_float(std) |
||||
field.valid = valid |
||||
|
||||
return fix |
||||
|
||||
def liveLocationMsg(self): |
||||
fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P, self.calibrated) |
||||
# experimentally found these values, no false positives in 20k minutes of driving |
||||
old_mean, new_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST//2]), np.mean(self.posenet_stds[POSENET_STD_HIST//2:]) |
||||
std_spike = new_mean/old_mean > 4 and new_mean > 7 |
||||
|
||||
fix.posenetOK = not (std_spike and self.car_speed > 5) |
||||
fix.deviceStable = not self.device_fell |
||||
self.device_fell = False |
||||
|
||||
#fix.gpsWeek = self.time.week |
||||
#fix.gpsTimeOfWeek = self.time.tow |
||||
fix.unixTimestampMillis = self.unix_timestamp_millis |
||||
|
||||
if np.linalg.norm(fix.positionECEF.std) < 50 and self.calibrated: |
||||
fix.status = 'valid' |
||||
elif np.linalg.norm(fix.positionECEF.std) < 50: |
||||
fix.status = 'uncalibrated' |
||||
else: |
||||
fix.status = 'uninitialized' |
||||
return fix |
||||
|
||||
def update_kalman(self, time, kind, meas, R=None): |
||||
try: |
||||
self.kf.predict_and_observe(time, kind, meas, R) |
||||
except KalmanError: |
||||
cloudlog.error("Error in predict and observe, kalman reset") |
||||
self.reset_kalman() |
||||
|
||||
def handle_gps(self, current_time, log): |
||||
# ignore the message if the fix is invalid |
||||
if log.flags % 2 == 0: |
||||
return |
||||
|
||||
self.last_gps_fix = current_time |
||||
|
||||
self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude]) |
||||
ecef_pos = self.converter.ned2ecef([0, 0, 0]) |
||||
ecef_vel = self.converter.ned2ecef(np.array(log.vNED)) - ecef_pos |
||||
ecef_pos_R = np.diag([(3*log.verticalAccuracy)**2]*3) |
||||
ecef_vel_R = np.diag([(log.speedAccuracy)**2]*3) |
||||
|
||||
#self.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3)) |
||||
self.unix_timestamp_millis = log.timestamp |
||||
gps_est_error = np.sqrt((self.kf.x[0] - ecef_pos[0])**2 + |
||||
(self.kf.x[1] - ecef_pos[1])**2 + |
||||
(self.kf.x[2] - ecef_pos[2])**2) |
||||
|
||||
orientation_ecef = euler_from_quat(self.kf.x[States.ECEF_ORIENTATION]) |
||||
orientation_ned = ned_euler_from_ecef(ecef_pos, orientation_ecef) |
||||
orientation_ned_gps = np.array([0, 0, np.radians(log.bearingDeg)]) |
||||
orientation_error = np.mod(orientation_ned - orientation_ned_gps - np.pi, 2*np.pi) - np.pi |
||||
initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps)) |
||||
if np.linalg.norm(ecef_vel) > 5 and np.linalg.norm(orientation_error) > 1: |
||||
cloudlog.error("Locationd vs ubloxLocation orientation difference too large, kalman reset") |
||||
self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat) |
||||
self.update_kalman(current_time, ObservationKind.ECEF_ORIENTATION_FROM_GPS, initial_pose_ecef_quat) |
||||
elif gps_est_error > 50: |
||||
cloudlog.error("Locationd vs ubloxLocation position difference too large, kalman reset") |
||||
self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat) |
||||
|
||||
self.update_kalman(current_time, ObservationKind.ECEF_POS, ecef_pos, R=ecef_pos_R) |
||||
self.update_kalman(current_time, ObservationKind.ECEF_VEL, ecef_vel, R=ecef_vel_R) |
||||
|
||||
def handle_car_state(self, current_time, log): |
||||
self.speed_counter += 1 |
||||
|
||||
if self.speed_counter % SENSOR_DECIMATION == 0: |
||||
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, [log.vEgo]) |
||||
self.car_speed = abs(log.vEgo) |
||||
if log.vEgo == 0: |
||||
self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0]) |
||||
|
||||
def handle_cam_odo(self, current_time, log): |
||||
self.cam_counter += 1 |
||||
|
||||
if self.cam_counter % VISION_DECIMATION == 0: |
||||
rot_device = self.device_from_calib.dot(log.rot) |
||||
rot_device_std = self.device_from_calib.dot(log.rotStd) |
||||
self.update_kalman(current_time, |
||||
ObservationKind.CAMERA_ODO_ROTATION, |
||||
np.concatenate([rot_device, 10*rot_device_std])) |
||||
trans_device = self.device_from_calib.dot(log.trans) |
||||
trans_device_std = self.device_from_calib.dot(log.transStd) |
||||
self.posenet_speed = np.linalg.norm(trans_device) |
||||
self.posenet_stds[:-1] = self.posenet_stds[1:] |
||||
self.posenet_stds[-1] = trans_device_std[0] |
||||
self.update_kalman(current_time, |
||||
ObservationKind.CAMERA_ODO_TRANSLATION, |
||||
np.concatenate([trans_device, 10*trans_device_std])) |
||||
|
||||
def handle_sensors(self, current_time, log): |
||||
# TODO does not yet account for double sensor readings in the log |
||||
for sensor_reading in log: |
||||
sensor_time = 1e-9 * sensor_reading.timestamp |
||||
# TODO: handle messages from two IMUs at the same time |
||||
if sensor_reading.source == SensorSource.lsm6ds3: |
||||
continue |
||||
|
||||
# Gyro Uncalibrated |
||||
if sensor_reading.sensor == 5 and sensor_reading.type == 16: |
||||
self.gyro_counter += 1 |
||||
if self.gyro_counter % SENSOR_DECIMATION == 0: |
||||
v = sensor_reading.gyroUncalibrated.v |
||||
self.update_kalman(sensor_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]]) |
||||
|
||||
# Accelerometer |
||||
if sensor_reading.sensor == 1 and sensor_reading.type == 1: |
||||
# 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 |
||||
self.device_fell = self.device_fell or (np.linalg.norm(np.array(sensor_reading.acceleration.v) - np.array([10, 0, 0])) > 40) |
||||
|
||||
self.acc_counter += 1 |
||||
if self.acc_counter % SENSOR_DECIMATION == 0: |
||||
v = sensor_reading.acceleration.v |
||||
self.update_kalman(sensor_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]]) |
||||
|
||||
def handle_live_calib(self, current_time, log): |
||||
if len(log.rpyCalib): |
||||
self.calib = log.rpyCalib |
||||
self.device_from_calib = rot_from_euler(self.calib) |
||||
self.calib_from_device = self.device_from_calib.T |
||||
self.calibrated = log.calStatus == 1 |
||||
|
||||
def reset_kalman(self, current_time=None, init_orient=None, init_pos=None): |
||||
self.filter_time = current_time |
||||
init_x = LiveKalman.initial_x.copy() |
||||
# too nonlinear to init on completely wrong |
||||
if init_orient is not None: |
||||
init_x[3:7] = init_orient |
||||
if init_pos is not None: |
||||
init_x[:3] = init_pos |
||||
self.kf.init_state(init_x, covs=np.diag(LiveKalman.initial_P_diag), filter_time=current_time) |
||||
|
||||
self.observation_buffer = [] |
||||
|
||||
self.gyro_counter = 0 |
||||
self.acc_counter = 0 |
||||
self.speed_counter = 0 |
||||
self.cam_counter = 0 |
||||
|
||||
|
||||
def locationd_thread(sm, pm, disabled_logs=None): |
||||
gc.disable() |
||||
|
||||
if disabled_logs is None: |
||||
disabled_logs = [] |
||||
|
||||
if sm is None: |
||||
socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration', 'carState'] |
||||
sm = messaging.SubMaster(socks, ignore_alive=['gpsLocationExternal'], ignore_avg_freq=['sensorEvents']) |
||||
if pm is None: |
||||
pm = messaging.PubMaster(['liveLocationKalman']) |
||||
|
||||
params = Params() |
||||
localizer = Localizer(disabled_logs=disabled_logs) |
||||
|
||||
while True: |
||||
sm.update() |
||||
|
||||
for sock, updated in sm.updated.items(): |
||||
if updated and sm.valid[sock]: |
||||
t = sm.logMonoTime[sock] * 1e-9 |
||||
if sock == "sensorEvents": |
||||
localizer.handle_sensors(t, sm[sock]) |
||||
elif sock == "gpsLocationExternal": |
||||
localizer.handle_gps(t, sm[sock]) |
||||
elif sock == "carState": |
||||
localizer.handle_car_state(t, sm[sock]) |
||||
elif sock == "cameraOdometry": |
||||
localizer.handle_cam_odo(t, sm[sock]) |
||||
elif sock == "liveCalibration": |
||||
localizer.handle_live_calib(t, sm[sock]) |
||||
|
||||
if sm.updated['cameraOdometry']: |
||||
t = sm.logMonoTime['cameraOdometry'] |
||||
msg = messaging.new_message('liveLocationKalman') |
||||
msg.logMonoTime = t |
||||
|
||||
msg.liveLocationKalman = localizer.liveLocationMsg() |
||||
msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid() |
||||
msg.liveLocationKalman.sensorsOK = sm.alive['sensorEvents'] and sm.valid['sensorEvents'] |
||||
|
||||
gps_age = (t / 1e9) - localizer.last_gps_fix |
||||
msg.liveLocationKalman.gpsOK = gps_age < 1.0 |
||||
pm.send('liveLocationKalman', msg) |
||||
|
||||
if sm.frame % 1200 == 0 and msg.liveLocationKalman.gpsOK: # once a minute |
||||
location = { |
||||
'latitude': msg.liveLocationKalman.positionGeodetic.value[0], |
||||
'longitude': msg.liveLocationKalman.positionGeodetic.value[1], |
||||
'altitude': msg.liveLocationKalman.positionGeodetic.value[2], |
||||
} |
||||
params.put("LastGPSPosition", json.dumps(location)) |
||||
|
||||
|
||||
def main(sm=None, pm=None): |
||||
locationd_thread(sm, pm) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
import os |
||||
os.environ["OMP_NUM_THREADS"] = "1" |
||||
main() |
||||
@ -0,0 +1,140 @@ |
||||
#include "live_kf.h" |
||||
|
||||
using namespace EKFS; |
||||
using namespace Eigen; |
||||
|
||||
Eigen::Map<Eigen::VectorXd> get_mapvec(Eigen::VectorXd& vec) { |
||||
return Eigen::Map<Eigen::VectorXd>(vec.data(), vec.rows(), vec.cols()); |
||||
} |
||||
|
||||
Eigen::Map<MatrixXdr> get_mapmat(MatrixXdr& mat) { |
||||
return Eigen::Map<MatrixXdr>(mat.data(), mat.rows(), mat.cols()); |
||||
} |
||||
|
||||
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(std::vector<Eigen::VectorXd>& vec_vec) { |
||||
std::vector<Eigen::Map<Eigen::VectorXd>> res; |
||||
for (Eigen::VectorXd& vec : vec_vec) { |
||||
res.push_back(get_mapvec(vec)); |
||||
} |
||||
return res; |
||||
} |
||||
|
||||
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(std::vector<MatrixXdr>& mat_vec) { |
||||
std::vector<Eigen::Map<MatrixXdr>> res; |
||||
for (MatrixXdr& mat : mat_vec) { |
||||
res.push_back(get_mapmat(mat)); |
||||
} |
||||
return res; |
||||
} |
||||
|
||||
LiveKalman::LiveKalman() { |
||||
this->dim_state = 23; |
||||
this->dim_state_err = 22; |
||||
|
||||
this->initial_x = live_initial_x; |
||||
this->initial_P = live_initial_P_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<std::string>(), 0.2); |
||||
} |
||||
|
||||
void LiveKalman::init_state(VectorXd& state, 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(VectorXd& state, MatrixXdr& covs, double filter_time) { |
||||
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time); |
||||
} |
||||
|
||||
void LiveKalman::init_state(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(); |
||||
} |
||||
|
||||
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, std::vector<VectorXd> meas, std::vector<MatrixXdr> R) { |
||||
std::optional<Estimate> r; |
||||
switch (kind) { |
||||
case OBSERVATION_CAMERA_ODO_TRANSLATION: |
||||
r = this->predict_and_update_odo_trans(meas, t, kind); |
||||
break; |
||||
case OBSERVATION_CAMERA_ODO_ROTATION: |
||||
r = this->predict_and_update_odo_rot(meas, t, kind); |
||||
break; |
||||
case OBSERVATION_ODOMETRIC_SPEED: |
||||
r = this->predict_and_update_odo_speed(meas, t, kind); |
||||
break; |
||||
default: |
||||
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)); |
||||
break; |
||||
} |
||||
this->filter->normalize_state(STATE_ECEF_ORIENTATION_START, STATE_ECEF_ORIENTATION_END); |
||||
return r; |
||||
} |
||||
|
||||
std::optional<Estimate> LiveKalman::predict_and_update_odo_speed(std::vector<VectorXd> speed, double t, int kind) { |
||||
std::vector<MatrixXdr> R; |
||||
R.assign(speed.size(), (MatrixXdr(1, 1) << std::pow(0.2, 2)).finished().asDiagonal()); |
||||
return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(speed), get_vec_mapmat(R)); |
||||
} |
||||
|
||||
std::optional<Estimate> LiveKalman::predict_and_update_odo_trans(std::vector<VectorXd> trans, double t, int kind) { |
||||
std::vector<VectorXd> z; |
||||
std::vector<MatrixXdr> R; |
||||
for (VectorXd& trns : trans) { |
||||
assert(trns.size() == 6); // TODO remove
|
||||
z.push_back(trns.head(3)); |
||||
R.push_back(trns.segment<3>(3).array().square().matrix().asDiagonal()); |
||||
} |
||||
return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(z), get_vec_mapmat(R)); |
||||
} |
||||
|
||||
std::optional<Estimate> LiveKalman::predict_and_update_odo_rot(std::vector<VectorXd> rot, double t, int kind) { |
||||
std::vector<VectorXd> z; |
||||
std::vector<MatrixXdr> R; |
||||
for (VectorXd& rt : rot) { |
||||
assert(rt.size() == 6); // TODO remove
|
||||
z.push_back(rt.head(3)); |
||||
R.push_back(rt.segment<3>(3).array().square().matrix().asDiagonal()); |
||||
} |
||||
return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(z), get_vec_mapmat(R)); |
||||
} |
||||
|
||||
Eigen::VectorXd LiveKalman::get_initial_x() { |
||||
return this->initial_x; |
||||
} |
||||
|
||||
MatrixXdr LiveKalman::get_initial_P() { |
||||
return this->initial_P; |
||||
} |
||||
|
||||
MatrixXdr LiveKalman::H(VectorXd in) { |
||||
assert(in.size() == 6); |
||||
Matrix<double, 3, 6, Eigen::RowMajor> res; |
||||
this->filter->get_extra_routine("H")(in.data(), res.data()); |
||||
return res; |
||||
} |
||||
@ -0,0 +1,56 @@ |
||||
#pragma once |
||||
|
||||
#include <string> |
||||
#include <cmath> |
||||
#include <memory> |
||||
|
||||
#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(Eigen::VectorXd& vec); |
||||
Eigen::Map<MatrixXdr> get_mapmat(MatrixXdr& mat); |
||||
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(std::vector<Eigen::VectorXd>& vec_vec); |
||||
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(std::vector<MatrixXdr>& mat_vec); |
||||
|
||||
class LiveKalman { |
||||
public: |
||||
LiveKalman(); |
||||
|
||||
void init_state(Eigen::VectorXd& state, Eigen::VectorXd& covs_diag, double filter_time); |
||||
void init_state(Eigen::VectorXd& state, MatrixXdr& covs, double filter_time); |
||||
void init_state(Eigen::VectorXd& state, double filter_time); |
||||
|
||||
Eigen::VectorXd get_x(); |
||||
MatrixXdr get_P(); |
||||
std::vector<MatrixXdr> get_R(int kind, int n); |
||||
|
||||
std::optional<Estimate> predict_and_observe(double t, int kind, 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); |
||||
|
||||
Eigen::VectorXd get_initial_x(); |
||||
MatrixXdr get_initial_P(); |
||||
|
||||
MatrixXdr H(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 Q; // process noise
|
||||
std::unordered_map<int, MatrixXdr> obs_noise; |
||||
}; |
||||
Loading…
Reference in new issue