Merge remote-tracking branch 'origin/latpmv2' into depth1_mapmaker

Author: chervias

sotodlib/coords/pointing_model.py

@@ -125,11 +125,9 @@ def apply_pointing_model_lat(vers, params, ancil):
 
     if vers == 'lat_naive':
         return _new_boresight(ancil.samps, az=az, el=el, roll=roll)
-        
-    if vers == "lat_v1":
+    elif vers == "lat_v1":
         az1, el1, roll1 = model_lat_v1(params, az, el, roll)
         return _new_boresight(ancil.samps, az=az1, el=el1, roll=roll1)
-
     else:
         raise ValueError(f'Unimplemented pointing model "{vers}"')
 
@@ -156,6 +154,10 @@ def model_lat_v1(params, az, el, roll):
     - mir_center_{xi,eta}0: The (xi,eta) coordinate in the El-structure-centered
       focal plane that appears fixed when the mirrors are rotated about the ray from
       sky that hits the center of both mirrors.
+    - base_tilt_{cos,sin}: Base tilt coefficients, in radians. 
+    - el_sag_{quad,lin}: Dimensionless coefficients for the quadradtic
+      and linear components of the elevation sag.
+    - el_sag_pivot: The elevation in radians to treat as the sag's zero point.
 
     """
     _p = dict(param_defaults['lat_v1'])
@@ -217,7 +219,26 @@ def model_lat_v1(params, az, el, roll):
         * q_el_roll * q_el_axis_center
         * q_cr_roll * q_cr_center
     )
-    new_az, el, roll = quat.decompose_lonlat(q_hs)* np.array([-1, 1, 1])[..., None]
+    az, el, roll = quat.decompose_lonlat(q_hs)* np.array([-1, 1, 1])[..., None]
+    cr = el - roll - np.deg2rad(60)    
+    
+    # Base tilt
+    q_base_tilt = get_base_tilt_q(params['base_tilt_cos'], params['base_tilt_sin'])
+
+    # El sag
+    el_sag = params['el_sag_quad']*(el - params['el_sag_pivot'])**2
+    el_sag += params['el_sag_lin']*(el - params['el_sag_pivot'])
+    el += el_sag
+
+    # Lonlat rotation after v1 model and el sag is applied 
+    q_lonlat = quat.rotation_lonlat(-1 * az, el) * quat.euler(2, roll)
+
+    # Horizon Coordinates
+    q_hs = q_base_tilt * q_lonlat
+    new_az, el, _ = quat.decompose_lonlat(q_hs)* np.array([-1, 1, 1])[..., None]
+
+    # Get new roll with the modified el
+    roll = el - cr - np.deg2rad(60)
 
     # Make corrected az as close as possible to the input az.    
     change = ((new_az - az_orig) + np.pi) % (2 * np.pi) - np.pi
@@ -318,6 +339,11 @@ def model_sat_v1(params, az, el, roll):
         'cr_center_eta0': 0,
         'mir_center_xi0': 0,
         'mir_center_eta0': 0,
+        'base_tilt_cos': 0,
+        'base_tilt_sin': 0,
+        'el_sag_quad': 0,
+        'el_sag_lin': 0,
+        'el_sag_pivot': np.pi/2.,
     },
     'sat_v1' : {
         'enc_offset_az': 0.,

tests/test_pointing_model.py

@@ -145,5 +145,22 @@ def tpoint_base_tilt(an, aw, az, el):
             assert(abs(sig_meas - sig * DEG) < .001*DEG)
 
 
+    def test_lat_v1(self):
+        az, el, roll = full_vectors(az=np.linspace(-90, 90, 100),
+                                    el=60)
+        params = {'version': 'lat_v1'}
+        az1, el1, roll1 = to_deg(*pm.model_lat_v1(params, *to_rad(az, el, roll)))
+
+        # Backwards compatibility for not providing el sag or base tilt
+        params['enc_offset_az'] = 1. * DEG
+        az1, el1, roll1 = to_deg(*pm.model_lat_v1(params, *to_rad(az, el, roll)))
+        assert np.all(center_branch(az1 - az) > 0)
+
+        # Try an el sag
+        params['el_sag_lin'] = 1
+        az2, el2, roll2 = to_deg(*pm.model_lat_v1(params, *to_rad(az, el, roll)))
+        assert np.all(np.isclose(el/el2, 2))
+
+
 if __name__ == '__main__':
     unittest.main()