Coding simulation step of particle filter. Initialization is randomized (Only position, orientation must be also initialized). colors are saturated so there is a trouble with precision and type of variables