mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 18:53:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

Better poly (#1437)

Browse Source 
- better polyfitting
- no mpc cost change during lane change
- model trained with better ll gt, sim noise and guaranteed memories
This commit is contained in:
HaraldSchafer
2020-05-08 17:14:43 -07:00
committed by GitHub
parent 930738f15f
commit 65fcc7c45f
4 changed files with 16 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
models/supercombo.dlc
View File

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

4
models/supercombo.keras
View File

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

8
selfdrive/controls/lib/pathplanner.py
View File

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

24
selfdrive/modeld/models/driving.cc
View File

@@ -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));