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