openpilot v0.3.9 tweaks

old-commit-hash: 1ad9cc8c67

common/kalman/ekf.py

@@ -19,6 +19,7 @@ import numpy.matlib
 # update() should be called once per sensor, and can be called multiple times between predict steps.
 # Access and set the state of the filter directly with ekf.state and ekf.covar.
 
+
 class SensorReading:
   # Given a perfect model and no noise, data = obs_model * state
   def __init__(self, data, covar, obs_model):
@@ -33,7 +34,7 @@ class SensorReading:
 
 # A generic sensor class that does no pre-processing of data
 class SimpleSensor:
-  # obs_model can be 
+  # obs_model can be
   #   a full obesrvation model matrix, or
   #   an integer or tuple of indices into ekf.state, indicating which variables are being directly observed
   # covar can be
@@ -131,11 +132,11 @@ class EKF:
     # like update but knowing that measurment is a scalar
     # this avoids matrix inversions and speeds up (surprisingly) drived.py a lot
 
-    # innovation = reading.data - np.matmul(reading.obs_model, self.state)   
-    # innovation_covar = np.matmul(np.matmul(reading.obs_model, self.covar), reading.obs_model.T) + reading.covar    
-    # kalman_gain = np.matmul(self.covar, reading.obs_model.T)/innovation_covar   
-    # self.state += np.matmul(kalman_gain, innovation)   
-    # aux_mtrx = self.identity - np.matmul(kalman_gain, reading.obs_model)    
+    # innovation = reading.data - np.matmul(reading.obs_model, self.state)
+    # innovation_covar = np.matmul(np.matmul(reading.obs_model, self.covar), reading.obs_model.T) + reading.covar
+    # kalman_gain = np.matmul(self.covar, reading.obs_model.T)/innovation_covar
+    # self.state += np.matmul(kalman_gain, innovation)
+    # aux_mtrx = self.identity - np.matmul(kalman_gain, reading.obs_model)
     # self.covar =  np.matmul(aux_mtrx, np.matmul(self.covar, aux_mtrx.T)) + np.matmul(kalman_gain, np.matmul(reading.covar, kalman_gain.T))
 
     # written without np.matmul
@@ -174,7 +175,7 @@ class EKF:
 
     #! Clip covariance to avoid explosions
     self.covar = np.clip(self.covar,-1e10,1e10)
-    
+
   @abc.abstractmethod
   def calc_transfer_fun(self, dt):
     """Return a tuple with the transfer function and transfer function jacobian
@@ -190,6 +191,7 @@ class EKF:
       and using it during calcualtion of A and J
     """
 
+
 class FastEKF1D(EKF):
   """Fast version of EKF for 1D problems with scalar readings."""
 

common/kalman/simple_kalman.py

@@ -0,0 +1,23 @@
+import numpy as np
+
+
+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.x = x0
+    self.A = A
+    self.C = C
+    self.K = K
+
+    self.A_K = self.A - np.dot(self.K, self.C)
+
+    # 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)
+    return self.x

common/params.py

@@ -57,6 +57,7 @@ keys = {
   "IsMetric": TxType.PERSISTANT,
   "IsRearViewMirror": TxType.PERSISTANT,
   "IsFcwEnabled": TxType.PERSISTANT,
+  "HasAcceptedTerms": TxType.PERSISTANT,
 # written: visiond
 # read:    visiond, controlsd
   "CalibrationParams": TxType.PERSISTANT,