selfdrive/locationd

selfdrive/locationd/.gitignore

@@ -0,0 +1,4 @@
+ubloxd
+ubloxd_test
+params_learner
+paramsd

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)
+  
+
+
+

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
+

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()

selfdrive/locationd/kalman/.gitignore

@@ -0,0 +1,4 @@
+lane.cpp
+gnss.cpp
+loc*.cpp
+pos_computer*.cpp

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()

selfdrive/locationd/kalman/compute_pos.c

@@ -0,0 +1,51 @@
+#include <eigen3/Eigen/QR>    
+#include <eigen3/Eigen/Dense>    
+#include <iostream>    
+typedef Eigen::Matrix<double, KDIM*2, 3, Eigen::RowMajor> R3M;
+typedef Eigen::Matrix<double, KDIM*2, 1> R1M;
+typedef Eigen::Matrix<double, 3, 1> O1M;
+typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> 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));
+}

selfdrive/locationd/kalman/ekf_c.c

@@ -0,0 +1,123 @@
+#include <eigen3/Eigen/Dense>
+#include <iostream>
+
+typedef Eigen::Matrix<double, DIM, DIM, Eigen::RowMajor> DDM;
+typedef Eigen::Matrix<double, EDIM, EDIM, Eigen::RowMajor> EEM;
+typedef Eigen::Matrix<double, DIM, EDIM, Eigen::RowMajor> DEM;
+
+void predict(double *in_x, double *in_P, double *in_Q, double dt) {
+  typedef Eigen::Matrix<double, MEDIM, MEDIM, Eigen::RowMajor> RRM;
+  
+  double nx[DIM] = {0};
+  double in_F[EDIM*EDIM] = {0};
+
+  // functions from sympy
+  f_fun(in_x, dt, nx);
+  F_fun(in_x, dt, in_F);
+
+
+  EEM F(in_F);
+  EEM P(in_P);
+  EEM Q(in_Q);
+
+  RRM F_main = F.topLeftCorner(MEDIM, MEDIM);
+  P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose();
+  P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM);
+  P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose();
+
+  P = P + dt*Q;
+
+  // copy out state
+  memcpy(in_x, nx, DIM * sizeof(double));
+  memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
+}
+
+// note: extra_args dim only correct when null space projecting
+// otherwise 1
+template <int ZDIM, int EADIM, bool MAHA_TEST>
+void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) {
+  typedef Eigen::Matrix<double, ZDIM, ZDIM, Eigen::RowMajor> ZZM;
+  typedef Eigen::Matrix<double, ZDIM, DIM, Eigen::RowMajor> ZDM;
+  typedef Eigen::Matrix<double, Eigen::Dynamic, EDIM, Eigen::RowMajor> XEM;
+  //typedef Eigen::Matrix<double, EDIM, ZDIM, Eigen::RowMajor> EZM;
+  typedef Eigen::Matrix<double, Eigen::Dynamic, 1> X1M;
+  typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> XXM;
+
+  double in_hx[ZDIM] = {0};
+  double in_H[ZDIM * DIM] = {0};
+  double in_H_mod[EDIM * DIM] = {0};
+  double delta_x[EDIM] = {0};
+  double x_new[DIM] = {0};
+
+
+  // state x, P
+  Eigen::Matrix<double, ZDIM, 1> z(in_z);
+  EEM P(in_P);
+  ZZM pre_R(in_R);
+  
+  // functions from sympy
+  h_fun(in_x, in_ea, in_hx);
+  H_fun(in_x, in_ea, in_H);
+  ZDM pre_H(in_H); 
+  
+  // get y (y = z - hx)
+  Eigen::Matrix<double, ZDIM, 1> pre_y(in_hx); pre_y = z - pre_y;
+  X1M y; XXM H; XXM R;
+  if (Hea_fun){
+    typedef Eigen::Matrix<double, ZDIM, EADIM, Eigen::RowMajor> ZAM;
+    double in_Hea[ZDIM * EADIM] = {0};
+    Hea_fun(in_x, in_ea, in_Hea);
+    ZAM Hea(in_Hea);
+    XXM A = Hea.transpose().fullPivLu().kernel();
+   
+
+    y = A.transpose() * pre_y;
+    H = A.transpose() * pre_H;
+    R = A.transpose() * pre_R * A;
+  } else {
+    y = pre_y;
+    H = pre_H;
+    R = pre_R;
+  }
+  // get modified H
+  H_mod_fun(in_x, in_H_mod);
+  DEM H_mod(in_H_mod);
+  XEM H_err = H * H_mod;
+  
+  // Do mahalobis distance test
+  if (MAHA_TEST){
+    XXM a = (H_err * P * H_err.transpose() + R).inverse();
+    double maha_dist = y.transpose() * a * y;
+    if (maha_dist > MAHA_THRESHOLD){
+      R = 1.0e16 * R;
+    }
+  }
+
+  // Outlier resilient weighting
+  double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum());
+
+  // kalman gains and I_KH
+  XXM S = ((H_err * P) * H_err.transpose()) + R/weight;
+  XEM KT = S.fullPivLu().solve(H_err * P.transpose());
+  //EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE?
+  //EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose();
+  //std::cout << "Here is the matrix rot:\n" << K << std::endl;
+  EEM I_KH = Eigen::Matrix<double, EDIM, EDIM>::Identity() - (KT.transpose() * H_err);
+
+  // update state by injecting dx
+  Eigen::Matrix<double, EDIM, 1> dx(delta_x);
+  dx  = (KT.transpose() * y);
+  memcpy(delta_x, dx.data(), EDIM * sizeof(double));
+  err_fun(in_x, delta_x, x_new);
+  Eigen::Matrix<double, DIM, 1> x(x_new);
+ 
+  // update cov 
+  P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT);
+
+  // copy out state
+  memcpy(in_x, x.data(), DIM * sizeof(double));
+  memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
+  memcpy(in_z, y.data(), y.rows() * sizeof(double));
+}
+
+

selfdrive/locationd/kalman/ekf_sym.py

@@ -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]

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));
+}

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
+

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()

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)

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')

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)

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()

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)

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)

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)

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()

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()

selfdrive/locationd/locationd_yawrate.cc

@@ -0,0 +1,163 @@
+#include <iostream>
+#include <cmath>
+
+#include <capnp/message.h>
+#include <capnp/serialize-packed.h>
+#include <eigen3/Eigen/Dense>
+
+#include "cereal/gen/cpp/log.capnp.h"
+
+#include "locationd_yawrate.h"
+
+
+void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &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<cereal::SensorEventData>::Reader sensor_events, double current_time) {
+  for (cereal::SensorEventData::Reader sensor_event : sensor_events){
+    if (sensor_event.getSensor() == 5 && sensor_event.getType() == 16) {
+      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<const capnp::word> view((const capnp::word*)data, len);
+    capnp::FlatArrayMessageReader msg(view);
+    cereal::Event::Reader event = msg.getRoot<cereal::Event>();
+
+    Localizer * loc = (Localizer*) localizer;
+    loc->handle_log(event);
+  }
+
+  double localizer_get_yaw(void * localizer) {
+    Localizer * loc = (Localizer*) localizer;
+    return loc->x[0];
+  }
+  double localizer_get_bias(void * localizer) {
+    Localizer * loc = (Localizer*) localizer;
+    return loc->x[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));
+  }
+}

selfdrive/locationd/locationd_yawrate.h

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <eigen3/Eigen/Dense>
+#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<double, 1, 4> C_posenet;
+  Eigen::Matrix<double, 1, 4> C_gyro;
+
+  double R_gyro;
+
+  void update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas);
+  void handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time);
+  void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time);
+  void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time);
+
+public:
+  Eigen::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);
+
+};

selfdrive/locationd/params_learner.cc

@@ -0,0 +1,118 @@
+#include <algorithm>
+#include <cmath>
+#include <iostream>
+
+#include <capnp/message.h>
+#include <capnp/serialize-packed.h>
+#include "cereal/gen/cpp/log.capnp.h"
+#include "cereal/gen/cpp/car.capnp.h"
+#include "params_learner.h"
+
+// #define DEBUG
+
+template <typename T>
+T clip(const T& n, const T& lower, const T& upper) {
+  return std::max(lower, std::min(n, upper));
+}
+
+ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params,
+                             double angle_offset,
+                             double stiffness_factor,
+                             double steer_ratio,
+                             double learning_rate) :
+  ao(angle_offset * DEGREES_TO_RADIANS),
+  slow_ao(angle_offset * DEGREES_TO_RADIANS),
+  x(stiffness_factor),
+  sR(steer_ratio) {
+
+  cF0 = car_params.getTireStiffnessFront();
+  cR0 = car_params.getTireStiffnessRear();
+
+  l = car_params.getWheelbase();
+  m = car_params.getMass();
+
+  aF = car_params.getCenterToFront();
+  aR = l - aF;
+
+  min_sr = MIN_SR * car_params.getSteerRatio();
+  max_sr = MAX_SR * car_params.getSteerRatio();
+  min_sr_th = MIN_SR_TH * car_params.getSteerRatio();
+  max_sr_th = MAX_SR_TH * car_params.getSteerRatio();
+  alpha1 = 0.01 * learning_rate;
+  alpha2 = 0.0005 * learning_rate;
+  alpha3 = 0.1 * learning_rate;
+  alpha4 = 1.0 * learning_rate;
+}
+
+bool ParamsLearner::update(double psi, double u, double sa) {
+  if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 90.)) {
+    double ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2));
+    double new_ao = ao - alpha1 * ao_diff;
+
+    double slow_ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2));
+    double new_slow_ao = slow_ao - alpha2 * slow_ao_diff;
+
+    double new_x = x - alpha3 * (-2.0*cF0*cR0*l*m*pow(u, 3)*(slow_ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 3)));
+    double new_sR = sR - alpha4 * (-2.0*cF0*cR0*l*u*x*(slow_ao - sa)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 3)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2)));
+
+    ao = new_ao;
+    slow_ao = new_slow_ao;
+    x = new_x;
+    sR = new_sR;
+  }
+
+#ifdef DEBUG
+  std::cout << "Instant AO: " << (RADIANS_TO_DEGREES * ao) << "\tAverage AO: " << (RADIANS_TO_DEGREES * slow_ao);
+  std::cout << "\tStiffness: " << x << "\t sR: " << sR << std::endl;
+#endif
+
+  ao = clip(ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET);
+  slow_ao = clip(slow_ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET);
+  x = clip(x, MIN_STIFFNESS, MAX_STIFFNESS);
+  sR = clip(sR, min_sr, max_sr);
+
+  bool valid = fabs(slow_ao) < MAX_ANGLE_OFFSET_TH;
+  valid = valid && sR > min_sr_th;
+  valid = valid && sR < max_sr_th;
+  return valid;
+}
+
+
+extern "C" {
+  void *params_learner_init(size_t len, char * params, double angle_offset, double stiffness_factor, double steer_ratio, double learning_rate) {
+
+    auto amsg = kj::heapArray<capnp::word>((len / sizeof(capnp::word)) + 1);
+    memcpy(amsg.begin(), params, len);
+
+    capnp::FlatArrayMessageReader cmsg(amsg);
+    cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
+
+    ParamsLearner * p = new ParamsLearner(car_params, angle_offset, stiffness_factor, steer_ratio, learning_rate);
+    return (void*)p;
+  }
+
+  bool params_learner_update(void * params_learner, double psi, double u, double sa) {
+    ParamsLearner * p = (ParamsLearner*) params_learner;
+    return p->update(psi, u, sa);
+  }
+
+  double params_learner_get_ao(void * params_learner){
+    ParamsLearner * p = (ParamsLearner*) params_learner;
+    return p->ao;
+  }
+
+  double params_learner_get_x(void * params_learner){
+    ParamsLearner * p = (ParamsLearner*) params_learner;
+    return p->x;
+  }
+
+  double params_learner_get_slow_ao(void * params_learner){
+    ParamsLearner * p = (ParamsLearner*) params_learner;
+    return p->slow_ao;
+  }
+
+  double params_learner_get_sR(void * params_learner){
+    ParamsLearner * p = (ParamsLearner*) params_learner;
+    return p->sR;
+  }
+}

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);
+};

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

selfdrive/locationd/paramsd.cc

