0

I'm implementing a particle filter on python. I'm working on a function called "sample motion model" This function would take the particles, the motion model (odometry in my case) and some noise ans "move" all particles acording to these models. My code:

def sample_motion_model(self, odom_model):
    rospy.loginfo('[PF] sample_motion_model')

    # Tomo los valores de la odometría
    delta_rot1 = odom_model.vector.x
    delta_tran = odom_model.vector.y
    delta_rot2 = odom_model.vector.z
    # print(delta_rot1, delta_tran, delta_rot2)
    # the motion noise parameters: [alpha1, alpha2, alpha3, alpha4]
    noise = [0.05, 0.05, 0.025, 0.025]

    # standard deviations of motion noise
    sigma_delta_rot1 = noise[0] * abs(delta_rot1) + noise[1] * delta_tran
    sigma_delta_tran = noise[2] * delta_tran + noise[3] * (abs(delta_rot1) + abs(delta_rot2))
    sigma_delta_rot2 = noise[0] * abs(delta_rot2) + noise[1] * delta_tran

    # "move" each particle according to the odometry measurements plus sampled noise
    # to generate new particle set
    new_particles = []
    for particle in self.particles:
        new_particle = dict()
        #sample noisy motions
        noisy_delta_rot1 = delta_rot1 + np.random.normal(0, sigma_delta_rot1)
        noisy_delta_tran = delta_tran + np.random.normal(0, sigma_delta_tran)
        noisy_delta_rot2 = delta_rot2 + np.random.normal(0, sigma_delta_rot2)
        #calculate new particle pose
        new_particle['x'] = particle['x'] + noisy_delta_tran * np.cos(particle['theta'] + noisy_delta_rot1)
        new_particle['y'] = particle['y'] + noisy_delta_tran * np.sin(particle['theta'] + noisy_delta_rot1)
        new_particle['theta'] = particle['theta'] + noisy_delta_rot1 + noisy_delta_rot2
        new_particles.append(new_particle)
    
    self.particles = new_particles

is working ok, but it takes to much time to execute. If I call this function at 10Hz then I can move just 150 particles (on an odroid xu4 SBC) I need to move much more particles. So I'm asking if there is some trick in order to make thos code to run faster.

EDIT: This is a working example: https://github.com/elgarbe/particle_filter inside code, run particle_filter.py It's not the same code that I have on my SBC, but the function sample_motion_model, the one I'm interested in optimize is prety much the same.

Thank

Leonardo Garberoglio
  • 3
  • 3
  • Look at this [question](https://stackoverflow.com/questions/582336/how-can-you-profile-a-python-script) on how to profile a python script. Please also edit your question to include a [mcve] so it's possible to assist you. – import random Apr 13 '21 at 02:51
  • I added a repo with a working code. I don't understand how to profile sample_motion_model function from your link. – Leonardo Garberoglio Apr 13 '21 at 17:36
  • You should be able to run `python -m cProfile -s cumtime particle_filter.py` to profile your code and sort the output by cumulative time. Also, it might be better to use the [matplotlib animation module](https://matplotlib.org/stable/api/animation_api.html). – import random Apr 14 '21 at 05:04
  • Ok, thank! I'm thinking on remove dictionaries and use arrays for the data and, instead of use for loop try to use more matrix operations – Leonardo Garberoglio Apr 15 '21 at 14:25

0 Answers0