feat(reservoir): spread learning to neighbor grid cells for smoother interpolation
Each learn() call now updates a 3×3 neighborhood (center w=1.0, cardinal w=0.3, diagonal w=0.1). count field changed to float64 to support fractional weights. Fills grid ~5× faster and eliminates one-sided interpolation at bin boundaries. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
@@ -11,7 +11,7 @@ type
|
|||||||
GridCell = object
|
GridCell = object
|
||||||
sumSin: float
|
sumSin: float
|
||||||
sumCos: float
|
sumCos: float
|
||||||
count: int
|
count: float64
|
||||||
|
|
||||||
LeadGrid* = object
|
LeadGrid* = object
|
||||||
cells: array[VPERP_BINS * DIST_BINS, GridCell] # 17 × 8 = 136 cells
|
cells: array[VPERP_BINS * DIST_BINS, GridCell] # 17 × 8 = 136 cells
|
||||||
@@ -26,7 +26,6 @@ proc cellIndex(vPerp: float, distance: float): int {.inline.} =
|
|||||||
|
|
||||||
proc forward*(grid: var LeadGrid, vPerp, distance: float): float =
|
proc forward*(grid: var LeadGrid, vPerp, distance: float): float =
|
||||||
## Returns circular mean offset in degrees, or -999.0 when no data.
|
## Returns circular mean offset in degrees, or -999.0 when no data.
|
||||||
let idx = cellIndex(vPerp, distance)
|
|
||||||
let vBin = clamp(int(vPerp + 8.5), 0, VPERP_BINS - 1)
|
let vBin = clamp(int(vPerp + 8.5), 0, VPERP_BINS - 1)
|
||||||
let dBin = clamp(int(distance / DIST_BAND), 0, DIST_BINS - 1)
|
let dBin = clamp(int(distance / DIST_BAND), 0, DIST_BINS - 1)
|
||||||
|
|
||||||
@@ -52,11 +51,24 @@ proc forward*(grid: var LeadGrid, vPerp, distance: float): float =
|
|||||||
result = radToDeg(arctan2(sinSum, cosSum))
|
result = radToDeg(arctan2(sinSum, cosSum))
|
||||||
|
|
||||||
proc learn*(grid: var LeadGrid, vPerp, distance, correctOffset: float) =
|
proc learn*(grid: var LeadGrid, vPerp, distance, correctOffset: float) =
|
||||||
let idx = cellIndex(vPerp, distance)
|
let s = sin(degToRad(correctOffset))
|
||||||
grid.cells[idx].sumSin += sin(degToRad(correctOffset))
|
let c = cos(degToRad(correctOffset))
|
||||||
grid.cells[idx].sumCos += cos(degToRad(correctOffset))
|
let vBin = clamp(int(vPerp + 8.5), 0, VPERP_BINS - 1)
|
||||||
inc grid.cells[idx].count
|
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
|
||||||
|
|
||||||
proc totalCount*(grid: LeadGrid): int =
|
proc totalCount*(grid: LeadGrid): int =
|
||||||
for c in grid.cells:
|
var s = 0.0
|
||||||
result += c.count
|
for c in grid.cells: s += c.count
|
||||||
|
result = int(s)
|
||||||
|
|||||||
Reference in New Issue
Block a user