@@ -0,0 +1,186 @@
+#include <future>
+#include <iostream>
+#include <cassert>
+#include <csignal>
+#include <unistd.h>
+
+
+#include <capnp/message.h>
+#include <capnp/serialize-packed.h>
+
+#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<capnp::word>((value_sz / sizeof(capnp::word)) + 1);
+  memcpy(amsg.begin(), value, value_sz);
+  free(value);
+
+  capnp::FlatArrayMessageReader cmsg(amsg);
+  cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
+
+  // Read params from previous run
+  const int result = read_db_value(NULL, "LiveParameters", &value, &value_sz);
+
+  std::string fingerprint = car_params.getCarFingerprint();
+  std::string vin = car_params.getCarVin();
+  double sR = car_params.getSteerRatio();
+  double x = 1.0;
+  double ao = 0.0;
+  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<capnp::word>((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<cereal::Event>();
+
+      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<cereal::Event>();
+        event.setLogMonoTime(nanos_since_boot());
+        auto live_params = event.initLiveParameters();
+        live_params.setValid(valid);
+        live_params.setYawRate(localizer.x[0]);
+        live_params.setGyroBias(localizer.x[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;
+}

selfdrive/locationd/test/.gitignore

@@ -0,0 +1 @@
+out/

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)

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 <tnt@246tNt.com>
+    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
+
+

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()

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('<BBBBH', PREAMBLE1, PREAMBLE2, msg_class, msg_id, length)
+    msg._buf = header + msg._buf
+
+    for r in msg._recs:
+      f2 = []
+      for f in self.fields2:
+        f2.append(r[f])
+      msg._buf += struct.pack(self.format2, *tuple(f2))
+    msg._buf += struct.pack('<BB', *msg.checksum(data=msg._buf[2:]))
+
+  def format(self, msg):
+    '''return a formatted string for a message'''
+    if not msg._unpacked:
+      self.unpack(msg)
+    ret = self.name + ': '
+    for f in self.fields:
+      (fieldname, alen) = ArrayParse(f)
+      if not fieldname in msg._fields:
+        continue
+      v = msg._fields[fieldname]
+      if isinstance(v, list):
+        ret += '%s=[' % fieldname
+        for a in range(alen):
+          ret += '%s, ' % v[a]
+        ret = ret[:-2] + '], '
+      elif isinstance(v, str):
+        ret += '%s="%s", ' % (f, v.rstrip(' \0'))
+      else:
+        ret += '%s=%s, ' % (f, v)
+    for r in msg._recs:
+      ret += '[ '
+      for f in self.fields2:
+        v = r[f]
+        ret += '%s=%s, ' % (f, v)
+      ret = ret[:-2] + ' ], '
+    return ret[:-2]
+
+
+# list of supported message types.
+msg_types = {
+  (CLASS_ACK, MSG_ACK_ACK):
+  UBloxDescriptor('ACK_ACK', '<BB', ['clsID', 'msgID']),
+  (CLASS_ACK, MSG_ACK_NACK):
+  UBloxDescriptor('ACK_NACK', '<BB', ['clsID', 'msgID']),
+  (CLASS_CFG, MSG_CFG_USB):
+  UBloxDescriptor('CFG_USB', '<HHHHHH32s32s32s', [
+    'vendorID', 'productID', 'reserved1', 'reserved2', 'powerConsumption', 'flags',
+    'vendorString', 'productString', 'serialNumber'
+  ]),
+  (CLASS_CFG, MSG_CFG_PRT):
+  UBloxDescriptor('CFG_PRT', '<BBHIIHHHH', [
+    'portID', 'reserved0', 'txReady', 'mode', 'baudRate', 'inProtoMask', 'outProtoMask',
+    'reserved4', 'reserved5'
+  ]),
+  (CLASS_CFG, MSG_CFG_CFG):
+  UBloxDescriptor('CFG_CFG', '<III,B',
+                  ['clearMask', 'saveMask', 'loadMask', 'deviceMask']),
+  (CLASS_CFG, MSG_CFG_RXM):
+  UBloxDescriptor('CFG_RXM', '<BB',
+                  ['reserved1', 'lpMode']),
+  (CLASS_CFG, MSG_CFG_RST):
+  UBloxDescriptor('CFG_RST', '<HBB', ['navBbrMask ', 'resetMode', 'reserved1']),
+  (CLASS_CFG, MSG_CFG_SBAS):
+  UBloxDescriptor('CFG_SBAS', '<BBBBI',
+                  ['mode', 'usage', 'maxSBAS', 'scanmode2', 'scanmode1']),
+  (CLASS_CFG, MSG_CFG_GNSS):
+  UBloxDescriptor('CFG_GNSS', '<BBBB',
+                  ['msgVer', 'numTrkChHw', 'numTrkChUse',
+                   'numConfigBlocks'], 'numConfigBlocks', '<BBBBI',
+                  ['gnssId', 'resTrkCh', 'maxTrkCh', 'reserved1', 'flags']),
+  (CLASS_CFG, MSG_CFG_RATE):
+  UBloxDescriptor('CFG_RATE', '<HHH', ['measRate', 'navRate', 'timeRef']),
+  (CLASS_CFG, MSG_CFG_MSG):
+  UBloxDescriptor('CFG_MSG', '<BB6B', ['msgClass', 'msgId', 'rates[6]']),
+  (CLASS_NAV, MSG_NAV_POSLLH):
+  UBloxDescriptor('NAV_POSLLH', '<IiiiiII',
+                  ['iTOW', 'Longitude', 'Latitude', 'height', 'hMSL', 'hAcc', 'vAcc']),
+  (CLASS_NAV, MSG_NAV_VELNED):
+  UBloxDescriptor('NAV_VELNED', '<IiiiIIiII', [
+    'iTOW', 'velN', 'velE', 'velD', 'speed', 'gSpeed', 'heading', 'sAcc', 'cAcc'
+  ]),
+  (CLASS_NAV, MSG_NAV_DOP):
+  UBloxDescriptor('NAV_DOP', '<IHHHHHHH',
+                  ['iTOW', 'gDOP', 'pDOP', 'tDOP', 'vDOP', 'hDOP', 'nDOP', 'eDOP']),
+  (CLASS_NAV, MSG_NAV_STATUS):
+  UBloxDescriptor('NAV_STATUS', '<IBBBBII',
+                  ['iTOW', 'gpsFix', 'flags', 'fixStat', 'flags2', 'ttff', 'msss']),
+  (CLASS_NAV, MSG_NAV_SOL):
+  UBloxDescriptor('NAV_SOL', '<IihBBiiiIiiiIHBBI', [
+    'iTOW', 'fTOW', 'week', 'gpsFix', 'flags', 'ecefX', 'ecefY', 'ecefZ', 'pAcc',
+    'ecefVX', 'ecefVY', 'ecefVZ', 'sAcc', 'pDOP', 'reserved1', 'numSV', 'reserved2'
+  ]),
+  (CLASS_NAV, MSG_NAV_PVT):
+  UBloxDescriptor('NAV_PVT', '<IHBBBBBBIiBBBBiiiiIIiiiiiIIH6BihH', [
+    'iTOW', 'year', 'month', 'day', 'hour', 'min', 'sec', 'valid', 'tAcc', 'nano',
+    'fixType', 'flags', 'flags2', 'numSV', 'lon', 'lat', 'height', 'hMSL', 'hAcc', 'vAcc',
+    'velN', 'velE', 'velD', 'gSpeed', 'headMot', 'sAcc', 'headAcc', 'pDOP',
+    'reserverd1[6]', 'headVeh', 'magDec', 'magAcc'
+  ]),
+  (CLASS_NAV, MSG_NAV_POSUTM):
+  UBloxDescriptor('NAV_POSUTM', '<Iiiibb',
+                  ['iTOW', 'East', 'North', 'Alt', 'Zone', 'Hem']),
+  (CLASS_NAV, MSG_NAV_SBAS):
+  UBloxDescriptor('NAV_SBAS', '<IBBbBBBBB', [
+    'iTOW', 'geo', 'mode', 'sys', 'service', 'cnt', 'reserved01', 'reserved02',
+    'reserved03'
+  ], 'cnt', 'BBBBBBhHh', [
+    'svid', 'flags', 'udre', 'svSys', 'svService', 'reserved1', 'prc', 'reserved2', 'ic'
+  ]),
+  (CLASS_NAV, MSG_NAV_POSECEF):
+  UBloxDescriptor('NAV_POSECEF', '<IiiiI', ['iTOW', 'ecefX', 'ecefY', 'ecefZ', 'pAcc']),
+  (CLASS_NAV, MSG_NAV_VELECEF):
+  UBloxDescriptor('NAV_VELECEF', '<IiiiI', ['iTOW', 'ecefVX', 'ecefVY', 'ecefVZ',
+                                            'sAcc']),
+  (CLASS_NAV, MSG_NAV_TIMEGPS):
+  UBloxDescriptor('NAV_TIMEGPS', '<IihbBI',
+                  ['iTOW', 'fTOW', 'week', 'leapS', 'valid', 'tAcc']),
+  (CLASS_NAV, MSG_NAV_TIMEUTC):
+  UBloxDescriptor('NAV_TIMEUTC', '<IIiHBBBBBB', [
+    'iTOW', 'tAcc', 'nano', 'year', 'month', 'day', 'hour', 'min', 'sec', 'valid'
+  ]),
+  (CLASS_NAV, MSG_NAV_CLOCK):
+  UBloxDescriptor('NAV_CLOCK', '<IiiII', ['iTOW', 'clkB', 'clkD', 'tAcc', 'fAcc']),
+  (CLASS_NAV, MSG_NAV_DGPS):
+  UBloxDescriptor('NAV_DGPS', '<IihhBBH',
+                  ['iTOW', 'age', 'baseId', 'baseHealth', 'numCh', 'status', 'reserved1'],
+                  'numCh', '<BBHff', ['svid', 'flags', 'ageC', 'prc', 'prrc']),
+  (CLASS_NAV, MSG_NAV_SVINFO):
+  UBloxDescriptor('NAV_SVINFO', '<IBBH', ['iTOW', 'numCh', 'globalFlags',
+                                          'reserved2'], 'numCh', '<BBBBBbhi',
+                  ['chn', 'svid', 'flags', 'quality', 'cno', 'elev', 'azim', 'prRes']),
+  (CLASS_RXM, MSG_RXM_SVSI):
+  UBloxDescriptor('RXM_SVSI', '<IhBB', ['iTOW', 'week', 'numVis', 'numSV'], 'numSV',
+                  '<BBhbB', ['svid', 'svFlag', 'azim', 'elev', 'age']),
+  (CLASS_RXM, MSG_RXM_EPH):
+  UBloxDescriptor('RXM_EPH', '<II , 8I 8I 8I',
+                  ['svid', 'how', 'sf1d[8]', 'sf2d[8]', 'sf3d[8]']),
+  (CLASS_AID, MSG_AID_EPH):
+  UBloxDescriptor('AID_EPH', '<II , 8I 8I 8I',
+                  ['svid', 'how', 'sf1d[8]', 'sf2d[8]', 'sf3d[8]']),
+  (CLASS_AID, MSG_AID_HUI):
+  UBloxDescriptor('AID_HUI', '<Iddi 6h 8f I',
+                  ['health', 'utcA0', 'utcA1', 'utcTOW', 'utcWNT', 'utcLS', 'utcWNF',
+                   'utcDN', 'utcLSF', 'utcSpare', 'klobA0', 'klobA1', 'klobA2', 'klobA3',
+                   'klobB0', 'klobB1', 'klobB2', 'klobB3', 'flags']),
+  (CLASS_AID, MSG_AID_AOP):
+  UBloxDescriptor('AID_AOP', '<B47B , 48B 48B 48B',
+                  ['svid', 'data[47]', 'optional0[48]', 'optional1[48]',
+                   'optional1[48]']),
+  (CLASS_RXM, MSG_RXM_RAW):
+  UBloxDescriptor('RXM_RAW', '<dHbBB3B', [
+    'rcvTow', 'week', 'leapS', 'numMeas', 'recStat', 'reserved1[3]'
+  ], 'numMeas', '<ddfBBBBHBBBBBB', [
+    'prMes', 'cpMes', 'doMes', 'gnssId', 'svId', 'sigId', 'freqId', 'locktime', 'cno',
+    'prStdev', 'cpStdev', 'doStdev', 'trkStat', 'reserved3'
+  ]),
+  (CLASS_RXM, MSG_RXM_SFRB):
+  UBloxDescriptor('RXM_SFRB', '<BB10I', ['chn', 'svid', 'dwrd[10]']),
+  (CLASS_RXM, MSG_RXM_SFRBX):
+  UBloxDescriptor('RXM_SFRBX', '<8B', ['gnssId', 'svid', 'reserved1', 'freqId', 'numWords',
+      'reserved2', 'version', 'reserved3'], 'numWords', 'I', ['dwrd']),
+  (CLASS_AID, MSG_AID_ALM):
+  UBloxDescriptor('AID_ALM', '<II', '_remaining', 'I', ['dwrd']),
+  (CLASS_RXM, MSG_RXM_ALM):
+  UBloxDescriptor('RXM_ALM', '<II , 8I', ['svid', 'week', 'dwrd[8]']),
+  (CLASS_CFG, MSG_CFG_ODO):
+  UBloxDescriptor('CFG_ODO', '<B3BBB6BBB2BBB2B', [
+    'version', 'reserved1[3]', 'flags', 'odoCfg', 'reserverd2[6]', 'cogMaxSpeed',
+    'cogMaxPosAcc', 'reserved3[2]', 'velLpGain', 'cogLpGain', 'reserved[2]'
+  ]),
+  (CLASS_CFG, MSG_CFG_NAV5):
+  UBloxDescriptor('CFG_NAV5', '<HBBiIbBHHHHBBIII', [
+    'mask', 'dynModel', 'fixMode', 'fixedAlt', 'fixedAltVar', 'minElev', 'drLimit',
+    'pDop', 'tDop', 'pAcc', 'tAcc', 'staticHoldThresh', 'dgpsTimeOut', 'reserved2',
+    'reserved3', 'reserved4'
+  ]),
+  (CLASS_CFG, MSG_CFG_NAVX5):
+  UBloxDescriptor('CFG_NAVX5', '<HHIBBBBBBBBBBHIBBBBBBHII', [
+    'version', 'mask1', 'reserved0', 'reserved1', 'reserved2', 'minSVs', 'maxSVs',
+    'minCNO', 'reserved5', 'iniFix3D', 'reserved6', 'reserved7', 'reserved8',
+    'wknRollover', 'reserved9', 'reserved10', 'reserved11', 'usePPP', 'useAOP',
+    'reserved12', 'reserved13', 'aopOrbMaxErr', 'reserved3', 'reserved4'
+  ]),
+  (CLASS_MON, MSG_MON_HW):
+  UBloxDescriptor('MON_HW', '<IIIIHHBBBBIB25BHIII', [
+    'pinSel', 'pinBank', 'pinDir', 'pinVal', 'noisePerMS', 'agcCnt', 'aStatus', 'aPower',
+    'flags', 'reserved1', 'usedMask', 'VP[25]', 'jamInd', 'reserved3', 'pinInq', 'pullH',
+    'pullL'
+  ]),
+  (CLASS_MON, MSG_MON_HW2):
+  UBloxDescriptor('MON_HW2', '<bBbBB3BI8BI4B', [
+    'ofsI', 'magI', 'ofsQ', 'magQ', 'cfgSource', 'reserved1[3]', 'lowLevCfg',
+    'reserved2[8]', 'postStatus', 'reserved3[4]'
+  ]),
+  (CLASS_MON, MSG_MON_SCHD):
+  UBloxDescriptor('MON_SCHD', '<IIIIHHHBB', [
+    'tskRun', 'tskSchd', 'tskOvrr', 'tskReg', 'stack', 'stackSize', 'CPUIdle', 'flySly',
+    'ptlSly'
+  ]),
+  (CLASS_MON, MSG_MON_VER):
+  UBloxDescriptor('MON_VER', '<30s10s,30s', ['swVersion', 'hwVersion', 'romVersion'],
+                  '_remaining', '30s', ['extension']),
+  (CLASS_TIM, MSG_TIM_TP):
+  UBloxDescriptor('TIM_TP', '<IIiHBB',
+                  ['towMS', 'towSubMS', 'qErr', 'week', 'flags', 'reserved1']),
+  (CLASS_TIM, MSG_TIM_TM2):
+  UBloxDescriptor('TIM_TM2', '<BBHHHIIIII', [
+    'ch', 'flags', 'count', 'wnR', 'wnF', 'towMsR', 'towSubMsR', 'towMsF', 'towSubMsF',
+    'accEst'
+  ]),
+  (CLASS_TIM, MSG_TIM_SVIN):
+  UBloxDescriptor('TIM_SVIN', '<IiiiIIBBH', [
+    'dur', 'meanX', 'meanY', 'meanZ', 'meanV', 'obs', 'valid', 'active', 'reserved1'
+  ]),
+  (CLASS_INF, MSG_INF_ERROR):
+  UBloxDescriptor('INF_ERR', '<18s', ['str']),
+  (CLASS_INF, MSG_INF_DEBUG):
+  UBloxDescriptor('INF_DEBUG', '<18s', ['str'])
+}
+
+
+class UBloxMessage:
+  '''UBlox message class - holds a UBX binary message'''
+
+  def __init__(self):
+    self._buf = b""
+    self._fields = {}
+    self._recs = []
+    self._unpacked = False
+    self.debug_level = 1
+
+  def __str__(self):
+    '''format a message as a string'''
+    if not self.valid():
+      return 'UBloxMessage(INVALID)'
+    type = self.msg_type()
+    if type in msg_types:
+      return msg_types[type].format(self)
+    return 'UBloxMessage(UNKNOWN %s, %u)' % (str(type), self.msg_length())
+
+  def as_dict(self):
+    '''format a message as a string'''
+    if not self.valid():
+      return 'UBloxMessage(INVALID)'
+    type = self.msg_type()
+    if type in msg_types:
+      return msg_types[type].format(self)
+    return 'UBloxMessage(UNKNOWN %s, %u)' % (str(type), self.msg_length())
+
+  def __getattr__(self, name):
+    '''allow access to message fields'''
+    try:
+      return self._fields[name]
+    except KeyError:
+      if name == 'recs':
+        return self._recs
+      raise AttributeError(name)
+
+  def __setattr__(self, name, value):
+    '''allow access to message fields'''
+    if name.startswith('_'):
+      self.__dict__[name] = value
+    else:
+      self._fields[name] = value
+
+  def have_field(self, name):
+    '''return True if a message contains the given field'''
+    return name in self._fields
+
+  def debug(self, level, msg):
+    '''write a debug message'''
+    if self.debug_level >= 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('<H', self._buf[4:6])
+    return payload_length
+
+  def valid_so_far(self):
+    '''check if the message is valid so far'''
+    if len(self._buf) > 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('<BB', self._buf[-2:])
+    return ck_a == ck_a2 and ck_b == ck_b2
+
+  def needed_bytes(self):
+    '''return number of bytes still needed'''
+    if len(self._buf) < 6:
+      return 8 - len(self._buf)
+    return self.msg_length() + 8 - len(self._buf)
+
+  def valid(self):
+    '''check if a message is valid'''
+    return len(self._buf) >= 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('<BBBBH', 0xb5, 0x62, msg_class, msg_id, len(payload))
+    msg._buf += payload
+    (ck_a, ck_b) = msg.checksum(msg._buf[2:])
+    msg._buf += struct.pack('<BB', ck_a, ck_b)
+    self.send(msg)
+
+  def configure_solution_rate(self, rate_ms=200, nav_rate=1, timeref=0):
+    '''configure the solution rate in milliseconds'''
+    payload = struct.pack('<HHH', rate_ms, nav_rate, timeref)
+    self.send_message(CLASS_CFG, MSG_CFG_RATE, payload)
+
+  def configure_message_rate(self, msg_class, msg_id, rate):
+    '''configure the message rate for a given message'''
+    payload = struct.pack('<BBB', msg_class, msg_id, rate)
+    self.send_message(CLASS_CFG, MSG_CFG_SET_RATE, payload)
+
+  def configure_port(self, port=1, inMask=3, outMask=3, mode=2240, baudrate=None):
+    '''configure a IO port'''
+    if baudrate is None:
+      baudrate = self.baudrate
+    payload = struct.pack('<BBH8BHHBBBB', port, 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0, inMask,
+                          outMask, 0, 0, 0, 0)
+    self.send_message(CLASS_CFG, MSG_CFG_PRT, payload)
+
+  def configure_loadsave(self, clearMask=0, saveMask=0, loadMask=0, deviceMask=0):
+    '''configure configuration load/save'''
+    payload = struct.pack('<IIIB', clearMask, saveMask, loadMask, deviceMask)
+    self.send_message(CLASS_CFG, MSG_CFG_CFG, payload)
+
+  def configure_poll(self, msg_class, msg_id, payload=''):
+    '''poll a configuration message'''
+    self.send_message(msg_class, msg_id, payload)
+
+  def configure_poll_port(self, portID=None):
+    '''poll a port configuration'''
+    if portID is None:
+      self.configure_poll(CLASS_CFG, MSG_CFG_PRT)
+    else:
+      self.configure_poll(CLASS_CFG, MSG_CFG_PRT, struct.pack('<B', portID))
+
+  def configure_min_max_sats(self, min_sats=4, max_sats=32):
+    '''Set the minimum/maximum number of satellites for a solution in the NAVX5 message'''
+    payload = struct.pack('<HHIBBBBBBBBBBHIBBBBBBHII', 0, 4, 0, 0, 0, min_sats, max_sats,
+                          0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
+    self.send_message(CLASS_CFG, MSG_CFG_NAVX5, payload)
+
+  def module_reset(self, set, mode):
+    ''' Reset the module for hot/warm/cold start'''
+    payload = struct.pack('<HBB', set, mode, 0)
+    self.send_message(CLASS_CFG, MSG_CFG_RST, payload)

selfdrive/locationd/test/ubloxd.py

@@ -0,0 +1,287 @@
+#!/usr/bin/env python3
+
+import os
+import serial
+from selfdrive.locationd.test import ublox
+import time
+import datetime
+import struct
+import sys
+from cereal import log
+from common import realtime
+import cereal.messaging as messaging
+from selfdrive.locationd.test.ephemeris import EphemerisData, GET_FIELD_U
+
+panda = os.getenv("PANDA") is not None   # panda directly connected
+grey = not (os.getenv("EVAL") is not None)     # panda through boardd
+debug = os.getenv("DEBUG") is not None   # debug prints
+print_dB = os.getenv("PRINT_DB") is not None     # print antenna dB
+
+timeout = 1
+dyn_model = 4 # auto model
+baudrate = 460800
+ports = ["/dev/ttyACM0","/dev/ttyACM1"]
+rate = 100 # send new data every 100ms
+
+# which SV IDs we have seen and when we got iono
+svid_seen = {}
+svid_ephemeris = {}
+iono_seen = 0
+
+def configure_ublox(dev):
+  # configure ports  and solution parameters and rate
+  # TODO: configure constellations and channels to allow for 10Hz and high precision
+  dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1) # enable only UBX on USB
+  dev.configure_port(port=0, inMask=0, outMask=0) # disable DDC
+
+  if panda:
+    payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 1, 1, 0, 0, 0)
+    dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # enable UART
+  else:
+    payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 0, 0, 0, 0, 0)
+    dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # disable UART
+
+  dev.configure_port(port=4, inMask=0, outMask=0) # disable SPI
+  dev.configure_poll_port()
+  dev.configure_poll_port(ublox.PORT_SERIAL1)
+  dev.configure_poll_port(ublox.PORT_SERIAL2)
+  dev.configure_poll_port(ublox.PORT_USB)
+  dev.configure_solution_rate(rate_ms=rate)
+
+  # Configure solution
+  payload = struct.pack('<HBBIIBB4H6BH6B', 5, 4, 3, 0, 0,
+                                           0, 0, 0, 0, 0,
+                                           0, 0, 0, 0, 0,
+                                           0, 0, 0, 0, 0,
+                                           0, 0, 0, 0)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5, payload)
+  payload = struct.pack('<B3BBB6BBB2BBB2B', 0, 0, 0, 0, 1,
+                                            3, 0, 0, 0, 0,
+                                            0, 0, 0, 0, 0,
+                                            0, 0, 0, 0, 0)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO, payload)
+  #payload = struct.pack('<HHIBBBBBBBBBBH6BBB2BH4B3BB', 0, 8192, 0, 0, 0,
+  #                                                     0, 0, 0, 0, 0, 0,
+  #                                                     0, 0, 0, 0, 0, 0,
+  #                                                     0, 0, 0, 0, 0, 0,
+  #                                                     0, 0, 0, 0, 0, 0,
+  #                                                     0, 0, 0, 0)
+  #dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5, payload)
+
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO)
+
+  # Configure RAW and PVT messages to be sent every solution cycle
+  dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_PVT, 1)
+  dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_RAW, 1)
+  dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_SFRBX, 1)
+
+
+
+def int_to_bool_list(num):
+  # for parsing bool bytes
+  return [bool(num & (1<<n)) for n in range(8)]
+
+
+def gen_ephemeris(ephem_data):
+  ephem = {'ephemeris':
+          {'svId': ephem_data.svId,
+
+           'toc': ephem_data.toc,
+           'gpsWeek': ephem_data.gpsWeek,
+
+           'af0': ephem_data.af0,
+           'af1': ephem_data.af1,
+           'af2': ephem_data.af2,
+
+           'iode': ephem_data.iode,
+           'crs': ephem_data.crs,
+           'deltaN': ephem_data.deltaN,
+           'm0': ephem_data.M0,
+
+           'cuc': ephem_data.cuc,
+           'ecc': ephem_data.ecc,
+           'cus': ephem_data.cus,
+           'a': ephem_data.A,
+
+           'toe': ephem_data.toe,
+           'cic': ephem_data.cic,
+           'omega0': ephem_data.omega0,
+           'cis': ephem_data.cis,
+
+           'i0': ephem_data.i0,
+           'crc': ephem_data.crc,
+           'omega': ephem_data.omega,
+           'omegaDot': ephem_data.omega_dot,
+
+           'iDot': ephem_data.idot,
+
+           'tgd': ephem_data.Tgd,
+
+           'ionoCoeffsValid': ephem_data.ionoCoeffsValid,
+           'ionoAlpha': ephem_data.ionoAlpha,
+           'ionoBeta': ephem_data.ionoBeta}}
+  return log.Event.new_message(ubloxGnss=ephem)
+
+
+def gen_solution(msg):
+  msg_data = msg.unpack()[0] # Solutions do not have any data in repeated blocks
+  timestamp = int(((datetime.datetime(msg_data['year'],
+                                      msg_data['month'],
+                                      msg_data['day'],
+                                      msg_data['hour'],
+                                      msg_data['min'],
+                                      msg_data['sec'])
+                 - datetime.datetime(1970,1,1)).total_seconds())*1e+03
+                 + msg_data['nano']*1e-06)
+  gps_fix = {'bearing': msg_data['headMot']*1e-05,  # heading of motion in degrees
+             'altitude': msg_data['height']*1e-03,  # altitude above ellipsoid
+             'latitude': msg_data['lat']*1e-07,  # latitude in degrees
+             'longitude': msg_data['lon']*1e-07,  # longitude in degrees
+             'speed': msg_data['gSpeed']*1e-03,  # ground speed in meters
+             'accuracy': msg_data['hAcc']*1e-03,  # horizontal accuracy (1 sigma?)
+             'timestamp': timestamp,  # UTC time in ms since start of UTC stime
+             'vNED': [msg_data['velN']*1e-03,
+                     msg_data['velE']*1e-03,
+                     msg_data['velD']*1e-03],  # velocity in NED frame in m/s
+             'speedAccuracy': msg_data['sAcc']*1e-03,  # speed accuracy in m/s
+             'verticalAccuracy': msg_data['vAcc']*1e-03,  # vertical accuracy in meters
+             'bearingAccuracy': msg_data['headAcc']*1e-05,  # heading accuracy in degrees
+             'source': 'ublox',
+             'flags': msg_data['flags'],
+  }
+  return log.Event.new_message(gpsLocationExternal=gps_fix)
+
+def gen_nav_data(msg, nav_frame_buffer):
+  # TODO this stuff needs to be parsed and published.
+  # refer to https://www.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_%28UBX-13003221%29.pdf
+  # section 9.1
+  msg_meta_data, measurements = msg.unpack()
+
+  # parse GPS ephem
+  gnssId = msg_meta_data['gnssId']
+  if gnssId  == 0:
+    svId =  msg_meta_data['svid']
+    subframeId =  GET_FIELD_U(measurements[1]['dwrd'], 3, 8)
+    words = []
+    for m in measurements:
+      words.append(m['dwrd'])
+
+    # parse from
+    if subframeId == 1:
+      nav_frame_buffer[gnssId][svId] = {}
+      nav_frame_buffer[gnssId][svId][subframeId] = words
+    elif subframeId-1 in nav_frame_buffer[gnssId][svId]:
+      nav_frame_buffer[gnssId][svId][subframeId] = words
+    if len(nav_frame_buffer[gnssId][svId]) == 5:
+      ephem_data = EphemerisData(svId, nav_frame_buffer[gnssId][svId])
+      return gen_ephemeris(ephem_data)
+
+
+
+
+def gen_raw(msg):
+  # meta data is in first part of tuple
+  # list of measurements is in second part
+  msg_meta_data, measurements = msg.unpack()
+  measurements_parsed = []
+  for m in measurements:
+    trackingStatus_bools = int_to_bool_list(m['trkStat'])
+    trackingStatus = {'pseudorangeValid': trackingStatus_bools[0],
+                      'carrierPhaseValid': trackingStatus_bools[1],
+                      'halfCycleValid': trackingStatus_bools[2],
+                      'halfCycleSubtracted': trackingStatus_bools[3]}
+    measurements_parsed.append({
+        'svId': m['svId'],
+        'sigId': m['sigId'],
+        'pseudorange': m['prMes'],
+        'carrierCycles': m['cpMes'],
+        'doppler': m['doMes'],
+        'gnssId': m['gnssId'],
+        'glonassFrequencyIndex': m['freqId'],
+        'locktime': m['locktime'],
+        'cno': m['cno'],
+        'pseudorangeStdev': 0.01*(2**(m['prStdev'] & 15)), # weird scaling, might be wrong
+        'carrierPhaseStdev': 0.004*(m['cpStdev'] & 15),
+        'dopplerStdev': 0.002*(2**(m['doStdev'] & 15)), # weird scaling, might be wrong
+        'trackingStatus': trackingStatus})
+  if print_dB:
+    cnos = {}
+    for meas in measurements_parsed:
+      cnos[meas['svId']] = meas['cno']
+    print('Carrier to noise ratio for each sat: \n', cnos, '\n')
+  receiverStatus_bools = int_to_bool_list(msg_meta_data['recStat'])
+  receiverStatus = {'leapSecValid': receiverStatus_bools[0],
+                    'clkReset': receiverStatus_bools[2]}
+  raw_meas = {'measurementReport': {'rcvTow': msg_meta_data['rcvTow'],
+                'gpsWeek': msg_meta_data['week'],
+                'leapSeconds': msg_meta_data['leapS'],
+                'receiverStatus': receiverStatus,
+                'numMeas': msg_meta_data['numMeas'],
+                'measurements': measurements_parsed}}
+  return log.Event.new_message(ubloxGnss=raw_meas)
+
+def init_reader():
+  port_counter = 0
+  while True:
+    try:
+      dev = ublox.UBlox(ports[port_counter], baudrate=baudrate, timeout=timeout, panda=panda, grey=grey)
+      configure_ublox(dev)
+      return dev
+    except serial.serialutil.SerialException as e:
+      print(e)
+      port_counter = (port_counter + 1)%len(ports)
+      time.sleep(2)
+
+def handle_msg(dev, msg, nav_frame_buffer):
+  try:
+    if debug:
+      print(str(msg))
+      sys.stdout.flush()
+    if msg.name() == 'NAV_PVT':
+      sol = gen_solution(msg)
+      sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+      gpsLocationExternal.send(sol.to_bytes())
+    elif msg.name() == 'RXM_RAW':
+      raw = gen_raw(msg)
+      raw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+      ubloxGnss.send(raw.to_bytes())
+    elif msg.name() == 'RXM_SFRBX':
+      nav = gen_nav_data(msg, nav_frame_buffer)
+      if nav is not None:
+        nav.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+        ubloxGnss.send(nav.to_bytes())
+
+    else:
+      print("UNKNNOWN MESSAGE:", msg.name())
+  except ublox.UBloxError as e:
+    print(e)
+
+  #if dev is not None and dev.dev is not None:
+  #  dev.close()
+
+def main(gctx=None):
+  global gpsLocationExternal, ubloxGnss
+  nav_frame_buffer = {}
+  nav_frame_buffer[0] = {}
+  for i in range(1,33):
+    nav_frame_buffer[0][i] = {}
+
+
+  gpsLocationExternal = messaging.pub_sock('gpsLocationExternal')
+  ubloxGnss = messaging.pub_sock('ubloxGnss')
+
+  dev = init_reader()
+  while True:
+    try:
+      msg = dev.receive_message()
+    except serial.serialutil.SerialException as e:
+      print(e)
+      dev.close()
+      dev = init_reader()
+    if msg is not None:
+      handle_msg(dev, msg, nav_frame_buffer)
+
+if __name__ == "__main__":
+  main()

