mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-28 01:52:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Radard: pdf not cdf (#26978)

Browse Source 
Update radard.py

according the the equation, it is the probability density function (pdf) , not the cumulative distribution function (cdf)
old-commit-hash: f25239f7c516b19782926423754db7c31a29bd75
This commit is contained in:
apache2046
2023-01-17 02:22:09 +08:00
committed by GitHub
parent 2c22914bf8
commit b9f9f948bc
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/radard.py
+4 -4
View File
@@ -35,7 +35,7 @@ class KalmanParams():
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)
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
def prob(c):
prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xStd[0])
prob_y = laplacian_cdf(c.yRel, -lead.y[0], lead.yStd[0])
prob_v = laplacian_cdf(c.vRel + v_ego, lead.v[0], lead.vStd[0])
prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0])
prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0])
prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0])
# This is isn't exactly right, but good heuristic
return prob_d * prob_y * prob_v