diff --git a/selfdrive/locationd/.gitignore b/selfdrive/locationd/.gitignore new file mode 100644 index 0000000000..6ea757462e --- /dev/null +++ b/selfdrive/locationd/.gitignore @@ -0,0 +1,4 @@ +ubloxd +ubloxd_test +params_learner +paramsd \ No newline at end of file diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript new file mode 100644 index 0000000000..fca12d1324 --- /dev/null +++ b/selfdrive/locationd/SConscript @@ -0,0 +1,25 @@ +Import('env', 'common', 'messaging') +loc_objs = [ + "locationd_yawrate.cc", + "params_learner.cc", + "paramsd.cc"] +loc_libs = [messaging, 'zmq', common, 'capnp', 'kj', 'json', 'json11', 'pthread'] + +env.Program("paramsd", loc_objs, LIBS=loc_libs) +env.SharedLibrary("locationd", loc_objs, LIBS=loc_libs) + +env.Program("ubloxd", [ + "ubloxd.cc", + "ublox_msg.cc", + "ubloxd_main.cc"], + LIBS=loc_libs) + +env.Program("ubloxd_test", [ + "ubloxd_test.cc", + "ublox_msg.cc", + "ubloxd_main.cc"], + LIBS=loc_libs) + + + + diff --git a/selfdrive/locationd/__init__.py b/selfdrive/locationd/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/locationd/calibration_helpers.py b/selfdrive/locationd/calibration_helpers.py new file mode 100644 index 0000000000..f290900759 --- /dev/null +++ b/selfdrive/locationd/calibration_helpers.py @@ -0,0 +1,11 @@ +import math + +class Filter: + MIN_SPEED = 7 # m/s (~15.5mph) + MAX_YAW_RATE = math.radians(3) # per second + +class Calibration: + UNCALIBRATED = 0 + CALIBRATED = 1 + INVALID = 2 + diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py new file mode 100755 index 0000000000..cd92a5f630 --- /dev/null +++ b/selfdrive/locationd/calibrationd.py @@ -0,0 +1,164 @@ +#!/usr/bin/env python3 + +import os +import copy +import json +import numpy as np +import cereal.messaging as messaging +from selfdrive.locationd.calibration_helpers import Calibration +from selfdrive.swaglog import cloudlog +from common.params import Params, put_nonblocking +from common.transformations.model import model_height +from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \ + get_calib_from_vp, H, W, FOCAL + +MPH_TO_MS = 0.44704 +MIN_SPEED_FILTER = 15 * MPH_TO_MS +MAX_SPEED_STD = 1.5 +MAX_YAW_RATE_FILTER = np.radians(2) # per second + +# This is all 20Hz, blocks needed for efficiency +BLOCK_SIZE = 100 +INPUTS_NEEDED = 5 # allow to update VP every so many frames +INPUTS_WANTED = 20 # We want a little bit more than we need for stability +WRITE_CYCLES = 10 # write every 1000 cycles +VP_INIT = np.array([W/2., H/2.]) + +# These validity corners were chosen by looking at 1000 +# and taking most extreme cases with some margin. +VP_VALIDITY_CORNERS = np.array([[W//2 - 120, 300], [W//2 + 120, 520]]) +DEBUG = os.getenv("DEBUG") is not None + + +def is_calibration_valid(vp): + return vp[0] > VP_VALIDITY_CORNERS[0,0] and vp[0] < VP_VALIDITY_CORNERS[1,0] and \ + vp[1] > VP_VALIDITY_CORNERS[0,1] and vp[1] < VP_VALIDITY_CORNERS[1,1] + + +def sanity_clip(vp): + if np.isnan(vp).any(): + vp = VP_INIT + return [np.clip(vp[0], VP_VALIDITY_CORNERS[0,0] - 20, VP_VALIDITY_CORNERS[1,0] + 20), + np.clip(vp[1], VP_VALIDITY_CORNERS[0,1] - 20, VP_VALIDITY_CORNERS[1,1] + 20)] + + +def intrinsics_from_vp(vp): + return np.array([ + [FOCAL, 0., vp[0]], + [ 0., FOCAL, vp[1]], + [ 0., 0., 1.]]) + + +class Calibrator(): + def __init__(self, param_put=False): + self.param_put = param_put + self.vp = copy.copy(VP_INIT) + self.vps = np.zeros((INPUTS_WANTED, 2)) + self.idx = 0 + self.block_idx = 0 + self.valid_blocks = 0 + self.cal_status = Calibration.UNCALIBRATED + self.just_calibrated = False + + # Read calibration + calibration_params = Params().get("CalibrationParams") + if calibration_params: + try: + calibration_params = json.loads(calibration_params) + self.vp = np.array(calibration_params["vanishing_point"]) + if not np.isfinite(self.vp).all(): + self.vp = copy.copy(VP_INIT) + self.vps = np.tile(self.vp, (INPUTS_WANTED, 1)) + self.valid_blocks = calibration_params['valid_blocks'] + if not np.isfinite(self.valid_blocks) or self.valid_blocks < 0: + self.valid_blocks = 0 + self.update_status() + except Exception: + cloudlog.exception("CalibrationParams file found but error encountered") + + def update_status(self): + start_status = self.cal_status + if self.valid_blocks < INPUTS_NEEDED: + self.cal_status = Calibration.UNCALIBRATED + else: + self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID + end_status = self.cal_status + + self.just_calibrated = False + if start_status == Calibration.UNCALIBRATED and end_status == Calibration.CALIBRATED: + self.just_calibrated = True + + def handle_cam_odom(self, trans, rot, trans_std, rot_std): + if ((trans[0] > MIN_SPEED_FILTER) and + (trans_std[0] < MAX_SPEED_STD) and + (abs(rot[2]) < MAX_YAW_RATE_FILTER)): + # intrinsics are not eon intrinsics, since this is calibrated frame + intrinsics = intrinsics_from_vp(self.vp) + new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans)) + new_vp = new_vp[:2]/new_vp[2] + + self.vps[self.block_idx] = (self.idx*self.vps[self.block_idx] + (BLOCK_SIZE - self.idx) * new_vp) / float(BLOCK_SIZE) + self.idx = (self.idx + 1) % BLOCK_SIZE + if self.idx == 0: + self.block_idx += 1 + self.valid_blocks = max(self.block_idx, self.valid_blocks) + self.block_idx = self.block_idx % INPUTS_WANTED + raw_vp = np.mean(self.vps[:max(1, self.valid_blocks)], axis=0) + self.vp = sanity_clip(raw_vp) + self.update_status() + + if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated): + cal_params = {"vanishing_point": list(self.vp), + "valid_blocks": self.valid_blocks} + put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8')) + return new_vp + else: + return None + + def send_data(self, pm): + calib = get_calib_from_vp(self.vp) + extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height) + + cal_send = messaging.new_message() + cal_send.init('liveCalibration') + cal_send.liveCalibration.calStatus = self.cal_status + cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) + cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()] + cal_send.liveCalibration.rpyCalib = [float(x) for x in calib] + + pm.send('liveCalibration', cal_send) + + +def calibrationd_thread(sm=None, pm=None): + if sm is None: + sm = messaging.SubMaster(['cameraOdometry']) + + if pm is None: + pm = messaging.PubMaster(['liveCalibration']) + + calibrator = Calibrator(param_put=True) + + send_counter = 0 + while 1: + sm.update() + + if sm.updated['cameraOdometry']: + new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'].trans, + sm['cameraOdometry'].rot, + sm['cameraOdometry'].transStd, + sm['cameraOdometry'].rotStd) + if DEBUG and new_vp is not None: + print('got new vp', new_vp) + + # decimate outputs for efficiency + if (send_counter % 5) == 0: + calibrator.send_data(pm) + send_counter += 1 + + +def main(sm=None, pm=None): + calibrationd_thread(sm, pm) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/locationd/kalman/.gitignore b/selfdrive/locationd/kalman/.gitignore new file mode 100644 index 0000000000..9d357c72cc --- /dev/null +++ b/selfdrive/locationd/kalman/.gitignore @@ -0,0 +1,4 @@ +lane.cpp +gnss.cpp +loc*.cpp +pos_computer*.cpp diff --git a/selfdrive/locationd/kalman/__init__.py b/selfdrive/locationd/kalman/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/locationd/kalman/chi2_lookup.py b/selfdrive/locationd/kalman/chi2_lookup.py new file mode 100644 index 0000000000..f45b4577f5 --- /dev/null +++ b/selfdrive/locationd/kalman/chi2_lookup.py @@ -0,0 +1,21 @@ +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() diff --git a/selfdrive/locationd/kalman/chi2_lookup_table.npy b/selfdrive/locationd/kalman/chi2_lookup_table.npy new file mode 100644 index 0000000000..73f068e4b4 --- /dev/null +++ b/selfdrive/locationd/kalman/chi2_lookup_table.npy @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0cc301b3a918cce76bd119a219e31722c6a03fa837927eb10fd84e877966cc3e +size 156928 diff --git a/selfdrive/locationd/kalman/compute_pos.c b/selfdrive/locationd/kalman/compute_pos.c new file mode 100644 index 0000000000..f77783da74 --- /dev/null +++ b/selfdrive/locationd/kalman/compute_pos.c @@ -0,0 +1,51 @@ +#include +#include +#include +typedef Eigen::Matrix R3M; +typedef Eigen::Matrix R1M; +typedef Eigen::Matrix O1M; +typedef Eigen::Matrix M3D; + +void gauss_newton(double *in_x, double *in_poses, double *in_img_positions) { + + double res[KDIM*2] = {0}; + double jac[KDIM*6] = {0}; + + O1M x(in_x); + O1M delta; + int counter = 0; + while ((delta.squaredNorm() > 0.0001 and counter < 30) or counter == 0){ + res_fun(in_x, in_poses, in_img_positions, res); + jac_fun(in_x, in_poses, in_img_positions, jac); + R1M E(res); R3M J(jac); + delta = (J.transpose()*J).inverse() * J.transpose() * E; + x = x - delta; + memcpy(in_x, x.data(), 3 * sizeof(double)); + counter = counter + 1; + } +} + + +void compute_pos(double *to_c, double *poses, double *img_positions, double *param, double *pos) { + param[0] = img_positions[KDIM*2-2]; + param[1] = img_positions[KDIM*2-1]; + param[2] = 0.1; + gauss_newton(param, poses, img_positions); + + Eigen::Quaterniond q; + q.w() = poses[KDIM*7-4]; + q.x() = poses[KDIM*7-3]; + q.y() = poses[KDIM*7-2]; + q.z() = poses[KDIM*7-1]; + M3D RC(to_c); + Eigen::Matrix3d R = q.normalized().toRotationMatrix(); + Eigen::Matrix3d rot = R * RC.transpose(); + + pos[0] = param[0]/param[2]; + pos[1] = param[1]/param[2]; + pos[2] = 1.0/param[2]; + O1M ecef_offset(poses + KDIM*7-7); + O1M ecef_output(pos); + ecef_output = rot*ecef_output + ecef_offset; + memcpy(pos, ecef_output.data(), 3 * sizeof(double)); +} diff --git a/selfdrive/locationd/kalman/ekf_c.c b/selfdrive/locationd/kalman/ekf_c.c new file mode 100644 index 0000000000..e524f81ecf --- /dev/null +++ b/selfdrive/locationd/kalman/ekf_c.c @@ -0,0 +1,123 @@ +#include +#include + +typedef Eigen::Matrix DDM; +typedef Eigen::Matrix EEM; +typedef Eigen::Matrix DEM; + +void predict(double *in_x, double *in_P, double *in_Q, double dt) { + typedef Eigen::Matrix 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 +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 ZZM; + typedef Eigen::Matrix ZDM; + typedef Eigen::Matrix XEM; + //typedef Eigen::Matrix EZM; + typedef Eigen::Matrix X1M; + typedef Eigen::Matrix 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 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 pre_y(in_hx); pre_y = z - pre_y; + X1M y; XXM H; XXM R; + if (Hea_fun){ + typedef Eigen::Matrix 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::Identity() - (KT.transpose() * H_err); + + // update state by injecting dx + Eigen::Matrix 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 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)); +} + + diff --git a/selfdrive/locationd/kalman/ekf_sym.py b/selfdrive/locationd/kalman/ekf_sym.py new file mode 100644 index 0000000000..7ca6f9ef6f --- /dev/null +++ b/selfdrive/locationd/kalman/ekf_sym.py @@ -0,0 +1,562 @@ +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 range(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(): + 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 self.filter_time is not None and 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 range(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 range(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] diff --git a/selfdrive/locationd/kalman/feature_handler.c b/selfdrive/locationd/kalman/feature_handler.c new file mode 100644 index 0000000000..eeef6f8a25 --- /dev/null +++ b/selfdrive/locationd/kalman/feature_handler.c @@ -0,0 +1,56 @@ +bool sane(double track [K + 1][5]) { + double diffs_x [K-1]; + double diffs_y [K-1]; + int i; + for (i = 0; i < K-1; i++) { + diffs_x[i] = fabs(track[i+2][2] - track[i+1][2]); + diffs_y[i] = fabs(track[i+2][3] - track[i+1][3]); + } + for (i = 1; i < K-1; i++) { + if (((diffs_x[i] > 0.05 or diffs_x[i-1] > 0.05) and + (diffs_x[i] > 2*diffs_x[i-1] or + diffs_x[i] < .5*diffs_x[i-1])) or + ((diffs_y[i] > 0.05 or diffs_y[i-1] > 0.05) and + (diffs_y[i] > 2*diffs_y[i-1] or + diffs_y[i] < .5*diffs_y[i-1]))){ + return false; + } + } + return true; +} + +void merge_features(double *tracks, double *features, long long *empty_idxs) { + double feature_arr [3000][5]; + memcpy(feature_arr, features, 3000 * 5 * sizeof(double)); + double track_arr [6000][K + 1][5]; + memcpy(track_arr, tracks, (K+1) * 6000 * 5 * sizeof(double)); + int match; + int empty_idx = 0; + int idx; + for (int i = 0; i < 3000; i++) { + match = feature_arr[i][4]; + if (track_arr[match][0][1] == match and track_arr[match][0][2] == 0){ + track_arr[match][0][0] = track_arr[match][0][0] + 1; + track_arr[match][0][1] = feature_arr[i][1]; + track_arr[match][0][2] = 1; + idx = track_arr[match][0][0]; + memcpy(track_arr[match][idx], feature_arr[i], 5 * sizeof(double)); + if (idx == K){ + // label complete + track_arr[match][0][3] = 1; + if (sane(track_arr[match])){ + // label valid + track_arr[match][0][4] = 1; + } + } + } else { + // gen new track with this feature + track_arr[empty_idxs[empty_idx]][0][0] = 1; + track_arr[empty_idxs[empty_idx]][0][1] = feature_arr[i][1]; + track_arr[empty_idxs[empty_idx]][0][2] = 1; + memcpy(track_arr[empty_idxs[empty_idx]][1], feature_arr[i], 5 * sizeof(double)); + empty_idx = empty_idx + 1; + } + } + memcpy(tracks, track_arr, (K+1) * 6000 * 5 * sizeof(double)); +} diff --git a/selfdrive/locationd/kalman/feature_handler.py b/selfdrive/locationd/kalman/feature_handler.py new file mode 100644 index 0000000000..7489aefd79 --- /dev/null +++ b/selfdrive/locationd/kalman/feature_handler.py @@ -0,0 +1,323 @@ +import common.transformations.orientation as orient +import numpy as np +import scipy.optimize as opt +import time +import os +from bisect import bisect_left +from common.sympy_helpers import sympy_into_c, quat_matrix_l +from common.ffi_wrapper import ffi_wrap, wrap_compiled, compile_code + +EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__)) + + +def sane(track): + img_pos = track[1:,2:4] + diffs_x = abs(img_pos[1:,0] - img_pos[:-1,0]) + diffs_y = abs(img_pos[1:,1] - img_pos[:-1,1]) + for i in range(1, len(diffs_x)): + if ((diffs_x[i] > 0.05 or diffs_x[i-1] > 0.05) and \ + (diffs_x[i] > 2*diffs_x[i-1] or \ + diffs_x[i] < .5*diffs_x[i-1])) or \ + ((diffs_y[i] > 0.05 or diffs_y[i-1] > 0.05) and \ + (diffs_y[i] > 2*diffs_y[i-1] or \ + diffs_y[i] < .5*diffs_y[i-1])): + return False + return True + +class FeatureHandler(): + def __init__(self, K): + self.MAX_TRACKS=6000 + self.K = K + #Array of tracks, each track + #has K 5D features preceded + #by 5 params that inidicate + #[f_idx, last_idx, updated, complete, valid] + # f_idx: idx of current last feature in track + # idx of of last feature in frame + # bool for whether this track has been update + # bool for whether this track is complete + # bool for whether this track is valid + self.tracks = np.zeros((self.MAX_TRACKS, K+1, 5)) + self.tracks[:] = np.nan + + # Wrap c code for slow matching + c_header = "\nvoid merge_features(double *tracks, double *features, long long *empty_idxs);" + c_code = "#define K %d\n" % K + c_code += "\n" + open(os.path.join(EXTERNAL_PATH, "feature_handler.c")).read() + ffi, lib = ffi_wrap('feature_handler', c_code, c_header) + def merge_features_c(tracks, features, empty_idxs): + lib.merge_features(ffi.cast("double *", tracks.ctypes.data), + ffi.cast("double *", features.ctypes.data), + ffi.cast("long long *", empty_idxs.ctypes.data)) + + #self.merge_features = self.merge_features_python + self.merge_features = merge_features_c + + def reset(self): + self.tracks[:] = np.nan + + def merge_features_python(self, tracks, features, empty_idxs): + empty_idx = 0 + for f in features: + match_idx = int(f[4]) + if tracks[match_idx, 0, 1] == match_idx and tracks[match_idx, 0 ,2] == 0: + tracks[match_idx, 0, 0] += 1 + tracks[match_idx, 0, 1] = f[1] + tracks[match_idx, 0, 2] = 1 + tracks[match_idx, int(tracks[match_idx, 0, 0])] = f + if tracks[match_idx, 0, 0] == self.K: + tracks[match_idx, 0, 3] = 1 + if sane(tracks[match_idx]): + tracks[match_idx, 0, 4] = 1 + else: + if empty_idx == len(empty_idxs): + print('need more empty space') + continue + tracks[empty_idxs[empty_idx], 0, 0] = 1 + tracks[empty_idxs[empty_idx], 0, 1] = f[1] + tracks[empty_idxs[empty_idx], 0, 2] = 1 + tracks[empty_idxs[empty_idx], 1] = f + empty_idx += 1 + + def update_tracks(self, features): + t0 = time.time() + last_idxs = np.copy(self.tracks[:,0,1]) + real = np.isfinite(last_idxs) + self.tracks[last_idxs[real].astype(int)] = self.tracks[real] + mask = np.ones(self.MAX_TRACKS, np.bool) + mask[last_idxs[real].astype(int)] = 0 + empty_idxs = np.arange(self.MAX_TRACKS)[mask] + self.tracks[empty_idxs] = np.nan + self.tracks[:,0,2] = 0 + self.merge_features(self.tracks, features, empty_idxs) + + def handle_features(self, features): + self.update_tracks(features) + valid_idxs = self.tracks[:,0,4] == 1 + complete_idxs = self.tracks[:,0,3] == 1 + stale_idxs = self.tracks[:,0,2] == 0 + valid_tracks = self.tracks[valid_idxs] + self.tracks[complete_idxs] = np.nan + self.tracks[stale_idxs] = np.nan + return valid_tracks[:,1:,:4].reshape((len(valid_tracks), self.K*4)) + +def generate_residual(K): + import sympy as sp + from common.sympy_helpers import quat_rotate + x_sym = sp.MatrixSymbol('abr', 3,1) + poses_sym = sp.MatrixSymbol('poses', 7*K,1) + img_pos_sym = sp.MatrixSymbol('img_positions', 2*K,1) + alpha, beta, rho = x_sym + to_c = sp.Matrix(orient.rot_matrix(-np.pi/2, -np.pi/2, 0)) + pos_0 = sp.Matrix(np.array(poses_sym[K*7-7:K*7-4])[:,0]) + q = poses_sym[K*7-4:K*7] + quat_rot = quat_rotate(*q) + rot_g_to_0 = to_c*quat_rot.T + rows = [] + for i in range(K): + pos_i = sp.Matrix(np.array(poses_sym[i*7:i*7+3])[:,0]) + q = poses_sym[7*i+3:7*i+7] + quat_rot = quat_rotate(*q) + rot_g_to_i = to_c*quat_rot.T + rot_0_to_i = rot_g_to_i*(rot_g_to_0.T) + trans_0_to_i = rot_g_to_i*(pos_0 - pos_i) + funct_vec = rot_0_to_i*sp.Matrix([alpha, beta, 1]) + rho*trans_0_to_i + h1, h2, h3 = funct_vec + rows.append(h1/h3 - img_pos_sym[i*2 +0]) + rows.append(h2/h3 - img_pos_sym[i*2 + 1]) + img_pos_residual_sym = sp.Matrix(rows) + + # sympy into c + sympy_functions = [] + sympy_functions.append(('res_fun', img_pos_residual_sym, [x_sym, poses_sym, img_pos_sym])) + sympy_functions.append(('jac_fun', img_pos_residual_sym.jacobian(x_sym), [x_sym, poses_sym, img_pos_sym])) + + return sympy_functions + + +def generate_orient_error_jac(K): + import sympy as sp + from common.sympy_helpers import quat_rotate + x_sym = sp.MatrixSymbol('abr', 3,1) + dtheta = sp.MatrixSymbol('dtheta', 3,1) + delta_quat = sp.Matrix(np.ones(4)) + delta_quat[1:,:] = sp.Matrix(0.5*dtheta[0:3,:]) + poses_sym = sp.MatrixSymbol('poses', 7*K,1) + img_pos_sym = sp.MatrixSymbol('img_positions', 2*K,1) + alpha, beta, rho = x_sym + to_c = sp.Matrix(orient.rot_matrix(-np.pi/2, -np.pi/2, 0)) + pos_0 = sp.Matrix(np.array(poses_sym[K*7-7:K*7-4])[:,0]) + q = quat_matrix_l(poses_sym[K*7-4:K*7])*delta_quat + quat_rot = quat_rotate(*q) + rot_g_to_0 = to_c*quat_rot.T + rows = [] + for i in range(K): + pos_i = sp.Matrix(np.array(poses_sym[i*7:i*7+3])[:,0]) + q = quat_matrix_l(poses_sym[7*i+3:7*i+7])*delta_quat + quat_rot = quat_rotate(*q) + rot_g_to_i = to_c*quat_rot.T + rot_0_to_i = rot_g_to_i*(rot_g_to_0.T) + trans_0_to_i = rot_g_to_i*(pos_0 - pos_i) + funct_vec = rot_0_to_i*sp.Matrix([alpha, beta, 1]) + rho*trans_0_to_i + h1, h2, h3 = funct_vec + rows.append(h1/h3 - img_pos_sym[i*2 +0]) + rows.append(h2/h3 - img_pos_sym[i*2 + 1]) + img_pos_residual_sym = sp.Matrix(rows) + + # sympy into c + sympy_functions = [] + sympy_functions.append(('orient_error_jac', img_pos_residual_sym.jacobian(dtheta), [x_sym, poses_sym, img_pos_sym, dtheta])) + + return sympy_functions + + +class LstSqComputer(): + def __init__(self, K, MIN_DEPTH=2, MAX_DEPTH=500, debug=False): + self.to_c = orient.rot_matrix(-np.pi/2, -np.pi/2, 0) + self.MAX_DEPTH = MAX_DEPTH + self.MIN_DEPTH = MIN_DEPTH + self.debug = debug + self.name = 'pos_computer_' + str(K) + if debug: + self.name += '_debug' + + try: + dir_path = os.path.dirname(__file__) + deps = [dir_path + '/' + 'feature_handler.py', + dir_path + '/' + 'compute_pos.c'] + + outs = [dir_path + '/' + self.name + '.o', + dir_path + '/' + self.name + '.so', + dir_path + '/' + self.name + '.cpp'] + out_times = list(map(os.path.getmtime, outs)) + dep_times = list(map(os.path.getmtime, deps)) + rebuild = os.getenv("REBUILD", False) + if min(out_times) < max(dep_times) or rebuild: + list(map(os.remove, outs)) + # raise the OSError if removing didnt + # raise one to start the compilation + raise OSError() + except OSError as e: + # gen c code for sympy functions + sympy_functions = generate_residual(K) + #if debug: + # sympy_functions.extend(generate_orient_error_jac(K)) + header, code = sympy_into_c(sympy_functions) + + # ffi wrap c code + extra_header = "\nvoid compute_pos(double *to_c, double *in_poses, double *in_img_positions, double *param, double *pos);" + code += "\n#define KDIM %d\n" % K + header += "\n" + extra_header + code += "\n" + open(os.path.join(EXTERNAL_PATH, "compute_pos.c")).read() + compile_code(self.name, code, header, EXTERNAL_PATH) + ffi, lib = wrap_compiled(self.name, EXTERNAL_PATH) + + # wrap c functions + #if debug: + #def orient_error_jac(x, poses, img_positions, dtheta): + # out = np.zeros(((K*2, 3)), dtype=np.float64) + # lib.orient_error_jac(ffi.cast("double *", x.ctypes.data), + # ffi.cast("double *", poses.ctypes.data), + # ffi.cast("double *", img_positions.ctypes.data), + # ffi.cast("double *", dtheta.ctypes.data), + # ffi.cast("double *", out.ctypes.data)) + # return out + #self.orient_error_jac = orient_error_jac + def residual_jac(x, poses, img_positions): + out = np.zeros(((K*2, 3)), dtype=np.float64) + lib.jac_fun(ffi.cast("double *", x.ctypes.data), + ffi.cast("double *", poses.ctypes.data), + ffi.cast("double *", img_positions.ctypes.data), + ffi.cast("double *", out.ctypes.data)) + return out + def residual(x, poses, img_positions): + out = np.zeros((K*2), dtype=np.float64) + lib.res_fun(ffi.cast("double *", x.ctypes.data), + ffi.cast("double *", poses.ctypes.data), + ffi.cast("double *", img_positions.ctypes.data), + ffi.cast("double *", out.ctypes.data)) + return out + self.residual = residual + self.residual_jac = residual_jac + + def compute_pos_c(poses, img_positions): + pos = np.zeros(3, dtype=np.float64) + param = np.zeros(3, dtype=np.float64) + # Can't be a view for the ctype + img_positions = np.copy(img_positions) + lib.compute_pos(ffi.cast("double *", self.to_c.ctypes.data), + ffi.cast("double *", poses.ctypes.data), + ffi.cast("double *", img_positions.ctypes.data), + ffi.cast("double *", param.ctypes.data), + ffi.cast("double *", pos.ctypes.data)) + return pos, param + self.compute_pos_c = compute_pos_c + + def compute_pos(self, poses, img_positions, debug=False): + pos, param = self.compute_pos_c(poses, img_positions) + #pos, param = self.compute_pos_python(poses, img_positions) + depth = 1/param[2] + if debug: + if not self.debug: + raise NotImplementedError("This is not a debug computer") + #orient_err_jac = self.orient_error_jac(param, poses, img_positions, np.zeros(3)).reshape((-1,2,3)) + jac = self.residual_jac(param, poses, img_positions).reshape((-1,2,3)) + res = self.residual(param, poses, img_positions).reshape((-1,2)) + return pos, param, res, jac #, orient_err_jac + elif (self.MIN_DEPTH < depth < self.MAX_DEPTH): + return pos + else: + return None + + def gauss_newton(self, fun, jac, x, args): + poses, img_positions = args + delta = 1 + counter = 0 + while abs(np.linalg.norm(delta)) > 1e-4 and counter < 30: + delta = np.linalg.pinv(jac(x, poses, img_positions)).dot(fun(x, poses, img_positions)) + x = x - delta + counter += 1 + return [x] + + def compute_pos_python(self, poses, img_positions, check_quality=False): + # This procedure is also described + # in the MSCKF paper (Mourikis et al. 2007) + x = np.array([img_positions[-1][0], + img_positions[-1][1], 0.1]) + res = opt.leastsq(self.residual, x, Dfun=self.residual_jac, args=(poses, img_positions)) # scipy opt + #res = self.gauss_newton(self.residual, self.residual_jac, x, (poses, img_positions)) # diy gauss_newton + + alpha, beta, rho = res[0] + rot_0_to_g = (orient.rotations_from_quats(poses[-1,3:])).dot(self.to_c.T) + return (rot_0_to_g.dot(np.array([alpha, beta, 1])))/rho + poses[-1,:3] + + + + +''' +EXPERIMENTAL CODE +''' +def unroll_shutter(img_positions, poses, v, rot_rates, ecef_pos): + # only speed correction for now + t_roll = 0.016 # 16ms rolling shutter? + vroll, vpitch, vyaw = rot_rates + A = 0.5*np.array([[-1, -vroll, -vpitch, -vyaw], + [vroll, 0, vyaw, -vpitch], + [vpitch, -vyaw, 0, vroll], + [vyaw, vpitch, -vroll, 0]]) + q_dot = A.dot(poses[-1][3:7]) + v = np.append(v, q_dot) + v = np.array([v[0], v[1], v[2],0,0,0,0]) + current_pose = poses[-1] + v*0.05 + poses = np.vstack((current_pose, poses)) + dt = -img_positions[:,1]*t_roll/0.48 + errs = project(poses, ecef_pos) - project(poses + np.atleast_2d(dt).T.dot(np.atleast_2d(v)), ecef_pos) + return img_positions - errs + +def project(poses, ecef_pos): + img_positions = np.zeros((len(poses), 2)) + for i, p in enumerate(poses): + cam_frame = orient.rotations_from_quats(p[3:]).T.dot(ecef_pos - p[:3]) + img_positions[i] = np.array([cam_frame[1]/cam_frame[0], cam_frame[2]/cam_frame[0]]) + return img_positions + diff --git a/selfdrive/locationd/kalman/gnss_kf.py b/selfdrive/locationd/kalman/gnss_kf.py new file mode 100755 index 0000000000..4701e80a8d --- /dev/null +++ b/selfdrive/locationd/kalman/gnss_kf.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +import numpy as np +from . import gnss_model + +from .kalman_helpers import ObservationKind +from .ekf_sym import EKF_sym +from selfdrive.locationd.kalman.loc_kf import parse_pr, parse_prr + +class States(): + ECEF_POS = slice(0,3) # x, y and z in ECEF in meters + ECEF_VELOCITY = slice(3,6) + CLOCK_BIAS = slice(6, 7) # clock bias in light-meters, + CLOCK_DRIFT = slice(7, 8) # clock drift in light-meters/s, + CLOCK_ACCELERATION = slice(8, 9) # clock acceleration in light-meters/s**2 + GLONASS_BIAS = slice(9, 10) # clock drift in light-meters/s, + GLONASS_FREQ_SLOPE = slice(10, 11) # GLONASS bias in m expressed as bias + freq_num*freq_slope + + +class GNSSKalman(): + def __init__(self, N=0, max_tracks=3000): + x_initial = np.array([-2712700.6008, -4281600.6679, 3859300.1830, + 0, 0, 0, + 0, 0, 0, + 0, 0]) + + # state covariance + P_initial = np.diag([10000**2, 10000**2, 10000**2, + 10**2, 10**2, 10**2, + (2000000)**2, (100)**2, (0.5)**2, + (10)**2, (1)**2]) + + # process noise + Q = np.diag([0.3**2, 0.3**2, 0.3**2, + 3**2, 3**2, 3**2, + (.1)**2, (0)**2, (0.01)**2, + .1**2, (.01)**2]) + + self.dim_state = x_initial.shape[0] + + # mahalanobis outlier rejection + maha_test_kinds = []#ObservationKind.PSEUDORANGE_RATE, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_GLONASS] + + name = 'gnss' + gnss_model.gen_model(name, self.dim_state, maha_test_kinds) + + # init filter + self.filter = EKF_sym(name, Q, x_initial, P_initial, self.dim_state, self.dim_state, maha_test_kinds=maha_test_kinds) + + @property + def x(self): + return self.filter.state() + + @property + def P(self): + return self.filter.covs() + + def predict(self, t): + return self.filter.predict(t) + + def rts_smooth(self, estimates): + return self.filter.rts_smooth(estimates, norm_quats=False) + + 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.PSEUDORANGE_GPS or kind == ObservationKind.PSEUDORANGE_GLONASS: + r = self.predict_and_update_pseudorange(data, t, kind) + elif kind == ObservationKind.PSEUDORANGE_RATE_GPS or kind == ObservationKind.PSEUDORANGE_RATE_GLONASS: + r = self.predict_and_update_pseudorange_rate(data, t, kind) + return r + + def predict_and_update_pseudorange(self, meas, t, kind): + R = np.zeros((len(meas), 1, 1)) + sat_pos_freq = np.zeros((len(meas), 4)) + z = np.zeros((len(meas), 1)) + for i, m in enumerate(meas): + z_i, R_i, sat_pos_freq_i = parse_pr(m) + sat_pos_freq[i,:] = sat_pos_freq_i + z[i,:] = z_i + R[i,:,:] = R_i + return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_freq) + + def predict_and_update_pseudorange_rate(self, meas, t, kind): + R = np.zeros((len(meas), 1, 1)) + z = np.zeros((len(meas), 1)) + sat_pos_vel = np.zeros((len(meas), 6)) + for i, m in enumerate(meas): + z_i, R_i, sat_pos_vel_i = parse_prr(m) + sat_pos_vel[i] = sat_pos_vel_i + R[i,:,:] = R_i + z[i, :] = z_i + return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_vel) + + +if __name__ == "__main__": + GNSSKalman() diff --git a/selfdrive/locationd/kalman/gnss_model.py b/selfdrive/locationd/kalman/gnss_model.py new file mode 100644 index 0000000000..4a5f6fd2f6 --- /dev/null +++ b/selfdrive/locationd/kalman/gnss_model.py @@ -0,0 +1,93 @@ +import numpy as np +import sympy as sp + +import os +from .kalman_helpers import ObservationKind +from .ekf_sym import gen_code +from common.sympy_helpers import cross, euler_rotate, quat_rotate, quat_matrix_l, quat_matrix_r + +def gen_model(name, dim_state, maha_test_kinds): + + # 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 + '/' + 'gnss_model.py', + dir_path + '/' + 'gnss_kf.py'] + + outs = [dir_path + '/' + name + '.o', + dir_path + '/' + name + '.so', + dir_path + '/' + name + '.cpp'] + out_times = list(map(os.path.getmtime, outs)) + dep_times = list(map(os.path.getmtime, deps)) + rebuild = os.getenv("REBUILD", False) + if min(out_times) > max(dep_times) and not rebuild: + return + list(map(os.remove, outs)) + except OSError as e: + pass + + # make functions and jacobians with sympy + # state variables + state_sym = sp.MatrixSymbol('state', dim_state, 1) + state = sp.Matrix(state_sym) + x,y,z = state[0:3,:] + v = state[3:6,:] + vx, vy, vz = v + cb, cd, ca = state[6:9,:] + glonass_bias, glonass_freq_slope = state[9:11,:] + + dt = sp.Symbol('dt') + + state_dot = sp.Matrix(np.zeros((dim_state, 1))) + state_dot[:3,:] = v + state_dot[6,0] = cd + state_dot[7,0] = ca + + # Basic descretization, 1st order intergrator + # Can be pretty bad if dt is big + f_sym = state + dt*state_dot + + + # + # Observation functions + # + + # extra args + sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1) + sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1) + sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1) + orb_epos_sym = sp.MatrixSymbol('orb_epos_sym', 3, 1) + + # expand extra args + sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym + sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:] + los_x, los_y, los_z = sat_los_sym + orb_x, orb_y, orb_z = orb_epos_sym + + h_pseudorange_sym = sp.Matrix([sp.sqrt( + (x - sat_x)**2 + + (y - sat_y)**2 + + (z - sat_z)**2) + + cb]) + + h_pseudorange_glonass_sym = sp.Matrix([sp.sqrt( + (x - sat_x)**2 + + (y - sat_y)**2 + + (z - sat_z)**2) + + cb + glonass_bias + glonass_freq_slope*glonass_freq]) + + los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z])) + los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 + los_vector[2]**2) + h_pseudorange_rate_sym = sp.Matrix([los_vector[0]*(sat_vx - vx) + + los_vector[1]*(sat_vy - vy) + + los_vector[2]*(sat_vz - vz) + + cd]) + + obs_eqs = [[h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS, sat_pos_freq_sym], + [h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym], + [h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym], + [h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym]] + + gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds) diff --git a/selfdrive/locationd/kalman/kalman_helpers.py b/selfdrive/locationd/kalman/kalman_helpers.py new file mode 100644 index 0000000000..b4d22dce7f --- /dev/null +++ b/selfdrive/locationd/kalman/kalman_helpers.py @@ -0,0 +1,165 @@ +import numpy as np +import os +from bisect import bisect +from tqdm import tqdm + + +class ObservationKind(): + 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 + 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 + fig = plt.figure(figsize=(10,20)) + fig.suptitle('Residuals of ' + data_name, fontsize=24) + n = len(list(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') diff --git a/selfdrive/locationd/kalman/loc_kf.py b/selfdrive/locationd/kalman/loc_kf.py new file mode 100755 index 0000000000..ce7b4e759c --- /dev/null +++ b/selfdrive/locationd/kalman/loc_kf.py @@ -0,0 +1,323 @@ +#!/usr/bin/env python3 +import numpy as np +from . import loc_model + +from .kalman_helpers import ObservationKind +from .ekf_sym import EKF_sym +from .feature_handler import LstSqComputer, unroll_shutter +from laika.raw_gnss import GNSSMeasurement + + +def parse_prr(m): + sat_pos_vel_i = np.concatenate((m[GNSSMeasurement.SAT_POS], + m[GNSSMeasurement.SAT_VEL])) + R_i = np.atleast_2d(m[GNSSMeasurement.PRR_STD]**2) + z_i = m[GNSSMeasurement.PRR] + return z_i, R_i, sat_pos_vel_i + + +def parse_pr(m): + pseudorange = m[GNSSMeasurement.PR] + pseudorange_stdev = m[GNSSMeasurement.PR_STD] + sat_pos_freq_i = np.concatenate((m[GNSSMeasurement.SAT_POS], + np.array([m[GNSSMeasurement.GLONASS_FREQ]]))) + z_i = np.atleast_1d(pseudorange) + R_i = np.atleast_2d(pseudorange_stdev**2) + return z_i, R_i, sat_pos_freq_i + + +class States(): + ECEF_POS = slice(0,3) # x, y and z in ECEF in meters + ECEF_ORIENTATION = slice(3,7) # quat for pose of phone in ecef + ECEF_VELOCITY = slice(7,10) # ecef velocity in m/s + ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s + CLOCK_BIAS = slice(13, 14) # clock bias in light-meters, + CLOCK_DRIFT = slice(14, 15) # clock drift in light-meters/s, + GYRO_BIAS = slice(15, 18) # roll, pitch and yaw biases + ODO_SCALE = slice(18, 19) # odometer scale + ACCELERATION = slice(19, 22) # Acceleration in device frame in m/s**2 + FOCAL_SCALE = slice(22, 23) # focal length scale + IMU_OFFSET = slice(23,26) # imu offset angles in radians + GLONASS_BIAS = slice(26,27) # GLONASS bias in m expressed as bias + freq_num*freq_slope + GLONASS_FREQ_SLOPE = slice(27, 28) # GLONASS bias in m expressed as bias + freq_num*freq_slope + CLOCK_ACCELERATION = slice(28, 29) # clock acceleration in light-meters/s**2, + + +class LocKalman(): + def __init__(self, N=0, max_tracks=3000): + x_initial = np.array([-2.7e6, 4.2e6, 3.8e6, + 1, 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 0, 0, + 0, 0, 0, + 1, + 0, 0, 0, + 1, + 0, 0, 0, + 0, 0, + 0]) + + # state covariance + P_initial = np.diag([10000**2, 10000**2, 10000**2, + 10**2, 10**2, 10**2, + 10**2, 10**2, 10**2, + 1**2, 1**2, 1**2, + (200000)**2, (100)**2, + 0.05**2, 0.05**2, 0.05**2, + 0.02**2, + 1**2, 1**2, 1**2, + 0.01**2, + (0.01)**2, (0.01)**2, (0.01)**2, + 10**2, 1**2, + 0.05**2]) + + # process noise + Q = np.diag([0.03**2, 0.03**2, 0.03**2, + 0.0**2, 0.0**2, 0.0**2, + 0.0**2, 0.0**2, 0.0**2, + 0.1**2, 0.1**2, 0.1**2, + (.1)**2, (0.0)**2, + (0.005/100)**2, (0.005/100)**2, (0.005/100)**2, + (0.02/100)**2, + 3**2, 3**2, 3**2, + 0.001**2, + (0.05/60)**2, (0.05/60)**2, (0.05/60)**2, + (.1)**2, (.01)**2, + 0.005**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]), + ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5*2]), + ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]), + ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]), + ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]), + ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])} + + # MSCKF stuff + self.N = N + self.dim_main = x_initial.shape[0] + self.dim_augment = 7 + self.dim_main_err = P_initial.shape[0] + self.dim_augment_err = 6 + self.dim_state = self.dim_main + self.dim_augment*self.N + self.dim_state_err = self.dim_main_err + self.dim_augment_err*self.N + + # mahalanobis outlier rejection + maha_test_kinds = [ObservationKind.ORB_FEATURES] #, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE] + + name = 'loc_%d' % N + loc_model.gen_model(name, N, self.dim_main, self.dim_main_err, + self.dim_augment, self.dim_augment_err, + self.dim_state, self.dim_state_err, + maha_test_kinds) + + if self.N > 0: + x_initial, P_initial, Q = self.pad_augmented(x_initial, P_initial, Q) + self.computer = LstSqComputer(N) + self.max_tracks = max_tracks + + # init filter + self.filter = EKF_sym(name, Q, x_initial, P_initial, self.dim_main, self.dim_main_err, + N, self.dim_augment, self.dim_augment_err, maha_test_kinds) + + @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): + return self.filter.predict(t) + + def rts_smooth(self, estimates): + return self.filter.rts_smooth(estimates, norm_quats=True) + + def pad_augmented(self, x, P, Q=None): + if x.shape[0] == self.dim_main and self.N > 0: + x = np.pad(x, (0, self.N*self.dim_augment), mode='constant') + x[self.dim_main+3::7] = 1 + if P.shape[0] == self.dim_main_err and self.N > 0: + P = np.pad(P, [(0, self.N*self.dim_augment_err), (0, self.N*self.dim_augment_err)], mode='constant') + P[self.dim_main_err:, self.dim_main_err:] = 10e20*np.eye(self.dim_augment_err *self.N) + if Q is None: + return x, P + else: + Q = np.pad(Q, [(0, self.N*self.dim_augment_err), (0, self.N*self.dim_augment_err)], mode='constant') + return x, P, Q + + 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() + state, P = self.pad_augmented(state, P) + 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.PSEUDORANGE_GPS or kind == ObservationKind.PSEUDORANGE_GLONASS: + r = self.predict_and_update_pseudorange(data, t, kind) + elif kind == ObservationKind.PSEUDORANGE_RATE_GPS or kind == ObservationKind.PSEUDORANGE_RATE_GLONASS: + r = self.predict_and_update_pseudorange_rate(data, t, kind) + elif kind == ObservationKind.ORB_POINT: + r = self.predict_and_update_orb(data, t, kind) + elif kind == ObservationKind.ORB_FEATURES: + r = self.predict_and_update_orb_features(data, t, kind) + elif kind == ObservationKind.MSCKF_TEST: + r = self.predict_and_update_msckf_test(data, t, kind) + elif kind == ObservationKind.FEATURE_TRACK_TEST: + r = self.predict_and_update_feature_track_test(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))) + # Normalize quats + quat_norm = np.linalg.norm(self.filter.x[3:7,0]) + # Should not continue if the quats behave this weirdly + if not 0.1 < quat_norm < 10: + raise RuntimeError("Sir! The filter's gone all wobbly!") + self.filter.x[3:7,0] = self.filter.x[3:7,0]/quat_norm + for i in range(self.N): + d1 = self.dim_main + d3 = self.dim_augment + self.filter.x[d1+d3*i+3:d1+d3*i+7] /= np.linalg.norm(self.filter.x[d1+i*d3 + 3:d1+i*d3 + 7,0]) + 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 range(n): + R[i,:,:] = obs_noise + return R + + def predict_and_update_pseudorange(self, meas, t, kind): + R = np.zeros((len(meas), 1, 1)) + sat_pos_freq = np.zeros((len(meas), 4)) + z = np.zeros((len(meas), 1)) + for i, m in enumerate(meas): + z_i, R_i, sat_pos_freq_i = parse_pr(m) + sat_pos_freq[i,:] = sat_pos_freq_i + z[i,:] = z_i + R[i,:,:] = R_i + return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_freq) + + + def predict_and_update_pseudorange_rate(self, meas, t, kind): + R = np.zeros((len(meas), 1, 1)) + z = np.zeros((len(meas), 1)) + sat_pos_vel = np.zeros((len(meas), 6)) + for i, m in enumerate(meas): + z_i, R_i, sat_pos_vel_i = parse_prr(m) + sat_pos_vel[i] = sat_pos_vel_i + R[i,:,:] = R_i + z[i, :] = z_i + return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_vel) + + def predict_and_update_orb(self, orb, t, kind): + true_pos = orb[:,2:] + z = orb[:,:2] + R = np.zeros((len(orb), 2, 2)) + for i, _ in enumerate(z): + R[i,:,:] = np.diag([10**2, 10**2]) + return self.filter.predict_and_update_batch(t, kind, z, R, true_pos) + + 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) + + def predict_and_update_orb_features(self, tracks, t, kind): + k = 2*(self.N+1) + R = np.zeros((len(tracks), k, k)) + z = np.zeros((len(tracks), k)) + ecef_pos = np.zeros((len(tracks), 3)) + ecef_pos[:] = np.nan + poses = self.x[self.dim_main:].reshape((-1,7)) + times = tracks.reshape((len(tracks),self.N+1, 4))[:,:,0] + good_counter = 0 + if times.any() and np.allclose(times[0,:-1], self.filter.augment_times, rtol=1e-6): + for i, track in enumerate(tracks): + img_positions = track.reshape((self.N+1, 4))[:,2:] + # TODO not perfect as last pose not used + #img_positions = unroll_shutter(img_positions, poses, self.filter.state()[7:10], self.filter.state()[10:13], ecef_pos[i]) + ecef_pos[i] = self.computer.compute_pos(poses, img_positions[:-1]) + z[i] = img_positions.flatten() + R[i,:,:] = np.diag([0.005**2]*(k)) + if np.isfinite(ecef_pos[i][0]): + good_counter += 1 + if good_counter > self.max_tracks: + break + good_idxs = np.all(np.isfinite(ecef_pos),axis=1) + # have to do some weird stuff here to keep + # to have the observations input from mesh3d + # consistent with the outputs of the filter + # Probably should be replaced, not sure how. + ret = self.filter.predict_and_update_batch(t, kind, z[good_idxs], R[good_idxs], ecef_pos[good_idxs], augment=True) + if ret is None: + return + y_full = np.zeros((z.shape[0], z.shape[1] - 3)) + #print sum(good_idxs), len(tracks) + if sum(good_idxs) > 0: + y_full[good_idxs] = np.array(ret[6]) + ret = ret[:6] + (y_full, z, ecef_pos) + return ret + + def predict_and_update_msckf_test(self, test_data, t, kind): + assert self.N > 0 + z = test_data + R = np.zeros((len(test_data), len(z[0]), len(z[0]))) + ecef_pos = [self.x[:3]] + for i, _ in enumerate(z): + R[i,:,:] = np.diag([0.1**2]*len(z[0])) + ret = self.filter.predict_and_update_batch(t, kind, z, R, ecef_pos) + self.filter.augment() + return ret + + def maha_test_pseudorange(self, x, P, meas, kind, maha_thresh=.3): + bools = [] + for i, m in enumerate(meas): + z, R, sat_pos_freq = parse_pr(m) + bools.append(self.filter.maha_test(x, P, kind, z, R, extra_args=sat_pos_freq, maha_thresh=maha_thresh)) + return np.array(bools) + + def maha_test_pseudorange_rate(self, x, P, meas, kind, maha_thresh=.999): + bools = [] + for i, m in enumerate(meas): + z, R, sat_pos_vel = parse_prr(m) + bools.append(self.filter.maha_test(x, P, kind, z, R, extra_args=sat_pos_vel, maha_thresh=maha_thresh)) + return np.array(bools) + + +if __name__ == "__main__": + LocKalman(N=4) diff --git a/selfdrive/locationd/kalman/loc_local_kf.py b/selfdrive/locationd/kalman/loc_local_kf.py new file mode 100755 index 0000000000..f39902ad2e --- /dev/null +++ b/selfdrive/locationd/kalman/loc_local_kf.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python3 +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(): + 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(): + 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 range(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() diff --git a/selfdrive/locationd/kalman/loc_local_model.py b/selfdrive/locationd/kalman/loc_local_model.py new file mode 100644 index 0000000000..0a9f72e5db --- /dev/null +++ b/selfdrive/locationd/kalman/loc_local_model.py @@ -0,0 +1,80 @@ +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 = list(map(os.path.getmtime, outs)) + dep_times = list(map(os.path.getmtime, deps)) + rebuild = os.getenv("REBUILD", False) + if min(out_times) > max(dep_times) and not rebuild: + return + list(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) diff --git a/selfdrive/locationd/kalman/loc_model.py b/selfdrive/locationd/kalman/loc_model.py new file mode 100644 index 0000000000..6255c2aaa8 --- /dev/null +++ b/selfdrive/locationd/kalman/loc_model.py @@ -0,0 +1,254 @@ +import numpy as np +import sympy as sp +import os + +from laika.constants import EARTH_GM +from .kalman_helpers import ObservationKind +from .ekf_sym import gen_code +from common.sympy_helpers import cross, euler_rotate, quat_rotate, quat_matrix_l, quat_matrix_r + +def gen_model(name, N, dim_main, dim_main_err, + dim_augment, dim_augment_err, + dim_state, dim_state_err, + maha_test_kinds): + + + # 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_model.py', + dir_path + '/' + 'loc_kf.py'] + + outs = [dir_path + '/' + name + '.o', + dir_path + '/' + name + '.so', + dir_path + '/' + name + '.cpp'] + out_times = list(map(os.path.getmtime, outs)) + dep_times = list(map(os.path.getmtime, deps)) + rebuild = os.getenv("REBUILD", False) + if min(out_times) > max(dep_times) and not rebuild: + return + list(map(os.remove, outs)) + except OSError as e: + pass + + # make functions and jacobians with sympy + # state variables + state_sym = sp.MatrixSymbol('state', dim_state, 1) + state = sp.Matrix(state_sym) + x,y,z = state[0:3,:] + q = state[3:7,:] + v = state[7:10,:] + vx, vy, vz = v + omega = state[10:13,:] + vroll, vpitch, vyaw = omega + cb, cd = state[13:15,:] + roll_bias, pitch_bias, yaw_bias = state[15:18,:] + odo_scale = state[18,:] + acceleration = state[19:22,:] + focal_scale = state[22,:] + imu_angles= state[23:26,:] + glonass_bias, glonass_freq_slope = state[26:28,:] + ca = state[28,0] + + dt = sp.Symbol('dt') + + # calibration and attitude rotation matrices + quat_rot = quat_rotate(*q) + + # Got the quat predict equations from here + # A New Quaternion-Based Kalman Filter for + # Real-Time Attitude Estimation Using the Two-Step + # Geometrically-Intuitive Correction Algorithm + A = 0.5*sp.Matrix([[0, -vroll, -vpitch, -vyaw], + [vroll, 0, vyaw, -vpitch], + [vpitch, -vyaw, 0, vroll], + [vyaw, vpitch, -vroll, 0]]) + q_dot = A * q + + # Time derivative of the state as a function of state + state_dot = sp.Matrix(np.zeros((dim_state, 1))) + state_dot[:3,:] = v + state_dot[3:7,:] = q_dot + state_dot[7:10,0] = quat_rot * acceleration + state_dot[13,0] = cd + state_dot[14,0] = ca + + # Basic descretization, 1st order intergrator + # Can be pretty bad if dt is big + f_sym = state + dt*state_dot + + state_err_sym = sp.MatrixSymbol('state_err',dim_state_err,1) + state_err = sp.Matrix(state_err_sym) + quat_err = state_err[3:6,:] + v_err = state_err[6:9,:] + omega_err = state_err[9:12,:] + cd_err = state_err[13,:] + acceleration_err = state_err[18:21,:] + ca_err = state_err[27,:] + + # Time derivative of the state error as a function of state error and state + quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2]) + q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err) + state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1))) + state_err_dot[:3,:] = v_err + state_err_dot[3:6,:] = q_err_dot + state_err_dot[6:9,:] = quat_err_matrix * quat_rot * (acceleration + acceleration_err) + state_err_dot[12,:] = cd_err + state_err_dot[13,:] = ca_err + f_err_sym = state_err + dt*state_err_dot + + # convenient indexing + # q idxs are for quats and p idxs are for other + q_idxs = [[3, dim_augment]] + [[dim_main + n*dim_augment + 3, dim_main + (n+1)*dim_augment] for n in range(N)] + q_err_idxs = [[3, dim_augment_err]] + [[dim_main_err + n*dim_augment_err + 3, dim_main_err + (n+1)*dim_augment_err] for n in range(N)] + p_idxs = [[0, 3]] + [[dim_augment, dim_main]] + [[dim_main + n*dim_augment , dim_main + n*dim_augment + 3] for n in range(N)] + p_err_idxs = [[0, 3]] + [[dim_augment_err, dim_main_err]] + [[dim_main_err + n*dim_augment_err, dim_main_err + n*dim_augment_err + 3] for n in range(N)] + + # Observation matrix modifier + H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) + for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): + H_mod_sym[p_idx[0]:p_idx[1],p_err_idx[0]:p_err_idx[1]] = np.eye(p_idx[1]-p_idx[0]) + for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): + H_mod_sym[q_idx[0]:q_idx[1],q_err_idx[0]:q_err_idx[1]] = 0.5*quat_matrix_r(state[q_idx[0]:q_idx[1]])[:,1:] + + + # these error functions are defined so that say there + # is a nominal x and true x: + # true x = err_function(nominal x, delta x) + # delta x = inv_err_function(nominal x, true x) + nom_x = sp.MatrixSymbol('nom_x',dim_state,1) + true_x = sp.MatrixSymbol('true_x',dim_state,1) + delta_x = sp.MatrixSymbol('delta_x',dim_state_err,1) + + err_function_sym = sp.Matrix(np.zeros((dim_state,1))) + for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): + delta_quat = sp.Matrix(np.ones((4))) + delta_quat[1:,:] = sp.Matrix(0.5*delta_x[q_err_idx[0]: q_err_idx[1],:]) + err_function_sym[q_idx[0]:q_idx[1],0] = quat_matrix_r(nom_x[q_idx[0]:q_idx[1],0])*delta_quat + for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): + err_function_sym[p_idx[0]:p_idx[1],:] = sp.Matrix(nom_x[p_idx[0]:p_idx[1],:] + delta_x[p_err_idx[0]:p_err_idx[1],:]) + + inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err,1))) + for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): + inv_err_function_sym[p_err_idx[0]:p_err_idx[1],0] = sp.Matrix(-nom_x[p_idx[0]:p_idx[1],0] + true_x[p_idx[0]:p_idx[1],0]) + for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): + delta_quat = quat_matrix_r(nom_x[q_idx[0]:q_idx[1],0]).T*true_x[q_idx[0]:q_idx[1],0] + inv_err_function_sym[q_err_idx[0]:q_err_idx[1],0] = sp.Matrix(2*delta_quat[1:]) + + eskf_params = [[err_function_sym, nom_x, delta_x], + [inv_err_function_sym, nom_x, true_x], + H_mod_sym, f_err_sym, state_err_sym] + + + + # + # Observation functions + # + + # extra args + sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1) + sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1) + sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1) + orb_epos_sym = sp.MatrixSymbol('orb_epos_sym', 3, 1) + + # expand extra args + sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym + sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:] + los_x, los_y, los_z = sat_los_sym + orb_x, orb_y, orb_z = orb_epos_sym + + h_pseudorange_sym = sp.Matrix([sp.sqrt( + (x - sat_x)**2 + + (y - sat_y)**2 + + (z - sat_z)**2) + + cb]) + + h_pseudorange_glonass_sym = sp.Matrix([sp.sqrt( + (x - sat_x)**2 + + (y - sat_y)**2 + + (z - sat_z)**2) + + cb + glonass_bias + glonass_freq_slope*glonass_freq]) + + los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z])) + los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 + los_vector[2]**2) + h_pseudorange_rate_sym = sp.Matrix([los_vector[0]*(sat_vx - vx) + + los_vector[1]*(sat_vy - vy) + + los_vector[2]*(sat_vz - vz) + + cd]) + + imu_rot = euler_rotate(*imu_angles) + h_gyro_sym = imu_rot*sp.Matrix([vroll + roll_bias, + vpitch + pitch_bias, + vyaw + yaw_bias]) + + pos = sp.Matrix([x, y, z]) + gravity = quat_rot.T * ((EARTH_GM/((x**2 + y**2 + z**2)**(3.0/2.0)))*pos) + h_acc_sym = imu_rot*(gravity + acceleration) + h_phone_rot_sym = sp.Matrix([vroll, + vpitch, + vyaw]) + speed = vx**2 + vy**2 + vz**2 + h_speed_sym = sp.Matrix([sp.sqrt(speed)*odo_scale]) + + # orb stuff + orb_pos_sym = sp.Matrix([orb_x - x, orb_y - y, orb_z - z]) + orb_pos_rot_sym = quat_rot.T * orb_pos_sym + s = orb_pos_rot_sym[0] + h_orb_point_sym = sp.Matrix([(1/s)*(orb_pos_rot_sym[1]), + (1/s)*(orb_pos_rot_sym[2])]) + + h_pos_sym = sp.Matrix([x, y, z]) + h_imu_frame_sym = sp.Matrix(imu_angles) + + h_relative_motion = sp.Matrix(quat_rot.T * v) + + + 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_acc_sym, ObservationKind.PHONE_ACCEL, None], + [h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS, sat_pos_freq_sym], + [h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym], + [h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym], + [h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym], + [h_pos_sym, ObservationKind.ECEF_POS, None], + [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], + [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], + [h_imu_frame_sym, ObservationKind.IMU_FRAME, None], + [h_orb_point_sym, ObservationKind.ORB_POINT, orb_epos_sym]] + + # MSCKF configuration + if N > 0: + focal_scale =1 + # Add observation functions for orb feature tracks + track_epos_sym = sp.MatrixSymbol('track_epos_sym', 3, 1) + track_x, track_y, track_z = track_epos_sym + h_track_sym = sp.Matrix(np.zeros(((1 + N)*2, 1))) + track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z]) + track_pos_rot_sym = quat_rot.T * track_pos_sym + h_track_sym[-2:,:] = sp.Matrix([focal_scale*(track_pos_rot_sym[1]/track_pos_rot_sym[0]), + focal_scale*(track_pos_rot_sym[2]/track_pos_rot_sym[0])]) + + h_msckf_test_sym = sp.Matrix(np.zeros(((1 + N)*3, 1))) + h_msckf_test_sym[-3:,:] = sp.Matrix([track_x - x,track_y - y , track_z - z]) + + for n in range(N): + idx = dim_main + n*dim_augment + err_idx = dim_main_err + n*dim_augment_err + x, y, z = state[idx:idx+3] + q = state[idx+3:idx+7] + quat_rot = quat_rotate(*q) + track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z]) + track_pos_rot_sym = quat_rot.T * track_pos_sym + h_track_sym[n*2:n*2+2,:] = sp.Matrix([focal_scale*(track_pos_rot_sym[1]/track_pos_rot_sym[0]), + focal_scale*(track_pos_rot_sym[2]/track_pos_rot_sym[0])]) + h_msckf_test_sym[n*3:n*3+3,:] = sp.Matrix([track_x - x, track_y - y, track_z - z]) + obs_eqs.append([h_msckf_test_sym, ObservationKind.MSCKF_TEST, track_epos_sym]) + obs_eqs.append([h_track_sym, ObservationKind.ORB_FEATURES, track_epos_sym]) + obs_eqs.append([h_track_sym, ObservationKind.FEATURE_TRACK_TEST, track_epos_sym]) + msckf_params = [dim_main, dim_augment, dim_main_err, dim_augment_err, N, [ObservationKind.MSCKF_TEST, ObservationKind.ORB_FEATURES]] + else: + msckf_params = None + gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, msckf_params, maha_test_kinds) diff --git a/selfdrive/locationd/liblocationd_py.py b/selfdrive/locationd/liblocationd_py.py new file mode 100644 index 0000000000..9a56ecc7ab --- /dev/null +++ b/selfdrive/locationd/liblocationd_py.py @@ -0,0 +1,31 @@ +import os +import subprocess +from common.basedir import BASEDIR + +from cffi import FFI +from ctypes import cdll + +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) diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py new file mode 100755 index 0000000000..567ff0149f --- /dev/null +++ b/selfdrive/locationd/locationd.py @@ -0,0 +1,232 @@ +#!/usr/bin/env python3 +import os +import zmq +import numpy as np +from bisect import bisect_right +import cereal.messaging as messaging +from selfdrive.swaglog import cloudlog +from cereal.services import service_list +from common.transformations.orientation import rotations_from_quats, ecef_euler_from_ned, euler2quat, ned_euler_from_ecef, quat2euler +import common.transformations.coordinates as coord + +import laika.raw_gnss as gnss +from laika.astro_dog import AstroDog + +from selfdrive.locationd.kalman.loc_kf import LocKalman +from selfdrive.locationd.kalman.kalman_helpers import ObservationKind +os.environ["OMP_NUM_THREADS"] = "1" + + +class Localizer(): + def __init__(self, disabled_logs=[], dog=None): + self.kf = LocKalman(0) + self.reset_kalman() + if dog: + self.dog = dog + else: + self.dog = AstroDog(auto_update=True) + self.max_age = .2 # seconds + self.disabled_logs = disabled_logs + self.week = None + + def liveLocationMsg(self, time): + fix = messaging.log.LiveLocationData.new_message() + predicted_state = self.kf.x + fix_ecef = predicted_state[0:3] + fix_pos_geo = coord.ecef2geodetic(fix_ecef) + fix.lat = float(fix_pos_geo[0]) + fix.lon = float(fix_pos_geo[1]) + fix.alt = float(fix_pos_geo[2]) + + fix.speed = float(np.linalg.norm(predicted_state[7:10])) + + orientation_ned_euler = ned_euler_from_ecef(fix_ecef, quat2euler(predicted_state[3:7])) + fix.roll = float(orientation_ned_euler[0]*180/np.pi) + fix.pitch = float(orientation_ned_euler[1]*180/np.pi) + fix.heading = float(orientation_ned_euler[2]*180/np.pi) + + fix.gyro = [float(predicted_state[10]), float(predicted_state[11]), float(predicted_state[12])] + fix.accel = [float(predicted_state[19]), float(predicted_state[20]), float(predicted_state[21])] + local_vel = rotations_from_quats(predicted_state[3:7]).T.dot(predicted_state[7:10]) + fix.pitchCalibration = float((np.arctan2(local_vel[2], local_vel[0]))*180/np.pi) + fix.yawCalibration = float((np.arctan2(local_vel[1], local_vel[0]))*180/np.pi) + fix.imuFrame = [(180/np.pi)*float(predicted_state[23]), (180/np.pi)*float(predicted_state[24]), (180/np.pi)*float(predicted_state[25])] + 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)) + #print len(self.observation_buffer), idx, self.kf.filter.filter_time, time + while self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age: + if self.filter_ready: + self.kf.predict_and_observe(*self.observation_buffer.pop(0)) + else: + self.observation_buffer.pop(0) + + def handle_gps(self, log, current_time): + self.converter = coord.LocalCoord.from_geodetic([log.gpsLocationExternal.latitude, log.gpsLocationExternal.longitude, log.gpsLocationExternal.altitude]) + fix_ecef = self.converter.ned2ecef([0,0,0]) + # initing with bad bearing allowed, maybe bad? + if not self.filter_ready and len(list(self.dog.orbits.keys())) >6: # and log.gpsLocationExternal.speed > 5: + self.filter_ready = True + initial_ecef = fix_ecef + initial_state = np.zeros(29) + gps_bearing = log.gpsLocationExternal.bearing*(np.pi/180) + initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_bearing]) + initial_pose_ecef_quat = euler2quat(initial_pose_ecef) + gps_speed = log.gpsLocationExternal.speed + quat_uncertainty = 0.2**2 + initial_pose_ecef_quat = euler2quat(initial_pose_ecef) + initial_state[:3] = initial_ecef + initial_state[3:7] = initial_pose_ecef_quat + initial_state[7:10] = rotations_from_quats(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0])) + initial_state[18] = 1 + initial_state[22] = 1 + covs_diag = np.array([10**2,10**2,10**2, + quat_uncertainty, quat_uncertainty, quat_uncertainty, + 2**2, 2**2, 2**2, + 1, 1, 1, + 20000000**2, 100**2, + 0.01**2, 0.01**2, 0.01**2, + 0.02**2, + 2**2, 2**2, 2**2, + .01**2, + 0.01**2, 0.01**2, 0.01**2, + 10**2, 1**2, + 0.2**2]) + self.kf.init_state(initial_state, covs=np.diag(covs_diag), filter_time=current_time) + print("Filter initialized") + elif self.filter_ready: + #self.update_kalman(current_time, ObservationKind.ECEF_POS, fix_ecef) + gps_est_error = np.sqrt((self.kf.x[0] - fix_ecef[0])**2 + + (self.kf.x[1] - fix_ecef[1])**2 + + (self.kf.x[2] - fix_ecef[2])**2) + if gps_est_error > 50: + cloudlog.info("Locationd vs ubloxLocation difference too large, kalman reset") + self.reset_kalman() + + def handle_car_state(self, log, current_time): + self.speed_counter += 1 + if self.speed_counter % 5==0: + self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, log.carState.vEgo) + if log.carState.vEgo == 0: + self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0]) + + def handle_ublox_gnss(self, log, current_time): + if hasattr(log.ubloxGnss, 'measurementReport'): + self.raw_gnss_counter += 1 + if True or self.raw_gnss_counter % 3==0: + processed_raw = gnss.process_measurements(gnss.read_raw_ublox(log.ubloxGnss.measurementReport), dog=self.dog) + corrected_raw = gnss.correct_measurements(processed_raw, self.kf.x[:3], dog=self.dog) + corrected_raw = np.array([c.as_array() for c in corrected_raw]).reshape((-1,14)) + self.update_kalman(current_time, ObservationKind.PSEUDORANGE_GPS, corrected_raw) + self.update_kalman(current_time, ObservationKind.PSEUDORANGE_RATE_GPS, corrected_raw) + #elif hasattr(log.ubloxGnss, 'ephemeris'): + # self.dog.add_ublox_ephems([log]) + # if len(self.dog.orbits.keys()) < 6: + # print 'Added ublox ephem now has ', len(self.dog.orbits.keys()) + + def handle_qcom_gnss(self, log, current_time): + if hasattr(log.qcomGnss, 'drSvPoly') and self.week is not None: + self.dog.add_qcom_ephems([log], self.week) + if len(list(self.dog.orbits.keys())) < 6: + print('Added qcom ephem now has ', len(list(self.dog.orbits.keys()))) + if hasattr(log.qcomGnss, 'drMeasurementReport') and log.qcomGnss.drMeasurementReport.source == "gps": + self.week = log.qcomGnss.drMeasurementReport.gpsWeek + + 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])) + pass + + 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.gyro_counter += 1 + if True or self.gyro_counter % 5==0: + if max(abs(self.kf.x[23:26])) > 0.07: + print('imu frame angles exceeded, correcting') + self.update_kalman(current_time, ObservationKind.IMU_FRAME, [0, 0, 0]) + self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-sensor_reading.gyro.v[2], -sensor_reading.gyro.v[1], -sensor_reading.gyro.v[0]]) + if sensor_reading.type == 1: + self.acc_counter += 1 + if True or self.acc_counter % 5==0: + self.update_kalman(current_time, ObservationKind.PHONE_ACCEL, [-sensor_reading.acceleration.v[2], -sensor_reading.acceleration.v[1], -sensor_reading.acceleration.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.handle_sensors(log, current_time) + elif typ == "gpsLocationExternal": + self.handle_gps(log, current_time) + elif typ == "carState": + self.handle_car_state(log, current_time) + elif typ == "ubloxGnss": + self.handle_ublox_gnss(log, current_time) + elif typ == "qcomGnss": + self.handle_qcom_gnss(log, current_time) + elif typ == "cameraOdometry": + self.handle_cam_odo(log, current_time) + + def reset_kalman(self): + self.filter_time = None + self.filter_ready = False + self.observation_buffer = [] + self.converter = None + self.gyro_counter = 0 + self.acc_counter = 0 + self.raw_gnss_counter = 0 + self.speed_counter = 0 + + +def locationd_thread(gctx, addr, disabled_logs): + poller = zmq.Poller() + + #carstate = messaging.sub_sock('carState', poller, addr=addr, conflate=True) + gpsLocationExternal = messaging.sub_sock('gpsLocationExternal', poller, addr=addr, conflate=True) + ubloxGnss = messaging.sub_sock('ubloxGnss', poller, addr=addr, conflate=True) + qcomGnss = messaging.sub_sock('qcomGnss', poller, addr=addr, conflate=True) + sensorEvents = messaging.sub_sock('sensorEvents', poller, addr=addr, conflate=True) + + liveLocation = messaging.pub_sock('liveLocation') + + localizer = Localizer(disabled_logs=disabled_logs) + print("init done") + + # buffer with all the messages that still need to be input into the kalman + while 1: + polld = poller.poll(timeout=1000) + for sock, mode in polld: + if mode != zmq.POLLIN: + continue + logs = messaging.drain_sock(sock) + for log in logs: + localizer.handle_log(log) + + if localizer.filter_ready and log.which == 'ubloxGnss': + msg = messaging.new_message() + msg.logMonoTime = log.logMonoTime + msg.init('liveLocation') + msg.liveLocation = localizer.liveLocationMsg(log.logMonoTime*1e-9) + liveLocation.send(msg.to_bytes()) + + +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('carState') + if IN_CAR: + addr = "192.168.5.11" + locationd_thread(gctx, addr, disabled_logs) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/locationd/locationd_local.py b/selfdrive/locationd/locationd_local.py new file mode 100755 index 0000000000..375728c468 --- /dev/null +++ b/selfdrive/locationd/locationd_local.py @@ -0,0 +1,206 @@ +#!/usr/bin/env python3 +import os +import zmq +import math +import json + +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 cereal.messaging as messaging +from selfdrive.swaglog import cloudlog +from selfdrive.controls.lib.vehicle_model import VehicleModel +from cereal.services import service_list +from selfdrive.locationd.kalman.loc_local_kf import LocLocalKalman +from selfdrive.locationd.kalman.kalman_helpers import ObservationKind +from selfdrive.locationd.params_learner import ParamsLearner + +DEBUG = False +kf = LocLocalKalman() # Make sure that model is generated on import time + + +LEARNING_RATE = 3 + + +class Localizer(): + 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) + + +def locationd_thread(gctx, addr, disabled_logs): + poller = zmq.Poller() + + controls_state_socket = messaging.sub_sock('controlsState', poller, addr=addr, conflate=True) + sensor_events_socket = messaging.sub_sock('sensorEvents', poller, addr=addr, conflate=True) + camera_odometry_socket = messaging.sub_sock('cameraOdometry', poller, addr=addr, conflate=True) + + kalman_odometry_socket = messaging.pub_sock('kalmanOdometry') + live_parameters_socket = messaging.pub_sock('liveParameters') + + 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() diff --git a/selfdrive/locationd/locationd_yawrate.cc b/selfdrive/locationd/locationd_yawrate.cc new file mode 100644 index 0000000000..b2b87557ce --- /dev/null +++ b/selfdrive/locationd/locationd_yawrate.cc @@ -0,0 +1,163 @@ +#include +#include + +#include +#include +#include + +#include "cereal/gen/cpp/log.capnp.h" + +#include "locationd_yawrate.h" + + +void Localizer::update_state(const Eigen::Matrix &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::Vector4d K = P * C.transpose() * (1.0 / S); + x = x + K * y; + P = (I - K * C) * P; +} + +void Localizer::handle_sensor_events(capnp::List::Reader sensor_events, double current_time) { + for (cereal::SensorEventData::Reader sensor_event : sensor_events){ + if (sensor_event.getSensor() == 5 && sensor_event.getType() == 16) { + sensor_data_time = current_time; + 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(30.0 *camera_odometry.getRotStd()[2], 2); + double meas = camera_odometry.getRot()[2]; + update_state(C_posenet, R, current_time, meas); + + auto trans = camera_odometry.getTrans(); + posenet_speed = sqrt(trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2]); + camera_odometry_time = current_time; +} + +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(); + controls_state_time = current_time; +} + + +Localizer::Localizer() { + // States: [yaw rate, yaw rate diff, gyro bias, gyro bias diff] + A << + 1, 1, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 1, + 0, 0, 0, 1; + I << + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1; + + Q << + 0, 0, 0, 0, + 0, pow(0.1, 2.0), 0, 0, + 0, 0, 0, 0, + 0, 0, pow(0.005 / 100.0, 2.0), 0; + P << + pow(100.0, 2.0), 0, 0, 0, + 0, pow(100.0, 2.0), 0, 0, + 0, 0, pow(100.0, 2.0), 0, + 0, 0, 0, pow(100.0, 2.0); + + C_posenet << 1, 0, 0, 0; + C_gyro << 1, 0, 1, 0; + x << 0, 0, 0, 0; + + R_gyro = pow(0.25, 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 view((const capnp::word*)data, len); + capnp::FlatArrayMessageReader msg(view); + cereal::Event::Reader event = msg.getRoot(); + + 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[2]; + } + + 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)); + } +} diff --git a/selfdrive/locationd/locationd_yawrate.h b/selfdrive/locationd/locationd_yawrate.h new file mode 100644 index 0000000000..9c6885241b --- /dev/null +++ b/selfdrive/locationd/locationd_yawrate.h @@ -0,0 +1,37 @@ +#pragma once + +#include +#include "cereal/gen/cpp/log.capnp.h" + +#define DEGREES_TO_RADIANS 0.017453292519943295 + +class Localizer +{ + Eigen::Matrix4d A; + Eigen::Matrix4d I; + Eigen::Matrix4d Q; + Eigen::Matrix C_posenet; + Eigen::Matrix C_gyro; + + double R_gyro; + + void update_state(const Eigen::Matrix &C, const double R, double current_time, double meas); + void handle_sensor_events(capnp::List::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::Vector4d x; + Eigen::Matrix4d P; + double steering_angle = 0; + double car_speed = 0; + double posenet_speed = 0; + double prev_update_time = -1; + double controls_state_time = -1; + double sensor_data_time = -1; + double camera_odometry_time = -1; + + Localizer(); + void handle_log(cereal::Event::Reader event); + +}; diff --git a/selfdrive/locationd/params_learner.cc b/selfdrive/locationd/params_learner.cc new file mode 100644 index 0000000000..66f378a1da --- /dev/null +++ b/selfdrive/locationd/params_learner.cc @@ -0,0 +1,118 @@ +#include +#include +#include + +#include +#include +#include "cereal/gen/cpp/log.capnp.h" +#include "cereal/gen/cpp/car.capnp.h" +#include "params_learner.h" + +// #define DEBUG + +template +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((len / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), params, len); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::CarParams::Reader car_params = cmsg.getRoot(); + + 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; + } +} diff --git a/selfdrive/locationd/params_learner.h b/selfdrive/locationd/params_learner.h new file mode 100644 index 0000000000..4d97551b3b --- /dev/null +++ b/selfdrive/locationd/params_learner.h @@ -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); +}; diff --git a/selfdrive/locationd/params_learner.py b/selfdrive/locationd/params_learner.py new file mode 100644 index 0000000000..0aae5f0900 --- /dev/null +++ b/selfdrive/locationd/params_learner.py @@ -0,0 +1,84 @@ +import math +from common.numpy_fast import clip + +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 + +DEBUG = False + + +class ParamsLearner(): + 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: % .6f Avg. AO % .6f" % (math.degrees(self.ao), math.degrees(self.slow_ao)) + s5 = "Stiffnes: % .6f x" % self.x + s6 = "sR: %.4f" % self.sR + print("{0} {1} {2}".format(s4, s5, s6)) + + + 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 diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc new file mode 100644 index 0000000000..6109c271c3 --- /dev/null +++ b/selfdrive/locationd/paramsd.cc @@ -0,0 +1,186 @@ +#include +#include +#include +#include +#include + + +#include +#include + +#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 "messaging.hpp" +#include "locationd_yawrate.h" +#include "params_learner.h" + + +void sigpipe_handler(int sig) { + LOGE("SIGPIPE received"); +} + + +int main(int argc, char *argv[]) { + signal(SIGPIPE, (sighandler_t)sigpipe_handler); + + Context * c = Context::create(); + SubSocket * controls_state_sock = SubSocket::create(c, "controlsState"); + SubSocket * sensor_events_sock = SubSocket::create(c, "sensorEvents"); + SubSocket * camera_odometry_sock = SubSocket::create(c, "cameraOdometry"); + PubSocket * live_parameters_sock = PubSocket::create(c, "liveParameters"); + + assert(controls_state_sock != NULL); + assert(sensor_events_sock != NULL); + assert(camera_odometry_sock != NULL); + assert(live_parameters_sock != NULL); + + Poller * poller = Poller::create({controls_state_sock, sensor_events_sock, camera_odometry_sock}); + + Localizer localizer; + + // 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((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(); + + // 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; + double posenet_invalid_count = 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){ + for (auto s : poller->poll(100)){ + Message * msg = s->receive(); + + auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg->getData(), msg->getSize()); + + capnp::FlatArrayMessageReader capnp_msg(amsg); + cereal::Event::Reader event = capnp_msg.getRoot(); + + localizer.handle_log(event); + + auto which = event.which(); + // Throw vision failure if posenet and odometric speed too different + if (which == cereal::Event::CAMERA_ODOMETRY){ + if (std::abs(localizer.posenet_speed - localizer.car_speed) > std::max(0.4 * localizer.car_speed, 5.0)) { + posenet_invalid_count++; + } else { + posenet_invalid_count = 0; + } + } else 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 camera_odometry_age = localizer.controls_state_time - localizer.camera_odometry_time; + + 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(); + 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[2]); + 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); + live_params.setPosenetSpeed(localizer.posenet_speed); + live_params.setPosenetValid((posenet_invalid_count < 4) && (camera_odometry_age < 5.0)); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + live_parameters_sock->send((char*)bytes.begin(), bytes.size()); + + // 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(NULL, "LiveParameters", out.c_str(), out.length()); + }); + } + } + delete msg; + } + } + + delete live_parameters_sock; + delete controls_state_sock; + delete camera_odometry_sock; + delete sensor_events_sock; + delete poller; + delete c; + + return 0; +} diff --git a/selfdrive/locationd/test/.gitignore b/selfdrive/locationd/test/.gitignore new file mode 100644 index 0000000000..89f9ac04aa --- /dev/null +++ b/selfdrive/locationd/test/.gitignore @@ -0,0 +1 @@ +out/ diff --git a/selfdrive/locationd/test/__init__.py b/selfdrive/locationd/test/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/locationd/test/ci_test.py b/selfdrive/locationd/test/ci_test.py new file mode 100755 index 0000000000..5b23e8326a --- /dev/null +++ b/selfdrive/locationd/test/ci_test.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 +import subprocess +import os +import sys +import argparse +import tempfile + +from selfdrive.locationd.test.ubloxd_py_test import parser_test +from selfdrive.locationd.test.ubloxd_regression_test import compare_results + + +def mkdirs_exists_ok(path): + try: + os.makedirs(path) + except OSError: + if not os.path.isdir(path): + raise + + +def main(args): + cur_dir = os.path.dirname(os.path.realpath(__file__)) + ubloxd_dir = os.path.join(cur_dir, '../') + + cc_output_dir = os.path.join(args.output_dir, 'cc') + mkdirs_exists_ok(cc_output_dir) + + py_output_dir = os.path.join(args.output_dir, 'py') + mkdirs_exists_ok(py_output_dir) + + archive_file = os.path.join(cur_dir, args.stream_gz_file) + + try: + print('Extracting stream file') + subprocess.check_call(['tar', 'zxf', archive_file], cwd=tempfile.gettempdir()) + stream_file_path = os.path.join(tempfile.gettempdir(), 'ubloxRaw.stream') + + if not os.path.isfile(stream_file_path): + print('Extract file failed') + sys.exit(-3) + + print('Run regression test - CC parser...') + if args.valgrind: + subprocess.check_call(["valgrind", "--leak-check=full", os.path.join(ubloxd_dir, 'ubloxd_test'), stream_file_path, cc_output_dir]) + else: + subprocess.check_call([os.path.join(ubloxd_dir, 'ubloxd_test'), stream_file_path, cc_output_dir]) + + print('Running regression test - py parser...') + parser_test(stream_file_path, py_output_dir) + + print('Running regression test - compare result...') + r = compare_results(cc_output_dir, py_output_dir) + + print('All done!') + + subprocess.check_call(["rm", stream_file_path]) + subprocess.check_call(["rm", '-rf', cc_output_dir]) + subprocess.check_call(["rm", '-rf', py_output_dir]) + sys.exit(r) + + except subprocess.CalledProcessError as e: + print('CI test failed with {}'.format(e.returncode)) + sys.exit(e.returncode) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Ubloxd CI test", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + + parser.add_argument("stream_gz_file", nargs='?', default='ubloxRaw.tar.gz', + help="UbloxRaw data stream zip file") + + parser.add_argument("output_dir", nargs='?', default='out', + help="Output events temp directory") + + parser.add_argument("--valgrind", default=False, action='store_true', + help="Run in valgrind") + + args = parser.parse_args() + main(args) diff --git a/selfdrive/locationd/test/ephemeris.py b/selfdrive/locationd/test/ephemeris.py new file mode 100644 index 0000000000..dd8155e19a --- /dev/null +++ b/selfdrive/locationd/test/ephemeris.py @@ -0,0 +1,137 @@ +def GET_FIELD_U(w, nb, pos): + return (((w) >> (pos)) & ((1 << (nb)) - 1)) + + +def twos_complement(v, nb): + sign = v >> (nb - 1) + value = v + if sign != 0: + value = value - (1 << nb) + return value + + +def GET_FIELD_S(w, nb, pos): + v = GET_FIELD_U(w, nb, pos) + return twos_complement(v, nb) + + +def extract_uint8(v, b): + return (v >> (8 * (3 - b))) & 255 + +def extract_int8(v, b): + value = extract_uint8(v, b) + if value > 127: + value -= 256 + return value + +class EphemerisData: + '''container for parsing a AID_EPH message + Thanks to Sylvain Munaut + http://cgit.osmocom.org/cgit/osmocom-lcs/tree/gps.c +on of this parser + + See IS-GPS-200F.pdf Table 20-III for the field meanings, scaling factors and + field widths + ''' + + def __init__(self, svId, subframes): + from math import pow + self.svId = svId + week_no = GET_FIELD_U(subframes[1][2+0], 10, 20) + # code_on_l2 = GET_FIELD_U(subframes[1][0], 2, 12) + # sv_ura = GET_FIELD_U(subframes[1][0], 4, 8) + # sv_health = GET_FIELD_U(subframes[1][0], 6, 2) + # l2_p_flag = GET_FIELD_U(subframes[1][1], 1, 23) + t_gd = GET_FIELD_S(subframes[1][2+4], 8, 6) + iodc = (GET_FIELD_U(subframes[1][2+0], 2, 6) << 8) | GET_FIELD_U( + subframes[1][2+5], 8, 22) + + t_oc = GET_FIELD_U(subframes[1][2+5], 16, 6) + a_f2 = GET_FIELD_S(subframes[1][2+6], 8, 22) + a_f1 = GET_FIELD_S(subframes[1][2+6], 16, 6) + a_f0 = GET_FIELD_S(subframes[1][2+7], 22, 8) + + c_rs = GET_FIELD_S(subframes[2][2+0], 16, 6) + delta_n = GET_FIELD_S(subframes[2][2+1], 16, 14) + m_0 = (GET_FIELD_S(subframes[2][2+1], 8, 6) << 24) | GET_FIELD_U( + subframes[2][2+2], 24, 6) + c_uc = GET_FIELD_S(subframes[2][2+3], 16, 14) + e = (GET_FIELD_U(subframes[2][2+3], 8, 6) << 24) | GET_FIELD_U(subframes[2][2+4], 24, 6) + c_us = GET_FIELD_S(subframes[2][2+5], 16, 14) + a_powhalf = (GET_FIELD_U(subframes[2][2+5], 8, 6) << 24) | GET_FIELD_U( + subframes[2][2+6], 24, 6) + t_oe = GET_FIELD_U(subframes[2][2+7], 16, 14) + # fit_flag = GET_FIELD_U(subframes[2][7], 1, 7) + + c_ic = GET_FIELD_S(subframes[3][2+0], 16, 14) + omega_0 = (GET_FIELD_S(subframes[3][2+0], 8, 6) << 24) | GET_FIELD_U( + subframes[3][2+1], 24, 6) + c_is = GET_FIELD_S(subframes[3][2+2], 16, 14) + i_0 = (GET_FIELD_S(subframes[3][2+2], 8, 6) << 24) | GET_FIELD_U( + subframes[3][2+3], 24, 6) + c_rc = GET_FIELD_S(subframes[3][2+4], 16, 14) + w = (GET_FIELD_S(subframes[3][2+4], 8, 6) << 24) | GET_FIELD_U(subframes[3][5], 24, 6) + omega_dot = GET_FIELD_S(subframes[3][2+6], 24, 6) + idot = GET_FIELD_S(subframes[3][2+7], 14, 8) + + self._rsvd1 = GET_FIELD_U(subframes[1][2+1], 23, 6) + self._rsvd2 = GET_FIELD_U(subframes[1][2+2], 24, 6) + self._rsvd3 = GET_FIELD_U(subframes[1][2+3], 24, 6) + self._rsvd4 = GET_FIELD_U(subframes[1][2+4], 16, 14) + self.aodo = GET_FIELD_U(subframes[2][2+7], 5, 8) + + # Definition of Pi used in the GPS coordinate system + gpsPi = 3.1415926535898 + + # now form variables in radians, meters and seconds etc + self.Tgd = t_gd * pow(2, -31) + self.A = pow(a_powhalf * pow(2, -19), 2.0) + self.cic = c_ic * pow(2, -29) + self.cis = c_is * pow(2, -29) + self.crc = c_rc * pow(2, -5) + self.crs = c_rs * pow(2, -5) + self.cuc = c_uc * pow(2, -29) + self.cus = c_us * pow(2, -29) + self.deltaN = delta_n * pow(2, -43) * gpsPi + self.ecc = e * pow(2, -33) + self.i0 = i_0 * pow(2, -31) * gpsPi + self.idot = idot * pow(2, -43) * gpsPi + self.M0 = m_0 * pow(2, -31) * gpsPi + self.omega = w * pow(2, -31) * gpsPi + self.omega_dot = omega_dot * pow(2, -43) * gpsPi + self.omega0 = omega_0 * pow(2, -31) * gpsPi + self.toe = t_oe * pow(2, 4) + + # clock correction information + self.toc = t_oc * pow(2, 4) + self.gpsWeek = week_no + self.af0 = a_f0 * pow(2, -31) + self.af1 = a_f1 * pow(2, -43) + self.af2 = a_f2 * pow(2, -55) + + iode1 = GET_FIELD_U(subframes[2][2+0], 8, 22) + iode2 = GET_FIELD_U(subframes[3][2+7], 8, 22) + self.valid = (iode1 == iode2) and (iode1 == (iodc & 0xff)) + self.iode = iode1 + + if GET_FIELD_U(subframes[4][2+0], 6, 22) == 56 and \ + GET_FIELD_U(subframes[4][2+0], 2, 28) == 1 and \ + GET_FIELD_U(subframes[5][2+0], 2, 28) == 1: + a0 = GET_FIELD_S(subframes[4][2], 8, 14) * pow(2, -30) + a1 = GET_FIELD_S(subframes[4][2], 8, 6) * pow(2, -27) + a2 = GET_FIELD_S(subframes[4][3], 8, 22) * pow(2, -24) + a3 = GET_FIELD_S(subframes[4][3], 8, 14) * pow(2, -24) + b0 = GET_FIELD_S(subframes[4][3], 8, 6) * pow(2, 11) + b1 = GET_FIELD_S(subframes[4][4], 8, 22) * pow(2, 14) + b2 = GET_FIELD_S(subframes[4][4], 8, 14) * pow(2, 16) + b3 = GET_FIELD_S(subframes[4][4], 8, 6) * pow(2, 16) + + self.ionoAlpha = [a0, a1, a2, a3] + self.ionoBeta = [b0, b1, b2, b3] + self.ionoCoeffsValid = True + else: + self.ionoAlpha = [] + self.ionoBeta = [] + self.ionoCoeffsValid = False + + diff --git a/selfdrive/locationd/test/test_params_learner.py b/selfdrive/locationd/test/test_params_learner.py new file mode 100755 index 0000000000..a3c8487858 --- /dev/null +++ b/selfdrive/locationd/test/test_params_learner.py @@ -0,0 +1,53 @@ +#!/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, t 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() diff --git a/selfdrive/locationd/test/ublox.py b/selfdrive/locationd/test/ublox.py new file mode 100644 index 0000000000..496207d401 --- /dev/null +++ b/selfdrive/locationd/test/ublox.py @@ -0,0 +1,996 @@ +#!/usr/bin/env python3 +''' +UBlox binary protocol handling + +Copyright Andrew Tridgell, October 2012 +Released under GNU GPL version 3 or later + +WARNING: This code has originally intended for +ublox version 7, it has been adapted to work +for ublox version 8, not all functions may work. +''' + + +import struct +import time, os + +# protocol constants +PREAMBLE1 = 0xb5 +PREAMBLE2 = 0x62 + +# message classes +CLASS_NAV = 0x01 +CLASS_RXM = 0x02 +CLASS_INF = 0x04 +CLASS_ACK = 0x05 +CLASS_CFG = 0x06 +CLASS_MON = 0x0A +CLASS_AID = 0x0B +CLASS_TIM = 0x0D +CLASS_ESF = 0x10 + +# ACK messages +MSG_ACK_NACK = 0x00 +MSG_ACK_ACK = 0x01 + +# NAV messages +MSG_NAV_POSECEF = 0x1 +MSG_NAV_POSLLH = 0x2 +MSG_NAV_STATUS = 0x3 +MSG_NAV_DOP = 0x4 +MSG_NAV_SOL = 0x6 +MSG_NAV_PVT = 0x7 +MSG_NAV_POSUTM = 0x8 +MSG_NAV_VELNED = 0x12 +MSG_NAV_VELECEF = 0x11 +MSG_NAV_TIMEGPS = 0x20 +MSG_NAV_TIMEUTC = 0x21 +MSG_NAV_CLOCK = 0x22 +MSG_NAV_SVINFO = 0x30 +MSG_NAV_AOPSTATUS = 0x60 +MSG_NAV_DGPS = 0x31 +MSG_NAV_DOP = 0x04 +MSG_NAV_EKFSTATUS = 0x40 +MSG_NAV_SBAS = 0x32 +MSG_NAV_SOL = 0x06 + +# RXM messages +MSG_RXM_RAW = 0x15 +MSG_RXM_SFRB = 0x11 +MSG_RXM_SFRBX = 0x13 +MSG_RXM_SVSI = 0x20 +MSG_RXM_EPH = 0x31 +MSG_RXM_ALM = 0x30 +MSG_RXM_PMREQ = 0x41 + +# AID messages +MSG_AID_ALM = 0x30 +MSG_AID_EPH = 0x31 +MSG_AID_ALPSRV = 0x32 +MSG_AID_AOP = 0x33 +MSG_AID_DATA = 0x10 +MSG_AID_ALP = 0x50 +MSG_AID_DATA = 0x10 +MSG_AID_HUI = 0x02 +MSG_AID_INI = 0x01 +MSG_AID_REQ = 0x00 + +# CFG messages +MSG_CFG_PRT = 0x00 +MSG_CFG_ANT = 0x13 +MSG_CFG_DAT = 0x06 +MSG_CFG_EKF = 0x12 +MSG_CFG_ESFGWT = 0x29 +MSG_CFG_CFG = 0x09 +MSG_CFG_USB = 0x1b +MSG_CFG_RATE = 0x08 +MSG_CFG_SET_RATE = 0x01 +MSG_CFG_NAV5 = 0x24 +MSG_CFG_FXN = 0x0E +MSG_CFG_INF = 0x02 +MSG_CFG_ITFM = 0x39 +MSG_CFG_MSG = 0x01 +MSG_CFG_NAVX5 = 0x23 +MSG_CFG_NMEA = 0x17 +MSG_CFG_NVS = 0x22 +MSG_CFG_PM2 = 0x3B +MSG_CFG_PM = 0x32 +MSG_CFG_RINV = 0x34 +MSG_CFG_RST = 0x04 +MSG_CFG_RXM = 0x11 +MSG_CFG_SBAS = 0x16 +MSG_CFG_TMODE2 = 0x3D +MSG_CFG_TMODE = 0x1D +MSG_CFG_TPS = 0x31 +MSG_CFG_TP = 0x07 +MSG_CFG_GNSS = 0x3E +MSG_CFG_ODO = 0x1E + +# ESF messages +MSG_ESF_MEAS = 0x02 +MSG_ESF_STATUS = 0x10 + +# INF messages +MSG_INF_DEBUG = 0x04 +MSG_INF_ERROR = 0x00 +MSG_INF_NOTICE = 0x02 +MSG_INF_TEST = 0x03 +MSG_INF_WARNING = 0x01 + +# MON messages +MSG_MON_SCHD = 0x01 +MSG_MON_HW = 0x09 +MSG_MON_HW2 = 0x0B +MSG_MON_IO = 0x02 +MSG_MON_MSGPP = 0x06 +MSG_MON_RXBUF = 0x07 +MSG_MON_RXR = 0x21 +MSG_MON_TXBUF = 0x08 +MSG_MON_VER = 0x04 + +# TIM messages +MSG_TIM_TP = 0x01 +MSG_TIM_TM2 = 0x03 +MSG_TIM_SVIN = 0x04 +MSG_TIM_VRFY = 0x06 + +# port IDs +PORT_DDC = 0 +PORT_SERIAL1 = 1 +PORT_SERIAL2 = 2 +PORT_USB = 3 +PORT_SPI = 4 + +# dynamic models +DYNAMIC_MODEL_PORTABLE = 0 +DYNAMIC_MODEL_STATIONARY = 2 +DYNAMIC_MODEL_PEDESTRIAN = 3 +DYNAMIC_MODEL_AUTOMOTIVE = 4 +DYNAMIC_MODEL_SEA = 5 +DYNAMIC_MODEL_AIRBORNE1G = 6 +DYNAMIC_MODEL_AIRBORNE2G = 7 +DYNAMIC_MODEL_AIRBORNE4G = 8 + +#reset items +RESET_HOT = 0 +RESET_WARM = 1 +RESET_COLD = 0xFFFF + +RESET_HW = 0 +RESET_SW = 1 +RESET_SW_GPS = 2 +RESET_HW_GRACEFUL = 4 +RESET_GPS_STOP = 8 +RESET_GPS_START = 9 + + +class UBloxError(Exception): + '''Ublox error class''' + + def __init__(self, msg): + Exception.__init__(self, msg) + self.message = msg + + +class UBloxAttrDict(dict): + '''allow dictionary members as attributes''' + + def __init__(self): + dict.__init__(self) + + def __getattr__(self, name): + try: + return self.__getitem__(name) + except KeyError: + raise AttributeError(name) + + def __setattr__(self, name, value): + if name in self.__dict__: + # allow set on normal attributes + dict.__setattr__(self, name, value) + else: + self.__setitem__(name, value) + + +def ArrayParse(field): + '''parse an array descriptor''' + arridx = field.find('[') + if arridx == -1: + return (field, -1) + alen = int(field[arridx + 1:-1]) + fieldname = field[:arridx] + return (fieldname, alen) + + +class UBloxDescriptor: + '''class used to describe the layout of a UBlox message''' + + def __init__(self, + name, + msg_format, + fields=None, + count_field=None, + format2=None, + fields2=None): + if fields is None: + fields = [] + + self.name = name + self.msg_format = msg_format + self.fields = fields + self.count_field = count_field + self.format2 = format2 + self.fields2 = fields2 + + def unpack(self, msg): + '''unpack a UBloxMessage, creating the .fields and ._recs attributes in msg''' + msg._fields = {} + + # unpack main message blocks. A comm + formats = self.msg_format.split(',') + buf = msg._buf[6:-2] + count = 0 + msg._recs = [] + fields = self.fields[:] + + for fmt in formats: + size1 = struct.calcsize(fmt) + if size1 > len(buf): + raise UBloxError("%s INVALID_SIZE1=%u" % (self.name, len(buf))) + f1 = list(struct.unpack(fmt, buf[:size1])) + i = 0 + while i < len(f1): + field = fields.pop(0) + (fieldname, alen) = ArrayParse(field) + if alen == -1: + msg._fields[fieldname] = f1[i] + if self.count_field == fieldname: + count = int(f1[i]) + i += 1 + else: + msg._fields[fieldname] = [0] * alen + for a in range(alen): + msg._fields[fieldname][a] = f1[i] + i += 1 + buf = buf[size1:] + if len(buf) == 0: + break + + if self.count_field == '_remaining': + count = len(buf) // struct.calcsize(self.format2) + + if count == 0: + msg._unpacked = True + if len(buf) != 0: + raise UBloxError("EXTRA_BYTES=%u" % len(buf)) + return + + size2 = struct.calcsize(self.format2) + for c in range(count): + r = UBloxAttrDict() + if size2 > len(buf): + raise UBloxError("INVALID_SIZE=%u, " % len(buf)) + f2 = list(struct.unpack(self.format2, buf[:size2])) + for i in range(len(self.fields2)): + r[self.fields2[i]] = f2[i] + buf = buf[size2:] + msg._recs.append(r) + if len(buf) != 0: + raise UBloxError("EXTRA_BYTES=%u" % len(buf)) + msg._unpacked = True + + def pack(self, msg, msg_class=None, msg_id=None): + '''pack a UBloxMessage from the .fields and ._recs attributes in msg''' + f1 = [] + if msg_class is None: + msg_class = msg.msg_class() + if msg_id is None: + msg_id = msg.msg_id() + msg._buf = '' + + fields = self.fields[:] + for f in fields: + (fieldname, alen) = ArrayParse(f) + if not fieldname in msg._fields: + break + if alen == -1: + f1.append(msg._fields[fieldname]) + else: + for a in range(alen): + f1.append(msg._fields[fieldname][a]) + try: + # try full length message + fmt = self.msg_format.replace(',', '') + msg._buf = struct.pack(fmt, *tuple(f1)) + except Exception: + # try without optional part + fmt = self.msg_format.split(',')[0] + msg._buf = struct.pack(fmt, *tuple(f1)) + + length = len(msg._buf) + if msg._recs: + length += len(msg._recs) * struct.calcsize(self.format2) + header = struct.pack('= level: + print(msg) + + def unpack(self): + '''unpack a message''' + if not self.valid(): + raise UBloxError('INVALID MESSAGE') + type = self.msg_type() + if not type in msg_types: + raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf))) + msg_types[type].unpack(self) + return self._fields, self._recs + + def pack(self): + '''pack a message''' + if not self.valid(): + raise UBloxError('INVALID MESSAGE') + type = self.msg_type() + if not type in msg_types: + raise UBloxError('Unknown message %s' % str(type)) + msg_types[type].pack(self) + + def name(self): + '''return the short string name for a message''' + if not self.valid(): + raise UBloxError('INVALID MESSAGE') + type = self.msg_type() + if not type in msg_types: + raise UBloxError('Unknown message %s length=%u' % (str(type), len(self._buf))) + return msg_types[type].name + + def msg_class(self): + '''return the message class''' + return self._buf[2] + + def msg_id(self): + '''return the message id within the class''' + return self._buf[3] + + def msg_type(self): + '''return the message type tuple (class, id)''' + return (self.msg_class(), self.msg_id()) + + def msg_length(self): + '''return the payload length''' + (payload_length, ) = struct.unpack(' 0 and self._buf[0] != PREAMBLE1: + return False + if len(self._buf) > 1 and self._buf[1] != PREAMBLE2: + self.debug(1, "bad pre2") + return False + if self.needed_bytes() == 0 and not self.valid(): + if len(self._buf) > 8: + self.debug(1, "bad checksum len=%u needed=%u" % (len(self._buf), + self.needed_bytes())) + else: + self.debug(1, "bad len len=%u needed=%u" % (len(self._buf), self.needed_bytes())) + return False + return True + + def add(self, bytes): + '''add some bytes to a message''' + self._buf += bytes + while not self.valid_so_far() and len(self._buf) > 0: + '''handle corrupted streams''' + self._buf = self._buf[1:] + if self.needed_bytes() < 0: + self._buf = "" + + def checksum(self, data=None): + '''return a checksum tuple for a message''' + if data is None: + data = self._buf[2:-2] + #cs = 0 + ck_a = 0 + ck_b = 0 + for i in data: + ck_a = (ck_a + i) & 0xFF + ck_b = (ck_b + ck_a) & 0xFF + return (ck_a, ck_b) + + def valid_checksum(self): + '''check if the checksum is OK''' + (ck_a, ck_b) = self.checksum() + #d = self._buf[2:-2] + (ck_a2, ck_b2) = struct.unpack('= 8 and self.needed_bytes() == 0 and self.valid_checksum() + + +class UBlox: + '''main UBlox control class. + + port can be a file (for reading only) or a serial device + ''' + + def __init__(self, port, baudrate=115200, timeout=0, panda=False, grey=False): + + self.serial_device = port + self.baudrate = baudrate + self.use_sendrecv = False + self.read_only = False + self.debug_level = 0 + + if panda: + from panda import Panda, PandaSerial + + self.panda = Panda() + + # resetting U-Blox module + self.panda.set_esp_power(0) + time.sleep(0.1) + self.panda.set_esp_power(1) + time.sleep(0.5) + + # can't set above 9600 now... + self.baudrate = 9600 + self.dev = PandaSerial(self.panda, 1, self.baudrate) + + self.baudrate = 460800 + print("upping baud:",self.baudrate) + self.send_nmea("$PUBX,41,1,0007,0003,%u,0" % self.baudrate) + time.sleep(0.1) + + self.dev = PandaSerial(self.panda, 1, self.baudrate) + elif grey: + import cereal.messaging as messaging + + class BoarddSerial(): + def __init__(self): + self.ubloxRaw = messaging.sub_sock('ubloxRaw') + self.buf = "" + + def read(self, n): + for msg in messaging.drain_sock(self.ubloxRaw, len(self.buf) < n): + self.buf += msg.ubloxRaw + ret = self.buf[:n] + self.buf = self.buf[n:] + return ret + + + def write(self, dat): + pass + + self.dev = BoarddSerial() + else: + if self.serial_device.startswith("tcp:"): + import socket + a = self.serial_device.split(':') + destination_addr = (a[1], int(a[2])) + self.dev = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.dev.connect(destination_addr) + self.dev.setblocking(1) + self.dev.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1) + self.use_sendrecv = True + elif os.path.isfile(self.serial_device): + self.read_only = True + self.dev = open(self.serial_device, mode='rb') + else: + import serial + self.dev = serial.Serial( + self.serial_device, + baudrate=self.baudrate, + dsrdtr=False, + rtscts=False, + xonxoff=False, + timeout=timeout) + + self.logfile = None + self.log = None + self.preferred_dynamic_model = None + self.preferred_usePPP = None + self.preferred_dgps_timeout = None + + def close(self): + '''close the device''' + self.dev.close() + self.dev = None + + def set_debug(self, debug_level): + '''set debug level''' + self.debug_level = debug_level + + def debug(self, level, msg): + '''write a debug message''' + if self.debug_level >= level: + print(msg) + + def set_logfile(self, logfile, append=False): + '''setup logging to a file''' + if self.log is not None: + self.log.close() + self.log = None + self.logfile = logfile + if self.logfile is not None: + if append: + mode = 'ab' + else: + mode = 'wb' + self.log = open(self.logfile, mode=mode) + + def set_preferred_dynamic_model(self, model): + '''set the preferred dynamic model for receiver''' + self.preferred_dynamic_model = model + if model is not None: + self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) + + def set_preferred_dgps_timeout(self, timeout): + '''set the preferred DGPS timeout for receiver''' + self.preferred_dgps_timeout = timeout + if timeout is not None: + self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) + + def set_preferred_usePPP(self, usePPP): + '''set the preferred usePPP setting for the receiver''' + if usePPP is None: + self.preferred_usePPP = None + return + self.preferred_usePPP = int(usePPP) + self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5) + + def nmea_checksum(self, msg): + d = msg[1:] + cs = 0 + for i in d: + cs ^= ord(i) + return cs + + def write(self, buf): + '''write some bytes''' + if not self.read_only: + if self.use_sendrecv: + return self.dev.send(buf) + return self.dev.write(buf) + + def read(self, n): + '''read some bytes''' + if self.use_sendrecv: + import socket + try: + return self.dev.recv(n) + except socket.error: + return '' + return self.dev.read(n) + + def send_nmea(self, msg): + if not self.read_only: + s = msg + "*%02X" % self.nmea_checksum(msg) + "\r\n" + self.write(s) + + def set_binary(self): + '''put a UBlox into binary mode using a NMEA string''' + if not self.read_only: + print("try set binary at %u" % self.baudrate) + self.send_nmea("$PUBX,41,0,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,1,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,2,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,3,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,4,0007,0001,%u,0" % self.baudrate) + self.send_nmea("$PUBX,41,5,0007,0001,%u,0" % self.baudrate) + + def disable_nmea(self): + ''' stop sending all types of nmea messages ''' + self.send_nmea("$PUBX,40,GSV,1,1,1,1,1,0") + self.send_nmea("$PUBX,40,GGA,0,0,0,0,0,0") + self.send_nmea("$PUBX,40,GSA,0,0,0,0,0,0") + self.send_nmea("$PUBX,40,VTG,0,0,0,0,0,0") + self.send_nmea("$PUBX,40,TXT,0,0,0,0,0,0") + self.send_nmea("$PUBX,40,RMC,0,0,0,0,0,0") + + def seek_percent(self, pct): + '''seek to the given percentage of a file''' + self.dev.seek(0, 2) + filesize = self.dev.tell() + self.dev.seek(pct * 0.01 * filesize) + + def special_handling(self, msg): + '''handle automatic configuration changes''' + if msg.name() == 'CFG_NAV5': + msg.unpack() + sendit = False + pollit = False + if self.preferred_dynamic_model is not None and msg.dynModel != self.preferred_dynamic_model: + msg.dynModel = self.preferred_dynamic_model + sendit = True + pollit = True + if self.preferred_dgps_timeout is not None and msg.dgpsTimeOut != self.preferred_dgps_timeout: + msg.dgpsTimeOut = self.preferred_dgps_timeout + self.debug(2, "Setting dgpsTimeOut=%u" % msg.dgpsTimeOut) + sendit = True + # we don't re-poll for this one, as some receivers refuse to set it + if sendit: + msg.pack() + self.send(msg) + if pollit: + self.configure_poll(CLASS_CFG, MSG_CFG_NAV5) + if msg.name() == 'CFG_NAVX5' and self.preferred_usePPP is not None: + msg.unpack() + if msg.usePPP != self.preferred_usePPP: + msg.usePPP = self.preferred_usePPP + msg.mask = 1 << 13 + msg.pack() + self.send(msg) + self.configure_poll(CLASS_CFG, MSG_CFG_NAVX5) + + def receive_message(self, ignore_eof=False): + '''blocking receive of one ublox message''' + msg = UBloxMessage() + while True: + n = msg.needed_bytes() + b = self.read(n) + if not b: + if ignore_eof: + time.sleep(0.01) + continue + if len(msg._buf) > 0: + self.debug(1, "dropping %d bytes" % len(msg._buf)) + return None + msg.add(b) + if self.log is not None: + self.log.write(b) + self.log.flush() + if msg.valid(): + self.special_handling(msg) + return msg + + def receive_message_noerror(self, ignore_eof=False): + '''blocking receive of one ublox message, ignoring errors''' + try: + return self.receive_message(ignore_eof=ignore_eof) + except UBloxError as e: + print(e) + return None + except OSError as e: + # Occasionally we get hit with 'resource temporarily unavailable' + # messages here on the serial device, catch them too. + print(e) + return None + + def send(self, msg): + '''send a preformatted ublox message''' + if not msg.valid(): + self.debug(1, "invalid send") + return + if not self.read_only: + self.write(msg._buf) + + def send_message(self, msg_class, msg_id, payload): + '''send a ublox message with class, id and payload''' + msg = UBloxMessage() + msg._buf = struct.pack(' 1e-3: + print('Gps vNED mismatch', o, n) + return + elif o != n: + print('Gps mismatch', attr, o, n) + return -4 + elif events1[i].which() == 'ubloxGnss': + old_gnss = events1[i].ubloxGnss + gnss = events2[i].ubloxGnss + if old_gnss.which() == 'measurementReport' and gnss.which() == 'measurementReport': + attrs = ['gpsWeek', 'leapSeconds', 'measurements', 'numMeas', 'rcvTow', 'receiverStatus', 'schema'] + for attr in attrs: + o = getattr(old_gnss.measurementReport, attr) + n = getattr(gnss.measurementReport, attr) + if str(o) != str(n): + print('measurementReport {} mismatched'.format(attr)) + return -5 + if not (str(old_gnss.measurementReport) == str(gnss.measurementReport)): + print('Gnss measurementReport mismatched!') + print('gnss measurementReport old', old_gnss.measurementReport.measurements) + print('gnss measurementReport new', gnss.measurementReport.measurements) + return -6 + elif old_gnss.which() == 'ephemeris' and gnss.which() == 'ephemeris': + if not (str(old_gnss.ephemeris) == str(gnss.ephemeris)): + print('Gnss ephemeris mismatched!') + print('gnss ephemeris old', old_gnss.ephemeris) + print('gnss ephemeris new', gnss.ephemeris) + return -7 + print('All {} events matched!'.format(len(events1))) + return 0 + + +if __name__ == "__main__": + args = get_arg_parser().parse_args(sys.argv[1:]) + compare_results(args.dir1, args.dir2) diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc new file mode 100644 index 0000000000..ae78eb7192 --- /dev/null +++ b/selfdrive/locationd/ublox_msg.cc @@ -0,0 +1,374 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "cereal/gen/cpp/log.capnp.h" + +#include "common/params.h" +#include "common/swaglog.h" +#include "common/timing.h" + +#include "ublox_msg.h" + +#define UBLOX_MSG_SIZE(hdr) (*(uint16_t *)&hdr[4]) +#define GET_FIELD_U(w, nb, pos) (((w) >> (pos)) & ((1<<(nb))-1)) + +namespace ublox { + +inline int twos_complement(uint32_t v, uint32_t nb) { + int sign = v >> (nb - 1); + int value = v; + if(sign != 0) + value = value - (1 << nb); + return value; +} + +inline int GET_FIELD_S(uint32_t w, uint32_t nb, uint32_t pos) { + int v = GET_FIELD_U(w, nb, pos); + return twos_complement(v, nb); +} + +class EphemerisData { + public: + EphemerisData(uint8_t svId, subframes_map subframes) { + this->svId = svId; + int week_no = GET_FIELD_U(subframes[1][2+0], 10, 20); + int t_gd = GET_FIELD_S(subframes[1][2+4], 8, 6); + int iodc = (GET_FIELD_U(subframes[1][2+0], 2, 6) << 8) | GET_FIELD_U( + subframes[1][2+5], 8, 22); + + int t_oc = GET_FIELD_U(subframes[1][2+5], 16, 6); + int a_f2 = GET_FIELD_S(subframes[1][2+6], 8, 22); + int a_f1 = GET_FIELD_S(subframes[1][2+6], 16, 6); + int a_f0 = GET_FIELD_S(subframes[1][2+7], 22, 8); + + int c_rs = GET_FIELD_S(subframes[2][2+0], 16, 6); + int delta_n = GET_FIELD_S(subframes[2][2+1], 16, 14); + int m_0 = (GET_FIELD_S(subframes[2][2+1], 8, 6) << 24) | GET_FIELD_U( + subframes[2][2+2], 24, 6); + int c_uc = GET_FIELD_S(subframes[2][2+3], 16, 14); + int e = (GET_FIELD_U(subframes[2][2+3], 8, 6) << 24) | GET_FIELD_U(subframes[2][2+4], 24, 6); + int c_us = GET_FIELD_S(subframes[2][2+5], 16, 14); + uint32_t a_powhalf = (GET_FIELD_U(subframes[2][2+5], 8, 6) << 24) | GET_FIELD_U( + subframes[2][2+6], 24, 6); + int t_oe = GET_FIELD_U(subframes[2][2+7], 16, 14); + + int c_ic = GET_FIELD_S(subframes[3][2+0], 16, 14); + int omega_0 = (GET_FIELD_S(subframes[3][2+0], 8, 6) << 24) | GET_FIELD_U( + subframes[3][2+1], 24, 6); + int c_is = GET_FIELD_S(subframes[3][2+2], 16, 14); + int i_0 = (GET_FIELD_S(subframes[3][2+2], 8, 6) << 24) | GET_FIELD_U( + subframes[3][2+3], 24, 6); + int c_rc = GET_FIELD_S(subframes[3][2+4], 16, 14); + int w = (GET_FIELD_S(subframes[3][2+4], 8, 6) << 24) | GET_FIELD_U(subframes[3][5], 24, 6); + int omega_dot = GET_FIELD_S(subframes[3][2+6], 24, 6); + int idot = GET_FIELD_S(subframes[3][2+7], 14, 8); + + this->_rsvd1 = GET_FIELD_U(subframes[1][2+1], 23, 6); + this->_rsvd2 = GET_FIELD_U(subframes[1][2+2], 24, 6); + this->_rsvd3 = GET_FIELD_U(subframes[1][2+3], 24, 6); + this->_rsvd4 = GET_FIELD_U(subframes[1][2+4], 16, 14); + this->aodo = GET_FIELD_U(subframes[2][2+7], 5, 8); + + double gpsPi = 3.1415926535898; + + // now form variables in radians, meters and seconds etc + this->Tgd = t_gd * pow(2, -31); + this->A = pow(a_powhalf * pow(2, -19), 2.0); + this->cic = c_ic * pow(2, -29); + this->cis = c_is * pow(2, -29); + this->crc = c_rc * pow(2, -5); + this->crs = c_rs * pow(2, -5); + this->cuc = c_uc * pow(2, -29); + this->cus = c_us * pow(2, -29); + this->deltaN = delta_n * pow(2, -43) * gpsPi; + this->ecc = e * pow(2, -33); + this->i0 = i_0 * pow(2, -31) * gpsPi; + this->idot = idot * pow(2, -43) * gpsPi; + this->M0 = m_0 * pow(2, -31) * gpsPi; + this->omega = w * pow(2, -31) * gpsPi; + this->omega_dot = omega_dot * pow(2, -43) * gpsPi; + this->omega0 = omega_0 * pow(2, -31) * gpsPi; + this->toe = t_oe * pow(2, 4); + + this->toc = t_oc * pow(2, 4); + this->gpsWeek = week_no; + this->af0 = a_f0 * pow(2, -31); + this->af1 = a_f1 * pow(2, -43); + this->af2 = a_f2 * pow(2, -55); + + uint32_t iode1 = GET_FIELD_U(subframes[2][2+0], 8, 22); + uint32_t iode2 = GET_FIELD_U(subframes[3][2+7], 8, 22); + this->valid = (iode1 == iode2) && (iode1 == (iodc & 0xff)); + this->iode = iode1; + + if (GET_FIELD_U(subframes[4][2+0], 6, 22) == 56 && + GET_FIELD_U(subframes[4][2+0], 2, 28) == 1 && + GET_FIELD_U(subframes[5][2+0], 2, 28) == 1) { + double a0 = GET_FIELD_S(subframes[4][2], 8, 14) * pow(2, -30); + double a1 = GET_FIELD_S(subframes[4][2], 8, 6) * pow(2, -27); + double a2 = GET_FIELD_S(subframes[4][3], 8, 22) * pow(2, -24); + double a3 = GET_FIELD_S(subframes[4][3], 8, 14) * pow(2, -24); + double b0 = GET_FIELD_S(subframes[4][3], 8, 6) * pow(2, 11); + double b1 = GET_FIELD_S(subframes[4][4], 8, 22) * pow(2, 14); + double b2 = GET_FIELD_S(subframes[4][4], 8, 14) * pow(2, 16); + double b3 = GET_FIELD_S(subframes[4][4], 8, 6) * pow(2, 16); + this->ionoAlpha[0] = a0;this->ionoAlpha[1] = a1;this->ionoAlpha[2] = a2;this->ionoAlpha[3] = a3; + this->ionoBeta[0] = b0;this->ionoBeta[1] = b1;this->ionoBeta[2] = b2;this->ionoBeta[3] = b3; + this->ionoCoeffsValid = true; + } else { + this->ionoCoeffsValid = false; + } + } + uint16_t svId; + double Tgd, A, cic, cis, crc, crs, cuc, cus, deltaN, ecc, i0, idot, M0, omega, omega_dot, omega0, toe, toc; + uint32_t gpsWeek, iode, _rsvd1, _rsvd2, _rsvd3, _rsvd4, aodo; + double af0, af1, af2; + bool valid; + double ionoAlpha[4], ionoBeta[4]; + bool ionoCoeffsValid; +}; + +UbloxMsgParser::UbloxMsgParser() :bytes_in_parse_buf(0) { + nav_frame_buffer[0U] = std::map(); + for(int i = 1;i < 33;i++) + nav_frame_buffer[0U][i] = subframes_map(); +} + +inline int UbloxMsgParser::needed_bytes() { + // Msg header incomplete? + if(bytes_in_parse_buf < UBLOX_HEADER_SIZE) + return UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf; + uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE; + // too much data + if(needed < (uint16_t)bytes_in_parse_buf) + return -1; + return needed - (uint16_t)bytes_in_parse_buf; +} + +inline bool UbloxMsgParser::valid_cheksum() { + uint8_t ck_a = 0, ck_b = 0; + for(int i = 2; i < bytes_in_parse_buf - UBLOX_CHECKSUM_SIZE;i++) { + ck_a = (ck_a + msg_parse_buf[i]) & 0xFF; + ck_b = (ck_b + ck_a) & 0xFF; + } + if(ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) { + LOGD("Checksum a mismtach: %02X, %02X", ck_a, msg_parse_buf[6]); + return false; + } + if(ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) { + LOGD("Checksum b mismtach: %02X, %02X", ck_b, msg_parse_buf[7]); + return false; + } + return true; +} + +inline bool UbloxMsgParser::valid() { + return bytes_in_parse_buf >= UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE && + needed_bytes() == 0 && + valid_cheksum(); +} + +inline bool UbloxMsgParser::valid_so_far() { + if(bytes_in_parse_buf > 0 && msg_parse_buf[0] != PREAMBLE1) { + //LOGD("PREAMBLE1 invalid, %02X.", msg_parse_buf[0]); + return false; + } + if(bytes_in_parse_buf > 1 && msg_parse_buf[1] != PREAMBLE2) { + //LOGD("PREAMBLE2 invalid, %02X.", msg_parse_buf[1]); + return false; + } + if(needed_bytes() == 0 && !valid()) + return false; + return true; +} + +kj::Array UbloxMsgParser::gen_solution() { + nav_pvt_msg *msg = (nav_pvt_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; + capnp::MallocMessageBuilder msg_builder; + cereal::Event::Builder event = msg_builder.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto gpsLoc = event.initGpsLocationExternal(); + gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX); + gpsLoc.setFlags(msg->flags); + gpsLoc.setLatitude(msg->lat * 1e-07); + gpsLoc.setLongitude(msg->lon * 1e-07); + gpsLoc.setAltitude(msg->height * 1e-03); + gpsLoc.setSpeed(msg->gSpeed * 1e-03); + gpsLoc.setBearing(msg->headMot * 1e-5); + gpsLoc.setAccuracy(msg->hAcc * 1e-03); + std::tm timeinfo = std::tm(); + timeinfo.tm_year = msg->year - 1900; + timeinfo.tm_mon = msg->month - 1; + timeinfo.tm_mday = msg->day; + timeinfo.tm_hour = msg->hour; + timeinfo.tm_min = msg->min; + timeinfo.tm_sec = msg->sec; + std::time_t utc_tt = timegm(&timeinfo); + gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano * 1e-06); + float f[] = { msg->velN * 1e-03f, msg->velE * 1e-03f, msg->velD * 1e-03f }; + kj::ArrayPtr ap(&f[0], sizeof(f) / sizeof(f[0])); + gpsLoc.setVNED(ap); + gpsLoc.setVerticalAccuracy(msg->vAcc * 1e-03); + gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03); + gpsLoc.setBearingAccuracy(msg->headAcc * 1e-05); + return capnp::messageToFlatArray(msg_builder); +} + +inline bool bit_to_bool(uint8_t val, int shifts) { + return (val & (1 << shifts)) ? true : false; +} + +kj::Array UbloxMsgParser::gen_raw() { + rxm_raw_msg *msg = (rxm_raw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; + if(bytes_in_parse_buf != ( + UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg) + msg->numMeas * sizeof(rxm_raw_msg_extra) + UBLOX_CHECKSUM_SIZE + )) { + LOGD("Invalid measurement size %u, %u, %u, %u", msg->numMeas, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg)); + return kj::Array(); + } + rxm_raw_msg_extra *measurements = (rxm_raw_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg)]; + capnp::MallocMessageBuilder msg_builder; + cereal::Event::Builder event = msg_builder.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto gnss = event.initUbloxGnss(); + auto mr = gnss.initMeasurementReport(); + mr.setRcvTow(msg->rcvTow); + mr.setGpsWeek(msg->week); + mr.setLeapSeconds(msg->leapS); + mr.setGpsWeek(msg->week); + auto mb = mr.initMeasurements(msg->numMeas); + for(int8_t i = 0; i < msg->numMeas; i++) { + mb[i].setSvId(measurements[i].svId); + mb[i].setSigId(measurements[i].sigId); + mb[i].setPseudorange(measurements[i].prMes); + mb[i].setCarrierCycles(measurements[i].cpMes); + mb[i].setDoppler(measurements[i].doMes); + mb[i].setGnssId(measurements[i].gnssId); + mb[i].setGlonassFrequencyIndex(measurements[i].freqId); + mb[i].setLocktime(measurements[i].locktime); + mb[i].setCno(measurements[i].cno); + mb[i].setPseudorangeStdev(0.01*(pow(2, (measurements[i].prStdev & 15)))); // weird scaling, might be wrong + mb[i].setCarrierPhaseStdev(0.004*(measurements[i].cpStdev & 15)); + mb[i].setDopplerStdev(0.002*(pow(2, (measurements[i].doStdev & 15)))); // weird scaling, might be wrong + auto ts = mb[i].initTrackingStatus(); + ts.setPseudorangeValid(bit_to_bool(measurements[i].trkStat, 0)); + ts.setCarrierPhaseValid(bit_to_bool(measurements[i].trkStat, 1)); + ts.setHalfCycleValid(bit_to_bool(measurements[i].trkStat, 2)); + ts.setHalfCycleSubtracted(bit_to_bool(measurements[i].trkStat, 3)); + } + + mr.setNumMeas(msg->numMeas); + auto rs = mr.initReceiverStatus(); + rs.setLeapSecValid(bit_to_bool(msg->recStat, 0)); + rs.setClkReset(bit_to_bool(msg->recStat, 2)); + return capnp::messageToFlatArray(msg_builder); +} + +kj::Array UbloxMsgParser::gen_nav_data() { + rxm_sfrbx_msg *msg = (rxm_sfrbx_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; + if(bytes_in_parse_buf != ( + UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg) + msg->numWords * sizeof(rxm_sfrbx_msg_extra) + UBLOX_CHECKSUM_SIZE + )) { + LOGD("Invalid sfrbx words size %u, %u, %u, %u", msg->numWords, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg)); + return kj::Array(); + } + rxm_sfrbx_msg_extra *measurements = (rxm_sfrbx_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg)]; + if(msg->gnssId == 0) { + uint8_t subframeId = GET_FIELD_U(measurements[1].dwrd, 3, 8); + std::vector words; + for(int i = 0; i < msg->numWords;i++) + words.push_back(measurements[i].dwrd); + + if(subframeId == 1) { + nav_frame_buffer[msg->gnssId][msg->svid] = subframes_map(); + nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words; + } else if(nav_frame_buffer[msg->gnssId][msg->svid].find(subframeId-1) != nav_frame_buffer[msg->gnssId][msg->svid].end()) + nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words; + if(nav_frame_buffer[msg->gnssId][msg->svid].size() == 5) { + EphemerisData ephem_data(msg->svid, nav_frame_buffer[msg->gnssId][msg->svid]); + capnp::MallocMessageBuilder msg_builder; + cereal::Event::Builder event = msg_builder.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto gnss = event.initUbloxGnss(); + auto eph = gnss.initEphemeris(); + eph.setSvId(ephem_data.svId); + eph.setToc(ephem_data.toc); + eph.setGpsWeek(ephem_data.gpsWeek); + eph.setAf0(ephem_data.af0); + eph.setAf1(ephem_data.af1); + eph.setAf2(ephem_data.af2); + eph.setIode(ephem_data.iode); + eph.setCrs(ephem_data.crs); + eph.setDeltaN(ephem_data.deltaN); + eph.setM0(ephem_data.M0); + eph.setCuc(ephem_data.cuc); + eph.setEcc(ephem_data.ecc); + eph.setCus(ephem_data.cus); + eph.setA(ephem_data.A); + eph.setToe(ephem_data.toe); + eph.setCic(ephem_data.cic); + eph.setOmega0(ephem_data.omega0); + eph.setCis(ephem_data.cis); + eph.setI0(ephem_data.i0); + eph.setCrc(ephem_data.crc); + eph.setOmega(ephem_data.omega); + eph.setOmegaDot(ephem_data.omega_dot); + eph.setIDot(ephem_data.idot); + eph.setTgd(ephem_data.Tgd); + eph.setIonoCoeffsValid(ephem_data.ionoCoeffsValid); + if(ephem_data.ionoCoeffsValid) { + kj::ArrayPtr apa(&ephem_data.ionoAlpha[0], sizeof(ephem_data.ionoAlpha) / sizeof(ephem_data.ionoAlpha[0])); + eph.setIonoAlpha(apa); + kj::ArrayPtr apb(&ephem_data.ionoBeta[0], sizeof(ephem_data.ionoBeta) / sizeof(ephem_data.ionoBeta[0])); + eph.setIonoBeta(apb); + } else { + eph.setIonoAlpha(kj::ArrayPtr()); + eph.setIonoBeta(kj::ArrayPtr()); + } + return capnp::messageToFlatArray(msg_builder); + } + } + return kj::Array(); +} + +bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) { + int needed = needed_bytes(); + if(needed > 0) { + bytes_consumed = min((size_t)needed, incoming_data_len ); + // Add data to buffer + memcpy(msg_parse_buf + bytes_in_parse_buf, incoming_data, bytes_consumed); + bytes_in_parse_buf += bytes_consumed; + } else { + bytes_consumed = incoming_data_len; + } + // Validate msg format, detect invalid header and invalid checksum. + while(!valid_so_far() && bytes_in_parse_buf != 0) { + //LOGD("Drop corrupt data, remained in buf: %u", bytes_in_parse_buf); + // Corrupted msg, drop a byte. + bytes_in_parse_buf -= 1; + if(bytes_in_parse_buf > 0) + memmove(&msg_parse_buf[0], &msg_parse_buf[1], bytes_in_parse_buf); + } + // There is redundant data at the end of buffer, reset the buffer. + if(needed_bytes() == -1) + bytes_in_parse_buf = 0; + return valid(); +} + +} diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h new file mode 100644 index 0000000000..eca6186d4a --- /dev/null +++ b/selfdrive/locationd/ublox_msg.h @@ -0,0 +1,150 @@ +#pragma once + +#include +#include "messaging.hpp" + +#define min(x, y) ((x) <= (y) ? (x) : (y)) + +// NAV_PVT +typedef struct __attribute__((packed)) { + uint32_t iTOW; + uint16_t year; + int8_t month; + int8_t day; + int8_t hour; + int8_t min; + int8_t sec; + int8_t valid; + uint32_t tAcc; + int32_t nano; + int8_t fixType; + int8_t flags; + int8_t flags2; + int8_t numSV; + int32_t lon; + int32_t lat; + int32_t height; + int32_t hMSL; + uint32_t hAcc; + uint32_t vAcc; + int32_t velN; + int32_t velE; + int32_t velD; + int32_t gSpeed; + int32_t headMot; + uint32_t sAcc; + uint32_t headAcc; + uint16_t pDOP; + int8_t reserverd1[6]; + int32_t headVeh; + int16_t magDec; + uint16_t magAcc; +} nav_pvt_msg; + +// RXM_RAW +typedef struct __attribute__((packed)) { + double rcvTow; + uint16_t week; + int8_t leapS; + int8_t numMeas; + int8_t recStat; + int8_t reserved1[3]; +} rxm_raw_msg; + +// Extra data count is in numMeas +typedef struct __attribute__((packed)) { + double prMes; + double cpMes; + float doMes; + int8_t gnssId; + int8_t svId; + int8_t sigId; + int8_t freqId; + uint16_t locktime; + int8_t cno; + int8_t prStdev; + int8_t cpStdev; + int8_t doStdev; + int8_t trkStat; + int8_t reserved3; +} rxm_raw_msg_extra; +// RXM_SFRBX +typedef struct __attribute__((packed)) { + int8_t gnssId; + int8_t svid; + int8_t reserved1; + int8_t freqId; + int8_t numWords; + int8_t reserved2; + int8_t version; + int8_t reserved3; +} rxm_sfrbx_msg; + +// Extra data count is in numWords +typedef struct __attribute__((packed)) { + uint32_t dwrd; +} rxm_sfrbx_msg_extra; + +namespace ublox { + // protocol constants + const uint8_t PREAMBLE1 = 0xb5; + const uint8_t PREAMBLE2 = 0x62; + + // message classes + const uint8_t CLASS_NAV = 0x01; + const uint8_t CLASS_RXM = 0x02; + + // NAV messages + const uint8_t MSG_NAV_PVT = 0x7; + + // RXM messages + const uint8_t MSG_RXM_RAW = 0x15; + const uint8_t MSG_RXM_SFRBX = 0x13; + + const int UBLOX_HEADER_SIZE = 6; + const int UBLOX_CHECKSUM_SIZE = 2; + const int UBLOX_MAX_MSG_SIZE = 65536; + + typedef std::map> subframes_map; + + class UbloxMsgParser { + public: + + UbloxMsgParser(); + kj::Array gen_solution(); + kj::Array gen_raw(); + + kj::Array gen_nav_data(); + bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed); + inline void reset() {bytes_in_parse_buf = 0;} + inline uint8_t msg_class() { + return msg_parse_buf[2]; + } + + inline uint8_t msg_id() { + return msg_parse_buf[3]; + } + inline int needed_bytes(); + + void hexdump(uint8_t *d, int l) { + for (int i = 0; i < l; i++) { + if (i%0x10 == 0 && i != 0) printf("\n"); + printf("%02X ", d[i]); + } + printf("\n"); + } + private: + inline bool valid_cheksum(); + inline bool valid(); + inline bool valid_so_far(); + + uint8_t msg_parse_buf[UBLOX_HEADER_SIZE + UBLOX_MAX_MSG_SIZE]; + int bytes_in_parse_buf; + std::map> nav_frame_buffer; + }; + +} + +typedef Message * (*poll_ubloxraw_msg_func)(Poller *poller); +typedef int (*send_gps_event_func)(PubSocket *s, const void *buf, size_t len); +int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func); diff --git a/selfdrive/locationd/ubloxd.cc b/selfdrive/locationd/ubloxd.cc new file mode 100644 index 0000000000..2c28f14228 --- /dev/null +++ b/selfdrive/locationd/ubloxd.cc @@ -0,0 +1,47 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "messaging.hpp" +#include +#include "cereal/gen/cpp/log.capnp.h" + +#include "common/params.h" +#include "common/swaglog.h" +#include "common/timing.h" + +#include "ublox_msg.h" + +const long ZMQ_POLL_TIMEOUT = 1000; // In miliseconds + +Message * poll_ubloxraw_msg(Poller * poller) { + auto p = poller->poll(ZMQ_POLL_TIMEOUT); + + if (p.size()) { + return p[0]->receive(); + } else { + return NULL; + } +} + + +int send_gps_event(PubSocket *s, const void *buf, size_t len) { + return s->send((char*)buf, len); +} + +int main() { + return ubloxd_main(poll_ubloxraw_msg, send_gps_event); +} diff --git a/selfdrive/locationd/ubloxd_main.cc b/selfdrive/locationd/ubloxd_main.cc new file mode 100644 index 0000000000..497b69820c --- /dev/null +++ b/selfdrive/locationd/ubloxd_main.cc @@ -0,0 +1,115 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "messaging.hpp" +#include +#include "cereal/gen/cpp/log.capnp.h" + +#include "common/params.h" +#include "common/swaglog.h" +#include "common/timing.h" + +#include "ublox_msg.h" + +volatile sig_atomic_t do_exit = 0; // Flag for process exit on signal + +void set_do_exit(int sig) { + do_exit = 1; +} + +using namespace ublox; + +int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) { + LOGW("starting ubloxd"); + signal(SIGINT, (sighandler_t) set_do_exit); + signal(SIGTERM, (sighandler_t) set_do_exit); + + UbloxMsgParser parser; + + Context * c = Context::create(); + PubSocket * gpsLocationExternal = PubSocket::create(c, "gpsLocationExternal"); + PubSocket * ubloxGnss = PubSocket::create(c, "ubloxGnss"); + SubSocket * ubloxRaw = SubSocket::create(c, "ubloxRaw"); + + assert(gpsLocationExternal != NULL); + assert(ubloxGnss != NULL); + assert(ubloxRaw != NULL); + + Poller * poller = Poller::create({ubloxRaw}); + + + while (!do_exit) { + Message * msg = poll_func(poller); + if (msg == NULL) continue; + + auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg->getData(), msg->getSize()); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + + const uint8_t *data = event.getUbloxRaw().begin(); + size_t len = event.getUbloxRaw().size(); + size_t bytes_consumed = 0; + while(bytes_consumed < len && !do_exit) { + size_t bytes_consumed_this_time = 0U; + if(parser.add_data(data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) { + // New message available + if(parser.msg_class() == CLASS_NAV) { + if(parser.msg_id() == MSG_NAV_PVT) { + //LOGD("MSG_NAV_PVT"); + auto words = parser.gen_solution(); + if(words.size() > 0) { + auto bytes = words.asBytes(); + send_func(gpsLocationExternal, bytes.begin(), bytes.size()); + } + } else + LOGW("Unknown nav msg id: 0x%02X", parser.msg_id()); + } else if(parser.msg_class() == CLASS_RXM) { + if(parser.msg_id() == MSG_RXM_RAW) { + //LOGD("MSG_RXM_RAW"); + auto words = parser.gen_raw(); + if(words.size() > 0) { + auto bytes = words.asBytes(); + send_func(ubloxGnss, bytes.begin(), bytes.size()); + } + } else if(parser.msg_id() == MSG_RXM_SFRBX) { + //LOGD("MSG_RXM_SFRBX"); + auto words = parser.gen_nav_data(); + if(words.size() > 0) { + auto bytes = words.asBytes(); + send_func(ubloxGnss, bytes.begin(), bytes.size()); + } + } else + LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id()); + } else + LOGW("Unknown msg class: 0x%02X", parser.msg_class()); + parser.reset(); + } + bytes_consumed += bytes_consumed_this_time; + } + delete msg; + } + + delete poller; + delete ubloxRaw; + delete ubloxGnss; + delete gpsLocationExternal; + delete c; + + return 0; +} diff --git a/selfdrive/locationd/ubloxd_test.cc b/selfdrive/locationd/ubloxd_test.cc new file mode 100644 index 0000000000..68fe6351ea --- /dev/null +++ b/selfdrive/locationd/ubloxd_test.cc @@ -0,0 +1,102 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "messaging.hpp" +#include "impl_zmq.hpp" +#include +#include "cereal/gen/cpp/log.capnp.h" + +#include "common/params.h" +#include "common/swaglog.h" +#include "common/timing.h" +#include "common/util.h" +#include "ublox_msg.h" + +using namespace ublox; +extern volatile sig_atomic_t do_exit; + +void write_file(std::string fpath, uint8_t *data, int len) { + FILE* f = fopen(fpath.c_str(), "wb"); + if (!f) { + std::cout << "Open " << fpath << " failed" << std::endl; + return; + } + fwrite(data, len, 1, f); + fclose(f); +} + +static size_t len = 0U; +static size_t consumed = 0U; +static uint8_t *data = NULL; +static int save_idx = 0; +static std::string prefix; + +Message * poll_ubloxraw_msg(Poller * poller) { + assert(poller); + + size_t consuming = min(len - consumed, 128); + if(consumed < len) { + // create message + capnp::MallocMessageBuilder msg_builder; + cereal::Event::Builder event = msg_builder.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto ublox_raw = event.initUbloxRaw(consuming); + memcpy(ublox_raw.begin(), (void *)(data + consumed), consuming); + + auto words = capnp::messageToFlatArray(msg_builder); + auto bytes = words.asBytes(); + + Message * msg = new ZMQMessage(); + msg->init((char*)bytes.begin(), bytes.size()); + consumed += consuming; + return msg; + } else { + do_exit = 1; + return NULL; + } +} + +int send_gps_event(PubSocket *s, const void *buf, size_t len) { + assert(s); + write_file(prefix + "/" + std::to_string(save_idx), (uint8_t *)buf, len); + save_idx++; + return len; +} + +int main(int argc, char** argv) { + if(argc < 3) { + printf("Format: ubloxd_test stream_file_path save_prefix\n"); + return 0; + } + // Parse 11360 msgs, generate 9452 events + data = (uint8_t *)read_file(argv[1], &len); + if(data == NULL) { + LOGE("Read file %s failed\n", argv[1]); + return -1; + } + prefix = argv[2]; + ubloxd_main(poll_ubloxraw_msg, send_gps_event); + free(data); + printf("Generated %d cereal events\n", save_idx); + if(save_idx != 9452) { + printf("Event count error: %d\n", save_idx); + return -1; + } + return 0; +}