Skip to content
Snippets Groups Projects
Commit 0f1de9fe authored by Davis, Duane T's avatar Davis, Duane T
Browse files

ROBOT_MATH: Added a low-variance sampler function

parent d19d71e7
No related branches found
No related tags found
No related merge requests found
......@@ -5,6 +5,7 @@
import math
import random
import numpy
import copy
# Some potentially useful constants
......@@ -277,6 +278,29 @@ def projectSensorReturnToWorld(rng, theta_sense, state, x_sense):
return ( x_return, y_return )
def lowVarianceSampler(hypotheses, weights):
''' Implements the low-variance sampling algorithm to generate an updated
set of hypotheses reflecting the probabilistic weights of the initial set
@param hypotheses: ordered list (or tuple) of hypotheses
@param weights: ordered list (or tuple) of probabilistic hypothesis weights
@return a list of new probabilistically sampled hypotheses
'''
M = len(hypotheses)
M_inverse = 1.0/M
scaler = sum(weights) # Used to make sure that the weights add up to 1
r0 = random.uniform(0.0, M_inverse)
i = 0
c = weights[i]/scaler
result = []
for m in range(0, M):
r = r0 + m * M_inverse # Iterate by 1/M for each addition
while r > c: # Shift to the right hypothesis to add if required
i += 1
c += weights[i]/scaler
result.append(copy.deepcopy(hypotheses[i]))
return result
# Vector functions
def scalar_multiply(v1, s):
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment