Radard: pdf not cdf (#26978)

Update radard.py

according the the equation, it is the probability density function (pdf) , not the cumulative distribution function (cdf)
old-commit-hash: f25239f7c5
beeps
apache2046 2 years ago committed by GitHub
parent 2c22914bf8
commit b9f9f948bc
  1. 8
      selfdrive/controls/radard.py

@ -35,7 +35,7 @@ class KalmanParams():
self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]] self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]]
def laplacian_cdf(x, mu, b): def laplacian_pdf(x, mu, b):
b = max(b, 1e-4) b = max(b, 1e-4)
return math.exp(-abs(x-mu)/b) return math.exp(-abs(x-mu)/b)
@ -45,9 +45,9 @@ def match_vision_to_cluster(v_ego, lead, clusters):
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
def prob(c): def prob(c):
prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xStd[0]) prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0])
prob_y = laplacian_cdf(c.yRel, -lead.y[0], lead.yStd[0]) prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0])
prob_v = laplacian_cdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0])
# This is isn't exactly right, but good heuristic # This is isn't exactly right, but good heuristic
return prob_d * prob_y * prob_v return prob_d * prob_y * prob_v

Loading…
Cancel
Save