remove old params learner (#1918)
Willem Melching
GitHub
parent
b16e90c781
commit
e0e7c7486d
|
|
@ -280,12 +280,6 @@ selfdrive/locationd/models/constants.py
|
|||
selfdrive/locationd/calibrationd.py
|
||||
selfdrive/locationd/calibration_helpers.py
|
||||
|
||||
selfdrive/locationd/locationd_yawrate.cc
|
||||
selfdrive/locationd/locationd_yawrate.h
|
||||
selfdrive/locationd/params_learner.cc
|
||||
selfdrive/locationd/params_learner.h
|
||||
selfdrive/locationd/paramsd.cc
|
||||
|
||||
selfdrive/logcatd/SConscript
|
||||
selfdrive/logcatd/logcatd.cc
|
||||
|
||||
|
|
|
|||
|
|
@ -1,12 +1,6 @@
|
|||
Import('env', 'common', 'cereal', 'messaging')
|
||||
loc_objs = [
|
||||
"locationd_yawrate.cc",
|
||||
"params_learner.cc",
|
||||
"paramsd.cc"]
|
||||
loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'json11', 'pthread']
|
||||
|
||||
env.Program("paramsd", loc_objs, LIBS=loc_libs)
|
||||
env.SharedLibrary("locationd", loc_objs, LIBS=loc_libs)
|
||||
loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread']
|
||||
|
||||
env.Program("ubloxd", [
|
||||
"ubloxd.cc",
|
||||
|
|
|
|||
|
|
@ -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