Added gradient free belief
Fixed Random trajs
working good
sped up ekf'

J
Optimized best trajectories