bearing_from_mag.py 文件源码

def mitsuta_mean(self, angles_array):
        # Function meant to work with degrees, covert inputs
        # from radians to degrees and output from degrees to radians
        D = math.degrees(angles_array[0])
        mysum = D
        for val in angles_array[1:]:
            val = math.degrees(val)
            delta = val - D
            if delta < -180.0:
                D = D + delta + 360.0
            elif delta < 180.0:
                D = D + delta
            else:
                D = D + delta - 360.0
            mysum = mysum + D
        m = mysum / len(angles_array)

        avg = math.radians((m + 360.0) % 360.0)
        # make sure avg is between -pi and pi
        if avg > math.pi:
            avg = avg - 2.0 * math.pi
        elif avg < -math.pi:
            avg = avg + 2.0 * math.pi

        return avg