SirStone/SirRoboGarage
1
0
Fork 0
Code Issues 28 Pull Requests Actions Projects Releases Wiki Activity

fix(reservoir): proper bilinear interpolation in forward(), revert neighbor spread in learn()

Browse Source 
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Davide Cappellini
2026-09-17 08:16:20 +02:00
parent 10a0696176
commit 34f952ce20
1 changed files with 25 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
SNNBot_garage/src/reservoir.nim
+25 -42
View File
@@ -19,54 +19,37 @@ type
proc initLeadGrid*(): LeadGrid =
result = LeadGrid()
proc cellIndex(vPerp: float, distance: float): int {.inline.} =
let vBin = clamp(int(vPerp + 8.5), 0, VPERP_BINS - 1)
let dBin = clamp(int(distance / DIST_BAND), 0, DIST_BINS - 1)
result = vBin * DIST_BINS + dBin
proc forward*(grid: var LeadGrid, vPerp, distance: float): float =
## Returns circular mean offset in degrees, or -999.0 when no data.
let vBin = clamp(int(vPerp + 8.5), 0, VPERP_BINS - 1)
let dBin = clamp(int(distance / DIST_BAND), 0, DIST_BINS - 1)
# Accumulate weighted contributions: direct cell (w=1), cardinal neighbors (w=0.5), diagonals (w=0.25)
var sinSum = 0.0
var cosSum = 0.0
var totalW = 0.0
for dv in -1 .. 1:
for dd in -1 .. 1:
let vb = vBin + dv
let db = dBin + dd
if vb < 0 or vb >= VPERP_BINS or db < 0 or db >= DIST_BINS: continue
let c = grid.cells[vb * DIST_BINS + db]
if c.count == 0: continue
let w = if dv == 0 and dd == 0: 1.0
elif dv == 0 or dd == 0: 0.5
else: 0.25
sinSum += w * c.sumSin / float(c.count)
cosSum += w * c.sumCos / float(c.count)
totalW += w
if totalW == 0.0:
return -999.0
## Returns bilinear-interpolated circular mean offset in degrees, or -999.0 when no data.
let vCont = clamp(vPerp + 8.0, 0.0, 15.999)
let dCont = clamp(distance / DIST_BAND, 0.0, 6.999)
let v0 = int(floor(vCont)); let v1 = min(v0 + 1, VPERP_BINS - 1)
let d0 = int(floor(dCont)); let d1 = min(d0 + 1, DIST_BINS - 1)
let fv = vCont - float(v0)
let fd = dCont - float(d0)
let corners = [(v0, d0, (1-fv)*(1-fd)),
(v1, d0, fv*(1-fd)),
(v0, d1, (1-fv)*fd),
(v1, d1, fv*fd)]
var sinSum, cosSum, totalW = 0.0
for (vi, di, w) in corners:
let cell = grid.cells[vi * DIST_BINS + di]
if cell.count == 0: continue
sinSum += w * cell.sumSin / cell.count
cosSum += w * cell.sumCos / cell.count
totalW += w
if totalW == 0.0: return -999.0
result = radToDeg(arctan2(sinSum, cosSum))
proc learn*(grid: var LeadGrid, vPerp, distance, correctOffset: float) =
let s = sin(degToRad(correctOffset))
let c = cos(degToRad(correctOffset))
let vBin = clamp(int(vPerp + 8.5), 0, VPERP_BINS - 1)
let dBin = clamp(int(distance / DIST_BAND), 0, DIST_BINS - 1)
for dv in -1 .. 1:
for dd in -1 .. 1:
let vb = vBin + dv
let db = dBin + dd
if vb < 0 or vb >= VPERP_BINS or db < 0 or db >= DIST_BINS: continue
let w = if dv == 0 and dd == 0: 1.0
elif dv == 0 or dd == 0: 0.3
else: 0.1
let i = vb * DIST_BINS + db
grid.cells[i].sumSin += w * s
grid.cells[i].sumCos += w * c
grid.cells[i].count += w
let vBin = clamp(int(floor(vPerp + 8.0)), 0, VPERP_BINS - 1)
let dBin = clamp(int(floor(distance / DIST_BAND)), 0, DIST_BINS - 1)
let i = vBin * DIST_BINS + dBin
grid.cells[i].sumSin += s
grid.cells[i].sumCos += c
grid.cells[i].count += 1.0
proc totalCount*(grid: LeadGrid): int =
var s = 0.0