|
|
|
@ -30,6 +30,17 @@ static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder me
|
|
|
|
|
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() {
|
|
|
|
|
this->kf = std::make_unique<LiveKalman>();
|
|
|
|
|
this->reset_kalman();
|
|
|
|
@ -64,11 +75,12 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
|
|
|
|
|
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();
|
|
|
|
|
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_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();
|
|
|
|
|
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();
|
|
|
|
@ -88,7 +100,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
|
|
|
|
|
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 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);
|
|
|
|
|
//orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
|
|
|
|
@ -170,11 +182,8 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
|
|
|
|
|
|
|
|
|
|
// 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]) });
|
|
|
|
|
}
|
|
|
|
|
auto v = sensor_reading.getGyroUncalibrated().getV();
|
|
|
|
|
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) });
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Accelerometer
|
|
|
|
@ -185,10 +194,7 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
|
|
|
|
|
// 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]) });
|
|
|
|
|
}
|
|
|
|
|
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { Vector3d(-v[2], -v[1], -v[0]) });
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
@ -239,36 +245,33 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::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.getVEgoRaw()).finished() });
|
|
|
|
|
this->car_speed = std::abs(log.getVEgo());
|
|
|
|
|
if (this->car_speed < 1e-3) {
|
|
|
|
|
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_ODOMETRIC_SPEED, { (VectorXd(1) << log.getVEgoRaw()).finished() });
|
|
|
|
|
this->car_speed = std::abs(log.getVEgo());
|
|
|
|
|
if (this->car_speed < 1e-3) {
|
|
|
|
|
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++;
|
|
|
|
|
VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
|
|
|
|
|
VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
|
|
|
|
|
|
|
|
|
|
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 rot_calib_std = floatlist2vector(log.getRotStd());
|
|
|
|
|
VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
|
|
|
|
|
|
|
|
|
|
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_calib_std[0]);
|
|
|
|
|
|
|
|
|
|
this->posenet_stds.pop_front();
|
|
|
|
|
this->posenet_stds.push_back(trans_device_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;
|
|
|
|
|
VectorXd rot_device_std = rotate_std(this->device_from_calib, rot_calib_std);
|
|
|
|
|
VectorXd trans_device_std = rotate_std(this->device_from_calib, trans_calib_std);
|
|
|
|
|
|
|
|
|
|
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() });
|
|
|
|
|
}
|
|
|
|
|
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() });
|
|
|
|
|
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) {
|
|
|
|
@ -285,6 +288,14 @@ void Localizer::reset_kalman(double current_time) {
|
|
|
|
|
this->reset_kalman(current_time, init_x.segment<4>(3), init_x.head(3));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
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::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();
|
|
|
|
@ -293,11 +304,6 @@ void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd
|
|
|
|
|
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) {
|
|
|
|
@ -322,6 +328,7 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
|
|
|
|
|
} else if (log.isLiveCalibration()) {
|
|
|
|
|
this->handle_live_calib(t, log.getLiveCalibration());
|
|
|
|
|
}
|
|
|
|
|
this->finite_check();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
|
|
|
|
|