implementing particle filter
particle filter works somewhat

the CalculateWeight function is not good at all. Need to add the math.
added time measurements