From b9f9f948bc3f1bbfea6ea0fb7be35494305bafe8 Mon Sep 17 00:00:00 2001 From: apache2046 Date: Tue, 17 Jan 2023 02:22:09 +0800 Subject: [PATCH] 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: f25239f7c516b19782926423754db7c31a29bd75 --- selfdrive/controls/radard.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index db87604e98..6a0902e510 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -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