Willem Melching
5 years ago
committed by
GitHub
parent
ea052d61b3
commit
488bbcaaa4
9 changed files with 1 additions and 562 deletions
@ -1,28 +0,0 @@ |
|||||||
import os |
|
||||||
|
|
||||||
from cffi import FFI |
|
||||||
|
|
||||||
locationd_dir = os.path.dirname(os.path.abspath(__file__)) |
|
||||||
liblocationd_fn = os.path.join(locationd_dir, "liblocationd.so") |
|
||||||
|
|
||||||
|
|
||||||
ffi = FFI() |
|
||||||
ffi.cdef(""" |
|
||||||
void *localizer_init(void); |
|
||||||
void localizer_handle_log(void * localizer, const unsigned char * data, size_t len); |
|
||||||
double localizer_get_yaw(void * localizer); |
|
||||||
double localizer_get_bias(void * localizer); |
|
||||||
double localizer_get_t(void * localizer); |
|
||||||
void *params_learner_init(size_t len, char * params, double angle_offset, double stiffness_factor, double steer_ratio, double learning_rate); |
|
||||||
bool params_learner_update(void * params_learner, double psi, double u, double sa); |
|
||||||
double params_learner_get_ao(void * params_learner); |
|
||||||
double params_learner_get_slow_ao(void * params_learner); |
|
||||||
double params_learner_get_x(void * params_learner); |
|
||||||
double params_learner_get_sR(void * params_learner); |
|
||||||
double * localizer_get_P(void * localizer); |
|
||||||
void localizer_set_P(void * localizer, double * P); |
|
||||||
double * localizer_get_state(void * localizer); |
|
||||||
void localizer_set_state(void * localizer, double * state); |
|
||||||
""") |
|
||||||
|
|
||||||
liblocationd = ffi.dlopen(liblocationd_fn) |
|
||||||
@ -1,150 +0,0 @@ |
|||||||
#include <iostream> |
|
||||||
#include <cmath> |
|
||||||
|
|
||||||
#include <capnp/message.h> |
|
||||||
#include <capnp/serialize-packed.h> |
|
||||||
#include <eigen3/Eigen/Dense> |
|
||||||
|
|
||||||
#include "cereal/gen/cpp/log.capnp.h" |
|
||||||
|
|
||||||
#include "locationd_yawrate.h" |
|
||||||
|
|
||||||
|
|
||||||
void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) { |
|
||||||
double dt = current_time - prev_update_time; |
|
||||||
|
|
||||||
if (dt < 0) { |
|
||||||
dt = 0; |
|
||||||
} else { |
|
||||||
prev_update_time = current_time; |
|
||||||
} |
|
||||||
|
|
||||||
x = A * x; |
|
||||||
P = A * P * A.transpose() + dt * Q; |
|
||||||
|
|
||||||
double y = meas - C * x; |
|
||||||
double S = R + C * P * C.transpose(); |
|
||||||
Eigen::Vector2d K = P * C.transpose() * (1.0 / S); |
|
||||||
x = x + K * y; |
|
||||||
P = (I - K * C) * P; |
|
||||||
} |
|
||||||
|
|
||||||
void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time) { |
|
||||||
for (cereal::SensorEventData::Reader sensor_event : sensor_events){ |
|
||||||
if (sensor_event.getSensor() == 5 && sensor_event.getType() == 16) { |
|
||||||
double meas = -sensor_event.getGyroUncalibrated().getV()[0]; |
|
||||||
update_state(C_gyro, R_gyro, current_time, meas); |
|
||||||
} |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) { |
|
||||||
double R = pow(5 * camera_odometry.getRotStd()[2], 2); |
|
||||||
double meas = camera_odometry.getRot()[2]; |
|
||||||
update_state(C_posenet, R, current_time, meas); |
|
||||||
} |
|
||||||
|
|
||||||
void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) { |
|
||||||
steering_angle = controls_state.getAngleSteers() * DEGREES_TO_RADIANS; |
|
||||||
car_speed = controls_state.getVEgo(); |
|
||||||
} |
|
||||||
|
|
||||||
|
|
||||||
Localizer::Localizer() { |
|
||||||
// States: [yaw rate, gyro bias]
|
|
||||||
A << |
|
||||||
1, 0, |
|
||||||
0, 1; |
|
||||||
|
|
||||||
Q << |
|
||||||
pow(.1, 2.0), 0, |
|
||||||
0, pow(0.05/ 100.0, 2.0), |
|
||||||
P << |
|
||||||
pow(10000.0, 2.0), 0, |
|
||||||
0, pow(10000.0, 2.0); |
|
||||||
|
|
||||||
I << |
|
||||||
1, 0, |
|
||||||
0, 1; |
|
||||||
|
|
||||||
C_posenet << 1, 0; |
|
||||||
C_gyro << 1, 1; |
|
||||||
x << 0, 0; |
|
||||||
|
|
||||||
R_gyro = pow(0.025, 2.0); |
|
||||||
} |
|
||||||
|
|
||||||
void Localizer::handle_log(cereal::Event::Reader event) { |
|
||||||
double current_time = event.getLogMonoTime() / 1.0e9; |
|
||||||
|
|
||||||
// Initialize update_time on first update
|
|
||||||
if (prev_update_time < 0) { |
|
||||||
prev_update_time = current_time; |
|
||||||
} |
|
||||||
|
|
||||||
auto type = event.which(); |
|
||||||
switch(type) { |
|
||||||
case cereal::Event::CONTROLS_STATE: |
|
||||||
handle_controls_state(event.getControlsState(), current_time); |
|
||||||
break; |
|
||||||
case cereal::Event::CAMERA_ODOMETRY: |
|
||||||
handle_camera_odometry(event.getCameraOdometry(), current_time); |
|
||||||
break; |
|
||||||
case cereal::Event::SENSOR_EVENTS: |
|
||||||
handle_sensor_events(event.getSensorEvents(), current_time); |
|
||||||
break; |
|
||||||
default: |
|
||||||
break; |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
|
|
||||||
extern "C" { |
|
||||||
void *localizer_init(void) { |
|
||||||
Localizer * localizer = new Localizer; |
|
||||||
return (void*)localizer; |
|
||||||
} |
|
||||||
|
|
||||||
void localizer_handle_log(void * localizer, const unsigned char * data, size_t len) { |
|
||||||
const kj::ArrayPtr<const capnp::word> view((const capnp::word*)data, len); |
|
||||||
capnp::FlatArrayMessageReader msg(view); |
|
||||||
cereal::Event::Reader event = msg.getRoot<cereal::Event>(); |
|
||||||
|
|
||||||
Localizer * loc = (Localizer*) localizer; |
|
||||||
loc->handle_log(event); |
|
||||||
} |
|
||||||
|
|
||||||
double localizer_get_yaw(void * localizer) { |
|
||||||
Localizer * loc = (Localizer*) localizer; |
|
||||||
return loc->x[0]; |
|
||||||
} |
|
||||||
double localizer_get_bias(void * localizer) { |
|
||||||
Localizer * loc = (Localizer*) localizer; |
|
||||||
return loc->x[1]; |
|
||||||
} |
|
||||||
|
|
||||||
double * localizer_get_state(void * localizer) { |
|
||||||
Localizer * loc = (Localizer*) localizer; |
|
||||||
return loc->x.data(); |
|
||||||
} |
|
||||||
|
|
||||||
void localizer_set_state(void * localizer, double * state) { |
|
||||||
Localizer * loc = (Localizer*) localizer; |
|
||||||
memcpy(loc->x.data(), state, 4 * sizeof(double)); |
|
||||||
} |
|
||||||
|
|
||||||
double localizer_get_t(void * localizer) { |
|
||||||
Localizer * loc = (Localizer*) localizer; |
|
||||||
return loc->prev_update_time; |
|
||||||
} |
|
||||||
|
|
||||||
double * localizer_get_P(void * localizer) { |
|
||||||
Localizer * loc = (Localizer*) localizer; |
|
||||||
return loc->P.data(); |
|
||||||
} |
|
||||||
|
|
||||||
void localizer_set_P(void * localizer, double * P) { |
|
||||||
Localizer * loc = (Localizer*) localizer; |
|
||||||
memcpy(loc->P.data(), P, 16 * sizeof(double)); |
|
||||||
} |
|
||||||
} |
|
||||||
@ -1,33 +0,0 @@ |
|||||||
#pragma once |
|
||||||
|
|
||||||
#include <eigen3/Eigen/Dense> |
|
||||||
#include "cereal/gen/cpp/log.capnp.h" |
|
||||||
|
|
||||||
#define DEGREES_TO_RADIANS 0.017453292519943295 |
|
||||||
|
|
||||||
class Localizer |
|
||||||
{ |
|
||||||
Eigen::Matrix2d A; |
|
||||||
Eigen::Matrix2d I; |
|
||||||
Eigen::Matrix2d Q; |
|
||||||
Eigen::Matrix<double, 1, 2> C_posenet; |
|
||||||
Eigen::Matrix<double, 1, 2> C_gyro; |
|
||||||
|
|
||||||
double R_gyro; |
|
||||||
|
|
||||||
void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas); |
|
||||||
void handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time); |
|
||||||
void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time); |
|
||||||
void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time); |
|
||||||
|
|
||||||
public: |
|
||||||
Eigen::Vector2d x; |
|
||||||
Eigen::Matrix2d P; |
|
||||||
double steering_angle = 0; |
|
||||||
double car_speed = 0; |
|
||||||
double prev_update_time = -1; |
|
||||||
|
|
||||||
Localizer(); |
|
||||||
void handle_log(cereal::Event::Reader event); |
|
||||||
|
|
||||||
}; |
|
||||||
@ -1,118 +0,0 @@ |
|||||||
#include <algorithm> |
|
||||||
#include <cmath> |
|
||||||
#include <iostream> |
|
||||||
|
|
||||||
#include <capnp/message.h> |
|
||||||
#include <capnp/serialize-packed.h> |
|
||||||
#include "cereal/gen/cpp/log.capnp.h" |
|
||||||
#include "cereal/gen/cpp/car.capnp.h" |
|
||||||
#include "params_learner.h" |
|
||||||
|
|
||||||
// #define DEBUG
|
|
||||||
|
|
||||||
template <typename T> |
|
||||||
T clip(const T& n, const T& lower, const T& upper) { |
|
||||||
return std::max(lower, std::min(n, upper)); |
|
||||||
} |
|
||||||
|
|
||||||
ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params, |
|
||||||
double angle_offset, |
|
||||||
double stiffness_factor, |
|
||||||
double steer_ratio, |
|
||||||
double learning_rate) : |
|
||||||
ao(angle_offset * DEGREES_TO_RADIANS), |
|
||||||
slow_ao(angle_offset * DEGREES_TO_RADIANS), |
|
||||||
x(stiffness_factor), |
|
||||||
sR(steer_ratio) { |
|
||||||
|
|
||||||
cF0 = car_params.getTireStiffnessFront(); |
|
||||||
cR0 = car_params.getTireStiffnessRear(); |
|
||||||
|
|
||||||
l = car_params.getWheelbase(); |
|
||||||
m = car_params.getMass(); |
|
||||||
|
|
||||||
aF = car_params.getCenterToFront(); |
|
||||||
aR = l - aF; |
|
||||||
|
|
||||||
min_sr = MIN_SR * car_params.getSteerRatio(); |
|
||||||
max_sr = MAX_SR * car_params.getSteerRatio(); |
|
||||||
min_sr_th = MIN_SR_TH * car_params.getSteerRatio(); |
|
||||||
max_sr_th = MAX_SR_TH * car_params.getSteerRatio(); |
|
||||||
alpha1 = 0.01 * learning_rate; |
|
||||||
alpha2 = 0.0005 * learning_rate; |
|
||||||
alpha3 = 0.1 * learning_rate; |
|
||||||
alpha4 = 1.0 * learning_rate; |
|
||||||
} |
|
||||||
|
|
||||||
bool ParamsLearner::update(double psi, double u, double sa) { |
|
||||||
if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 90.)) { |
|
||||||
double ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2)); |
|
||||||
double new_ao = ao - alpha1 * ao_diff; |
|
||||||
|
|
||||||
double slow_ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2)); |
|
||||||
double new_slow_ao = slow_ao - alpha2 * slow_ao_diff; |
|
||||||
|
|
||||||
double new_x = x - alpha3 * (-2.0*cF0*cR0*l*m*pow(u, 3)*(slow_ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 3))); |
|
||||||
double new_sR = sR - alpha4 * (-2.0*cF0*cR0*l*u*x*(slow_ao - sa)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 3)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2))); |
|
||||||
|
|
||||||
ao = new_ao; |
|
||||||
slow_ao = new_slow_ao; |
|
||||||
x = new_x; |
|
||||||
sR = new_sR; |
|
||||||
} |
|
||||||
|
|
||||||
#ifdef DEBUG |
|
||||||
std::cout << "Instant AO: " << (RADIANS_TO_DEGREES * ao) << "\tAverage AO: " << (RADIANS_TO_DEGREES * slow_ao); |
|
||||||
std::cout << "\tStiffness: " << x << "\t sR: " << sR << std::endl; |
|
||||||
#endif |
|
||||||
|
|
||||||
ao = clip(ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET); |
|
||||||
slow_ao = clip(slow_ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET); |
|
||||||
x = clip(x, MIN_STIFFNESS, MAX_STIFFNESS); |
|
||||||
sR = clip(sR, min_sr, max_sr); |
|
||||||
|
|
||||||
bool valid = fabs(slow_ao) < MAX_ANGLE_OFFSET_TH; |
|
||||||
valid = valid && sR > min_sr_th; |
|
||||||
valid = valid && sR < max_sr_th; |
|
||||||
return valid; |
|
||||||
} |
|
||||||
|
|
||||||
|
|
||||||
extern "C" { |
|
||||||
void *params_learner_init(size_t len, char * params, double angle_offset, double stiffness_factor, double steer_ratio, double learning_rate) { |
|
||||||
|
|
||||||
auto amsg = kj::heapArray<capnp::word>((len / sizeof(capnp::word)) + 1); |
|
||||||
memcpy(amsg.begin(), params, len); |
|
||||||
|
|
||||||
capnp::FlatArrayMessageReader cmsg(amsg); |
|
||||||
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>(); |
|
||||||
|
|
||||||
ParamsLearner * p = new ParamsLearner(car_params, angle_offset, stiffness_factor, steer_ratio, learning_rate); |
|
||||||
return (void*)p; |
|
||||||
} |
|
||||||
|
|
||||||
bool params_learner_update(void * params_learner, double psi, double u, double sa) { |
|
||||||
ParamsLearner * p = (ParamsLearner*) params_learner; |
|
||||||
return p->update(psi, u, sa); |
|
||||||
} |
|
||||||
|
|
||||||
double params_learner_get_ao(void * params_learner){ |
|
||||||
ParamsLearner * p = (ParamsLearner*) params_learner; |
|
||||||
return p->ao; |
|
||||||
} |
|
||||||
|
|
||||||
double params_learner_get_x(void * params_learner){ |
|
||||||
ParamsLearner * p = (ParamsLearner*) params_learner; |
|
||||||
return p->x; |
|
||||||
} |
|
||||||
|
|
||||||
double params_learner_get_slow_ao(void * params_learner){ |
|
||||||
ParamsLearner * p = (ParamsLearner*) params_learner; |
|
||||||
return p->slow_ao; |
|
||||||
} |
|
||||||
|
|
||||||
double params_learner_get_sR(void * params_learner){ |
|
||||||
ParamsLearner * p = (ParamsLearner*) params_learner; |
|
||||||
return p->sR; |
|
||||||
} |
|
||||||
} |
|
||||||
@ -1,35 +0,0 @@ |
|||||||
#pragma once |
|
||||||
|
|
||||||
#define DEGREES_TO_RADIANS 0.017453292519943295 |
|
||||||
#define RADIANS_TO_DEGREES (1.0 / DEGREES_TO_RADIANS) |
|
||||||
|
|
||||||
#define MAX_ANGLE_OFFSET (10.0 * DEGREES_TO_RADIANS) |
|
||||||
#define MAX_ANGLE_OFFSET_TH (9.0 * DEGREES_TO_RADIANS) |
|
||||||
#define MIN_STIFFNESS 0.5 |
|
||||||
#define MAX_STIFFNESS 2.0 |
|
||||||
#define MIN_SR 0.5 |
|
||||||
#define MAX_SR 2.0 |
|
||||||
#define MIN_SR_TH 0.55 |
|
||||||
#define MAX_SR_TH 1.9 |
|
||||||
|
|
||||||
class ParamsLearner { |
|
||||||
double cF0, cR0; |
|
||||||
double aR, aF; |
|
||||||
double l, m; |
|
||||||
|
|
||||||
double min_sr, max_sr, min_sr_th, max_sr_th; |
|
||||||
double alpha1, alpha2, alpha3, alpha4; |
|
||||||
|
|
||||||
public: |
|
||||||
double ao; |
|
||||||
double slow_ao; |
|
||||||
double x, sR; |
|
||||||
|
|
||||||
ParamsLearner(cereal::CarParams::Reader car_params, |
|
||||||
double angle_offset, |
|
||||||
double stiffness_factor, |
|
||||||
double steer_ratio, |
|
||||||
double learning_rate); |
|
||||||
|
|
||||||
bool update(double psi, double u, double sa); |
|
||||||
}; |
|
||||||
@ -1,135 +0,0 @@ |
|||||||
#include <future> |
|
||||||
#include <iostream> |
|
||||||
#include <cassert> |
|
||||||
#include <csignal> |
|
||||||
#include <unistd.h> |
|
||||||
|
|
||||||
#include <capnp/serialize-packed.h> |
|
||||||
#include "json11.hpp" |
|
||||||
|
|
||||||
#include "common/swaglog.h" |
|
||||||
#include "common/params.h" |
|
||||||
#include "common/timing.h" |
|
||||||
|
|
||||||
#include "messaging.hpp" |
|
||||||
#include "locationd_yawrate.h" |
|
||||||
#include "params_learner.h" |
|
||||||
|
|
||||||
#include "common/util.h" |
|
||||||
|
|
||||||
void sigpipe_handler(int sig) { |
|
||||||
LOGE("SIGPIPE received"); |
|
||||||
} |
|
||||||
|
|
||||||
|
|
||||||
int main(int argc, char *argv[]) { |
|
||||||
signal(SIGPIPE, (sighandler_t)sigpipe_handler); |
|
||||||
|
|
||||||
SubMaster sm({"controlsState", "sensorEvents", "cameraOdometry"}); |
|
||||||
PubMaster pm({"liveParameters"}); |
|
||||||
|
|
||||||
Localizer localizer; |
|
||||||
|
|
||||||
// Read car params
|
|
||||||
std::vector<char> params; |
|
||||||
LOGW("waiting for params to set vehicle model"); |
|
||||||
while (true) { |
|
||||||
params = read_db_bytes("CarParams"); |
|
||||||
if (params.size() > 0) break; |
|
||||||
usleep(100*1000); |
|
||||||
} |
|
||||||
LOGW("got %d bytes CarParams", params.size()); |
|
||||||
|
|
||||||
// make copy due to alignment issues
|
|
||||||
auto amsg = kj::heapArray<capnp::word>((params.size() / sizeof(capnp::word)) + 1); |
|
||||||
memcpy(amsg.begin(), params.data(), params.size()); |
|
||||||
|
|
||||||
capnp::FlatArrayMessageReader cmsg(amsg); |
|
||||||
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>(); |
|
||||||
|
|
||||||
// Read params from previous run
|
|
||||||
std::string fingerprint = car_params.getCarFingerprint(); |
|
||||||
std::string vin = car_params.getCarVin(); |
|
||||||
double sR = car_params.getSteerRatio(); |
|
||||||
double x = 1.0; |
|
||||||
double ao = 0.0; |
|
||||||
std::vector<char> live_params = read_db_bytes("LiveParameters"); |
|
||||||
if (live_params.size() > 0){ |
|
||||||
std::string err; |
|
||||||
std::string str(live_params.begin(), live_params.end()); |
|
||||||
auto json = json11::Json::parse(str, err); |
|
||||||
if (json.is_null() || !err.empty()) { |
|
||||||
std::string log = "Error parsing json: " + err; |
|
||||||
LOGW(log.c_str()); |
|
||||||
} else { |
|
||||||
std::string new_fingerprint = json["carFingerprint"].string_value(); |
|
||||||
std::string new_vin = json["carVin"].string_value(); |
|
||||||
|
|
||||||
if (fingerprint == new_fingerprint && vin == new_vin) { |
|
||||||
std::string log = "Parameter starting with: " + str; |
|
||||||
LOGW(log.c_str()); |
|
||||||
|
|
||||||
sR = json["steerRatio"].number_value(); |
|
||||||
x = json["stiffnessFactor"].number_value(); |
|
||||||
ao = json["angleOffsetAverage"].number_value(); |
|
||||||
} |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
ParamsLearner learner(car_params, ao, x, sR, 1.0); |
|
||||||
|
|
||||||
// Main loop
|
|
||||||
int save_counter = 0; |
|
||||||
while (true){ |
|
||||||
if (sm.update(100) == 0) continue; |
|
||||||
|
|
||||||
if (sm.updated("controlsState")){ |
|
||||||
localizer.handle_log(sm["controlsState"]); |
|
||||||
save_counter++; |
|
||||||
|
|
||||||
double yaw_rate = -localizer.x[0]; |
|
||||||
bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle); |
|
||||||
|
|
||||||
double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao; |
|
||||||
double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao; |
|
||||||
|
|
||||||
capnp::MallocMessageBuilder msg; |
|
||||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
|
||||||
event.setLogMonoTime(nanos_since_boot()); |
|
||||||
auto live_params = event.initLiveParameters(); |
|
||||||
live_params.setValid(valid); |
|
||||||
live_params.setYawRate(localizer.x[0]); |
|
||||||
live_params.setGyroBias(localizer.x[1]); |
|
||||||
live_params.setAngleOffset(angle_offset_degrees); |
|
||||||
live_params.setAngleOffsetAverage(angle_offset_average_degrees); |
|
||||||
live_params.setStiffnessFactor(learner.x); |
|
||||||
live_params.setSteerRatio(learner.sR); |
|
||||||
|
|
||||||
pm.send("liveParameters", msg); |
|
||||||
|
|
||||||
// Save parameters every minute
|
|
||||||
if (save_counter % 6000 == 0) { |
|
||||||
json11::Json json = json11::Json::object { |
|
||||||
{"carVin", vin}, |
|
||||||
{"carFingerprint", fingerprint}, |
|
||||||
{"steerRatio", learner.sR}, |
|
||||||
{"stiffnessFactor", learner.x}, |
|
||||||
{"angleOffsetAverage", angle_offset_average_degrees}, |
|
||||||
}; |
|
||||||
|
|
||||||
std::string out = json.dump(); |
|
||||||
std::async(std::launch::async, |
|
||||||
[out]{ |
|
||||||
write_db_value("LiveParameters", out.c_str(), out.length()); |
|
||||||
}); |
|
||||||
} |
|
||||||
} |
|
||||||
if (sm.updated("sensorEvents")){ |
|
||||||
localizer.handle_log(sm["sensorEvents"]); |
|
||||||
} |
|
||||||
if (sm.updated("cameraOdometry")){ |
|
||||||
localizer.handle_log(sm["cameraOdometry"]); |
|
||||||
} |
|
||||||
} |
|
||||||
return 0; |
|
||||||
} |
|
||||||
@ -1,50 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
|
|
||||||
import numpy as np |
|
||||||
import unittest |
|
||||||
|
|
||||||
from selfdrive.car.honda.interface import CarInterface |
|
||||||
from selfdrive.car.honda.values import CAR |
|
||||||
from selfdrive.controls.lib.vehicle_model import VehicleModel |
|
||||||
from selfdrive.locationd.liblocationd_py import liblocationd # pylint: disable=no-name-in-module, import-error |
|
||||||
|
|
||||||
|
|
||||||
class TestParamsLearner(unittest.TestCase): |
|
||||||
def setUp(self): |
|
||||||
|
|
||||||
self.CP = CarInterface.get_params(CAR.CIVIC) |
|
||||||
bts = self.CP.to_bytes() |
|
||||||
|
|
||||||
self.params_learner = liblocationd.params_learner_init(len(bts), bts, 0.0, 1.0, self.CP.steerRatio, 1.0) |
|
||||||
|
|
||||||
def test_convergence(self): |
|
||||||
# Setup vehicle model with wrong parameters |
|
||||||
VM_sim = VehicleModel(self.CP) |
|
||||||
x_target = 0.75 |
|
||||||
sr_target = self.CP.steerRatio - 0.5 |
|
||||||
ao_target = -1.0 |
|
||||||
VM_sim.update_params(x_target, sr_target) |
|
||||||
|
|
||||||
# Run simulation |
|
||||||
times = np.arange(0, 15*3600, 0.01) |
|
||||||
angle_offset = np.radians(ao_target) |
|
||||||
steering_angles = np.radians(10 * np.sin(2 * np.pi * times / 100.)) + angle_offset |
|
||||||
speeds = 10 * np.sin(2 * np.pi * times / 1000.) + 25 |
|
||||||
|
|
||||||
for i, _ in enumerate(times): |
|
||||||
u = speeds[i] |
|
||||||
sa = steering_angles[i] |
|
||||||
psi = VM_sim.yaw_rate(sa - angle_offset, u) |
|
||||||
liblocationd.params_learner_update(self.params_learner, psi, u, sa) |
|
||||||
|
|
||||||
# Verify learned parameters |
|
||||||
sr = liblocationd.params_learner_get_sR(self.params_learner) |
|
||||||
ao_slow = np.degrees(liblocationd.params_learner_get_slow_ao(self.params_learner)) |
|
||||||
x = liblocationd.params_learner_get_x(self.params_learner) |
|
||||||
self.assertAlmostEqual(x_target, x, places=1) |
|
||||||
self.assertAlmostEqual(ao_target, ao_slow, places=1) |
|
||||||
self.assertAlmostEqual(sr_target, sr, places=1) |
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
unittest.main() |
|
||||||
Loading…
Reference in new issue