In [ ]:

    

import sys

import numpy as np
import math
from carSimulator import CarSimulator


    

In [ ]:

    

def sampleMultinomial_SUS(p, n):
    s = [0] * n
    sum = 0.
    ptr = np.random.uniform()
    j = 0
    for i in range(len(p)):
        sum += p[i] * n
        while(sum > ptr):
            s[j]=i
            j += 1
            ptr += 1.
    if j != n:
        print ("p not normalized?")
    return s

def resample(X, W):
    indices = sampleMultinomial_SUS(W, len(X))
    Xnew = X[indices]
    W = np.ones(W.shape) / len(X)
    return Xnew, W


    

In [ ]:

    

def main():

    sim = CarSimulator()
    u = [.1, .2]  # fixed control signal
    
    # 1) initialize particles
    N = 200
    X = np.random.normal(0., .5, (N,3))
    W = np.ones(N)/float(N)
    
    # you have access to:
    #   sim.observationNoise (use when evaluating a particle likelihood)
    #   sim.dynamicsNoise (use when propagating a particle)
    
    for t in range(1000):
        sim.step(u)
        y = sim.getRealNoisyObservation()
        
        # 2) resample weighted particles
        X, W = resample(X,W)
        
        # 3) "propagate" each particle using system dynamics (see internals of step function of carSimulator.py)
        #    you can add noise to the particles using 
        #    X += np.random.normal(0., sim.dynamicsNoise, X.shape)
         
            
        # 4) compute the likelihood weights for each particle
        #    to get the ideal observation for a state X[i] use this:
        #    y_expected = sim.getMeanObservationAtState(X[i])
    
    
        # draw particles
        sim.particlesToDraw = X
        
main()