In [52]:
(require gamble
math
racket/list)
(struct point (x y))
(define (point-minus p1 p2)
(point (- (point-x p1) (point-x p2))
(- (point-y p1) (point-y p2))))
(define (point-plus p1 p2)
(point (+ (point-x p1) (point-x p2))
(+ (point-y p1) (point-y p2))))
(define (square x) (* x x))
(define (dist-from-origin pt)
(sqrt (+ (square (point-x pt)) (square (point-y pt)))))
(define (angle-to origin object)
(let ([diff (point-minus object origin)])
(acos (/ (point-x diff) (dist-from-origin diff)))))
(define (distance-between pt1 pt2)
(let ([diff (point-minus pt2 pt1)])
(dist-from-origin diff)))
(define (in-angle-slice low high viewer-loc object-loc)
(let ([diff (point-minus object-loc viewer-loc)])
(let ([angle (angle-to-origin diff)])
(and (< low angle) (<= angle high)))))
(define (min-and-idx lst)
(argmin car (for/list ([e lst]
[i (in-naturals)])
(cons e i))))
(define (idx-of-min lst)
(cdr (min-and-idx lst)))
(define (closest-lm-and-range ranges lms)
(let ([range-and-idx (min-and-idx ranges)])
(let ([idx (cdr range-and-idx)]
[rng (car range-and-idx)])
(cons (list-ref lms idx) rng))))
(define (closest-lm ranges lms)
(let ([range-and-idx (min-and-idx ranges)])
(let ([idx (cdr range-and-idx)])
(list-ref lms idx))))
In [53]:
(closest-lm '(3 9 0 2 5) '(a b c d e))
Out[53]:
In [43]:
(define exogeneous-accelerations
(make-hash
(list
(cons 0 (point 0.1 0.1))
(cons 1 (point 0.09 0.11))
(cons 2 (point 0.08 0.12))
(cons 3 (point 0.07 0.13))
(cons 4 (point 0.06 0.14))
(cons 5 (point 0.05 0.15))
(cons 6 (point 0.04 0.16))
(cons 7 (point 0.03 0.17))
(cons 8 (point 0.02 0.18)))))
(define (agent-acceleration time)
(hash-ref exogeneous-accelerations time))
I can't remember the original, so I'm making some things up. Let's assume that if a landmark is the closest one in the observed angle slice, then the log-range returned is normally distributed around the true log distance. If no landmark is in the slice, then the range returned is (say) +inf.0.
In [58]:
(defmodel slam
(define location-noise-std-dev 0.05)
(define velocity-noise-std-dev 0.1)
(define obs-noise-std-dev 0.01)
(deflazy n-landmarks (poisson 3))
(deflazy landmarks (range (+ n-landmarks 1)))
(defmem (landmark-location landmark) (point (uniform -1 1) (uniform -1 1)))
(defmem (agent-location time)
(if (= time 0)
(point (uniform -1 1) (uniform -1 1))
(point-plus
(point (normal 0 location-noise-std-dev)
(normal 0 location-noise-std-dev))
(point-plus
(agent-location (- time 1))
(agent-velocity (- time 1))))))
#|
;; map-belief is a list of (mean cov) pairs?
(defmem (map-belief time)
(observation-xy...
assignment of obs to landmark...
(map-belief (- time 1))))
|#
(defmem (agent-velocity time)
(if (= time -1)
(point (normal 0 velocity-noise-std-dev)
(normal 0 velocity-noise-std-dev))
(point-plus
(point (normal 0 velocity-noise-std-dev)
(normal 0 velocity-noise-std-dev))
(point-plus
(agent-acceleration (- time 1))
(agent-velocity (- time 1))))))
(defmem (observed-xy view-angle-min view-angle-max viewer-position)
(let ([these-landmarks
(filter (lambda (lm)
(in-angle-slice view-angle-min view-angle-max viewer-position lm))
landmarks)])
(let ([ranges
(map
(lambda (lm) (distance-between view-position (landmark-position lm)))
these-landmarks)])
(let ([closest-landmark (closest-lm ranges these-landmarks)])
(point
(+ (point-x closest-landmark) (normal 0 obs-noise-std-dev))
(+ (point-y closest-landmark) (normal 0 obs-noise-std-dev))))))))
In [29]:
(define slam-sampler
(smc-sampler
(open-model slam)
))
Out[29]:
In [40]:
In [41]:
(idx-of-min '(1 6 2 4 8 2 0 9))
Out[41]:
In [ ]: