A computer-implemented method for determining an initial ego-pose for initialization of self-localization includes providing a plurality of particles in a map; grouping the particles in a plurality of clusters, each cluster comprising a respective subset of the plurality of particles; during particle filtering, injecting particles based on the plurality of clusters; and determining an initial ego-pose based on the particle filtering.