How to use the pymavlink.rotmat.Matrix3 function in pymavlink

def test_constructor(self):
        """Test the constructor functionality"""
        # Test the identity case
        q = [1, 0, 0, 0]
        euler = [0, 0, 0]
        dcm = Matrix3()
        self._helper_test_constructor(q, euler, dcm)

        # test a case with rotations around all angles (values from matlab)
        q = [0.774519052838329, 0.158493649053890, 0.591506350946110,
             0.158493649053890]
        euler = [np.radians(60), np.radians(60), np.radians(60)]
        dcm = Matrix3(Vector3(0.25, -0.058012701892219, 0.966506350946110),
                      Vector3(0.433012701892219, 0.899519052838329,
                              -0.058012701892219),
                      Vector3(-0.866025403784439, 0.433012701892219, 0.25))

        self._helper_test_constructor(q, euler, dcm)

def add_errors(mag, gyr, last_mag, deltat, total_error, rotations):
    for i in range(len(rotations)):
        if not rotations[i].is_90_degrees():
            continue
        r = rotations[i].r
        m = Matrix3()
        m.rotate(gyr * deltat)
        rmag1 = r * last_mag
        rmag2 = r * mag
        rmag3 = m.transposed() * rmag1
        err = rmag3 - rmag2
        total_error[i] += err.length()

def to_rotation_matrix(self):
        m = Matrix3()
        yy = self.y**2
        yz = self.y * self.z
        xx = self.x**2
        xy = self.x * self.y
        xz = self.x * self.z
        wx = self.w * self.x
        wy = self.w * self.y
        wz = self.w * self.z
        zz = self.z**2

        m.a.x = 1.0-2.0*(yy + zz)
        m.a.y =   2.0*(xy - wz)
        m.a.z =   2.0*(xz + wy)
        m.b.x =   2.0*(xy + wz)
        m.b.y = 1.0-2.0*(xx + zz)
        m.b.z =   2.0*(yz - wx)

return

        if m.get_type() != 'GIMBAL_REPORT':
            return

        needed = ['ATTITUDE', 'GLOBAL_POSITION_INT']
        for n in needed:
            if not n in self.master.messages:
                return

        # clear the camera icon
        self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('GimbalView'))

        gpi = self.master.messages['GLOBAL_POSITION_INT']
        att = self.master.messages['ATTITUDE']
        vehicle_dcm = Matrix3()
        vehicle_dcm.from_euler(att.roll, att.pitch, att.yaw)

        rotmat_copter_gimbal = Matrix3()
        rotmat_copter_gimbal.from_euler312(m.joint_roll, m.joint_el, m.joint_az)
        gimbal_dcm = vehicle_dcm * rotmat_copter_gimbal

        lat = gpi.lat * 1.0e-7
        lon = gpi.lon * 1.0e-7
        alt = gpi.relative_alt * 1.0e-3

        # ground plane
        ground_plane = Plane()

        # the position of the camera in the air, remembering its a right
        # hand coordinate system, so +ve z is down
        camera_point = Vector3(0, 0, -alt)

def remove_offsets(MAG, BAT, c):
    '''remove all corrections to get raw sensor data'''
    correction_matrix = Matrix3(Vector3(c.diag.x,    c.offdiag.x, c.offdiag.y),
                                Vector3(c.offdiag.x, c.diag.y,    c.offdiag.z),
                                Vector3(c.offdiag.y, c.offdiag.z, c.diag.z))
    try:
        correction_matrix = correction_matrix.invert()
    except Exception:
        return False

    field = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ)
    if BAT is not None and not math.isnan(BAT.Curr):
        field -= c.cmot * BAT.Curr
    field = correction_matrix * field
    field *= 1.0 / c.scaling
    field -= Vector3(MAG.OfsX, MAG.OfsY, MAG.OfsZ)

    if math.isnan(field.x) or math.isnan(field.y) or math.isnan(field.z):
        return False

def correct(MAG, BAT, c):
    '''correct a mag sample, returning a Vector3'''
    mag = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ)

    # add the given offsets
    mag += c.offsets

    # multiply by scale factor
    mag *= c.scaling

    # apply elliptical corrections
    mat = Matrix3(
        Vector3(c.diag.x,    c.offdiag.x,  c.offdiag.y),
        Vector3(c.offdiag.x, c.diag.y,     c.offdiag.z),
        Vector3(c.offdiag.y, c.offdiag.z,  c.diag.z))

    mag = mat * mag

    # apply compassmot corrections
    if BAT is not None and not math.isnan(BAT.Curr):
        mag += c.cmot * BAT.Curr

    return mag

def __init__(self):
        self.latitude = 0
        self.longitude = 0
        self.altitude = 0
        self.heading = 0
        self.velocity = Vector3()
        self.accel = Vector3()
        self.gyro = Vector3()
        self.attitude = Vector3()
        self.airspeed = 0
        self.dcm = Matrix3()
        self.timestamp_us = 1

def __init__(self, name, roll, pitch, yaw):
        self.name = name
        self.roll = roll
        self.pitch = pitch
        self.yaw = yaw
        self.r = Matrix3()
        self.r.from_euler(self.roll, self.pitch, self.yaw)