selfdrive/locationd/test/ubloxd_easy.py

@@ -0,0 +1,53 @@
+#!/usr/bin/env python3
+
+import os
+from selfdrive.locationd.test import ublox
+from common import realtime
+from selfdrive.locationd.test.ubloxd import gen_raw, gen_solution
+import zmq
+import cereal.messaging as messaging
+
+
+unlogger = os.getenv("UNLOGGER") is not None   # debug prints
+
+def main(gctx=None):
+  poller = zmq.Poller()
+
+  gpsLocationExternal = messaging.pub_sock('gpsLocationExternal')
+  ubloxGnss = messaging.pub_sock('ubloxGnss')
+
+  # ubloxRaw = messaging.sub_sock('ubloxRaw', poller)
+
+  # 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:
+        buff = log.ubloxRaw
+        time = log.logMonoTime
+        msg = ublox.UBloxMessage()
+        msg.add(buff)
+        if msg.valid():
+          if msg.name() == 'NAV_PVT':
+            sol = gen_solution(msg)
+            if unlogger:
+              sol.logMonoTime = time
+            else:
+              sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+            gpsLocationExternal.send(sol.to_bytes())
+          elif msg.name() == 'RXM_RAW':
+            raw = gen_raw(msg)
+            if unlogger:
+              raw.logMonoTime = time
+            else:
+              raw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+            ubloxGnss.send(raw.to_bytes())
+        else:
+          print("INVALID MESSAGE")
+
+
+if __name__ == "__main__":
+  main()

