remove old params learner (#1918)
Willem Melching
5 years ago
committed by
GitHub
parent
b16e90c781
commit
e0e7c7486d
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