def measurement_predict_function(objects, noise):
pdb.set_trace()
if magnitude(noise[1:]) == 0:
axis = noise[1:]
else:
axis = noise[1:]/magnitude(noise[1:])
theta = noise[0,0]
return [Angle3D(Quaternion(sin(theta/2)*axis[0,0],sin(theta/2)*axis[1,0],sin(theta/2)*axis[2,0],cos(theta/2))*objects[0].r)]
def calculateMean(cls, weights, angles):
# constrained optimization problem of sum_i d(x_i,mu)^2 over mu in unit complex space
def sumDist(r): # function to minimize
ret = 0
for i in range(len(angles)):
add = weights[i]*pow(magnitude(Angle(complex(*r)/magnitude(r)).log(angles[i])),2)
ret += add
return ret
r_mean = minimize(sumDist, [angles[0].r.real,angles[0].r.imag],constraints={'type': 'eq', 'fun': lambda r: magnitude(r)-1}).x
return Angle(complex(*r_mean)/magnitude(r_mean))
def sumDist(r): # function to minimize
ret = 0
for i in range(len(angles)):
add = weights[i]*pow(magnitude(Angle(complex(*r)/magnitude(r)).log(angles[i])),2)
ret += add
return ret