selfdrive/locationd/test/ubloxd_py_test.py

@@ -0,0 +1,77 @@
+import sys
+import os
+
+from selfdrive.locationd.test.ublox import UBloxMessage
+from selfdrive.locationd.test.ubloxd import gen_solution, gen_raw, gen_nav_data
+from common import realtime
+
+
+def mkdirs_exists_ok(path):
+  try:
+    os.makedirs(path)
+  except OSError:
+    if not os.path.isdir(path):
+      raise
+
+
+def parser_test(fn, prefix):
+  nav_frame_buffer = {}
+  nav_frame_buffer[0] = {}
+  for i in range(1, 33):
+    nav_frame_buffer[0][i] = {}
+
+  if not os.path.exists(prefix):
+    print('Prefix invalid')
+    sys.exit(-1)
+
+  with open(fn, 'rb') as f:
+    i = 0
+    saved_i = 0
+    msg = UBloxMessage()
+    while True:
+      n = msg.needed_bytes()
+      b = f.read(n)
+      if not b:
+        break
+      msg.add(b)
+      if msg.valid():
+        i += 1
+        if msg.name() == 'NAV_PVT':
+          sol = gen_solution(msg)
+          sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+          with open(os.path.join(prefix, str(saved_i)), 'wb') as f1:
+            f1.write(sol.to_bytes())
+            saved_i += 1
+        elif msg.name() == 'RXM_RAW':
+          raw = gen_raw(msg)
+          raw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+          with open(os.path.join(prefix, str(saved_i)), 'wb') as f1:
+            f1.write(raw.to_bytes())
+            saved_i += 1
+        elif msg.name() == 'RXM_SFRBX':
+          nav = gen_nav_data(msg, nav_frame_buffer)
+          if nav is not None:
+            nav.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+            with open(os.path.join(prefix, str(saved_i)), 'wb') as f1:
+              f1.write(nav.to_bytes())
+              saved_i += 1
+
+        msg = UBloxMessage()
+        msg.debug_level = 0
+    print('Parsed {} msgs'.format(i))
+    print('Generated {} cereal events'.format(saved_i))
+
+
+if __name__ == "__main__":
+  if len(sys.argv) < 3:
+    print('Format: ubloxd_py_test.py file_path prefix')
+    sys.exit(0)
+
+  fn = sys.argv[1]
+  if not os.path.isfile(fn):
+    print('File path invalid')
+    sys.exit(0)
+
+  prefix = sys.argv[2]
+  mkdirs_exists_ok(prefix)
+  parser_test(fn, prefix)

