Merge pull request #1162 from simonsobs/fix_pointing_model_pi_issue

Pointing model quat.deconstruct_lonlat issue. 
Fixed right now by minimizing the difference between original azimuth values and the corrected values.

Author: elle-shaw

sotodlib/coords/pointing_model.py

@@ -175,6 +175,7 @@ def model_sat_v1(params, az, el, roll):
             raise ValueError(f'Handling of model param "{k}" is not implemented.')
 
     # Construct offsetted encoders.
+    az_orig = az.copy()
     az_twist = params['az_rot'] * (el + params['enc_offset_el'])
     az = az + params['enc_offset_az'] + az_twist
     el = el + params['enc_offset_el'] 
@@ -195,7 +196,12 @@ def model_sat_v1(params, az, el, roll):
             * q_fp_offset * ~q_fp_rot * quat.euler(2, roll) * q_fp_rot)
 
     neg_az, el, roll = quat.decompose_lonlat(q_hs)
-    return -neg_az, el, roll
+
+    # Make corrected az as close as possible to the input az.
+    change = ((-neg_az - az_orig) + np.pi) % (2 * np.pi) - np.pi
+    new_az = az_orig + change
+
+    return new_az, el, roll
 
 
 # Support functions