calibration_fit.py 文件源码

def ComputeCoverage(sigma_points, bias):
    def ang(p):
        v = rotvecquat(vector.sub(p.compass, bias), vec2vec2quat(p.down, [0, 0, 1]))
        return math.atan2(v[1], v[0])

    angles = sorted(map(ang, sigma_points))
    #print 'angles', angles

    max_diff = 0
    for i in range(len(angles)):
        diff = -angles[i]
        j = i+1
        if j == len(angles):
            diff += 2*math.pi
            j = 0
        diff += angles[j]
        max_diff = max(max_diff, diff)
    return max_diff