selfdrive/locationd/test/ubloxd_regression_test.py

@@ -0,0 +1,96 @@
+#!/usr/bin/env python3
+import os
+import sys
+import argparse
+
+from cereal import log
+from common.basedir import BASEDIR
+os.environ['BASEDIR'] = BASEDIR
+
+
+def get_arg_parser():
+  parser = argparse.ArgumentParser(
+      description="Compare two result files",
+      formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+
+  parser.add_argument("dir1", nargs='?', default='/data/ubloxdc',
+                      help="Directory path 1 from which events are loaded")
+
+  parser.add_argument("dir2", nargs='?', default='/data/ubloxdpy',
+                      help="Directory path 2 from which msgs are loaded")
+
+  return parser
+
+
+def read_file(fn):
+  with open(fn, 'rb') as f:
+    return f.read()
+
+
+def compare_results(dir1, dir2):
+  onlyfiles1 = [f for f in os.listdir(dir1) if os.path.isfile(os.path.join(dir1, f))]
+  onlyfiles1.sort()
+
+  onlyfiles2 = [f for f in os.listdir(dir2) if os.path.isfile(os.path.join(dir2, f))]
+  onlyfiles2.sort()
+
+  if len(onlyfiles1) != len(onlyfiles2):
+    print('len mismatch: {} != {}'.format(len(onlyfiles1), len(onlyfiles2)))
+    return -1
+  events1 = [log.Event.from_bytes(read_file(os.path.join(dir1, f))) for f in onlyfiles1]
+  events2 = [log.Event.from_bytes(read_file(os.path.join(dir2, f))) for f in onlyfiles2]
+
+  for i in range(len(events1)):
+    if events1[i].which() != events2[i].which():
+      print('event {} type mismatch: {} != {}'.format(i, events1[i].which(), events2[i].which()))
+      return -2
+    if events1[i].which() == 'gpsLocationExternal':
+      old_gps = events1[i].gpsLocationExternal
+      gps = events2[i].gpsLocationExternal
+      # print(gps, old_gps)
+      attrs = ['flags', 'latitude', 'longitude', 'altitude', 'speed', 'bearing',
+               'accuracy', 'timestamp', 'source', 'vNED', 'verticalAccuracy', 'bearingAccuracy', 'speedAccuracy']
+      for attr in attrs:
+        o = getattr(old_gps, attr)
+        n = getattr(gps, attr)
+        if attr == 'vNED':
+          if len(o) != len(n):
+            print('Gps vNED len mismatch', o, n)
+            return -3
+          else:
+            for i in range(len(o)):
+              if abs(o[i] - n[i]) > 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)

