mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-08-21 16:23:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.6 release

Browse Source
This commit is contained in:
Vehicle Researcher
2019-06-28 21:11:30 +00:00
parent bbc7de2cbd
commit 7b469bc8be
974 changed files with 90130 additions and 100834 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/.gitignore
+1
View File
@@ -1,2 +1,3 @@
ubloxd
ubloxd_test
params_learner
selfdrive/locationd/Makefile
+43 -8
View File
@@ -13,28 +13,47 @@ WARN_FLAGS = -Werror=implicit-function-declaration \
-Werror=return-type \
-Werror=format-extra-args
CFLAGS = -std=gnu11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
CFLAGS = -std=gnu11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS) -Wall
CXXFLAGS = -std=c++11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS) -Wall
ZMQ_LIBS = -l:libczmq.a -l:libzmq.a
ifeq ($(ARCH),aarch64)
CFLAGS += -mcpu=cortex-a57
CXXFLAGS += -mcpu=cortex-a57
ZMQ_LIBS += -lgnustl_shared
endif
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
-l:libczmq.a -l:libzmq.a \
-lgnustl_shared
JSON_FLAGS = -I$(PHONELIBS)/json/src
JSON11_FLAGS = -I$(PHONELIBS)/json11
EXTRA_LIBS = -lpthread
ifeq ($(ARCH),x86_64)
ZMQ_FLAGS = -I$(BASEDIR)/phonelibs/zmq/x64/include
ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib \
-l:libczmq.a -l:libzmq.a
ZMQ_SHARED_LIBS = -L$(BASEDIR)/external/zmq/lib \
-lczmq -lzmq
else
EXTRA_LIBS += -llog -luuid
endif
.PHONY: all
all: ubloxd
all: ubloxd params_learner
include ../common/cereal.mk
LOC_OBJS = locationd_yawrate.o params_learner.o \
../common/swaglog.o \
../common/params.o \
../common/util.o \
$(PHONELIBS)/json11/json11.o \
$(PHONELIBS)/json/src/json.o \
$(CEREAL_OBJS)
LOC_DEPS := $(LOC_OBJS:.o=.d)
OBJS = ublox_msg.o \
ubloxd_main.o \
../common/swaglog.o \
@@ -45,6 +64,20 @@ OBJS = ublox_msg.o \
DEPS := $(OBJS:.o=.d) ubloxd.d ubloxd_test.d
liblocationd.so: $(LOC_OBJS)
@echo "[ LINK ] $@"
$(CXX) -shared -o '$@' $^ \
$(CEREAL_LIBS) \
$(ZMQ_SHARED_LIBS) \
$(EXTRA_LIBS)
params_learner: $(LOC_OBJS)
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
$(CEREAL_LIBS) \
$(ZMQ_LIBS) \
$(EXTRA_LIBS)
ubloxd: ubloxd.o $(OBJS)
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
@@ -65,6 +98,7 @@ ubloxd_test: ubloxd_test.o $(OBJS)
-Iinclude -I.. -I../.. \
$(CEREAL_CXXFLAGS) \
$(ZMQ_FLAGS) \
$(JSON11_FLAGS) \
$(JSON_FLAGS) \
-I../ \
-I../../ \
@@ -81,6 +115,7 @@ ubloxd_test: ubloxd_test.o $(OBJS)
.PHONY: clean
clean:
rm -f ubloxd ubloxd.d ubloxd.o ubloxd_test ubloxd_test.o ubloxd_test.d $(OBJS) $(DEPS)
rm -f ubloxd params_learner liblocationd.so ubloxd.d ubloxd.o ubloxd_test ubloxd_test.o ubloxd_test.d $(OBJS) $(LOC_OBJS) $(DEPS)
-include $(DEPS)
-include $(LOC_DEPS)
selfdrive/locationd/calibrationd.py
+4 -7
View File
@@ -1,6 +1,5 @@
#!/usr/bin/env python
import os
import zmq
import copy
import json
import numpy as np
@@ -9,7 +8,7 @@ from selfdrive.locationd.calibration_helpers import Calibration
from selfdrive.swaglog import cloudlog
from selfdrive.services import service_list
from common.params import Params
from common.transformations.model import model_height, get_camera_frame_from_model_frame, get_camera_frame_from_bigmodel_frame
from common.transformations.model import model_height, get_camera_frame_from_model_frame, get_camera_frame_from_medmodel_frame
from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
eon_intrinsics, get_calib_from_vp, H, W
@@ -85,7 +84,7 @@ class Calibrator(object):
extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
ke = eon_intrinsics.dot(extrinsic_matrix)
warp_matrix = get_camera_frame_from_model_frame(ke)
warp_matrix_big = get_camera_frame_from_bigmodel_frame(ke)
warp_matrix_big = get_camera_frame_from_medmodel_frame(ke)
cal_send = messaging.new_message()
cal_send.init('liveCalibration')
@@ -99,10 +98,8 @@ class Calibrator(object):
def calibrationd_thread(gctx=None, addr="127.0.0.1"):
context = zmq.Context()
cameraodometry = messaging.sub_sock(context, service_list['cameraOdometry'].port, addr=addr, conflate=True)
livecalibration = messaging.pub_sock(context, service_list['liveCalibration'].port)
cameraodometry = messaging.sub_sock(service_list['cameraOdometry'].port, addr=addr, conflate=True)
livecalibration = messaging.pub_sock(service_list['liveCalibration'].port)
calibrator = Calibrator(param_put=True)
# buffer with all the messages that still need to be input into the kalman
selfdrive/locationd/kalman/.gitignore
-4
View File
@@ -1,4 +0,0 @@
lane.cpp
gnss.cpp
loc*.cpp
pos_computer*.cpp
selfdrive/locationd/kalman/__init__.py
View File
selfdrive/locationd/kalman/chi2_lookup.py
-21
View File
@@ -1,21 +0,0 @@
import numpy as np
import os
def gen_chi2_ppf_lookup(max_dim=200):
from scipy.stats import chi2
table = np.zeros((max_dim, 98))
for dim in range(1,max_dim):
table[dim] = chi2.ppf(np.arange(.01, .99, .01), dim)
#outfile = open('chi2_lookup_table', 'w')
np.save('chi2_lookup_table', table)
def chi2_ppf(p, dim):
table = np.load(os.path.dirname(os.path.realpath(__file__)) + '/chi2_lookup_table.npy')
result = np.interp(p, np.arange(.01, .99, .01), table[dim])
return result
if __name__== "__main__":
gen_chi2_ppf_lookup()
selfdrive/locationd/kalman/chi2_lookup_table.npy
BIN
View File
Binary file not shown.
selfdrive/locationd/kalman/ekf_c.c
-124
View File
@@ -1,124 +0,0 @@
#include <eigen3/Eigen/Dense>
#include <iostream>
typedef Eigen::Matrix<double, DIM, DIM, Eigen::RowMajor> DDM;
typedef Eigen::Matrix<double, EDIM, EDIM, Eigen::RowMajor> EEM;
typedef Eigen::Matrix<double, DIM, EDIM, Eigen::RowMajor> DEM;
void predict(double *in_x, double *in_P, double *in_Q, double dt) {
typedef Eigen::Matrix<double, MEDIM, MEDIM, Eigen::RowMajor> RRM;
double nx[DIM] = {0};
double in_F[EDIM*EDIM] = {0};
// functions from sympy
f_fun(in_x, dt, nx);
F_fun(in_x, dt, in_F);
EEM F(in_F);
EEM P(in_P);
EEM Q(in_Q);
RRM F_main = F.topLeftCorner(MEDIM, MEDIM);
P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose();
P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM);
P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose();
P = P + dt*Q;
// copy out state
memcpy(in_x, nx, DIM * sizeof(double));
memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
}
// note: extra_args dim only correct when null space projecting
// otherwise 1
template <int ZDIM, int EADIM, bool MAHA_TEST>
void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) {
typedef Eigen::Matrix<double, ZDIM, ZDIM, Eigen::RowMajor> ZZM;
typedef Eigen::Matrix<double, ZDIM, DIM, Eigen::RowMajor> ZDM;
typedef Eigen::Matrix<double, ZDIM, EDIM, Eigen::RowMajor> ZEM;
typedef Eigen::Matrix<double, Eigen::Dynamic, EDIM, Eigen::RowMajor> XEM;
typedef Eigen::Matrix<double, EDIM, ZDIM, Eigen::RowMajor> EZM;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> X1M;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> XXM;
double in_hx[ZDIM] = {0};
double in_H[ZDIM * DIM] = {0};
double in_H_mod[EDIM * DIM] = {0};
double delta_x[EDIM] = {0};
double x_new[DIM] = {0};
// state x, P
Eigen::Matrix<double, ZDIM, 1> z(in_z);
EEM P(in_P);
ZZM pre_R(in_R);
// functions from sympy
h_fun(in_x, in_ea, in_hx);
H_fun(in_x, in_ea, in_H);
ZDM pre_H(in_H);
// get y (y = z - hx)
Eigen::Matrix<double, ZDIM, 1> pre_y(in_hx); pre_y = z - pre_y;
X1M y; XXM H; XXM R;
if (Hea_fun){
typedef Eigen::Matrix<double, ZDIM, EADIM, Eigen::RowMajor> ZAM;
double in_Hea[ZDIM * EADIM] = {0};
Hea_fun(in_x, in_ea, in_Hea);
ZAM Hea(in_Hea);
XXM A = Hea.transpose().fullPivLu().kernel();
y = A.transpose() * pre_y;
H = A.transpose() * pre_H;
R = A.transpose() * pre_R * A;
} else {
y = pre_y;
H = pre_H;
R = pre_R;
}
// get modified H
H_mod_fun(in_x, in_H_mod);
DEM H_mod(in_H_mod);
XEM H_err = H * H_mod;
// Do mahalobis distance test
if (MAHA_TEST){
XXM a = (H_err * P * H_err.transpose() + R).inverse();
double maha_dist = y.transpose() * a * y;
if (maha_dist > MAHA_THRESHOLD){
R = 1.0e16 * R;
}
}
// Outlier resilient weighting
double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum());
// kalman gains and I_KH
XXM S = ((H_err * P) * H_err.transpose()) + R/weight;
XEM KT = S.fullPivLu().solve(H_err * P.transpose());
//EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE?
//EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose();
//std::cout << "Here is the matrix rot:\n" << K << std::endl;
EEM I_KH = Eigen::Matrix<double, EDIM, EDIM>::Identity() - (KT.transpose() * H_err);
// update state by injecting dx
Eigen::Matrix<double, EDIM, 1> dx(delta_x);
dx = (KT.transpose() * y);
memcpy(delta_x, dx.data(), EDIM * sizeof(double));
err_fun(in_x, delta_x, x_new);
Eigen::Matrix<double, DIM, 1> x(x_new);
// update cov
P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT);
// copy out state
memcpy(in_x, x.data(), DIM * sizeof(double));
memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
memcpy(in_z, y.data(), y.rows() * sizeof(double));
}
selfdrive/locationd/kalman/ekf_sym.py
-562
View File
@@ -1,562 +0,0 @@
import os
from bisect import bisect_right
import sympy as sp
import numpy as np
from numpy import dot
from common.ffi_wrapper import compile_code, wrap_compiled
from common.sympy_helpers import sympy_into_c
from chi2_lookup import chi2_ppf
EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__))
def solve(a, b):
if a.shape[0] == 1 and a.shape[1] == 1:
#assert np.allclose(b/a[0][0], np.linalg.solve(a, b))
return b/a[0][0]
else:
return np.linalg.solve(a, b)
def null(H, eps=1e-12):
u, s, vh = np.linalg.svd(H)
padding = max(0,np.shape(H)[1]-np.shape(s)[0])
null_mask = np.concatenate(((s <= eps), np.ones((padding,),dtype=bool)),axis=0)
null_space = np.compress(null_mask, vh, axis=0)
return np.transpose(null_space)
def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[]):
# optional state transition matrix, H modifier
# and err_function if an error-state kalman filter (ESKF)
# is desired. Best described in "Quaternion kinematics
# for the error-state Kalman filter" by Joan Sola
if eskf_params:
err_eqs = eskf_params[0]
inv_err_eqs = eskf_params[1]
H_mod_sym = eskf_params[2]
f_err_sym = eskf_params[3]
x_err_sym = eskf_params[4]
else:
nom_x = sp.MatrixSymbol('nom_x',dim_x,1)
true_x = sp.MatrixSymbol('true_x',dim_x,1)
delta_x = sp.MatrixSymbol('delta_x',dim_x,1)
err_function_sym = sp.Matrix(nom_x + delta_x)
inv_err_function_sym = sp.Matrix(true_x - nom_x)
err_eqs = [err_function_sym, nom_x, delta_x]
inv_err_eqs = [inv_err_function_sym, nom_x, true_x]
H_mod_sym = sp.Matrix(np.eye(dim_x))
f_err_sym = f_sym
x_err_sym = x_sym
# This configures the multi-state augmentation
# needed for EKF-SLAM with MSCKF (Mourikis et al 2007)
if msckf_params:
msckf = True
dim_main = msckf_params[0] # size of the main state
dim_augment = msckf_params[1] # size of one augment state chunk
dim_main_err = msckf_params[2]
dim_augment_err = msckf_params[3]
N = msckf_params[4]
feature_track_kinds = msckf_params[5]
assert dim_main + dim_augment*N == dim_x
assert dim_main_err + dim_augment_err*N == dim_err
else:
msckf = False
dim_main = dim_x
dim_augment = 0
dim_main_err = dim_err
dim_augment_err = 0
N = 0
# linearize with jacobians
F_sym = f_err_sym.jacobian(x_err_sym)
for sym in x_err_sym:
F_sym = F_sym.subs(sym, 0)
for i in xrange(len(obs_eqs)):
obs_eqs[i].append(obs_eqs[i][0].jacobian(x_sym))
if msckf and obs_eqs[i][1] in feature_track_kinds:
obs_eqs[i].append(obs_eqs[i][0].jacobian(obs_eqs[i][2]))
else:
obs_eqs[i].append(None)
# collect sympy functions
sympy_functions = []
# error functions
sympy_functions.append(('err_fun', err_eqs[0], [err_eqs[1], err_eqs[2]]))
sympy_functions.append(('inv_err_fun', inv_err_eqs[0], [inv_err_eqs[1], inv_err_eqs[2]]))
# H modifier for ESKF updates
sympy_functions.append(('H_mod_fun', H_mod_sym, [x_sym]))
# state propagation function
sympy_functions.append(('f_fun', f_sym, [x_sym, dt_sym]))
sympy_functions.append(('F_fun', F_sym, [x_sym, dt_sym]))
# observation functions
for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs:
sympy_functions.append(('h_%d' % kind, h_sym, [x_sym, ea_sym]))
sympy_functions.append(('H_%d' % kind, H_sym, [x_sym, ea_sym]))
if msckf and kind in feature_track_kinds:
sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym]))
# Generate and wrap all th c code
header, code = sympy_into_c(sympy_functions)
extra_header = "#define DIM %d\n" % dim_x
extra_header += "#define EDIM %d\n" % dim_err
extra_header += "#define MEDIM %d\n" % dim_main_err
extra_header += "typedef void (*Hfun)(double *, double *, double *);\n"
extra_header += "\nvoid predict(double *x, double *P, double *Q, double dt);"
extra_post = ""
for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs:
if msckf and kind in feature_track_kinds:
He_str = 'He_%d' % kind
# ea_dim = ea_sym.shape[0]
else:
He_str = 'NULL'
# ea_dim = 1 # not really dim of ea but makes c function work
maha_thresh = chi2_ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection
maha_test = kind in maha_test_kinds
extra_post += """
void update_%d(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<%d,%d,%d>(in_x, in_P, h_%d, H_%d, %s, in_z, in_R, in_ea, MAHA_THRESH_%d);
}
""" % (kind, h_sym.shape[0], 3, maha_test, kind, kind, He_str, kind)
extra_header += "\nconst static double MAHA_THRESH_%d = %f;" % (kind, maha_thresh)
extra_header += "\nvoid update_%d(double *, double *, double *, double *, double *);" % kind
code += "\n" + extra_header
code += "\n" + open(os.path.join(EXTERNAL_PATH, "ekf_c.c")).read()
code += "\n" + extra_post
header += "\n" + extra_header
compile_code(name, code, header, EXTERNAL_PATH)
class EKF_sym(object):
def __init__(self, name, Q, x_initial, P_initial, dim_main, dim_main_err,
N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[]):
'''
Generates process function and all
observation functions for the kalman
filter.
'''
if N > 0:
self.msckf = True
else:
self.msckf = False
self.N = N
self.dim_augment = dim_augment
self.dim_augment_err = dim_augment_err
self.dim_main = dim_main
self.dim_main_err = dim_main_err
# state
x_initial = x_initial.reshape((-1, 1))
self.dim_x = x_initial.shape[0]
self.dim_err = P_initial.shape[0]
assert dim_main + dim_augment*N == self.dim_x
assert dim_main_err + dim_augment_err*N == self.dim_err
# kinds that should get mahalanobis distance
# tested for outlier rejection
self.maha_test_kinds = maha_test_kinds
# process noise
self.Q = Q
# rewind stuff
self.rewind_t = []
self.rewind_states = []
self.rewind_obscache = []
self.init_state(x_initial, P_initial, None)
ffi, lib = wrap_compiled(name, EXTERNAL_PATH)
kinds, self.feature_track_kinds = [], []
for func in dir(lib):
if func[:2] == 'h_':
kinds.append(int(func[2:]))
if func[:3] == 'He_':
self.feature_track_kinds.append(int(func[3:]))
# wrap all the sympy functions
def wrap_1lists(name):
func = eval("lib.%s" % name, {"lib":lib})
def ret(lst1, out):
func(ffi.cast("double *", lst1.ctypes.data),
ffi.cast("double *", out.ctypes.data))
return ret
def wrap_2lists(name):
func = eval("lib.%s" % name, {"lib":lib})
def ret(lst1, lst2, out):
func(ffi.cast("double *", lst1.ctypes.data),
ffi.cast("double *", lst2.ctypes.data),
ffi.cast("double *", out.ctypes.data))
return ret
def wrap_1list_1float(name):
func = eval("lib.%s" % name, {"lib":lib})
def ret(lst1, fl, out):
func(ffi.cast("double *", lst1.ctypes.data),
ffi.cast("double", fl),
ffi.cast("double *", out.ctypes.data))
return ret
self.f = wrap_1list_1float("f_fun")
self.F = wrap_1list_1float("F_fun")
self.err_function = wrap_2lists("err_fun")
self.inv_err_function = wrap_2lists("inv_err_fun")
self.H_mod = wrap_1lists("H_mod_fun")
self.hs, self.Hs, self.Hes = {}, {}, {}
for kind in kinds:
self.hs[kind] = wrap_2lists("h_%d" % kind)
self.Hs[kind] = wrap_2lists("H_%d" % kind)
if self.msckf and kind in self.feature_track_kinds:
self.Hes[kind] = wrap_2lists("He_%d" % kind)
# wrap the C++ predict function
def _predict_blas(x, P, dt):
lib.predict(ffi.cast("double *", x.ctypes.data),
ffi.cast("double *", P.ctypes.data),
ffi.cast("double *", self.Q.ctypes.data),
ffi.cast("double", dt))
return x, P
# wrap the C++ update function
def fun_wrapper(f, kind):
f = eval("lib.%s" % f, {"lib": lib})
def _update_inner_blas(x, P, z, R, extra_args):
f(ffi.cast("double *", x.ctypes.data),
ffi.cast("double *", P.ctypes.data),
ffi.cast("double *", z.ctypes.data),
ffi.cast("double *", R.ctypes.data),
ffi.cast("double *", extra_args.ctypes.data))
if self.msckf and kind in self.feature_track_kinds:
y = z[:-len(extra_args)]
else:
y = z
return x, P, y
return _update_inner_blas
self._updates = {}
for kind in kinds:
self._updates[kind] = fun_wrapper("update_%d" % kind, kind)
def _update_blas(x, P, kind, z, R, extra_args=[]):
return self._updates[kind](x, P, z, R, extra_args)
# assign the functions
self._predict = _predict_blas
#self._predict = self._predict_python
self._update = _update_blas
#self._update = self._update_python
def init_state(self, state, covs, filter_time):
self.x = np.array(state.reshape((-1, 1))).astype(np.float64)
self.P = np.array(covs).astype(np.float64)
self.filter_time = filter_time
self.augment_times = [0]*self.N
self.rewind_obscache = []
self.rewind_t = []
self.rewind_states = []
def augment(self):
# TODO this is not a generalized way of doing
# this and implies that the augmented states
# are simply the first (dim_augment_state)
# elements of the main state.
assert self.msckf
d1 = self.dim_main
d2 = self.dim_main_err
d3 = self.dim_augment
d4 = self.dim_augment_err
# push through augmented states
self.x[d1:-d3] = self.x[d1+d3:]
self.x[-d3:] = self.x[:d3]
assert self.x.shape == (self.dim_x, 1)
# push through augmented covs
assert self.P.shape == (self.dim_err, self.dim_err)
P_reduced = self.P
P_reduced = np.delete(P_reduced, np.s_[d2:d2+d4], axis=1)
P_reduced = np.delete(P_reduced, np.s_[d2:d2+d4], axis=0)
assert P_reduced.shape == (self.dim_err -d4, self.dim_err -d4)
to_mult = np.zeros((self.dim_err, self.dim_err - d4))
to_mult[:-d4,:] = np.eye(self.dim_err - d4)
to_mult[-d4:,:d4] = np.eye(d4)
self.P = to_mult.dot(P_reduced.dot(to_mult.T))
self.augment_times = self.augment_times[1:]
self.augment_times.append(self.filter_time)
assert self.P.shape == (self.dim_err, self.dim_err)
def state(self):
return np.array(self.x).flatten()
def covs(self):
return self.P
def rewind(self, t):
# find where we are rewinding to
idx = bisect_right(self.rewind_t, t)
assert self.rewind_t[idx-1] <= t
assert self.rewind_t[idx] > t # must be true, or rewind wouldn't be called
# set the state to the time right before that
self.filter_time = self.rewind_t[idx-1]
self.x[:] = self.rewind_states[idx-1][0]
self.P[:] = self.rewind_states[idx-1][1]
# return the observations we rewound over for fast forwarding
ret = self.rewind_obscache[idx:]
# throw away the old future
# TODO: is this making a copy?
self.rewind_t = self.rewind_t[:idx]
self.rewind_states = self.rewind_states[:idx]
self.rewind_obscache = self.rewind_obscache[:idx]
return ret
def checkpoint(self, obs):
# push to rewinder
self.rewind_t.append(self.filter_time)
self.rewind_states.append((np.copy(self.x), np.copy(self.P)))
self.rewind_obscache.append(obs)
# only keep a certain number around
REWIND_TO_KEEP = 512
self.rewind_t = self.rewind_t[-REWIND_TO_KEEP:]
self.rewind_states = self.rewind_states[-REWIND_TO_KEEP:]
self.rewind_obscache = self.rewind_obscache[-REWIND_TO_KEEP:]
def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False):
# TODO handle rewinding at this level"
# rewind
if t < self.filter_time:
if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] -1.0:
print("observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time))
return None
rewound = self.rewind(t)
else:
rewound = []
ret = self._predict_and_update_batch(t, kind, z, R, extra_args, augment)
# optional fast forward
for r in rewound:
self._predict_and_update_batch(*r)
return ret
def _predict_and_update_batch(self, t, kind, z, R, extra_args, augment=False):
"""The main kalman filter function
Predicts the state and then updates a batch of observations
dim_x: dimensionality of the state space
dim_z: dimensionality of the observation and depends on kind
n: number of observations
Args:
t (float): Time of observation
kind (int): Type of observation
z (vec [n,dim_z]): Measurements
R (mat [n,dim_z, dim_z]): Measurement Noise
extra_args (list, [n]): Values used in H computations
"""
# initialize time
if self.filter_time is None:
self.filter_time = t
# predict
dt = t - self.filter_time
assert dt >= 0
self.x, self.P = self._predict(self.x, self.P, dt)
self.filter_time = t
xk_km1, Pk_km1 = np.copy(self.x).flatten(), np.copy(self.P)
# update batch
y = []
for i in xrange(len(z)):
# these are from the user, so we canonicalize them
z_i = np.array(z[i], dtype=np.float64, order='F')
R_i = np.array(R[i], dtype=np.float64, order='F')
extra_args_i = np.array(extra_args[i], dtype=np.float64, order='F')
# update
self.x, self.P, y_i = self._update(self.x, self.P, kind, z_i, R_i, extra_args=extra_args_i)
y.append(y_i)
xk_k, Pk_k = np.copy(self.x).flatten(), np.copy(self.P)
if augment:
self.augment()
# checkpoint
self.checkpoint((t, kind, z, R, extra_args))
return xk_km1, xk_k, Pk_km1, Pk_k, t, kind, y, z, extra_args
def _predict_python(self, x, P, dt):
x_new = np.zeros(x.shape, dtype=np.float64)
self.f(x, dt, x_new)
F = np.zeros(P.shape, dtype=np.float64)
self.F(x, dt, F)
if not self.msckf:
P = dot(dot(F, P), F.T)
else:
# Update the predicted state covariance:
# Pk+1|k = |F*Pii*FT + Q*dt F*Pij |
# |PijT*FT Pjj |
# Where F is the jacobian of the main state
# predict function, Pii is the main state's
# covariance and Q its process noise. Pij
# is the covariance between the augmented
# states and the main state.
#
d2 = self.dim_main_err # known at compile time
F_curr = F[:d2, :d2]
P[:d2, :d2] = (F_curr.dot(P[:d2, :d2])).dot(F_curr.T)
P[:d2, d2:] = F_curr.dot(P[:d2, d2:])
P[d2:, :d2] = P[d2:, :d2].dot(F_curr.T)
P += dt*self.Q
return x_new, P
def _update_python(self, x, P, kind, z, R, extra_args=[]):
# init vars
z = z.reshape((-1, 1))
h = np.zeros(z.shape, dtype=np.float64)
H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64)
# C functions
self.hs[kind](x, extra_args, h)
self.Hs[kind](x, extra_args, H)
# y is the "loss"
y = z - h
# *** same above this line ***
if self.msckf and kind in self.Hes:
# Do some algebraic magic to decorrelate
He = np.zeros((z.shape[0], len(extra_args)), dtype=np.float64)
self.Hes[kind](x, extra_args, He)
# TODO: Don't call a function here, do projection locally
A = null(He.T)
y = A.T.dot(y)
H = A.T.dot(H)
R = A.T.dot(R.dot(A))
# TODO If nullspace isn't the dimension we want
if A.shape[1] + He.shape[1] != A.shape[0]:
print('Warning: null space projection failed, measurement ignored')
return x, P, np.zeros(A.shape[0] - He.shape[1])
# if using eskf
H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64)
self.H_mod(x, H_mod)
H = H.dot(H_mod)
# Do mahalobis distance test
# currently just runs on msckf observations
# could run on anything if needed
if self.msckf and kind in self.maha_test_kinds:
a = np.linalg.inv(H.dot(P).dot(H.T) + R)
maha_dist = y.T.dot(a.dot(y))
if maha_dist > chi2_ppf(0.95, y.shape[0]):
R = 10e16*R
# *** same below this line ***
# Outlier resilient weighting as described in:
# "A Kalman Filter for Robust Outlier Detection - Jo-Anne Ting, ..."
weight = 1 #(1.5)/(1 + np.sum(y**2)/np.sum(R))
S = dot(dot(H, P), H.T) + R/weight
K = solve(S, dot(H, P.T)).T
I_KH = np.eye(P.shape[0]) - dot(K, H)
# update actual state
delta_x = dot(K, y)
P = dot(dot(I_KH, P), I_KH.T) + dot(dot(K, R), K.T)
# inject observed error into state
x_new = np.zeros(x.shape, dtype=np.float64)
self.err_function(x, delta_x, x_new)
return x_new, P, y.flatten()
def maha_test(self, x, P, kind, z, R, extra_args=[], maha_thresh=0.95):
# init vars
z = z.reshape((-1, 1))
h = np.zeros(z.shape, dtype=np.float64)
H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64)
# C functions
self.hs[kind](x, extra_args, h)
self.Hs[kind](x, extra_args, H)
# y is the "loss"
y = z - h
# if using eskf
H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64)
self.H_mod(x, H_mod)
H = H.dot(H_mod)
a = np.linalg.inv(H.dot(P).dot(H.T) + R)
maha_dist = y.T.dot(a.dot(y))
if maha_dist > chi2_ppf(maha_thresh, y.shape[0]):
return False
else:
return True
def rts_smooth(self, estimates, norm_quats=False):
'''
Returns rts smoothed results of
kalman filter estimates
If the kalman state is augmented with
old states only the main state is smoothed
'''
xk_n = estimates[-1][0]
Pk_n = estimates[-1][2]
Fk_1 = np.zeros(Pk_n.shape, dtype=np.float64)
states_smoothed = [xk_n]
covs_smoothed = [Pk_n]
for k in xrange(len(estimates) - 2, -1, -1):
xk1_n = xk_n
if norm_quats:
xk1_n[3:7] /= np.linalg.norm(xk1_n[3:7])
Pk1_n = Pk_n
xk1_k, _, Pk1_k, _, t2, _, _, _, _ = estimates[k + 1]
_, xk_k, _, Pk_k, t1, _, _, _, _ = estimates[k]
dt = t2 - t1
self.F(xk_k, dt, Fk_1)
d1 = self.dim_main
d2 = self.dim_main_err
Ck = np.linalg.solve(Pk1_k[:d2,:d2], Fk_1[:d2,:d2].dot(Pk_k[:d2,:d2].T)).T
xk_n = xk_k
delta_x = np.zeros((Pk_n.shape[0], 1), dtype=np.float64)
self.inv_err_function(xk1_k, xk1_n, delta_x)
delta_x[:d2] = Ck.dot(delta_x[:d2])
x_new = np.zeros((xk_n.shape[0], 1), dtype=np.float64)
self.err_function(xk_k, delta_x, x_new)
xk_n[:d1] = x_new[:d1,0]
Pk_n = Pk_k
Pk_n[:d2,:d2] = Pk_k[:d2,:d2] + Ck.dot(Pk1_n[:d2,:d2] - Pk1_k[:d2,:d2]).dot(Ck.T)
states_smoothed.append(xk_n)
covs_smoothed.append(Pk_n)
return np.flipud(np.vstack(states_smoothed)), np.stack(covs_smoothed, 0)[::-1]
selfdrive/locationd/kalman/kalman_helpers.py
-165
View File
@@ -1,165 +0,0 @@
import numpy as np
import os
from bisect import bisect
from tqdm import tqdm
class ObservationKind(object):
UNKNOWN = 0
NO_OBSERVATION = 1
GPS_NED = 2
ODOMETRIC_SPEED = 3
PHONE_GYRO = 4
GPS_VEL = 5
PSEUDORANGE_GPS = 6
PSEUDORANGE_RATE_GPS = 7
SPEED = 8
NO_ROT = 9
PHONE_ACCEL = 10
ORB_POINT = 11
ECEF_POS = 12
CAMERA_ODO_TRANSLATION = 13
CAMERA_ODO_ROTATION = 14
ORB_FEATURES = 15
MSCKF_TEST = 16
FEATURE_TRACK_TEST = 17
LANE_PT = 18
IMU_FRAME = 19
PSEUDORANGE_GLONASS = 20
PSEUDORANGE_RATE_GLONASS = 21
PSEUDORANGE = 22
PSEUDORANGE_RATE = 23
names = ['Unknown',
'No observation',
'GPS NED',
'Odometric speed',
'Phone gyro',
'GPS velocity',
'GPS pseudorange',
'GPS pseudorange rate',
'Speed',
'No rotation',
'Phone acceleration',
'ORB point',
'ECEF pos',
'camera odometric translation',
'camera odometric rotation',
'ORB features',
'MSCKF test',
'Feature track test',
'Lane ecef point',
'imu frame eulers',
'GLONASS pseudorange',
'GLONASS pseudorange rate']
@classmethod
def to_string(cls, kind):
return cls.names[kind]
SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
ObservationKind.PSEUDORANGE_RATE_GPS,
ObservationKind.PSEUDORANGE_GLONASS,
ObservationKind.PSEUDORANGE_RATE_GLONASS]
def run_car_ekf_offline(kf, observations_by_kind):
from laika.raw_gnss import GNSSMeasurement # pylint: disable=import-error
observations = []
# create list of observations with element format: [kind, time, data]
for kind in observations_by_kind:
for t, data in zip(observations_by_kind[kind][0], observations_by_kind[kind][1]):
observations.append([t, kind, data])
observations.sort(key=lambda obs: obs[0])
times, estimates = run_observations_through_filter(kf, observations)
forward_states = np.stack(e[1] for e in estimates)
forward_covs = np.stack(e[3] for e in estimates)
smoothed_states, smoothed_covs = kf.rts_smooth(estimates)
observations_dict = {}
# TODO assuming observations and estimates
# are same length may not work with VO
for e in estimates:
t = e[4]
kind = str(int(e[5]))
res = e[6]
z = e[7]
ea = e[8]
if len(z) == 0:
continue
if kind not in observations_dict:
observations_dict[kind] = {}
observations_dict[kind]['t'] = np.array(len(z)*[t])
observations_dict[kind]['z'] = np.array(z)
observations_dict[kind]['ea'] = np.array(ea)
observations_dict[kind]['residual'] = np.array(res)
else:
observations_dict[kind]['t'] = np.append(observations_dict[kind]['t'], np.array(len(z)*[t]))
observations_dict[kind]['z'] = np.vstack((observations_dict[kind]['z'], np.array(z)))
observations_dict[kind]['ea'] = np.vstack((observations_dict[kind]['ea'], np.array(ea)))
observations_dict[kind]['residual'] = np.vstack((observations_dict[kind]['residual'], np.array(res)))
# add svIds to gnss data
for kind in map(str, SAT_OBS):
if int(kind) in observations_by_kind and kind in observations_dict:
observations_dict[kind]['svIds'] = np.array([])
observations_dict[kind]['CNO'] = np.array([])
observations_dict[kind]['std'] = np.array([])
for obs in observations_by_kind[int(kind)][1]:
observations_dict[kind]['svIds'] = np.append(observations_dict[kind]['svIds'],
np.array([obs[:,GNSSMeasurement.PRN]]))
observations_dict[kind]['std'] = np.append(observations_dict[kind]['std'],
np.array([obs[:,GNSSMeasurement.PR_STD]]))
return smoothed_states, smoothed_covs, forward_states, forward_covs, times, observations_dict
def run_observations_through_filter(kf, observations, filter_time=None):
estimates = []
for obs in tqdm(observations):
t = obs[0]
kind = obs[1]
data = obs[2]
estimates.append(kf.predict_and_observe(t, kind, data))
times = [x[4] for x in estimates]
return times, estimates
def save_residuals_plot(obs, save_path, data_name):
import matplotlib.pyplot as plt
import mpld3 # pylint: disable=import-error
fig = plt.figure(figsize=(10,20))
fig.suptitle('Residuals of ' + data_name, fontsize=24)
n = len(obs.keys())
start_times = [obs[kind]['t'][0] for kind in obs]
start_time = min(start_times)
xlims = [start_time + 3, start_time + 60]
for i, kind in enumerate(obs):
ax = fig.add_subplot(n, 1, i+1)
ax.set_xlim(xlims)
t = obs[kind]['t']
res = obs[kind]['residual']
start_idx = bisect(t, xlims[0])
if len(res) == start_idx:
continue
ylim = max(np.linalg.norm(res[start_idx:], axis=1))
ax.set_ylim([-ylim, ylim])
if int(kind) in SAT_OBS:
svIds = obs[kind]['svIds']
for svId in set(svIds):
svId_idx = (svIds == svId)
t = obs[kind]['t'][svId_idx]
res = obs[kind]['residual'][svId_idx]
ax.plot(t, res, label='SV ' + str(int(svId)))
ax.legend(loc='right')
else:
ax.plot(t, res)
plt.title('Residual of kind ' + ObservationKind.to_string(int(kind)), fontsize=20)
plt.tight_layout()
os.makedirs(save_path)
mpld3.save_html(fig, save_path + 'residuals_plot.html')
selfdrive/locationd/kalman/loc_local_kf.py
-128
View File
@@ -1,128 +0,0 @@
#!/usr/bin/env python
import numpy as np
from selfdrive.locationd.kalman import loc_local_model
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind
from selfdrive.locationd.kalman.ekf_sym import EKF_sym
class States(object):
VELOCITY = slice(0,3) # device frame velocity in m/s
ANGULAR_VELOCITY = slice(3, 6) # roll, pitch and yaw rates in device frame in radians/s
GYRO_BIAS = slice(6, 9) # roll, pitch and yaw biases
ODO_SCALE = slice(9, 10) # odometer scale
ACCELERATION = slice(10, 13) # Acceleration in device frame in m/s**2
class LocLocalKalman(object):
def __init__(self):
x_initial = np.array([0, 0, 0,
0, 0, 0,
0, 0, 0,
1,
0, 0, 0])
# state covariance
P_initial = np.diag([10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
0.05**2, 0.05**2, 0.05**2,
0.02**2,
1**2, 1**2, 1**2])
# process noise
Q = np.diag([0.0**2, 0.0**2, 0.0**2,
.01**2, .01**2, .01**2,
(0.005/100)**2, (0.005/100)**2, (0.005/100)**2,
(0.02/100)**2,
3**2, 3**2, 3**2])
self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2])}
# MSCKF stuff
self.dim_state = len(x_initial)
self.dim_main = self.dim_state
name = 'loc_local'
loc_local_model.gen_model(name, self.dim_state)
# init filter
self.filter = EKF_sym(name, Q, x_initial, P_initial, self.dim_main, self.dim_main)
@property
def x(self):
return self.filter.state()
@property
def t(self):
return self.filter.filter_time
@property
def P(self):
return self.filter.covs()
def predict(self, t):
if self.t:
# Does NOT modify filter state
return self.filter._predict(self.x, self.P, t - self.t)[0]
else:
raise RuntimeError("Request predict on filter with uninitialized time")
def rts_smooth(self, estimates):
return self.filter.rts_smooth(estimates, norm_quats=True)
def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
if covs_diag is not None:
P = np.diag(covs_diag)
elif covs is not None:
P = covs
else:
P = self.filter.covs()
self.filter.init_state(state, P, filter_time)
def predict_and_observe(self, t, kind, data):
if len(data) > 0:
data = np.atleast_2d(data)
if kind == ObservationKind.CAMERA_ODO_TRANSLATION:
r = self.predict_and_update_odo_trans(data, t, kind)
elif kind == ObservationKind.CAMERA_ODO_ROTATION:
r = self.predict_and_update_odo_rot(data, t, kind)
elif kind == ObservationKind.ODOMETRIC_SPEED:
r = self.predict_and_update_odo_speed(data, t, kind)
else:
r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))
return r
def get_R(self, kind, n):
obs_noise = self.obs_noise[kind]
dim = obs_noise.shape[0]
R = np.zeros((n, dim, dim))
for i in xrange(n):
R[i,:,:] = obs_noise
return R
def predict_and_update_odo_speed(self, speed, t, kind):
z = np.array(speed)
R = np.zeros((len(speed), 1, 1))
for i, _ in enumerate(z):
R[i,:,:] = np.diag([0.2**2])
return self.filter.predict_and_update_batch(t, kind, z, R)
def predict_and_update_odo_trans(self, trans, t, kind):
z = trans[:,:3]
R = np.zeros((len(trans), 3, 3))
for i, _ in enumerate(z):
R[i,:,:] = np.diag(trans[i,3:]**2)
return self.filter.predict_and_update_batch(t, kind, z, R)
def predict_and_update_odo_rot(self, rot, t, kind):
z = rot[:,:3]
R = np.zeros((len(rot), 3, 3))
for i, _ in enumerate(z):
R[i,:,:] = np.diag(rot[i,3:]**2)
return self.filter.predict_and_update_batch(t, kind, z, R)
if __name__ == "__main__":
LocLocalKalman()
selfdrive/locationd/kalman/loc_local_model.py
-80
View File
@@ -1,80 +0,0 @@
import numpy as np
import sympy as sp
import os
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind
from selfdrive.locationd.kalman.ekf_sym import gen_code
def gen_model(name, dim_state):
# check if rebuild is needed
try:
dir_path = os.path.dirname(__file__)
deps = [dir_path + '/' + 'ekf_c.c',
dir_path + '/' + 'ekf_sym.py',
dir_path + '/' + 'loc_local_model.py',
dir_path + '/' + 'loc_local_kf.py']
outs = [dir_path + '/' + name + '.o',
dir_path + '/' + name + '.so',
dir_path + '/' + name + '.cpp']
out_times = map(os.path.getmtime, outs)
dep_times = map(os.path.getmtime, deps)
rebuild = os.getenv("REBUILD", False)
if min(out_times) > max(dep_times) and not rebuild:
return
map(os.remove, outs)
except OSError:
pass
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
v = state[0:3,:]
omega = state[3:6,:]
vroll, vpitch, vyaw = omega
vx, vy, vz = v
roll_bias, pitch_bias, yaw_bias = state[6:9,:]
odo_scale = state[9,:]
accel = state[10:13,:]
dt = sp.Symbol('dt')
# Time derivative of the state as a function of state
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[:3,:] = accel
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = sp.Matrix(state + dt*state_dot)
#
# Observation functions
#
# extra args
#imu_rot = euler_rotate(*imu_angles)
#h_gyro_sym = imu_rot*sp.Matrix([vroll + roll_bias,
# vpitch + pitch_bias,
# vyaw + yaw_bias])
h_gyro_sym = sp.Matrix([vroll + roll_bias,
vpitch + pitch_bias,
vyaw + yaw_bias])
speed = vx**2 + vy**2 + vz**2
h_speed_sym = sp.Matrix([sp.sqrt(speed)*odo_scale])
h_relative_motion = sp.Matrix(v)
h_phone_rot_sym = sp.Matrix([vroll,
vpitch,
vyaw])
obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None],
[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None]]
gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state)
selfdrive/locationd/locationd_local.py
-285
View File
@@ -1,285 +0,0 @@
#!/usr/bin/env python
import os
import zmq
import math
import json
os.environ["OMP_NUM_THREADS"] = "1"
import numpy as np
from bisect import bisect_right
from cereal import car
from common.params import Params
from common.numpy_fast import clip
import selfdrive.messaging as messaging
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.services import service_list
from selfdrive.locationd.kalman.loc_local_kf import LocLocalKalman
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind
DEBUG = False
kf = LocLocalKalman() # Make sure that model is generated on import time
MAX_ANGLE_OFFSET = math.radians(10.)
MAX_ANGLE_OFFSET_TH = math.radians(9.)
MIN_STIFFNESS = 0.5
MAX_STIFFNESS = 2.0
MIN_SR = 0.5
MAX_SR = 2.0
MIN_SR_TH = 0.55
MAX_SR_TH = 1.9
LEARNING_RATE = 3
class Localizer(object):
def __init__(self, disabled_logs=None, dog=None):
self.kf = LocLocalKalman()
self.reset_kalman()
self.sensor_data_t = 0.0
self.max_age = .2 # seconds
self.calibration_valid = False
if disabled_logs is None:
self.disabled_logs = list()
else:
self.disabled_logs = disabled_logs
def reset_kalman(self):
self.filter_time = None
self.observation_buffer = []
self.converter = None
self.speed_counter = 0
self.sensor_counter = 0
def liveLocationMsg(self, time):
fix = messaging.log.KalmanOdometry.new_message()
predicted_state = self.kf.x
fix.trans = [float(predicted_state[0]), float(predicted_state[1]), float(predicted_state[2])]
fix.rot = [float(predicted_state[3]), float(predicted_state[4]), float(predicted_state[5])]
return fix
def update_kalman(self, time, kind, meas):
idx = bisect_right([x[0] for x in self.observation_buffer], time)
self.observation_buffer.insert(idx, (time, kind, meas))
while self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
self.kf.predict_and_observe(*self.observation_buffer.pop(0))
def handle_cam_odo(self, log, current_time):
self.update_kalman(current_time, ObservationKind.CAMERA_ODO_ROTATION, np.concatenate([log.cameraOdometry.rot,
log.cameraOdometry.rotStd]))
self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans,
log.cameraOdometry.transStd]))
def handle_controls_state(self, log, current_time):
self.speed_counter += 1
if self.speed_counter % 5 == 0:
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.controlsState.vEgo]))
def handle_sensors(self, log, current_time):
for sensor_reading in log.sensorEvents:
# TODO does not yet account for double sensor readings in the log
if sensor_reading.type == 4:
self.sensor_counter += 1
if self.sensor_counter % LEARNING_RATE == 0:
self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-sensor_reading.gyro.v[2], -sensor_reading.gyro.v[1], -sensor_reading.gyro.v[0]])
def handle_log(self, log):
current_time = 1e-9 * log.logMonoTime
typ = log.which
if typ in self.disabled_logs:
return
if typ == "sensorEvents":
self.sensor_data_t = current_time
self.handle_sensors(log, current_time)
elif typ == "controlsState":
self.handle_controls_state(log, current_time)
elif typ == "cameraOdometry":
self.handle_cam_odo(log, current_time)
class ParamsLearner(object):
def __init__(self, VM, angle_offset=0., stiffness_factor=1.0, steer_ratio=None, learning_rate=1.0):
self.VM = VM
self.ao = math.radians(angle_offset)
self.slow_ao = math.radians(angle_offset)
self.x = stiffness_factor
self.sR = VM.sR if steer_ratio is None else steer_ratio
self.MIN_SR = MIN_SR * self.VM.sR
self.MAX_SR = MAX_SR * self.VM.sR
self.MIN_SR_TH = MIN_SR_TH * self.VM.sR
self.MAX_SR_TH = MAX_SR_TH * self.VM.sR
self.alpha1 = 0.01 * learning_rate
self.alpha2 = 0.0005 * learning_rate
self.alpha3 = 0.1 * learning_rate
self.alpha4 = 1.0 * learning_rate
def get_values(self):
return {
'angleOffsetAverage': math.degrees(self.slow_ao),
'stiffnessFactor': self.x,
'steerRatio': self.sR,
}
def update(self, psi, u, sa):
cF0 = self.VM.cF
cR0 = self.VM.cR
aR = self.VM.aR
aF = self.VM.aF
l = self.VM.l
m = self.VM.m
x = self.x
ao = self.ao
sR = self.sR
# Gradient descent: learn angle offset, tire stiffness and steer ratio.
if u > 10.0 and abs(math.degrees(sa)) < 15.:
self.ao -= self.alpha1 * 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2)
ao = self.slow_ao
self.slow_ao -= self.alpha2 * 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2)
self.x -= self.alpha3 * -2.0*cF0*cR0*l*m*u**3*(ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**3)
self.sR -= self.alpha4 * -2.0*cF0*cR0*l*u*x*(ao - sa)*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**3*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2)
if DEBUG:
# s1 = "Measured yaw rate % .6f" % psi
# ao = 0.
# s2 = "Uncompensated yaw % .6f" % (1.0*u*(-ao + sa)/(l*sR*(1 - m*u**2*(aF*cF0*x - aR*cR0*x)/(cF0*cR0*l**2*x**2))))
# instant_ao = aF*m*psi*sR*u/(cR0*l*x) - aR*m*psi*sR*u/(cF0*l*x) - l*psi*sR/u + sa
s4 = "Instant AO: % .2f Avg. AO % .2f" % (math.degrees(self.ao), math.degrees(self.slow_ao))
s5 = "Stiffnes: % .3f x" % self.x
print("{0} {1}".format(s4, s5))
self.ao = clip(self.ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET)
self.slow_ao = clip(self.slow_ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET)
self.x = clip(self.x, MIN_STIFFNESS, MAX_STIFFNESS)
self.sR = clip(self.sR, self.MIN_SR, self.MAX_SR)
# don't check stiffness for validity, as it can change quickly if sR is off
valid = abs(self.slow_ao) < MAX_ANGLE_OFFSET_TH and \
self.sR > self.MIN_SR_TH and self.sR < self.MAX_SR_TH
return valid
def locationd_thread(gctx, addr, disabled_logs):
ctx = zmq.Context()
poller = zmq.Poller()
controls_state_socket = messaging.sub_sock(ctx, service_list['controlsState'].port, poller, addr=addr, conflate=True)
sensor_events_socket = messaging.sub_sock(ctx, service_list['sensorEvents'].port, poller, addr=addr, conflate=True)
camera_odometry_socket = messaging.sub_sock(ctx, service_list['cameraOdometry'].port, poller, addr=addr, conflate=True)
kalman_odometry_socket = messaging.pub_sock(ctx, service_list['kalmanOdometry'].port)
live_parameters_socket = messaging.pub_sock(ctx, service_list['liveParameters'].port)
params_reader = Params()
cloudlog.info("Parameter learner is waiting for CarParams")
CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
VM = VehicleModel(CP)
cloudlog.info("Parameter learner got CarParams: %s" % CP.carFingerprint)
params = params_reader.get("LiveParameters")
# Check if car model matches
if params is not None:
params = json.loads(params)
if (params.get('carFingerprint', None) != CP.carFingerprint) or (params.get('carVin', CP.carVin) != CP.carVin):
cloudlog.info("Parameter learner found parameters for wrong car.")
params = None
if params is None:
params = {
'carFingerprint': CP.carFingerprint,
'carVin': CP.carVin,
'angleOffsetAverage': 0.0,
'stiffnessFactor': 1.0,
'steerRatio': VM.sR,
}
params_reader.put("LiveParameters", json.dumps(params))
cloudlog.info("Parameter learner resetting to default values")
cloudlog.info("Parameter starting with: %s" % str(params))
localizer = Localizer(disabled_logs=disabled_logs)
learner = ParamsLearner(VM,
angle_offset=params['angleOffsetAverage'],
stiffness_factor=params['stiffnessFactor'],
steer_ratio=params['steerRatio'],
learning_rate=LEARNING_RATE)
i = 1
while True:
for socket, event in poller.poll(timeout=1000):
log = messaging.recv_one(socket)
localizer.handle_log(log)
if socket is controls_state_socket:
if not localizer.kf.t:
continue
if i % LEARNING_RATE == 0:
# controlsState is not updating the Kalman Filter, so update KF manually
localizer.kf.predict(1e-9 * log.logMonoTime)
predicted_state = localizer.kf.x
yaw_rate = -float(predicted_state[5])
steering_angle = math.radians(log.controlsState.angleSteers)
params_valid = learner.update(yaw_rate, log.controlsState.vEgo, steering_angle)
log_t = 1e-9 * log.logMonoTime
sensor_data_age = log_t - localizer.sensor_data_t
params = messaging.new_message()
params.init('liveParameters')
params.liveParameters.valid = bool(params_valid)
params.liveParameters.sensorValid = bool(sensor_data_age < 5.0)
params.liveParameters.angleOffset = float(math.degrees(learner.ao))
params.liveParameters.angleOffsetAverage = float(math.degrees(learner.slow_ao))
params.liveParameters.stiffnessFactor = float(learner.x)
params.liveParameters.steerRatio = float(learner.sR)
live_parameters_socket.send(params.to_bytes())
if i % 6000 == 0: # once a minute
params = learner.get_values()
params['carFingerprint'] = CP.carFingerprint
params['carVin'] = CP.carVin
params_reader.put("LiveParameters", json.dumps(params))
params_reader.put("ControlsParams", json.dumps({'angle_model_bias': log.controlsState.angleModelBias}))
i += 1
elif socket is camera_odometry_socket:
msg = messaging.new_message()
msg.init('kalmanOdometry')
msg.logMonoTime = log.logMonoTime
msg.kalmanOdometry = localizer.liveLocationMsg(log.logMonoTime * 1e-9)
kalman_odometry_socket.send(msg.to_bytes())
elif socket is sensor_events_socket:
pass
def main(gctx=None, addr="127.0.0.1"):
IN_CAR = os.getenv("IN_CAR", False)
disabled_logs = os.getenv("DISABLED_LOGS", "").split(",")
# No speed for now
disabled_logs.append('controlsState')
if IN_CAR:
addr = "192.168.5.11"
locationd_thread(gctx, addr, disabled_logs)
if __name__ == "__main__":
main()
selfdrive/locationd/locationd_yawrate.cc
+311
View File
@@ -0,0 +1,311 @@
#include <iostream>
#include <csignal>
#include <cmath>
#include <czmq.h>
#include <capnp/message.h>
#include <capnp/serialize-packed.h>
#include <eigen3/Eigen/Dense>
#include "json11.hpp"
#include "cereal/gen/cpp/log.capnp.h"
#include "common/swaglog.h"
#include "common/messaging.h"
#include "common/params.h"
#include "common/timing.h"
#include "params_learner.h"
const int num_polls = 3;
class Localizer
{
Eigen::Matrix2d A;
Eigen::Matrix2d I;
Eigen::Matrix2d Q;
Eigen::Matrix2d P;
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) {
double dt = current_time - prev_update_time;
prev_update_time = current_time;
if (dt < 1.0e-9) {
return;
}
// x = A * x;
// P = A * P * A.transpose() + dt * Q;
// Simplify because A is unity
P = P + 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 handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time) {
for (cereal::SensorEventData::Reader sensor_event : sensor_events){
if (sensor_event.getType() == 4) {
sensor_data_time = current_time;
double meas = -sensor_event.getGyro().getV()[0];
update_state(C_gyro, R_gyro, current_time, meas);
}
}
}
void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
double R = 250.0 * pow(camera_odometry.getRotStd()[2], 2);
double meas = camera_odometry.getRot()[2];
update_state(C_posenet, R, current_time, meas);
}
void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) {
steering_angle = controls_state.getAngleSteers() * DEGREES_TO_RADIANS;
car_speed = controls_state.getVEgo();
controls_state_time = current_time;
}
public:
Eigen::Vector2d x;
double steering_angle = 0;
double car_speed = 0;
double prev_update_time = -1;
double controls_state_time = -1;
double sensor_data_time = -1;
Localizer() {
A << 1, 0, 0, 1;
I << 1, 0, 0, 1;
Q << pow(0.1, 2.0), 0, 0, pow(0.005 / 100.0, 2.0);
P << pow(1.0, 2.0), 0, 0, pow(0.05, 2.0);
C_posenet << 1, 0;
C_gyro << 1, 1;
x << 0, 0;
R_gyro = pow(0.05, 2.0);
}
cereal::Event::Which handle_log(const unsigned char* msg_dat, size_t msg_size) {
const kj::ArrayPtr<const capnp::word> view((const capnp::word*)msg_dat, msg_size);
capnp::FlatArrayMessageReader msg(view);
cereal::Event::Reader event = msg.getRoot<cereal::Event>();
double current_time = event.getLogMonoTime() / 1.0e9;
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;
}
return type;
}
};
int main(int argc, char *argv[]) {
auto ctx = zmq_ctx_new();
auto controls_state_sock = sub_sock(ctx, "tcp://127.0.0.1:8007");
auto sensor_events_sock = sub_sock(ctx, "tcp://127.0.0.1:8003");
auto camera_odometry_sock = sub_sock(ctx, "tcp://127.0.0.1:8066");
auto live_parameters_sock = zsock_new_pub("@tcp://*:8064");
assert(live_parameters_sock);
auto live_parameters_sock_raw = zsock_resolve(live_parameters_sock);
int err;
Localizer localizer;
zmq_pollitem_t polls[num_polls] = {{0}};
polls[0].socket = controls_state_sock;
polls[0].events = ZMQ_POLLIN;
polls[1].socket = sensor_events_sock;
polls[1].events = ZMQ_POLLIN;
polls[2].socket = camera_odometry_sock;
polls[2].events = ZMQ_POLLIN;
// Read car params
char *value;
size_t value_sz = 0;
LOGW("waiting for params to set vehicle model");
while (true) {
read_db_value(NULL, "CarParams", &value, &value_sz);
if (value_sz > 0) break;
usleep(100*1000);
}
LOGW("got %d bytes CarParams", value_sz);
// make copy due to alignment issues
auto amsg = kj::heapArray<capnp::word>((value_sz / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), value, value_sz);
free(value);
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
// Read params from previous run
const int result = read_db_value(NULL, "LiveParameters", &value, &value_sz);
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;
if (result == 0){
auto str = std::string(value, value_sz);
free(value);
std::string err;
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){
int ret = zmq_poll(polls, num_polls, 100);
if (ret == 0){
continue;
} else if (ret < 0){
break;
}
for (int i=0; i < num_polls; i++) {
if (polls[i].revents) {
zmq_msg_t msg;
err = zmq_msg_init(&msg);
assert(err == 0);
err = zmq_msg_recv(&msg, polls[i].socket, 0);
assert(err >= 0);
// make copy due to alignment issues, will be freed on out of scope
auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
auto which = localizer.handle_log((const unsigned char*)amsg.begin(), amsg.size());
zmq_msg_close(&msg);
if (which == cereal::Event::CONTROLS_STATE){
save_counter++;
double yaw_rate = -localizer.x[0];
bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle);
// TODO: Fix in replay
double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time;
double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
// Send parameters at 10 Hz
if (save_counter % 10 == 0){
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.setSensorValid(sensor_data_age < 5.0);
live_params.setAngleOffset(angle_offset_degrees);
live_params.setAngleOffsetAverage(angle_offset_average_degrees);
live_params.setStiffnessFactor(learner.x);
live_params.setSteerRatio(learner.sR);
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
}
// 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();
write_db_value(NULL, "LiveParameters", out.c_str(), out.length());
}
}
}
}
}
zmq_close(controls_state_sock);
zmq_close(sensor_events_sock);
zmq_close(camera_odometry_sock);
zmq_close(live_parameters_sock_raw);
return 0;
}
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) {
Localizer * loc = (Localizer*) localizer;
loc->handle_log(data, len);
}
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_t(void * localizer) {
Localizer * loc = (Localizer*) localizer;
return loc->prev_update_time;
}
}
selfdrive/locationd/params_learner.cc
+75
View File
@@ -0,0 +1,75 @@
#include <algorithm>
#include <cmath>
#include <iostream>
#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 * 15.)) {
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;
}
selfdrive/locationd/params_learner.h
+35
View File
@@ -0,0 +1,35 @@
#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);
};
selfdrive/locationd/test/ublox.py
+1 -3
View File
@@ -719,14 +719,12 @@ class UBlox:
self.dev = PandaSerial(self.panda, 1, self.baudrate)
elif grey:
import zmq
from selfdrive.services import service_list
import selfdrive.messaging as messaging
class BoarddSerial(object):
def __init__(self):
context = zmq.Context()
self.ubloxRaw = messaging.sub_sock(context, service_list['ubloxRaw'].port)
self.ubloxRaw = messaging.sub_sock(service_list['ubloxRaw'].port)
self.buf = ""
def read(self, n):
selfdrive/locationd/test/ubloxd.py
+2 -4
View File
@@ -8,7 +8,6 @@ import struct
import sys
from cereal import log
from common import realtime
import zmq
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.locationd.test.ephemeris import EphemerisData, GET_FIELD_U
@@ -270,9 +269,8 @@ def main(gctx=None):
nav_frame_buffer[0][i] = {}
context = zmq.Context()
gpsLocationExternal = messaging.pub_sock(context, service_list['gpsLocationExternal'].port)
ubloxGnss = messaging.pub_sock(context, service_list['ubloxGnss'].port)
gpsLocationExternal = messaging.pub_sock(service_list['gpsLocationExternal'].port)
ubloxGnss = messaging.pub_sock(service_list['ubloxGnss'].port)
dev = init_reader()
while True:
selfdrive/locationd/test/ubloxd_easy.py
+3 -5
View File
@@ -11,14 +11,12 @@ from selfdrive.services import service_list
unlogger = os.getenv("UNLOGGER") is not None # debug prints
def main(gctx=None):
context = zmq.Context()
poller = zmq.Poller()
context = zmq.Context()
gpsLocationExternal = messaging.pub_sock(context, service_list['gpsLocationExternal'].port)
ubloxGnss = messaging.pub_sock(context, service_list['ubloxGnss'].port)
gpsLocationExternal = messaging.pub_sock(service_list['gpsLocationExternal'].port)
ubloxGnss = messaging.pub_sock(service_list['ubloxGnss'].port)
# ubloxRaw = messaging.sub_sock(context, service_list['ubloxRaw'].port, poller)
# ubloxRaw = messaging.sub_sock(service_list['ubloxRaw'].port, poller)
# buffer with all the messages that still need to be input into the kalman
while 1: