diff --git a/pysiaf/aperture.py b/pysiaf/aperture.py
index 4ecd1173..c7cbf0c5 100644
--- a/pysiaf/aperture.py
+++ b/pysiaf/aperture.py
@@ -376,7 +376,8 @@ def correct_for_dva(self, v2_arcsec, v3_arcsec, verbose=False):
         data = Table([v2_arcsec, v3_arcsec])
         tmp_file_in = os.path.join(os.environ['HOME'], 'hst_dva_temporary_file.txt')
         tmp_file_out = os.path.join(os.environ['HOME'], 'hst_dva_temporary_file_out.txt')
-        data.write(tmp_file_in, format='ascii.fixed_width_no_header', delimiter=' ', bookend=False)
+        data.write(tmp_file_in, format='ascii.fixed_width_no_header', delimiter=' ', bookend=False,
+                   overwrite=True)
 
         dva_source_dir = self._dva_parameters['dva_source_dir']
         parameter_file = self._dva_parameters['parameter_file']
@@ -391,6 +392,9 @@ def correct_for_dva(self, v2_arcsec, v3_arcsec, verbose=False):
         data = Table.read(tmp_file_out, format='ascii.no_header',
                           names=('v2_original', 'v3_original', 'v2_corrected', 'v3_corrected'))
 
+        if not np.issubdtype(data['v2_corrected'].dtype, np.floating):
+            raise RuntimeError('DVA correction failed. Output is not float. Please check inputs.')
+
         # clean up
         for filename in [tmp_file_in, tmp_file_out]:
             if os.path.isfile(filename):
@@ -899,7 +903,8 @@ def sci_to_det(self, x_sci, y_sci, *args):
                                                                             y_sci - self.YSciRef)
 
     def idl_to_tel(self, x_idl, y_idl, V3IdlYAngle_deg=None, V2Ref_arcsec=None, V3Ref_arcsec=None,
-                   method='planar_approximation', input_coordinates='tangent_plane'):
+                   method='planar_approximation', input_coordinates='tangent_plane',
+                   output_coordinates='tangent_plane', verbose=False):
         """Convert from ideal to telescope (V2/V3) coordinate system.
 
         By default, this implements the planar approximation, which is adequate for most
@@ -923,65 +928,87 @@ def idl_to_tel(self, x_idl, y_idl, V3IdlYAngle_deg=None, V2Ref_arcsec=None, V3Re
         V3Ref_arcsec : float
             overwrites self.V3Ref
         method : str
-            must be one of ['planar_approximation', 'spherical_transformation']
+            must be one of ['planar_approximation', 'spherical_transformation', 'spherical']
         input_coordinates : str
-            must be one of ['tangent_plane', 'spherical']
+            must be one of ['tangent_plane', 'spherical', 'planar']
 
         Returns
         -------
             tuple of floats containing V2, V3 coordinates in arcsec
 
         """
-        if method == 'planar_approximation':
+
+        if V3IdlYAngle_deg is None:
+            V3IdlYAngle_deg = self.V3IdlYAngle
+        if V2Ref_arcsec is None:
+            V2Ref_arcsec = self.V2Ref
+        if V3Ref_arcsec is None:
+            V3Ref_arcsec = self.V3Ref
+
+        if verbose:
+            print('Method: {}, input_coordinates {}, output_coordinates={}'.format(
+                method, input_coordinates, output_coordinates))
+
+        if method == 'spherical':
+            if input_coordinates == 'cartesian':
+                # define cartesian unit vector as in JWST-PLAN-006166, Section 5.7.1.1
+                # then apply 3D rotation matrix to tel
+                unit_vector_idl = rotations.unit_vector_from_cartesian(x=x_idl*u.arcsec, y=y_idl*u.arcsec)
+
+            if input_coordinates == 'polar':
+                # interpret idl coordinates as spherical, i.e. distortion polynomial includes deprojection
+                # unit_vector_idl = rotations.unit(x_idl* u.arcsec.to(u.deg), y_idl* u.arcsec.to(u.deg))
+                xi_idl = x_idl * u.arcsec
+                eta_idl = y_idl * u.arcsec
+                unit_vector_idl = rotations.unit_vector_sky(xi_idl, eta_idl)
+                unit_vector_idl[1] = self.VIdlParity * unit_vector_idl[1]
+
+            l_matrix = rotations.idl_to_tel_rotation_matrix(V2Ref_arcsec, V3Ref_arcsec, V3IdlYAngle_deg)
+
+            # transformation to cartesian unit vector in telescope frame
+            unit_vector_tel = np.dot(np.linalg.inv(l_matrix), unit_vector_idl)
+            # print(unit_vector_tel)
+
+            if output_coordinates == 'polar':
+                # get angular coordinates on idealized focal sphere
+                # nu2_arcsec, nu3_arcsec = rotations.v2v3(unit_vector_tel)
+                nu2, nu3 = rotations.polar_angles(unit_vector_tel)
+
+                v2, v3 = nu2.to(u.arcsec).value, nu3.to(u.arcsec).value
+            if output_coordinates == 'cartesian':
+                v2, v3 = unit_vector_tel[1]*u.rad.to(u.arcsec), unit_vector_tel[2]*u.rad.to(u.arcsec)
+
+        elif method == 'planar_approximation':
             if input_coordinates != 'tangent_plane':
-                raise RuntimeError('Output has to be in tangent plane.')
+                raise RuntimeError('Input has to be in tangent plane.')
             x_model, y_model = self.telescope_transform('idl', 'tel', V3IdlYAngle_deg, V2Ref_arcsec,
                                                         V3Ref_arcsec)
 
             v2 = x_model(x_idl, y_idl)
             v3 = y_model(x_idl, y_idl)
 
-        elif method == 'spherical_transformation':
-            if input_coordinates == 'spherical':
-                x_idl_spherical_deg, y_idl_spherical_deg = x_idl * u.arcsec.to(u.deg), \
-                                                           y_idl * u.arcsec.to(u.deg)
-
-            elif input_coordinates == 'tangent_plane':
-                # deproject coordinates before applying rotations
-                x_idl_spherical_deg, y_idl_spherical_deg = projection.deproject_from_tangent_plane(
-                    x_idl * u.arcsec.to(u.deg), y_idl * u.arcsec.to(u.deg), 0.0, 0.0)
-
-            # only matrix rotations, this transforms from a spherical to a spherical coordinate
-            # system. These matrices transform the V-reference point to the ideal reference point
-            M1 = rotations.rotate(3, -1 * self.V2Ref / 3600.)
-            M2 = rotations.rotate(2, self.V3Ref / 3600.)
-            M3 = rotations.rotate(1, self.V3IdlYAngle)
-            M4 = np.dot(M2, M1)
-            M = np.dot(M3, M4)
-
-            unit_vector = rotations.unit(x_idl_spherical_deg, y_idl_spherical_deg)
-
-            unit_vector[1] = self.VIdlParity * unit_vector[1]
-            rotated_vector = np.dot(np.linalg.inv(M), unit_vector)
-            v2, v3 = rotations.v2v3(rotated_vector)
+        else:
+            raise NotImplementedError
 
         if self._correct_dva:
+            if (self.observatory == 'HST') and ('FGS' in self.AperName):
+                raise NotImplementedError('DVA correction for HST FGS not supported')
             return self.correct_for_dva(v2, v3)
         else:
             return v2, v3
 
     def tel_to_idl(self, v2_arcsec, v3_arcsec, V3IdlYAngle_deg=None, V2Ref_arcsec=None,
                    V3Ref_arcsec=None, method='planar_approximation',
-                   output_coordinates='tangent_plane'):
+                   output_coordinates='tangent_plane', input_coordinates='tangent_plane'):
         """Convert from telescope (V2/V3) to ideal coordinate system.
 
-        By default, this implementats the planar approximation, which is adequate for most
+        By default, this implements the planar approximation, which is adequate for most
         purposes but may not be for all. Error is about 1.7 mas at 10 arcminutes from the tangent
         point. See JWST-STScI-1550 for more details.
         For higher accuracy, set method='spherical_transformation' in which case 3D matrix rotations
         are applied.
 
-        Also by default, the output coordinates are in a tangent plane with a reference points at
+        Also by default, the output coordinates are in a tangent plane with a reference point at
         the origin (0,0) of the ideal frame.
 
         Parameters
@@ -1016,35 +1043,36 @@ def tel_to_idl(self, v2_arcsec, v3_arcsec, V3IdlYAngle_deg=None, V2Ref_arcsec=No
         if method == 'planar_approximation':
             if output_coordinates != 'tangent_plane':
                 raise RuntimeError('Output has to be in tangent plane.')
+            if input_coordinates != 'tangent_plane':
+                raise RuntimeError('Input has to be in tangent plane.')
             x_model, y_model = self.telescope_transform('tel', 'idl', V3IdlYAngle_deg=V3IdlYAngle_deg,
                                                         V2Ref_arcsec=V2Ref_arcsec, V3Ref_arcsec=V3Ref_arcsec)
             return x_model(v2_arcsec - V2Ref_arcsec, v3_arcsec - V3Ref_arcsec), \
                    y_model(v2_arcsec - V2Ref_arcsec, v3_arcsec - V3Ref_arcsec)
 
-        elif method == 'spherical_transformation':
-            # only matrix rotations, this transforms from a spherical to a spherical coordinate
-            # system
-            M1 = rotations.rotate(3, -1 * self.V2Ref / 3600.)
-            M2 = rotations.rotate(2, self.V3Ref / 3600.)
-            M3 = rotations.rotate(1, self.V3IdlYAngle)
-            M4 = np.dot(M2, M1)
-            M = np.dot(M3, M4)
-            unit_vector = rotations.unit(v2_arcsec / 3600., v3_arcsec / 3600.)
-            rotated_vector = np.dot(M, unit_vector)
-            rotated_vector[1] = self.VIdlParity * rotated_vector[1]
-            x_idl_spherical_arcsec, y_idl_spherical_arcsec = rotations.v2v3(rotated_vector)
-
-            if output_coordinates == 'spherical':
-                return x_idl_spherical_arcsec, y_idl_spherical_arcsec
-
-            elif output_coordinates == 'tangent_plane':
-                # Add tangent-plane projection with reference point at origin (0,0) of ideal system
-                x_idl_tangent_deg, y_idl_tangent_deg = projection.project_to_tangent_plane(
-                    x_idl_spherical_arcsec * u.arcsec.to(u.deg),
-                    y_idl_spherical_arcsec * u.arcsec.to(u.deg), 0.0, 0.0)
-
-                return x_idl_tangent_deg * u.deg.to(u.arcsec), \
-                       y_idl_tangent_deg * u.deg.to(u.arcsec)
+        elif method == 'spherical':
+            if input_coordinates == 'cartesian':
+                unit_vector_tel = rotations.unit_vector_from_cartesian(y=v2_arcsec * u.arcsec,
+                                                                       z=v3_arcsec * u.arcsec)
+            elif input_coordinates == 'polar':
+                unit_vector_tel = rotations.unit_vector_sky(v2_arcsec * u.arcsec,
+                                                            v3_arcsec * u.arcsec)
+
+            l_matrix = rotations.idl_to_tel_rotation_matrix(V2Ref_arcsec, V3Ref_arcsec,
+                                                            V3IdlYAngle_deg)
+
+            unit_vector_idl = np.dot(l_matrix, unit_vector_tel)
+
+            if output_coordinates == 'cartesian':
+                x_idl_arcsec, y_idl_arcsec = unit_vector_idl[0] * u.rad.to(u.arcsec), unit_vector_idl[
+                1] * u.rad.to(u.arcsec)
+            elif output_coordinates == 'polar':
+                unit_vector_idl[1] = self.VIdlParity * unit_vector_idl[1]
+                x_idl, y_idl = rotations.polar_angles(unit_vector_idl)
+                x_idl_arcsec, y_idl_arcsec = x_idl.to(u.arcsec).value, y_idl.to(u.arcsec).value
+
+            return x_idl_arcsec, y_idl_arcsec
+
 
     def sci_to_idl(self, x_sci, y_sci):
         """Science to ideal frame transformation."""
@@ -1357,6 +1385,8 @@ def __init__(self):
         """Initialize HST aperture by inheriting from Aperture class."""
         super(HstAperture, self).__init__()
         self.observatory = 'HST'
+        self._fgs_use_rearranged_alignment_parameters = True
+        self._fgs_use_welter_method_to_plot = False
 
     # dictionary that allows to set attributes using JWST naming convention
     _hst_to_jwst_keys = ({'SI_mne': 'InstrName',
@@ -1408,10 +1438,10 @@ def __setattr__(self, key, value):
                             m, k]
                     m += 1
 
-    def _tvs_parameters(self, tvs=None):
+    def _tvs_parameters(self, tvs=None, units=None, apply_rearrangement=True):
         """Compute V2_tvs, V3_tvs, and V3angle_tvs from the TVS matrices stored in the database.
 
-        TVS matrices come from GSFC/SAC web page.
+        TVS matrices come from SOCPRD amu.rep files.
         Adapted from Colin Cox's ipython notebook received 11 Dec 2017.
 
         """
@@ -1427,24 +1457,38 @@ def _tvs_parameters(self, tvs=None):
 
         m1f = np.dot(np.transpose(self.tvs_flip_matrix), tvs)
 
-        v2_arcsec = 3600. * np.rad2deg(np.arctan2(m1f[0, 1], m1f[0, 0]))
-        v3_arcsec = 3600. * np.rad2deg(np.arcsin(m1f[0, 2]))
-        pa_deg = np.rad2deg(np.arctan2(m1f[1, 2], m1f[2, 2]))
-        return v2_arcsec, v3_arcsec, pa_deg, tvs
+        if units is None:
+            v2_arcsec = 3600. * np.rad2deg(np.arctan2(m1f[0, 1], m1f[0, 0]))
+            v3_arcsec = 3600. * np.rad2deg(np.arcsin(m1f[0, 2]))
+            pa_deg = np.rad2deg(np.arctan2(m1f[1, 2], m1f[2, 2]))
+            if (self._fgs_use_rearranged_alignment_parameters) & (apply_rearrangement):
+                pa, v2, v3 = self.rearrange_fgs_alignment_parameters(pa_deg, v2_arcsec, v3_arcsec, 'camera_to_fgs')
+                return v2, v3, pa, tvs
+            else:
+                return v2_arcsec, v3_arcsec, pa_deg, tvs
+        else:
+            raise NotImplementedError
+
 
     def closed_polygon_points(self, to_frame, rederive=False):
         """Compute closed polygon points of aperture outline."""
         if self.a_shape == 'PICK':
-            x0 = 0.
-            y0 = 0.
-            outer_points_x, outer_points_y = points_on_arc(x0, y0, self.maj,
-                                                           self.pi_angle - self.pi_ext,
-                                                           self.pi_angle + self.pi_ext, N=100)
-            inner_points_x, inner_points_y = points_on_arc(x0, y0, self.min,
-                                                           self.po_angle - self.po_ext,
-                                                           self.po_angle + self.po_ext, N=100)
-            x_Tel = np.hstack((outer_points_x, inner_points_x[::-1]))
-            y_Tel = np.hstack((outer_points_y, inner_points_y[::-1]))
+            # TODO: implement method based on the FGS cones defined in amu.rep file
+            if self._fgs_use_welter_method_to_plot is False:
+                #     use SIAF pickle description
+                x0 = 0.
+                y0 = 0.
+                outer_points_x, outer_points_y = points_on_arc(x0, y0, self.maj,
+                                                               self.pi_angle - self.pi_ext,
+                                                               self.pi_angle + self.pi_ext, N=100)
+                inner_points_x, inner_points_y = points_on_arc(x0, y0, self.min,
+                                                               self.po_angle - self.po_ext,
+                                                               self.po_angle + self.po_ext, N=100)
+                x_Tel = np.hstack((outer_points_x, inner_points_x[::-1]))
+                y_Tel = np.hstack((outer_points_y, inner_points_y[::-1]))
+
+            else:
+                raise NotImplementedError
 
             curve = SiafCoordinates(x_Tel, y_Tel, 'tel')
             points_x, points_y = self.convert(curve.x, curve.y, curve.frame, to_frame)
@@ -1456,8 +1500,59 @@ def closed_polygon_points(self, to_frame, rederive=False):
         return points_x[np.append(np.arange(len(points_x)), 0)], points_y[
             np.append(np.arange(len(points_y)), 0)]
 
+    def rearrange_fgs_alignment_parameters(self, pa_deg_in, v2_arcsec_in, v3_arcsec_in, direction):
+        """Convert to/from alignment parameters make FGS `look` like a regular camera aperture.
+
+        Parameters
+        ----------
+        pa_deg_in : float
+        v2_arcsec_in : float
+        v3_arcsec_in : float
+        direction : str
+            One of ['fgs_to_camera', ]
+
+        Returns
+        -------
+        pa, v2, v3 : tuple
+            re-arranged and sometimes sign-inverted alignment parameters
+
+        """
+        pa_deg = copy.deepcopy(pa_deg_in)
+        v2_arcsec = copy.deepcopy(v2_arcsec_in)
+        v3_arcsec = copy.deepcopy(v3_arcsec_in)
+
+        if direction not in ['fgs_to_camera', 'camera_to_fgs']:
+            raise ValueError
+
+        if self.AperName == 'FGS1':
+            # for FGS1 `forward` and `backward` transformation are the same
+            v2 = +1 * pa_deg * u.deg.to(u.arcsec)
+            v3 = -1 * v3_arcsec
+            pa = +1 * v2_arcsec * u.arcsec.to(u.deg)
+        else:
+            if direction == 'fgs_to_camera':
+                if self.AperName == 'FGS2':
+                    v2 = +1 * pa_deg * u.deg.to(u.arcsec)
+                    pa = -1 * v3_arcsec * u.arcsec.to(u.deg)
+                    v3 = -1 * v2_arcsec
+                elif self.AperName == 'FGS3':
+                    v2 = +1 * pa_deg * u.deg.to(u.arcsec)
+                    v3 = +1 * v3_arcsec
+                    pa = -1 * v2_arcsec * u.arcsec.to(u.deg)
+            elif direction == 'camera_to_fgs':
+                if self.AperName == 'FGS2':
+                    v3 = -1 * pa_deg * u.deg.to(u.arcsec)
+                    pa = +1 * v2_arcsec * u.arcsec.to(u.deg)
+                    v2 = -1 * v3_arcsec
+                elif self.AperName == 'FGS3':
+                    v2 = -1 * pa_deg * u.deg.to(u.arcsec)
+                    v3 = +1 * v3_arcsec
+                    pa = +1 * v2_arcsec * u.arcsec.to(u.deg)
+        return pa, v2, v3
+
+
     def compute_tvs_matrix(self, v2_arcsec=None, v3_arcsec=None, pa_deg=None, verbose=False):
-        """Compute the TVS matrix from 'virtual' v2,v3,pa parameters.
+        """Compute the TVS matrix from tvs-specific v2,v3,pa parameters.
 
         Parameters
         ----------
@@ -1478,14 +1573,21 @@ def compute_tvs_matrix(self, v2_arcsec=None, v3_arcsec=None, pa_deg=None, verbos
         """
         if v2_arcsec is None:
             v2_arcsec = self.db_tvs_v2_arcsec
-            if verbose:
-                print('{} using db_tvs_v2_arcsec'.format(self))
         if v3_arcsec is None:
             v3_arcsec = self.db_tvs_v3_arcsec
         if pa_deg is None:
             pa_deg = self.db_tvs_pa_deg
 
-        attitude = rotations.attitude(v2_arcsec, v3_arcsec, 0.0, 0.0, pa_deg)
+        if verbose:
+            print('Computing TVS with tvs_v2_arcsec={}, tvs_v3_arcsec={}, tvs_pa_deg={}'.format(v2_arcsec, v3_arcsec, pa_deg))
+
+        if self._fgs_use_rearranged_alignment_parameters:
+            pa, v2, v3 = self.rearrange_fgs_alignment_parameters(pa_deg, v2_arcsec, v3_arcsec, 'fgs_to_camera')
+            # attitude = rotations.attitude(v2, v3, 0.0, 0.0, pa)
+            attitude = rotations.attitude_matrix(v2, v3, 0.0, 0.0, pa)
+        else:
+            attitude = rotations.attitude(v2_arcsec, v3_arcsec, 0.0, 0.0, pa_deg)
+
         tvs = np.dot(self.tvs_flip_matrix, attitude)
         return tvs
 
@@ -1511,9 +1613,12 @@ def corners(self, to_frame, rederive=False):
         return self.convert(corners.x, corners.y, corners.frame, to_frame)
 
     def idl_to_tel(self, x_idl, y_idl, V3IdlYAngle_deg=None, V2Ref_arcsec=None, V3Ref_arcsec=None,
-                   method='planar_approximation', input_coordinates='tangent_plane'):
+                   method='planar_approximation', input_coordinates='tangent_plane',
+                   output_coordinates=None, verbose=False):
         """Convert ideal coordinates to telescope (V2/V3) coordinates for HST.
 
+        INPUT COORDINATES HAVE TO BE FGS OBJECT SPACE CARTESIAN X,Y coordinates
+
         For HST FGS, transformation is implemented using the FGS TVS matrix. Parameter names
         are overloaded for simplicity:
         tvs_pa_deg = V3IdlYAngle_deg
@@ -1543,7 +1648,7 @@ def idl_to_tel(self, x_idl, y_idl, V3IdlYAngle_deg=None, V2Ref_arcsec=None, V3Re
             Telescope coordinates in arcsec
 
         """
-        if 'FGS' in self.AperName:
+        if ('FGS' in self.AperName) and (self.AperType not in ['PSEUDO']):
             tvs_pa_deg = V3IdlYAngle_deg
             tvs_v2_arcsec = V2Ref_arcsec
             tvs_v3_arcsec = V3Ref_arcsec
@@ -1551,19 +1656,102 @@ def idl_to_tel(self, x_idl, y_idl, V3IdlYAngle_deg=None, V2Ref_arcsec=None, V3Re
             # treat V3IdlYAngle, V2Ref, V3Ref in the TVS-specific way
             tvs = self.compute_tvs_matrix(tvs_v2_arcsec, tvs_v3_arcsec, tvs_pa_deg)
 
-            # unit vector
-            x_rad = np.deg2rad(x_idl * u.arcsec.to(u.deg))
-            y_rad = np.deg2rad(y_idl * u.arcsec.to(u.deg))
-            xyz = np.array([x_rad, y_rad, np.sqrt(1. - (x_rad ** 2 + y_rad ** 2))])
+            if verbose:
+                print('Method: {}, input_coordinates {}, output_coordinates={}'.format(
+                    method, input_coordinates, output_coordinates))
+
+            if method == 'spherical':
+                # unit vector to be used with TVS matrix (see fgs_to_veh.f l496)
+                # this is a distortion corrected object space vector
+
+                if input_coordinates == 'cartesian':
+                    unit_vector_idl = rotations.unit_vector_from_cartesian(x=x_idl*u.arcsec, y=y_idl*u.arcsec)
+                else:
+                    raise ValueError('Input coordinates must be `cartesian`.')
 
-            v = np.rad2deg(np.dot(tvs, xyz)) * u.deg.to(u.arcsec)
-            return v[1], v[2]
+                # apply TVS alignment matrix to unit vector, produces Star Vector in ST vehicle space
+                unit_vector_tel = np.dot(tvs, unit_vector_idl)
+
+                if output_coordinates == 'polar':
+                    # polar coordinates on focal sphere
+                    nu2, nu3 = rotations.polar_angles(unit_vector_tel)
+                    v2_arcsec, v3_arcsec = nu2.to(u.arcsec).value, nu3.to(u.arcsec).value
+                elif output_coordinates == 'cartesian':
+                    v2_arcsec, v3_arcsec = unit_vector_tel[1]*u.rad.to(u.arcsec), unit_vector_tel[2]*u.rad.to(u.arcsec)
+
+                # temporary fix until default is set
+                elif output_coordinates is None:
+                    raise NotImplementedError
+
+            elif method == 'planar_approximation':
+                # unit vector
+                x_rad = np.deg2rad(x_idl * u.arcsec.to(u.deg))
+                y_rad = np.deg2rad(y_idl * u.arcsec.to(u.deg))
+                xyz = np.array([x_rad, y_rad, np.sqrt(1. - (x_rad ** 2 + y_rad ** 2))])
+
+                v = np.rad2deg(np.dot(tvs, xyz)) * u.deg.to(u.arcsec)
+                v2_arcsec, v3_arcsec = v[1], v[2]
+
+            else:
+                raise NotImplementedError
+
+            if (method == 'spherical_transformation') and (output_coordinates == 'tangent_plane'):
+                # tangent plane projection using tel boresight
+                v2_tangent_deg, v3_tangent_deg = projection.project_to_tangent_plane(v2_spherical_arcsec * u.arcsec.to(u.deg),
+                                                                                     v3_spherical_arcsec * u.arcsec.to(u.deg), 0.0, 0.0)
+                v2_arcsec, v3_arcsec = v2_tangent_deg*3600., v3_tangent_deg*3600.
+
+            return v2_arcsec, v3_arcsec
         else:
             return super(HstAperture, self).idl_to_tel(x_idl, y_idl,
                                                        V3IdlYAngle_deg=V3IdlYAngle_deg,
                                                        V2Ref_arcsec=V2Ref_arcsec,
                                                        V3Ref_arcsec=V3Ref_arcsec, method=method,
-                                                       input_coordinates=input_coordinates)
+                                                       input_coordinates=input_coordinates,
+                                                       output_coordinates=output_coordinates)
+
+
+    def tel_to_idl(self, v2_arcsec, v3_arcsec, V3IdlYAngle_deg=None, V2Ref_arcsec=None,
+                   V3Ref_arcsec=None, method='planar_approximation',
+                   output_coordinates='tangent_plane', input_coordinates='tangent_plane'):
+
+        if 'FGS' in self.AperName:
+            tvs_pa_deg = V3IdlYAngle_deg
+            tvs_v2_arcsec = V2Ref_arcsec
+            tvs_v3_arcsec = V3Ref_arcsec
+
+            # treat V3IdlYAngle, V2Ref, V3Ref in the TVS-specific way
+            tvs = self.compute_tvs_matrix(tvs_v2_arcsec, tvs_v3_arcsec, tvs_pa_deg)
+
+            if method == 'spherical':
+                if input_coordinates == 'cartesian':
+                    unit_vector_tel = rotations.unit_vector_from_cartesian(y=v2_arcsec * u.arcsec,
+                                                                           z=v3_arcsec * u.arcsec)
+                elif input_coordinates == 'polar':
+                    unit_vector_tel = rotations.unit_vector_sky(v2_arcsec * u.arcsec, v3_arcsec * u.arcsec)
+
+                # apply inverse TVS alignment matrix to unit vector, produces Star Vector in FGS object space
+                unit_vector_idl = np.dot(np.transpose(tvs), unit_vector_tel)
+
+                x_idl_arcsec, y_idl_arcsec = unit_vector_idl[0]*u.rad.to(u.arcsec), unit_vector_idl[1]*u.rad.to(u.arcsec)
+
+                return x_idl_arcsec, y_idl_arcsec
+
+            elif method == 'planar_approximation':
+                # unit vector
+                unit_vector_tel = rotations.unit_vector_from_cartesian(y=v2_arcsec*u.arcsec, z=v3_arcsec*u.arcsec)
+                unit_vector_idl = np.dot(np.transpose(tvs), unit_vector_tel)
+                x_idl_arcsec, y_idl_arcsec = unit_vector_idl[0]*u.rad.to(u.arcsec), unit_vector_idl[1]*u.rad.to(u.arcsec)
+
+                return x_idl_arcsec, y_idl_arcsec
+        else:
+            return super(HstAperture, self).idl_to_tel(v2_arcsec, v3_arcsec,
+                                                       V3IdlYAngle_deg=V3IdlYAngle_deg,
+                                                       V2Ref_arcsec=V2Ref_arcsec,
+                                                       V3Ref_arcsec=V3Ref_arcsec, method=method,
+                                                       input_coordinates=input_coordinates,
+                                                       output_coordinates=output_coordinates)
+
 
     def set_idl_reference_point(self, v2_ref, v3_ref, verbose=False):
         """Determine the reference point in the Ideal frame.
@@ -1601,11 +1789,8 @@ def set_idl_reference_point(self, v2_ref, v3_ref, verbose=False):
         inverted_tvs = np.linalg.inv(db_tvs)
 
         # construct the normalized vector of the reference point in the V/tel frame
-        tel_vector_rad = np.array([0., np.deg2rad(v2_ref / 3600.), np.deg2rad(v3_ref / 3600.)])
-        tel_vector_rad_normalized = tel_vector_rad.copy()
-        tel_vector_rad_normalized[0] = np.sqrt(1. - tel_vector_rad[1] ** 2 - tel_vector_rad[2] ** 2)
-
-        idl_vector_rad = np.dot(inverted_tvs, tel_vector_rad_normalized)
+        tel_vector_rad = rotations.unit_vector_sky(v2_ref*u.arcsec, v3_ref*u.arcsec)
+        idl_vector_rad = np.dot(inverted_tvs, tel_vector_rad)
         idl_vector_arcsec = np.rad2deg(idl_vector_rad) * 3600.
 
         self.idl_x_ref_arcsec = idl_vector_arcsec[0]
@@ -1636,6 +1821,10 @@ def set_tel_reference_point(self, verbose=True):
 
         This is after using those V2Ref and V3Ref attributes for TVS matrix update.
 
+        TODO
+        ----
+            - use new rotations.methods
+
         """
         # reference point in idl frame
         idl_vector_rad = np.deg2rad(
@@ -1661,8 +1850,8 @@ def set_tel_reference_point(self, verbose=True):
 
         if verbose:
             for attribute_name in 'V2Ref V3Ref V3IdlYAngle'.split():
-                print('Setting {} to {}'.format(attribute_name, getattr(self, attribute_name)))
-                print('Setting {} to {}'.format(attribute_name + '_corrected',
+                print('HST FGS fiducial point update: Setting {} back to {:2.2f} '.format(attribute_name, getattr(self, attribute_name)), end='')
+                print('and {} to {:2.2f}'.format(attribute_name + '_corrected',
                                                 getattr(self, attribute_name + '_corrected')))
 
 
diff --git a/pysiaf/iando/read.py b/pysiaf/iando/read.py
index da74d2e1..a008e175 100644
--- a/pysiaf/iando/read.py
+++ b/pysiaf/iando/read.py
@@ -286,6 +286,7 @@ def read_hst_fgs_amudotrep(file=None, version=None):
         'n_cones': re.compile(r'NUMBER OF CONES:   (?P<n_cones>\d)'),
         'date': re.compile(r'(?P<day>[ 123][0-9])-(?P<month>[A-Z][A-Z][A-Z])-(?P<year>[0-9][0-9])'),
         'cone_vector': re.compile(r'(CONE)*(CONE VECTOR)*(CONE ANGLE)'),
+        'cone_vector_tel': re.compile(r'(CONE)*(REVISED CONE VECTOR)*(PREVIOUS CONE VECTOR)'),
         'tvs': re.compile(r'(FGS TO ST TRANSFORMATION MATRICES)'),
     }
 
@@ -310,7 +311,12 @@ def read_hst_fgs_amudotrep(file=None, version=None):
                                    data_start=astropy_table_index+2, data_end=astropy_table_index + 2 + n_cones,
                                    guess=False, names=('CONE', 'X', 'Y', 'Z', 'CONE_ANGLE_DEG'))
                 # table.pprint()
-                data[fgs_id]['cone_parameters'] = table
+                data[fgs_id]['cone_parameters_fgs'] = table
+            elif key == 'cone_vector_tel':
+                table = Table.read(file, format='ascii.no_header', delimiter=' ',
+                                   data_start=astropy_table_index+2, data_end=astropy_table_index + 2 + n_cones,
+                                   guess=False, names=('CONE', 'V1', 'V2', 'V3', 'V1_PREV', 'V2_PREV', 'V3_PREV'))
+                data[fgs_id]['cone_parameters_tel'] = table
             elif key == 'tvs':
                 table = Table.read(file, format='ascii.no_header', delimiter=' ',
                                    data_start=astropy_table_index+2, data_end=astropy_table_index+2+3,
diff --git a/pysiaf/tests/test_aperture.py b/pysiaf/tests/test_aperture.py
index c797bc69..aea876cc 100644
--- a/pysiaf/tests/test_aperture.py
+++ b/pysiaf/tests/test_aperture.py
@@ -24,31 +24,74 @@ def siaf_objects():
     return siafs
 
 
-def test_idl_to_tel():
-    """Test the transformations between ideal and telescope frames that do not use the planar approximation."""
+def test_idl_to_tel(verbose=False):
+    """Test the transformations between ideal and telescope frames."""
     siaf = Siaf('NIRISS')
 
     x_idl, y_idl = get_grid_coordinates(10, (0, 0), 100)
 
-    verbose = False
-
     for aper_name in siaf.apertures.keys():
-        # aperture
         aperture = siaf[aper_name]
 
-        for input_coordinates in ['spherical', 'tangent_plane']:
-            v2, v3 = aperture.idl_to_tel(x_idl, y_idl, method='spherical_transformation', input_coordinates=input_coordinates)
-            x_idl_2, y_idl_2 = aperture.tel_to_idl(v2, v3, method='spherical_transformation', output_coordinates=input_coordinates)
-            x_diff = np.abs(x_idl - x_idl_2)
-            y_diff = np.abs(y_idl - y_idl_2)
-            if verbose:
-                print('Aperture {} {} x_diff {} y_diff {}'.format(aper_name, input_coordinates, np.max(x_diff), np.max(y_diff)))
-            if input_coordinates == 'spherical':
-                threshold = 1e-12
-            elif input_coordinates == 'tangent_plane':
-                threshold = 5e-10
-            assert np.max(x_diff) < threshold
-            assert np.max(y_diff) < threshold
+        for idl_to_tel_method in ['planar_approximation', 'spherical']:
+            if idl_to_tel_method == 'spherical':
+                input_coordinate_types = ['polar', 'cartesian']
+            else:
+                input_coordinate_types = ['tangent_plane']
+
+            for input_coordinates in input_coordinate_types:
+                v2, v3 = aperture.idl_to_tel(x_idl, y_idl, method=idl_to_tel_method, input_coordinates=input_coordinates, output_coordinates=input_coordinates)
+                x_idl_2, y_idl_2 = aperture.tel_to_idl(v2, v3, method=idl_to_tel_method, input_coordinates=input_coordinates, output_coordinates=input_coordinates)
+                x_diff = np.abs(x_idl - x_idl_2)
+                y_diff = np.abs(y_idl - y_idl_2)
+                if verbose:
+                    print('{} {}: Aperture {} {} x_diff {} y_diff {}'.format(idl_to_tel_method, input_coordinates, aper_name, input_coordinates, np.max(x_diff), np.max(y_diff)))
+                if idl_to_tel_method == 'planar_approximation':
+                    threshold = 7e-14
+                elif idl_to_tel_method == 'spherical':
+                    if input_coordinates == 'polar':
+                        threshold = 6e-13
+                    elif input_coordinates == 'cartesian':
+                        threshold = 5e-8
+                assert np.max(x_diff) < threshold
+                assert np.max(y_diff) < threshold
+
+
+def test_hst_fgs_idl_to_tel(verbose=False):
+    """Test the transformations between ideal and telescope frames."""
+
+    siaf = Siaf('HST')
+
+    x_idl, y_idl = get_grid_coordinates(2, (0, -50), 1000, y_width=400)
+
+    for aper_name in 'FGS1 FGS2 FGS3'.split():
+        aperture = siaf[aper_name]
+        for idl_to_tel_method in ['planar_approximation', 'spherical']:
+            if idl_to_tel_method == 'spherical':
+                input_coordinate_types = ['polar', 'cartesian']
+            else:
+                input_coordinate_types = ['tangent_plane']
+
+            for input_coordinates in input_coordinate_types:
+                if input_coordinates == 'polar':
+                    v2, v3 = aperture.idl_to_tel(x_idl, y_idl, method=idl_to_tel_method,
+                                                 input_coordinates='cartesian',
+                                                 output_coordinates=input_coordinates)
+                    x_idl_2, y_idl_2 = aperture.tel_to_idl(v2, v3, method=idl_to_tel_method,
+                                                           input_coordinates=input_coordinates,
+                                                           output_coordinates='cartesian')
+                else:
+                    v2, v3 = aperture.idl_to_tel(x_idl, y_idl, method=idl_to_tel_method, input_coordinates=input_coordinates, output_coordinates=input_coordinates)
+                    x_idl_2, y_idl_2 = aperture.tel_to_idl(v2, v3, method=idl_to_tel_method, input_coordinates=input_coordinates, output_coordinates=input_coordinates)
+                x_diff = np.abs(x_idl - x_idl_2)
+                y_diff = np.abs(y_idl - y_idl_2)
+                if verbose:
+                    print('{} {}: Aperture {} {} x_diff {} y_diff {}'.format(idl_to_tel_method, input_coordinates, aper_name, input_coordinates, np.max(x_diff), np.max(y_diff)))
+
+                threshold = 2.5e-13
+
+                assert np.max(x_diff) < threshold
+                assert np.max(y_diff) < threshold
 
 
 def test_jwst_aperture_transforms(siaf_objects, verbose=False, threshold=None):
diff --git a/pysiaf/tests/test_projection.py b/pysiaf/tests/test_projection.py
index fbbbc635..781bec47 100644
--- a/pysiaf/tests/test_projection.py
+++ b/pysiaf/tests/test_projection.py
@@ -31,7 +31,8 @@ def make_grid_coordinates(centre_deg=(0., 0.)):
 
 def test_tangent_plane_projection_roundtrip(grid_coordinates):
     """Transform from RA/Dec to tangent plane and back. Check that input is recovered."""
-    centre_deg = (80., -70.)   # setting this to 0,0 fails because of 0,360 deg wrapping
+    # centre_deg = (80., -70.)
+    centre_deg = (0., 0.)
     ra_deg, dec_deg = grid_coordinates(centre_deg=centre_deg)
 
     # project to detector pixel coordinates
@@ -47,7 +48,8 @@ def test_tangent_plane_projection_roundtrip(grid_coordinates):
 
 def test_project_to_tangent_plane(grid_coordinates):
     """Compare projection code built with astropy functions with independent implementation."""
-    centre_deg = (80., -70.)  # setting this to 0,0 fails because of 0,360 deg wrapping
+    # centre_deg = (80., -70.)
+    centre_deg = (0., 0.)
     ra_deg, dec_deg = grid_coordinates(centre_deg=centre_deg)
 
     x_1, y_1 = projection.project_to_tangent_plane(ra_deg, dec_deg, centre_deg[0], centre_deg[1])
@@ -60,7 +62,8 @@ def test_project_to_tangent_plane(grid_coordinates):
 
 def test_deproject_from_tangent_plane(grid_coordinates):
     """Compare projection code built with astropy functions with independent implementation."""
-    centre_deg = (80., -70.)  # setting this to 0,0 fails because of 0,360 deg wrapping
+    # centre_deg = (80., -70.)
+    centre_deg = (0., 0.)
     ra_deg, dec_deg = grid_coordinates(centre_deg=centre_deg)
 
     x, y = projection.project_to_tangent_plane(ra_deg, dec_deg, centre_deg[0], centre_deg[1])
@@ -144,9 +147,9 @@ def tangent_plane_deprojection(e, n, alpha_ref, delta_ref):
                         - np.sin(rho)*np.sin(d0)*np.cos(B))
     alpha = alpha_ref + np.rad2deg(dalpha)
 
-    if np.any(alpha < 0.0):
-        index = np.where(alpha < 0.0)
-        alpha[index] += 360.0    # set to range 0 to 360 deg
+    # if np.any(alpha < 0.0):
+    #     index = np.where(alpha < 0.0)
+    #     alpha[index] += 360.0    # set to range 0 to 360 deg
 
     delta_rad = np.arcsin(np.sin(d0)*np.cos(rho) + np.cos(d0)*np.sin(rho)*np.cos(B))
     delta = np.rad2deg(delta_rad)
diff --git a/pysiaf/tests/test_rotations.py b/pysiaf/tests/test_rotations.py
index 74c25381..45a863c3 100644
--- a/pysiaf/tests/test_rotations.py
+++ b/pysiaf/tests/test_rotations.py
@@ -4,11 +4,15 @@
 Authors
 -------
     Colin Cox
+    Johannes Sahlmann
 
 """
-import numpy as np
 from math import sin, cos, acos, pi
-from pysiaf.utils import rotations as rt
+import numpy as np
+
+import astropy.units as u
+
+from pysiaf.utils import rotations, tools
 
 # Some values used in both tests
 #  Set up some arbitrary values
@@ -24,6 +28,13 @@
 v2_array = np.array([200.0, -500.0, 300.0, -400.0])
 v3_array = np.array([300.0, -600.0, -400.0, 500.0])
 
+ra_deg = 20.0
+dec_deg = 70.0
+pa_deg = 15.0
+v2_arcsec = 200.0
+v3_arcsec = 300.0
+
+
 
 def test_attitude(verbose=False):
     """Test the properties of the attitude matrix in calculating positions and roll angles.
@@ -37,24 +48,24 @@ def test_attitude(verbose=False):
         set to true only if detailed print-out needed
 
     """
-    a = rt.attitude(v2, v3, ra, dec, roll)
+    a = rotations.attitude(v2, v3, ra, dec, roll)
     if verbose:
         print('attitude\n', a)
 
     #  Show that attitude matrix correctly connects given inputs in both directions
-    (ra_test1, dec_test1) = rt.pointing(a, v2, v3)
+    (ra_test1, dec_test1) = rotations.pointing(a, v2, v3)
     if verbose:
         print('RA and Dec %10.6f %10.6f %10.3e %10.3e ' % (ra_test1, dec_test1, ra_test1-ra, dec_test1 - dec))
     assert abs(ra_test1 - ra) < 1.0e-10 and abs(dec_test1 - dec) < 1.0e-10, 'Miscalculated RA or Dec'
 
-    (v2_test1, v3_test1) = rt.getv2v3(a, ra, dec)
+    (v2_test1, v3_test1) = rotations.getv2v3(a, ra, dec)
     if verbose:
         print('V2 V3 %10.6f %10.6f %10.3e %10.3e' % (v2_test1, v3_test1, v2_test1 - v2, v3_test1 - v3))
     assert abs(v2_test1 - v2) < 1.0e-10 and abs(v3_test1 - v3) < 1.0e-10, 'Miscalculated V2 or V3'
 
     #  Show that points away from defining position are correctly handled
-    (v2_test2, v3_test2) = rt.getv2v3(a, ra2, dec2)
-    (ra_test2, dec_test2) = rt.pointing(a, v2_test2, v3_test2)
+    (v2_test2, v3_test2) = rotations.getv2v3(a, ra2, dec2)
+    (ra_test2, dec_test2) = rotations.pointing(a, v2_test2, v3_test2)
     if verbose:
         print('Test 2 %10.6f %10.6f' % (v2_test2, v3_test2))
         print('Test2 RA and Dec %10.6f %10.6f' % (ra_test2, dec_test2))
@@ -62,11 +73,11 @@ def test_attitude(verbose=False):
     assert abs(ra_test2 - ra2) < 1.0e10 and abs(dec_test2 - dec2) < 1.0e-10, 'Miscalculated RA or Dec'
 
     #  Position angles at reference point
-    pa1 = rt.posangle(a, v2, v3)
-    pa2 = rt.sky_posangle(a, ra, dec)
+    pa1 = rotations.posangle(a, v2, v3)
+    pa2 = rotations.sky_posangle(a, ra, dec)
     #  and at displaced point
-    pa3 = rt.posangle(a, v2_test2, v3_test2)
-    pa4 = rt.sky_posangle(a, ra_test2, dec_test2)
+    pa3 = rotations.posangle(a, v2_test2, v3_test2)
+    pa4 = rotations.sky_posangle(a, ra_test2, dec_test2)
     if verbose:
         print('PA tests')
         print('%10.6f %10.6f %10.6f %10.3e' % (roll, pa1, pa2, pa1 - pa2))
@@ -75,16 +86,16 @@ def test_attitude(verbose=False):
     assert abs(pa3 - pa4) < 1.0e-10, 'Disagreement for displaced point position angles'
 
     # Test some functions with arrays as input
-    unit_vectors = rt.unit(ra_array, dec_array)
-    rd = rt.radec(unit_vectors, positive_ra=True)
-    rd_test = rt.pointing(a, v2_array, v3_array)
+    unit_vectors = rotations.unit(ra_array, dec_array)
+    rd = rotations.radec(unit_vectors, positive_ra=True)
+    rd_test = rotations.pointing(a, v2_array, v3_array)
     if verbose:
         print(unit_vectors)
         print('RD\n', rd)
         print(rd_test[0])
         print(rd_test[1])
 
-    v_test = rt.getv2v3(a, ra_array, dec_array)
+    v_test = rotations.getv2v3(a, ra_array, dec_array)
     if verbose:
         for i in range(len(ra_array)):
             print(v_test[0][i], v_test[1][i])
@@ -94,6 +105,59 @@ def test_attitude(verbose=False):
     assert abs(v_test[1][0] - v3_array[0] < 1.0e-10), 'V3 values do not match'
 
 
+def test_attitude_matrix():
+    """Compare original and new attitude matrix generator functions."""
+
+    ra = ra_deg * u.deg
+    dec = dec_deg * u.deg
+    pa = pa_deg * u.deg
+    v2 = v2_arcsec * u.arcsec
+    v3 = v3_arcsec * u.arcsec
+
+    attitude = rotations.attitude(v2_arcsec, v3_arcsec, ra_deg, dec_deg, pa_deg)
+    attitude_matrix = rotations.attitude_matrix(v2, v3, ra, dec, pa)
+
+    assert np.all(attitude==attitude_matrix)
+
+
+def test_sky_to_tel():
+    """Test application of the attitude matrix"""
+
+    # test with quantities
+    ra = ra_deg * u.deg
+    dec = dec_deg * u.deg
+    pa = pa_deg * u.deg
+    v2 = v2_arcsec * u.arcsec
+    v3 = v3_arcsec * u.arcsec
+
+    attitude = rotations.attitude_matrix(v2, v3, ra, dec, pa)
+    ra_2, dec_2 = rotations.tel_to_sky(attitude, *rotations.sky_to_tel(attitude, ra, dec))
+    assert np.abs((ra - ra_2).to(u.milliarcsecond).value) < 1e-6
+    assert np.abs((dec - dec_2).to(u.milliarcsecond).value) < 1e-6
+
+    # test without quantities
+    attitude = rotations.attitude_matrix(v2_arcsec, v3_arcsec, ra_deg, dec_deg, pa_deg)
+    ra_2, dec_2 = rotations.tel_to_sky(attitude, *rotations.sky_to_tel(attitude, ra_deg, dec_deg))
+    assert np.abs(ra_deg - ra_2.to(u.deg).value)*u.deg.to(u.milliarcsecond) < 1e-6
+    assert np.abs(dec_deg - dec_2.to(u.deg).value)*u.deg.to(u.milliarcsecond) < 1e-6
+
+    # test array inputs
+    n_side = 3
+    span = 2 * u.arcmin
+    x_width = span.to(u.deg).value
+    centre_deg = (ra_deg, dec_deg)
+    ra_array_deg, dec_array_deg = tools.get_grid_coordinates(n_side, centre_deg, x_width)
+
+    ra_array_2, dec_array_2 = rotations.tel_to_sky(attitude, *rotations.sky_to_tel(attitude, ra_array_deg*u.deg, dec_array_deg*u.deg))
+    assert np.all(np.abs(ra_array_deg*u.deg - ra_array_2) < 1e-6 * u.milliarcsecond)
+    assert np.all(np.abs(dec_array_deg*u.deg - dec_array_2) < 1e-6 * u.milliarcsecond)
+
+    ra_array_2, dec_array_2 = rotations.tel_to_sky(attitude, *rotations.sky_to_tel(attitude, ra_array_deg, dec_array_deg))
+    assert np.all(np.abs(ra_array_deg*u.deg - ra_array_2) < 1e-6 * u.milliarcsecond)
+    assert np.all(np.abs(dec_array_deg*u.deg - dec_array_2) < 1e-6 * u.milliarcsecond)
+
+
+
 def test_axial_rotation(verbose=False):
     """Compare vector transformation using the attitude matrix with a single rotation about an axis.
 
@@ -111,13 +175,13 @@ def test_axial_rotation(verbose=False):
     values = np.random.rand(2)
     alpha = 2*pi*values[0]
     beta = acos(2*values[1] - 1.0)
-    u = np.array([cos(alpha)*cos(beta), sin(alpha)*cos(beta), sin(beta)])
+    vector = np.array([cos(alpha)*cos(beta), sin(alpha)*cos(beta), sin(beta)])
 
-    a = rt.attitude(v2, v3, ra, dec, roll)
-    va = np.dot(a, u)    # Transform using attitude matrix
+    a = rotations.attitude(v2, v3, ra, dec, roll)
+    va = np.dot(a, vector)    # Transform using attitude matrix
 
-    (axis, phi, quaternion) = rt.rodrigues(a)  # obtain single rotation parameters equivalent to attitude matrix
-    vb = rt.axial_rotation(axis, phi, u)  # Transform using axial rotation
+    (axis, phi, quaternion) = rotations.rodrigues(a)  # obtain single rotation parameters equivalent to attitude matrix
+    vb = rotations.axial_rotation(axis, phi, vector)  # Transform using axial rotation
     dot_product = np.dot(va, vb)
 
     if verbose:
@@ -132,3 +196,25 @@ def test_axial_rotation(verbose=False):
         print('Dot product', dot_product)
 
     assert abs(dot_product - 1.0) < 1.0e-12, 'Transforms do not agree'
+
+
+def test_unit_vector_from_cartesian():
+    """Test unit vector construction."""
+
+    # scalar inputs
+    x = 0.1
+    y = 0.2
+    unit_vector = rotations.unit_vector_from_cartesian(x=x, y=y)
+    assert (np.linalg.norm(unit_vector) - 1) < 1e-14
+
+    # scalar inputs with unit
+    x = 200 * u.arcsec
+    y = -300 * u.arcsec
+    unit_vector = rotations.unit_vector_from_cartesian(x=x, y=y)
+    assert (np.linalg.norm(unit_vector) - 1) < 1e-14
+
+    # array inputs with unit
+    x = np.linspace(-100, 100, 10) * u.arcsec
+    y = np.linspace(-500, -100, 10) * u.arcsec
+    unit_vector = rotations.unit_vector_from_cartesian(x=x, y=y)
+    assert np.all(np.abs(np.linalg.norm(unit_vector, axis=0) - 1)) < 1e-14
diff --git a/pysiaf/tests/test_tools.py b/pysiaf/tests/test_tools.py
new file mode 100644
index 00000000..1448d521
--- /dev/null
+++ b/pysiaf/tests/test_tools.py
@@ -0,0 +1,61 @@
+#!/usr/bin/env python
+"""Tests for the pysiaf utils.tools functions.
+
+Authors
+-------
+
+    Johannes Sahlmann
+
+"""
+
+import astropy.units as u
+import numpy as np
+from numpy.testing import assert_allclose
+
+from ..siaf import Siaf
+from ..utils.tools import get_grid_coordinates
+from ..utils.tools import jwst_fgs_to_fgs_matrix
+from ..utils import rotations
+
+
+def test_jwst_fgs_to_fgs_matrix(verbose=False):
+    """Test 3D matrix transformation against planar approximation."""
+    rotation_1to2 = jwst_fgs_to_fgs_matrix(direction='fgs1_to_fgs2')
+
+    siaf = Siaf('fgs')
+    fgs1 = siaf['FGS1_FULL_OSS']
+    fgs2 = siaf['FGS2_FULL_OSS']
+
+    # FGS1 ideal coordinates
+    fgs1_x_idl, fgs1_y_idl = get_grid_coordinates(3, (0, 0), 100)
+
+    # transform to FGS ideal using planar approximation
+    fgs2_x_idl_planar, fgs2_y_idl_planar = fgs2.tel_to_idl(*fgs1.idl_to_tel(fgs1_x_idl, fgs1_y_idl))
+
+    if verbose:
+        print('')
+        for aperture in [fgs1, fgs2]:
+            for attribute in 'V2Ref V3Ref V3IdlYAngle'.split():
+                print('{} {} {}'.format(aperture.AperName, attribute, getattr(aperture, attribute)))
+
+        print('FGS1 idl')
+        print(fgs1_x_idl, fgs1_y_idl)
+        print('tel')
+        print(fgs1.idl_to_tel(fgs1_x_idl, fgs1_y_idl))
+        print('FGS2 idl')
+        print(fgs2_x_idl_planar, fgs2_y_idl_planar)
+
+    # transform to FGS ideal using 3D rotation matrix
+    fgs1_unit_vector_idl = rotations.unit_vector_sky(fgs1_x_idl * u.arcsec, fgs1_y_idl * u.arcsec)
+    fgs2_unit_vector_idl = np.dot(rotation_1to2, fgs1_unit_vector_idl)
+    fgs2_x_idl, fgs2_y_idl = rotations.polar_angles(fgs2_unit_vector_idl)[0].to(u.arcsec).value, \
+                             rotations.polar_angles(fgs2_unit_vector_idl)[1].to(u.arcsec).value
+    if verbose:
+        print(fgs2_x_idl, fgs2_y_idl)
+        np.set_printoptions(precision=15, suppress=True)
+        print(rotation_1to2)
+
+    # require agreement within 1.5 mas
+    absolute_tolerance = 1.5e-3
+    assert_allclose(fgs2_x_idl, fgs2_x_idl_planar, atol=absolute_tolerance)
+    assert_allclose(fgs2_y_idl, fgs2_y_idl_planar, atol=absolute_tolerance)
diff --git a/pysiaf/utils/projection.py b/pysiaf/utils/projection.py
index 07642b5e..e1a9d24b 100644
--- a/pysiaf/utils/projection.py
+++ b/pysiaf/utils/projection.py
@@ -6,8 +6,10 @@
 
 """
 
+import numpy as np
 from astropy.modeling import models as astmodels
 from astropy.modeling import rotations as astrotations
+import astropy.units as u
 
 
 def project_to_tangent_plane(ra, dec, ra_ref, dec_ref, scale=1.):
@@ -42,7 +44,10 @@ def project_to_tangent_plane(ra, dec, ra_ref, dec_ref, scale=1.):
 
     """
     # for zenithal projections, i.e. gnomonic, i.e. TAN:
-    lonpole = 180.
+    if isinstance(ra_ref, u.Quantity):
+        lonpole = 180. * u.deg
+    else:
+        lonpole = 180.
 
     # tangent plane projection from phi/theta to x,y
     tan = astmodels.Sky2Pix_TAN()
@@ -61,7 +66,7 @@ def project_to_tangent_plane(ra, dec, ra_ref, dec_ref, scale=1.):
     return x, y
 
 
-def deproject_from_tangent_plane(x, y, ra_ref, dec_ref, scale=1.):
+def deproject_from_tangent_plane(x, y, ra_ref, dec_ref, scale=1., unwrap=True):
     """Convert pixel coordinates into ra/dec coordinates using a tangent plane de-projection.
 
     The projection's reference point has to be specified.
@@ -69,10 +74,10 @@ def deproject_from_tangent_plane(x, y, ra_ref, dec_ref, scale=1.):
 
     Parameters
     ----------
-    x : float
+    x : float or array of floats
         Pixel coordinate (default is in decimal degrees, but depends on value of scale parameter)
         x/scale has to be degrees.
-    y : float
+    y : float or array of floats
         Pixel coordinate (default is in decimal degrees, but depends on value of scale parameter)
         x/scale has to be degrees.
     ra_ref : float
@@ -91,7 +96,10 @@ def deproject_from_tangent_plane(x, y, ra_ref, dec_ref, scale=1.):
 
     """
     # for zenithal projections, i.e. gnomonic, i.e. TAN
-    lonpole = 180.
+    if isinstance(ra_ref, u.Quantity):
+        lonpole = 180. * u.deg
+    else:
+        lonpole = 180.
 
     x = x / scale
     y = y / scale
@@ -107,4 +115,11 @@ def deproject_from_tangent_plane(x, y, ra_ref, dec_ref, scale=1.):
     # ra and dec
     ra, dec = rot_for_tan(phi, theta)
 
+    if unwrap:
+        if (np.ndim(ra) == 0):
+            if (ra > 180.):
+                ra -= 360.
+        else:
+            ra[ra > 180.] -= 360.
+
     return ra, dec
diff --git a/pysiaf/utils/rotations.py b/pysiaf/utils/rotations.py
index d677c426..ee9c84da 100755
--- a/pysiaf/utils/rotations.py
+++ b/pysiaf/utils/rotations.py
@@ -3,18 +3,23 @@
 Authors
 -------
     Colin Cox
+    Johannes Sahlmann
 
 """
 from __future__ import absolute_import, print_function, division
+import copy
 import numpy as np
 
+import astropy.units as u
+from astropy.modeling.rotations import rotation_matrix
+
 
 def attitude(v2, v3, ra, dec, pa):
     """Return rotation matrix that transforms from v2,v3 to RA,Dec.
 
     Makes a 3D rotation matrix which rotates a unit vector representing a v2,v3 position
     to a unit vector representing an RA, Dec pointing with an assigned position angle
-    Described in JWST-STScI-001550, SM-12, section 6.1.
+    Described in JWST-STScI-001550, SM-12, section 5.1.
 
     Parameters
     ----------
@@ -32,7 +37,7 @@ def attitude(v2, v3, ra, dec, pa):
     Returns
     -------
     m : numpy matrix
-        A (3 x 3) matrixrepresents the attitude of the telescope which points the given
+        A (3 x 3) matrix represents the attitude of the telescope which points the given
         V2V3 position to the indicated RA and Dec and with the V3 axis rotated by position angle pa
 
     """
@@ -55,7 +60,106 @@ def attitude(v2, v3, ra, dec, pa):
     return m
 
 
-def axial_rotation(ax, phi, u):
+def convert_quantity(x_in, to_unit, factor=1.):
+    """Check if astropy quantity and apply conversion factor
+
+    Parameters
+    ----------
+    x_in : float or quantity
+        input
+    to_unit : astropy.units unit
+        unit to convert to
+    factor : float
+        Factor to apply if input is not a quantity
+
+    Returns
+    -------
+    x_out : float
+        converted value
+
+    """
+    x = copy.deepcopy(x_in)
+    if isinstance(x, u.Quantity):
+        x_out = x.to(to_unit).value
+    else:
+        x_out = x * factor
+    return x_out
+
+
+def attitude_matrix(nu2, nu3, ra, dec, pa, convention='JWST'):
+    """Return attitude matrix.
+
+    Makes a 3D rotation matrix that transforms between telescope frame
+    and sky. It rotates a unit vector on the idealized focal sphere
+    (specified by the spherical coordinates nu2, nu3) to a unit vector
+    representing an RA, Dec pointing with an assigned position angle
+    measured at nu2, nu3.
+    See JWST-STScI-001550, SM-12, section 5.1.
+
+    Parameters
+    ----------
+    nu2 : float
+        an euler angle (default unit is arc-seconds)
+    nu3 : float
+        an euler angle (default unit is arc-seconds)
+    ra : float
+        Right Ascension on the sky in degrees
+    dec : float
+        Declination on the sky in degrees
+    pa : float
+        Position angle of V3 axis at nu2,nu3 measured from
+        North to East (default unit is degree)
+
+    Returns
+    -------
+    m : numpy matrix
+        the attitude matrix
+
+    """
+    if convention == 'JWST':
+        pa_sign = -1.
+
+    if isinstance(nu2, u.Quantity):
+        nu2_value = nu2.to(u.deg).value
+    else:
+        nu2_value = nu2 / 3600.
+
+    if isinstance(nu3, u.Quantity):
+        nu3_value = nu3.to(u.deg).value
+    else:
+        nu3_value = nu3 / 3600.
+
+    if isinstance(pa, u.Quantity):
+        pa_value = pa.to(u.deg).value
+    else:
+        pa_value = pa
+    if isinstance(ra, u.Quantity):
+        ra_value = ra.to(u.deg).value
+    else:
+        ra_value = ra
+    if isinstance(dec, u.Quantity):
+        dec_value = dec.to(u.deg).value
+    else:
+        dec_value = dec
+
+    # Get separate rotation matrices
+    # astropy's rotation matrix takes inverse sign compared to rotations.rotate
+    mv2 = rotation_matrix(-1*-nu2_value, axis='z')
+    mv3 = rotation_matrix(-1*nu3_value, axis='y')
+    mra = rotation_matrix(-1*ra_value, axis='z')
+    mdec = rotation_matrix(-1*-dec_value, axis='y')
+    mpa = rotation_matrix(-1*pa_sign*pa_value, axis='x')
+
+    # Combine as mra*mdec*mpa*mv3*mv2
+    m = np.dot(mv3, mv2)
+    m = np.dot(mpa, m)
+    m = np.dot(mdec, m)
+    m = np.dot(mra, m)
+
+    return m
+
+
+def axial_rotation(ax, phi, vector):
     """Apply direct rotation to a vector using Rodrigues' formula.
 
     Parameters
@@ -64,7 +168,7 @@ def axial_rotation(ax, phi, u):
         a unit vector represent a rotation axis
     phi : float
         angle in degrees to rotate original vector
-    u : float
+    vector : float
         array of size 3 representing any vector
 
     Returns
@@ -74,10 +178,59 @@ def axial_rotation(ax, phi, u):
 
     """
     rphi = np.radians(phi)
-    v = u*np.cos(rphi) + cross(ax, u) * np.sin(rphi) + ax * np.dot(ax, u) * (1-np.cos(rphi))
+    v = vector*np.cos(rphi) + cross(ax, vector) * np.sin(rphi) + ax * np.dot(ax, vector) * (1-np.cos(rphi))
     return v
 
 
+def sky_to_tel(attitude, ra, dec, verbose=False): #, return_cartesian=False):
+    """Transform from sky (RA, Dec) to telescope (nu2, nu3) angles.
+
+    Return nu2,nu3 position on the idealized focal sphere of any RA and
+    Dec using the inverse of attitude matrix.
+
+    Parameters
+    ----------
+    attitude : 3 by 3 float array
+        The attitude matrix.
+    ra : float (default unit is degree)
+        RA of sky position
+    dec : float (default unit is degree)
+        Dec of sky position
+
+    Returns
+    -------
+    nu2, nu3 : tuple of floats with quantity
+        spherical coordinates at matching position on the idealized focal sphere
+
+    """
+    # ra = convert_quantity(ra, u.deg)
+    # dec = convert_quantity(dec, u.deg)
+    if attitude.shape != (3,3):
+        raise ValueError('Attitude has to be 3x3 array.')
+
+    # if return_cartesian:
+    #     ra_rad = np.deg2rad(ra)
+    #     dec_rad = np.deg2rad(dec)
+    #     urd = np.array([np.sqrt(1. - (ra_rad ** 2 + dec_rad ** 2)), ra_rad, dec_rad])
+    # else:
+    #     urd = unit(ra, dec)
+
+    unit_vector_sky_side = unit_vector_sky(ra, dec)
+    if verbose:
+        print('Sky-side unit vector: {}'.format(unit_vector_sky_side))
+    inverse_attitude = np.transpose(attitude)
+
+    # apply transformation
+    unit_vector_tel = np.dot(inverse_attitude, unit_vector_sky_side)
+    if verbose:
+        print('Tel-side unit vector: {}'.format(unit_vector_tel))
+
+    # extract spherical coordinates
+    nu2, nu3 = polar_angles(unit_vector_tel)
+
+    return nu2, nu3
+
+
 def getv2v3(attitude, ra, dec):
     """Return v2,v3 position of any RA and Dec using the inverse of attitude matrix.
 
@@ -97,8 +250,10 @@ def getv2v3(attitude, ra, dec):
 
     """
     urd = unit(ra, dec)
+
     inverse_attitude = np.transpose(attitude)
     uv = np.dot(inverse_attitude, urd)
+
     v2, v3 = v2v3(uv)
 
     return v2, v3
@@ -126,7 +281,7 @@ def cross(a, b):
     return c
 
 
-def pointing(attitude, v2, v3, positive_ra=True, verbose=False):
+def pointing(attitude, v2, v3, positive_ra=True, input_cartesian=False):
     """Calculate where a v2v3 position points on the sky using the attitude matrix.
 
     Parameters
@@ -148,20 +303,75 @@ def pointing(attitude, v2, v3, positive_ra=True, verbose=False):
     """
     v2d = v2 / 3600.0
     v3d = v3 / 3600.0
-    if verbose:
-        print('POINTING v2v3')
-        print(v2)
-        print(v3)
-        print(v2d)
-        print(v3d)
-    v = unit(v2d, v3d)
+
+    # compute unit vector
+    if input_cartesian:
+        v2_rad = np.deg2rad(v2d)
+        v3_rad = np.deg2rad(v3d)
+        v = np.array([np.sqrt(1. - (v2_rad ** 2 + v3_rad ** 2)), v2_rad, v3_rad])
+    else:
+        v = unit(v2d, v3d)
+
+    # apply attitude transformation
     w = np.dot(attitude, v)
 
-    # tuple containing ra and dec in degrees
-    rd = radec(w, positive_ra=positive_ra)
+    # compute tuple containing ra and dec in degrees
+    if input_cartesian:
+        rd = np.rad2deg(w[1]), np.rad2deg(w[2])
+    else:
+        rd = radec(w, positive_ra=positive_ra)
+
     return rd
 
 
+def tel_to_sky(attitude, nu2, nu3, positive_ra=True):#, input_cartesian=False):
+    """Calculate where a nu2,nu3 position points on the sky.
+
+    Parameters
+    ----------
+    attitude : 3 by 3 float array
+        the telescope attitude matrix
+    nu2 : float or array of floats (default unit is arcsecond)
+        V2 coordinate in arc-seconds
+    nu3 : float or array of floats (default unit is arcsecond)
+        V3 coordinate in arc-seconds
+    positive_ra : bool.
+        If True forces ra value to be positive
+
+    Returns
+    -------
+    rd : tuple of floats with quantity
+        (ra, dec) - RA and Dec
+
+    """
+
+    nu2_deg = convert_quantity(nu2, u.deg, factor=u.arcsec.to(u.deg))
+    nu3_deg = convert_quantity(nu3, u.deg, factor=u.arcsec.to(u.deg))
+
+    # v2d = v2 / 3600.0
+    # v3d = v3 / 3600.0
+
+    # # compute unit vector
+    # if input_cartesian:
+    #     v2_rad = np.deg2rad(v2d)
+    #     v3_rad = np.deg2rad(v3d)
+    #     v = np.array([np.sqrt(1. - (v2_rad ** 2 + v3_rad ** 2)), v2_rad, v3_rad])
+    # else:
+    #     v = unit(v2d, v3d)
+    unit_vector_tel = unit_vector_sky(nu2_deg, nu3_deg)
+
+    # apply attitude transformation
+    unit_vector_sky_side = np.dot(attitude, unit_vector_tel)
+
+    # compute tuple containing ra and dec in degrees
+    # if input_cartesian:
+    #     rd = np.rad2deg(w[1]), np.rad2deg(w[2])
+    # else:
+    #     rd = radec(w, positive_ra=positive_ra)
+    ra, dec = polar_angles(unit_vector_sky_side, positive_azimuth=positive_ra)
+    return ra, dec
+
+
 def posangle(attitude, v2, v3):
     """Return the V3 angle at arbitrary v2,v3 using the attitude matrix.
 
@@ -194,36 +404,6 @@ def posangle(attitude, v2, v3):
     return pa
 
 
-def radec(u, positive_ra=False):
-    """Return RA and Dec in degrees corresponding to the unit vector u.
-
-    Parameters
-    ----------
-    u : a float array or list of length 3
-        represents a unit vector so should have unit magnitude
-        if not, the normalization is forced within the method
-    positive_ra : bool
-        indicating whether to force ra to be positive
-
-    Returns
-    -------
-    ra , dec : tuple of floats
-        RA and Dec in degrees corresponding to the unit vector u
-
-    """
-    assert len(u) == 3, 'Not a vector'
-    norm = np.sqrt(u[0] ** 2 + u[1] ** 2 + u[2] ** 2)  # Works for list or array
-    dec = np.degrees(np.arcsin(u[2] / norm))
-    ra = np.degrees(np.arctan2(u[1], u[0]))  # atan2 puts it in the correct quadrant
-    if positive_ra:
-        if np.isscalar(ra) and ra < 0.0:
-            ra += 360.0
-        if not np.isscalar(ra) and np.any(ra < 0.0):
-            index = np.where(ra < 0.0)[0]
-            ra[index] += 360.0
-    return ra, dec
-
-
 def rodrigues(attitude):
     """Interpret rotation matrix as a single rotation by angle phi around unit length axis.
 
@@ -390,7 +570,9 @@ def slew(v2t, v3t, v2a, v3a):
 
 
 def unit(ra, dec):
-    """Convert vector expressed in Euler angles to unit vector components.
+    """Convert inertial frame vector expressed in polar coordinates / Euler angles to unit vector components.
+
+    See Section 5 of JWST-STScI-001550 and Equation 4.1 of JWST-PLAN-006166.
 
     Parameters
     ----------
@@ -401,32 +583,229 @@ def unit(ra, dec):
 
     Returns
     -------
-    u : float array of length 3
-        the equivalent unit vector
+    vector : float array of length 3
+        the equivalent unit vector in the inertial frame
 
     """
-    rar = np.radians(ra)
-    decr = np.radians(dec)
-    u = np.array([np.cos(rar)*np.cos(decr), np.sin(rar)*np.cos(decr), np.sin(decr)])
-    return u
+    rar = np.deg2rad(ra)
+    decr = np.deg2rad(dec)
+    vector = np.array([np.cos(rar)*np.cos(decr), np.sin(rar)*np.cos(decr), np.sin(decr)])
+    return vector
+
+
+def unit_vector_sky(ra, dec):
+    """Return unit vector on the celestial sphere.
+
+    Parameters
+    ----------
+    ra : float or array of floats (default unit is degree)
+        RA of sky position
+    dec : float or array of floats (default unit is degree)
+        Dec of sky position
+
+    Returns
+    -------
+    vector : float array of length 3
+        the equivalent unit vector in the inertial frame
+
+    """
+    ra_rad = convert_quantity(ra, u.rad, factor=np.deg2rad(1.))
+    dec_rad = convert_quantity(dec, u.rad, factor=np.deg2rad(1.))
+    vector = np.array([np.cos(ra_rad)*np.cos(dec_rad), np.sin(ra_rad)*np.cos(dec_rad), np.sin(dec_rad)])
+    return vector
+
+
+def unit_vector_hst_fgs_object(rho, phi):
+    """Return unit vector on the celestial sphere.
+
+    This is according to the HST object space angle definitions,
+    CSC/TM-82/6045 1987, Section 4.1.2.2.4
+
+    Parameters
+    ----------
+    rho : float or array of floats (default unit is degree)
+        RA of sky position
+    phi : float or array of floats (default unit is degree)
+        Dec of sky position
+
+    Returns
+    -------
+    vector : float array of length 3
+        the equivalent unit vector in the inertial frame
+
+    """
+    rho_rad = convert_quantity(rho, u.rad, factor=np.deg2rad(1.))
+    phi_rad = convert_quantity(phi, u.rad, factor=np.deg2rad(1.))
+    vector = np.array([np.sin(rho_rad)*np.cos(phi_rad), np.sin(rho_rad)*np.sin(phi_rad), np.cos(rho_rad)])
+    return vector
+
+
+def radec(vector, positive_ra=False):
+    """Return RA and Dec in degrees corresponding to the unit vector vector.
+
+    Parameters
+    ----------
+    vector : a float array or list of length 3
+        represents a unit vector so should have unit magnitude
+        if not, the normalization is forced within the method
+    positive_ra : bool
+        indicating whether to force ra to be positive
 
+    Returns
+    -------
+    ra , dec : tuple of floats
+        RA and Dec in degrees corresponding to the unit vector vector
+
+    """
+    if len(vector) != 3:
+        raise ValueError('Input is not a 3D vector')
+    norm = np.sqrt(vector[0] ** 2 + vector[1] ** 2 + vector[2] ** 2)  # Works for list or array
+    ra = np.degrees(np.arctan2(vector[1], vector[0]))  # atan2 puts it in the correct quadrant
+    dec = np.degrees(np.arcsin(vector[2] / norm))
+    if positive_ra:
+        if np.isscalar(ra) and ra < 0.0:
+            ra += 360.0
+        if not np.isscalar(ra) and np.any(ra < 0.0):
+            index = np.where(ra < 0.0)[0]
+            ra[index] += 360.0
+    return ra, dec
 
-def v2v3(u):
-    """Convert unit vector to v2v3.
+
+def v2v3(vector):
+    """Compute v2,v3 polar coordinates corresponding to a unit vector in the rotated (telescope) frame.
+
+    See Section 5 of JWST-STScI-001550.
 
     Parameters
     ----------
-    u : float list or array of length 3
-        a unit vector.
+    vector : float list or array of length 3
+        unit vector of cartesian coordinates in the rotated (telescope) frame
 
     Returns
     -------
-    v2, v3 : tuple of floate
-        The same position represented by V2,V3 values in arc-sec.
+    v2, v3 : tuple of floats
+        The same position represented by V2, V3 values in arc-sec.
 
     """
-    assert len(u) == 3, 'Not a vector'
-    norm = np.sqrt(u[0]**2 + u[1]**2 + u[2]**2)  # Works for list or array
-    v2 = 3600*np.degrees(np.arctan2(u[1], u[0]))  # atan2 puts it in the correct quadrant
-    v3 = 3600*np.degrees(np.arcsin(u[2]/norm))
+    assert len(vector) == 3, 'Not a vector'
+
+    norm = np.sqrt(vector[0]**2 + vector[1]**2 + vector[2]**2)
+    v2 = 3600. * np.degrees(np.arctan2(vector[1], vector[0]))  # atan2 puts it in the correct quadrant
+    v3 = 3600. * np.degrees(np.arcsin(vector[2]/norm))
     return v2, v3
+
+
+def polar_angles(vector, positive_azimuth=False):
+    """Compute polar coordinates of an unit vector.
+
+    Parameters
+    ----------
+    vector : float list or array of length 3
+        3-component unit vector
+    positive_azimuth : bool
+        If True, the returned nu2 value is forced to be positive.
+
+    Returns
+    -------
+    nu2, nu3 : tuple of floats with astropy quantity
+        The same position represented by polar coordinates
+
+    """
+    if len(vector) != 3:
+        raise ValueError('Input is not a vector or an array of vectors')
+
+    norm = np.sqrt(vector[0]**2 + vector[1]**2 + vector[2]**2)
+    nu2 = np.arctan2(vector[1], vector[0]) * u.rad
+    nu3 = np.arcsin(vector[2]/norm) * u.rad
+
+    if positive_azimuth:
+        if np.isscalar(nu2.value) and nu2.value < 0.0:
+            nu2 += 360.0 * u.deg
+        if not np.isscalar(nu2.value) and np.any(nu2.value < 0.0):
+            index = np.where(nu2.value < 0.0)[0]
+            nu2[index] += 360.0 * u.deg
+    return nu2, nu3
+
+
+def unit_vector_from_cartesian(x=None, y=None, z=None):
+    """Return unit vector corresponding to two cartesian coordinates.
+
+    Array inputs are supported.
+
+    Parameters
+    ----------
+    x : float or quantity
+        cartesian unit vector X coordinate in radians
+    y : float or quantity
+        cartesian unit vector Y coordinate in radians
+    z : float or quantity
+        cartesian unit vector Z coordinate in radians
+
+    Returns
+    -------
+    unit_vector : numpy.ndarray
+        Unit vector
+
+    """
+    # check that two arguments are provided
+    if x is None:
+        if y is None or z is None:
+            raise TypeError('Function requires axactly two arguments.')
+    if y is None:
+        if x is None or z is None:
+            raise TypeError('Function requires axactly two arguments.')
+    if z is None:
+        if x is None or y is None:
+            raise TypeError('Function requires axactly two arguments.')
+
+    # convert to radian
+    if isinstance(x, u.Quantity):
+        x_rad = x.to(u.rad).value
+    else:
+        x_rad = x
+
+    if isinstance(y, u.Quantity):
+        y_rad = y.to(u.rad).value
+    else:
+        y_rad = y
+
+    if isinstance(z, u.Quantity):
+        z_rad = z.to(u.rad).value
+    else:
+        z_rad = z
+
+    # handle array and scalar inputs
+    if np.array([x_rad]).all() and np.array([y_rad]).all():
+        unit_vector = np.array([x_rad, y_rad, np.sqrt(1-(x_rad**2+y_rad**2))])
+    elif np.array([y_rad]).all() and np.array([z_rad]).all():
+        unit_vector = np.array([np.sqrt(1-(y_rad**2+z_rad**2)), y_rad, z_rad])
+    elif np.array([x_rad]).all() and np.array([z_rad]).all():
+        unit_vector = np.array([x_rad, np.sqrt(1-(x_rad**2+z_rad**2)), z_rad])
+
+    if np.any(np.isnan(unit_vector)):
+        raise ValueError('Invalid arguments. Inputs should be in radians.')
+
+    return unit_vector
+
+
+
+def idl_to_tel_rotation_matrix(V2Ref_arcsec, V3Ref_arcsec, V3IdlYAngle_deg):
+    """Return 3D rotation matrix for ideal to telescope transformation.
+
+    Parameters
+    ----------
+    V2Ref_arcsec
+    V3Ref_arcsec
+    V3IdlYAngle_deg
+
+    Returns
+    -------
+
+    """
+    M1 = rotate(3, -1 * V2Ref_arcsec / 3600.)
+    M2 = rotate(2, V3Ref_arcsec / 3600.)
+    M3 = rotate(1, V3IdlYAngle_deg)
+    M4 = np.dot(M2, M1)
+    M = np.dot(M3, M4)
+
+    return M
diff --git a/pysiaf/utils/tools.py b/pysiaf/utils/tools.py
index 9e3cfda1..d59ec6d4 100644
--- a/pysiaf/utils/tools.py
+++ b/pysiaf/utils/tools.py
@@ -9,12 +9,16 @@
 import copy
 import math
 from math import sin, cos, atan2, degrees, radians
+
+from astropy.table import Table
 import numpy as np
 
 from ..constants import V3_TO_YAN_OFFSET_DEG
 from ..iando import read
 from .polynomial import shift_coefficients, flip_y, flip_x, add_rotation, \
     prepend_rotation_to_polynomial, poly, print_triangle
+from ..siaf import Siaf
+from ..utils import rotations
 
 
 def an_to_tel(xan_arcsec, yan_arcsec):
@@ -278,7 +282,74 @@ def correct_V3SciYAngle(V3SciYAngle_deg):
     return V3SciYAngle_deg_corrected
 
 
-def get_grid_coordinates(n_side, centre, x_width, y_width=None):
+def jwst_fgs_to_fgs_matrix(direction='fgs2_to_fgs1', siaf=None, verbose=False):
+    """Return JWST FGS1_OSS to FGS2_OSS transformation matrix as stored in LoadsPRD.
+
+    Parameters
+    ----------
+    siaf : pysiaf.Siaf instance
+        JWST FGS SIAF content
+
+    Returns
+    -------
+    R12 : ndarray
+        rotation matrix
+
+    References
+    ----------
+        See JWST-PLAN-006166, Section 5.8.3 for definitions.
+
+        Implementation adapted from Cox' BetweenFGS.ipynb
+
+    """
+    if siaf is None:
+        siaf = Siaf('fgs')
+
+    if direction=='fgs2_to_fgs1':
+        fgs1 = siaf['FGS2_FULL_OSS']
+        fgs2 = siaf['FGS1_FULL_OSS']
+    elif direction=='fgs1_to_fgs2':
+        fgs1 = siaf['FGS1_FULL_OSS']
+        fgs2 = siaf['FGS2_FULL_OSS']
+
+    FGS1V2 = fgs1.V2Ref
+    FGS1V3 = fgs1.V3Ref
+    FGS1pa = fgs1.V3IdlYAngle
+
+    FGS2V2 = fgs2.V2Ref
+    FGS2V3 = fgs2.V3Ref
+    FGS2pa = fgs2.V3IdlYAngle
+
+    # Form RA = R3.R2.R1
+    R1 = rotations.rotate(1, -FGS1pa)
+    R2 = rotations.rotate(2, -FGS1V3 / 3600.0)
+    R3 = rotations.rotate(3, FGS1V2 / 3600.0)
+    RA = np.dot(R2, R1)
+    RA = np.dot(R3, RA)
+
+    # Form RB = R6.R5.R4
+    R4 = rotations.rotate(1, -FGS2pa)
+    R5 = rotations.rotate(2, -FGS2V3 / 3600.0)
+    R6 = rotations.rotate(3, FGS2V2 / 3600.0)
+    RB = np.dot(R5, R4)
+    RB = np.dot(R6, RB)
+
+    R12 = np.dot(np.transpose(RB), RA)
+    R21 = np.dot(np.transpose(RA), RB)
+    if verbose:
+        print('RA\n', RA)
+        print('RB\n', RB)
+        if direction == 'fgs1_to_fgs2':
+            print('\nTransform from FGS1_OSS to FGS2_OSS\nR12\n', R12)
+            print('\nTransform from FGS2_OSS to FGS1_OSS\nR21\n', R21)
+        elif direction == 'fgs2_to_fgs1':
+            print('\nTransform from FGS2_OSS to FGS1_OSS\nR12\n', R12)
+            print('\nTransform from FGS1_OSS to FGS2_OSS\nR21\n', R21)
+
+    return R12
+
+
+def get_grid_coordinates(n_side, centre, x_width, y_width=None, max_radius=None):
     """Return tuple of arrays that contain the coordinates on a regular grid.
 
     Parameters
@@ -309,6 +380,10 @@ def get_grid_coordinates(n_side, centre, x_width, y_width=None):
     x = x_mesh.flatten()
     y = y_mesh.flatten()
 
+    if max_radius is not None:
+        index = np.where(np.sqrt((x-centre[0])**2+(y-centre[1])**2) < max_radius)[0]
+        return x[index], y[index]
+
     return x, y
 
 
@@ -478,6 +553,43 @@ def set_reference_point_and_distortion(instrument, aperture, parent_aperture):
     return aperture
 
 
+def write_matrix_to_file(matrix, file, comments=None, format='jwst_fsw_patch_request'):
+    """Write the elements of a matrix to a text file.
+
+    Parameters
+    ----------
+    matrix : ndarray
+        the matrix
+    file : str
+        output file name
+    comments : dict
+        comments to include in the commented file header
+    format : str
+        Formatting of matrix elements in output
+
+    """
+    if format=='jwst_fsw_patch_request':
+        index = []
+        value = []
+        for row_index in range(matrix.shape[0]):
+            for column_index in range(matrix.shape[1]):
+                index.append('[{}][{}]'.format(row_index, column_index))
+                value.append('{:+.15f}'.format(matrix[row_index, column_index]))
+        table = Table()
+        table['[Row][Column]'] = index
+        table['Value'] = value
+        if comments is not None:
+            table.meta['comments'] = comments
+        table.write(file, format='ascii.fixed_width', delimiter=',', bookend=False, overwrite=True)
+
+    elif format is None:
+        table = Table(matrix)
+        if comments is not None:
+            table.meta['comments'] = comments
+        fstring = '{' + ','.join(["'{}':  '%.15f'".format(c) for c in table.colnames]) + '}'
+        table.write(file, format='ascii.fixed_width', delimiter=',', bookend=False, overwrite=True, formats=eval(fstring))
+
+
 def v3sciyangle_to_v3idlyangle(v3sciyangle):
     """Convert V3SciYAngle to V3IdlYAngle.