fix PSO planning algorithm and add MyParticle and MyFitnessFunction
classes