Better poly (#1437)

- better polyfitting
- no mpc cost change during lane change
- model trained with better ll gt, sim noise and guaranteed memories

models/supercombo.dlc

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:cad8bd4bf2f9a15126baf58812515822015fb88e8fed69c0f6270d5c92b9ddf8
-size 78605237
+oid sha256:40fbf72761dc0170a5473f4e95a10519bda43f069717c4dd3ee5f796596fc5ce
+size 52636596

models/supercombo.keras

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:07df32d582b9d3ae687c7af28717cee3fe036cccf6365eb7d174a4e86c521d28
-size 79200720
+oid sha256:9e810ee9b30e5153b3da0bf15a4f096677788a01ab4170e8d0cd3eca2c247973
+size 53232072

selfdrive/controls/lib/pathplanner.py

@@ -126,8 +126,8 @@ class PathPlanner():
 
       # starting
       elif self.lane_change_state == LaneChangeState.laneChangeStarting:
-        # fade out lanelines over .5s
-        self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0)
+        # fade out over .2s
+        self.lane_change_ll_prob = max(self.lane_change_ll_prob - DT_MDL/5, 0.0)
         # 98% certainty
         if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
           self.lane_change_state = LaneChangeState.laneChangeFinishing
@@ -154,10 +154,6 @@ class PathPlanner():
     if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
       self.LP.l_prob *= self.lane_change_ll_prob
       self.LP.r_prob *= self.lane_change_ll_prob
-      self.libmpc.init_weights(MPC_COST_LAT.PATH / 3.0, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)
-    else:
-      self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)
-
     self.LP.update_d_poly(v_ego)
 
     # account for actuation delay

selfdrive/modeld/models/driving.cc

@@ -19,7 +19,7 @@
 #define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE
 #define OUTPUT_SIZE  POSE_IDX + POSE_SIZE
 #ifdef TEMPORAL
-  #define TEMPORAL_SIZE 1024
+  #define TEMPORAL_SIZE 512
 #else
   #define TEMPORAL_SIZE 0
 #endif
@@ -135,18 +135,18 @@ void model_free(ModelState* s) {
   delete s->m;
 }
 
-void poly_fit(float *in_pts, float *in_stds, float *out) {
+void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) {
   // References to inputs
-  Eigen::Map<Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> > pts(in_pts, MODEL_PATH_DISTANCE);
-  Eigen::Map<Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> > std(in_stds, MODEL_PATH_DISTANCE);
+  Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1> > pts(in_pts, valid_len);
+  Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1> > std(in_stds, valid_len);
   Eigen::Map<Eigen::Matrix<float, POLYFIT_DEGREE - 1, 1> > p(out, POLYFIT_DEGREE - 1);
 
   float y0 = pts[0];
   pts = pts.array() - y0;
 
   // Build Least Squares equations
-  Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> lhs = vander.array().colwise() / std.array();
-  Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> rhs = pts.array() / std.array();
+  Eigen::Matrix<float, Eigen::Dynamic, POLYFIT_DEGREE - 1> lhs = vander.topRows(valid_len).array().colwise() / std.array();
+  Eigen::Matrix<float, Eigen::Dynamic, 1> rhs = pts.array() / std.array();
 
   // Improve numerical stability
   Eigen::Matrix<float, POLYFIT_DEGREE - 1, 1> scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum();
@@ -170,15 +170,11 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo
   float prob;
   float valid_len;
 
-  valid_len =  data[MODEL_PATH_DISTANCE*2];
+  // clamp to 5 and 192
+  valid_len =  fmin(192, fmax(5, data[MODEL_PATH_DISTANCE*2]));
   for (int i=0; i<MODEL_PATH_DISTANCE; i++) {
     points_arr[i] = data[i] + offset;
-    // Always do at least 5 points
-    if (i < 5 || i < valid_len) {
-      stds_arr[i] = softplus(data[MODEL_PATH_DISTANCE + i]);
-    } else {
-      stds_arr[i] = 1.0e3; 
-    }
+    stds_arr[i] = softplus(data[MODEL_PATH_DISTANCE + i]);
   }
   if (has_prob) {
     prob =  sigmoid(data[MODEL_PATH_DISTANCE*2 + 1]);
@@ -186,7 +182,7 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo
     prob = 1.0;
   }
   std = softplus(data[MODEL_PATH_DISTANCE]);
-  poly_fit(points_arr, stds_arr, poly_arr);
+  poly_fit(points_arr, stds_arr, poly_arr, valid_len);
 
   if (std::getenv("DEBUG")){
     kj::ArrayPtr<const float> stds(&stds_arr[0], ARRAYSIZE(stds_arr));