selfdrive/locationd/ublox_msg.cc

@@ -0,0 +1,374 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <sched.h>
+#include <sys/time.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <assert.h>
+#include <math.h>
+#include <ctime>
+#include <chrono>
+#include <map>
+#include <vector>
+#include <algorithm>
+
+#include <capnp/serialize.h>
+#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<uint8_t, subframes_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<capnp::word> 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<cereal::Event>();
+  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<const float> 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<capnp::word> 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<capnp::word>();
+  }
+  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<cereal::Event>();
+  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<capnp::word> 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<capnp::word>();
+  }
+  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<uint32_t> 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<cereal::Event>();
+      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<const double> apa(&ephem_data.ionoAlpha[0], sizeof(ephem_data.ionoAlpha) / sizeof(ephem_data.ionoAlpha[0]));
+        eph.setIonoAlpha(apa);
+        kj::ArrayPtr<const double> apb(&ephem_data.ionoBeta[0], sizeof(ephem_data.ionoBeta) / sizeof(ephem_data.ionoBeta[0]));
+        eph.setIonoBeta(apb);
+      } else {
+        eph.setIonoAlpha(kj::ArrayPtr<const double>());
+        eph.setIonoBeta(kj::ArrayPtr<const double>());
+      }
+      return capnp::messageToFlatArray(msg_builder);
+    }
+  }
+  return kj::Array<capnp::word>();
+}
+
+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();
+}
+
+}

