IQ.Pilot Prebuilt Release @ 27f668a

artifacts/package_runtime/iqdbc/car/common/basedir.py

@@ -0,0 +1,4 @@
+import os
+
+
+BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../"))

artifacts/package_runtime/iqdbc/car/common/conversions.py

@@ -0,0 +1,20 @@
+import numpy as np
+
+
+class Conversions:
+  # Speed
+  MPH_TO_KPH = 1.609344
+  KPH_TO_MPH = 1. / MPH_TO_KPH
+  MS_TO_KPH = 3.6
+  KPH_TO_MS = 1. / MS_TO_KPH
+  MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
+  MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
+  MS_TO_KNOTS = 1.9438
+  KNOTS_TO_MS = 1. / MS_TO_KNOTS
+
+  # Angle
+  DEG_TO_RAD = np.pi / 180.
+  RAD_TO_DEG = 1. / DEG_TO_RAD
+
+  # Mass
+  LB_TO_KG = 0.453592

artifacts/package_runtime/iqdbc/car/common/filter_simple.py

@@ -0,0 +1,44 @@
+class FirstOrderFilter:
+  # first order filter
+  def __init__(self, x0, rc, dt, initialized=True):
+    self.x = x0
+    self._dt = dt
+    self.update_alpha(rc)
+    self.initialized = initialized
+
+  def update_dt(self, dt):
+    self._dt = dt
+    self.update_alpha(self._rc)
+
+  def update_alpha(self, rc):
+    self._rc = rc
+    self._alpha = self._dt / (self._rc + self._dt)
+
+  def update(self, x):
+    if self.initialized:
+      self.x = (1. - self._alpha) * self.x + self._alpha * x
+    else:
+      self.initialized = True
+      self.x = x
+    return self.x
+
+
+class HighPassFilter:
+  # technically a band-pass filter
+  def __init__(self, x0, rc1, rc2, dt, initialized=True):
+    self.x = x0
+    self._f1 = FirstOrderFilter(x0, rc1, dt, initialized)
+    self._f2 = FirstOrderFilter(x0, rc2, dt, initialized)
+    assert rc2 > rc1, "rc2 must be greater than rc1"
+
+  def update_dt(self, dt):
+    self._f1.update_dt(dt)
+    self._f2.update_dt(dt)
+
+  def update_alpha(self, rc1, rc2):
+    self._f1.update_alpha(rc1)
+    self._f2.update_alpha(rc2)
+
+  def update(self, x):
+    self.x = self._f1.update(x) - self._f2.update(x)
+    return self.x

artifacts/package_runtime/iqdbc/car/common/numpy_fast.py

@@ -0,0 +1,20 @@
+def clip(x, lo, hi):
+  return max(lo, min(hi, x))
+
+def interp(x, xp, fp):
+  N = len(xp)
+
+  def get_interp(xv):
+    hi = 0
+    while hi < N and xv > xp[hi]:
+      hi += 1
+    low = hi - 1
+    return fp[-1] if hi == N and xv > xp[low] else (
+      fp[0] if hi == 0 else
+      (xv - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low])
+
+  return [get_interp(v) for v in x] if hasattr(x, '__iter__') else get_interp(x)
+
+def mean(x):
+  return sum(x) / len(x)
+

artifacts/package_runtime/iqdbc/car/common/pid.py

@@ -0,0 +1,71 @@
+import numpy as np
+from numbers import Number
+
+
+class PIDController:
+  def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
+    self._k_p = k_p
+    self._k_i = k_i
+    self._k_d = k_d
+    self.k_f = k_f   # feedforward gain
+    if isinstance(self._k_p, Number):
+      self._k_p = [[0], [self._k_p]]
+    if isinstance(self._k_i, Number):
+      self._k_i = [[0], [self._k_i]]
+    if isinstance(self._k_d, Number):
+      self._k_d = [[0], [self._k_d]]
+
+    self.pos_limit = pos_limit
+    self.neg_limit = neg_limit
+
+    self.i_unwind_rate = 0.3 / rate
+    self.i_rate = 1.0 / rate
+    self.speed = 0.0
+
+    self.reset()
+
+  @property
+  def k_p(self):
+    return np.interp(self.speed, self._k_p[0], self._k_p[1])
+
+  @property
+  def k_i(self):
+    return np.interp(self.speed, self._k_i[0], self._k_i[1])
+
+  @property
+  def k_d(self):
+    return np.interp(self.speed, self._k_d[0], self._k_d[1])
+
+  @property
+  def error_integral(self):
+    return self.i/self.k_i
+
+  def reset(self):
+    self.p = 0.0
+    self.i = 0.0
+    self.d = 0.0
+    self.f = 0.0
+    self.control = 0
+
+  def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False):
+    self.speed = speed
+
+    self.p = float(error) * self.k_p
+    self.f = feedforward * self.k_f
+    self.d = error_rate * self.k_d
+
+    if override:
+      self.i -= self.i_unwind_rate * float(np.sign(self.i))
+    else:
+      if not freeze_integrator:
+        self.i = self.i + error * self.k_i * self.i_rate
+
+        # Clip i to prevent exceeding control limits
+        control_no_i = self.p + self.d + self.f
+        control_no_i = np.clip(control_no_i, self.neg_limit, self.pos_limit)
+        self.i = np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
+
+    control = self.p + self.i + self.d + self.f
+
+    self.control = np.clip(control, self.neg_limit, self.pos_limit)
+    return self.control

artifacts/package_runtime/iqdbc/car/common/simple_kalman.py

@@ -0,0 +1,54 @@
+import numpy as np
+
+
+def get_kalman_gain(dt, A, C, Q, R, iterations=100):
+  P = np.zeros_like(Q)
+  for _ in range(iterations):
+    P = A.dot(P).dot(A.T) + dt * Q
+    S = C.dot(P).dot(C.T) + R
+    K = P.dot(C.T).dot(np.linalg.inv(S))
+    P = (np.eye(len(P)) - K.dot(C)).dot(P)
+  return K
+
+
+class KF1D:
+  # this EKF assumes constant covariance matrix, so calculations are much simpler
+  # the Kalman gain also needs to be precomputed using the control module
+
+  def __init__(self, x0, A, C, K):
+    self.x0_0 = x0[0][0]
+    self.x1_0 = x0[1][0]
+    self.A0_0 = A[0][0]
+    self.A0_1 = A[0][1]
+    self.A1_0 = A[1][0]
+    self.A1_1 = A[1][1]
+    self.C0_0 = C[0]
+    self.C0_1 = C[1]
+    self.K0_0 = K[0][0]
+    self.K1_0 = K[1][0]
+
+    self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0
+    self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1
+    self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0
+    self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1
+
+    # K matrix needs to  be pre-computed as follow:
+    # import control
+    # (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R)
+    # self.K = np.transpose(K)
+
+  def update(self, meas):
+    #self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas)
+    x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas
+    x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas
+    self.x0_0 = x0_0
+    self.x1_0 = x1_0
+    return [self.x0_0, self.x1_0]
+
+  @property
+  def x(self):
+    return [[self.x0_0], [self.x1_0]]
+
+  def set_x(self, x):
+    self.x0_0 = x[0][0]
+    self.x1_0 = x[1][0]