selfdrive/locationd/ublox_msg.h

@@ -0,0 +1,150 @@
+#pragma once
+
+#include <stdint.h>
+#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<uint8_t, std::vector<uint32_t>> subframes_map;
+
+  class UbloxMsgParser {
+    public:
+
+      UbloxMsgParser();
+      kj::Array<capnp::word> gen_solution();
+      kj::Array<capnp::word> gen_raw();
+
+      kj::Array<capnp::word> 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<uint8_t, std::map<uint8_t, subframes_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);

selfdrive/locationd/ubloxd.cc

@@ -0,0 +1,47 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <sched.h>
+#include <sys/time.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <assert.h>
+#include <math.h>
+#include <ctime>
+#include <chrono>
+#include <map>
+#include <vector>
+
+#include "messaging.hpp"
+#include <capnp/serialize.h>
+#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);
+}

selfdrive/locationd/ubloxd_main.cc

@@ -0,0 +1,115 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <sched.h>
+#include <sys/time.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <assert.h>
+#include <math.h>
+#include <ctime>
+#include <chrono>
+#include <map>
+#include <vector>
+
+#include "messaging.hpp"
+#include <capnp/serialize.h>
+#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<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
+    memcpy(amsg.begin(), msg->getData(), msg->getSize());
+
+    capnp::FlatArrayMessageReader cmsg(amsg);
+    cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+
+    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;
+}

selfdrive/locationd/ubloxd_test.cc

@@ -0,0 +1,102 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <sched.h>
+#include <sys/time.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <assert.h>
+#include <math.h>
+#include <ctime>
+#include <chrono>
+#include <map>
+#include <vector>
+#include <iostream>
+
+#include "messaging.hpp"
+#include "impl_zmq.hpp"
+#include <capnp/serialize.h>
+#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<cereal::Event>();
+    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;
+}