replace secant method with Newton method for zero-doppler time computation

sarsen/apps.py

@@ -10,24 +10,20 @@
 
 logger = logging.getLogger(__name__)
 
-
-SPEED_OF_LIGHT = 299_792_458.0  # m / s
+from scipy.constants import speed_of_light
 
 
 def simulate_acquisition(
     dem_ecef: xr.DataArray,
-    position_ecef: xr.DataArray,
+    pos: xr.DataArray,
+    azimuth_time: Optional[xr.DataArray] = None,
     include_variables: Container[str] = (),
+    **kwargs,
 ) -> xr.Dataset:
     """Compute the image coordinates of the DEM given the satellite orbit."""
-    orbit_interpolator = orbit.OrbitPolyfitIterpolator.from_position(position_ecef)
-    position_ecef = orbit_interpolator.position()
-    velocity_ecef = orbit_interpolator.velocity()
-
-    acquisition = geocoding.backward_geocode(dem_ecef, position_ecef, velocity_ecef)
-
+    acquisition = geocoding.backward_geocode(dem_ecef, pos, t0=azimuth_time)
     slant_range = (acquisition.dem_distance**2).sum(dim="axis") ** 0.5
-    slant_range_time = 2.0 / SPEED_OF_LIGHT * slant_range
+    slant_range_time = 2.0 / speed_of_light * slant_range
 
     acquisition["slant_range_time"] = slant_range_time
 
@@ -39,19 +35,19 @@ def simulate_acquisition(
 
     for data_var_name in acquisition.data_vars:
         if include_variables and data_var_name not in include_variables:
-            acquisition = acquisition.drop_vars(data_var_name)  # type: ignore
+            acquisition = acquisition.drop_vars(data_var_name)
 
     # drop coordinates that are not associated with any data variable
     for coord_name in acquisition.coords:
         if all(coord_name not in dv.coords for dv in acquisition.data_vars.values()):
-            acquisition = acquisition.drop_vars(coord_name)  # type: ignore
+            acquisition = acquisition.drop_vars(coord_name)
 
     return acquisition
 
 
 def map_simulate_acquisition(
     dem_ecef: xr.DataArray,
-    position_ecef: xr.DataArray,
+    pos: xr.DataArray,
     template_raster: xr.DataArray,
     correct_radiometry: Optional[str] = None,
 ) -> xr.Dataset:
@@ -70,7 +66,7 @@ def map_simulate_acquisition(
         simulate_acquisition,
         dem_ecef,
         kwargs={
-            "position_ecef": position_ecef,
+            "pos": pos,
             "include_variables": include_variables,
         },
         template=acquisition_template,
@@ -90,16 +86,23 @@ def do_terrain_correction(
     logger.info("pre-process DEM")
 
     dem_ecef = xr.map_blocks(
-        scene.convert_to_dem_ecef, dem_raster, kwargs={"source_crs": dem_raster.rio.crs}
+        scene.convert_to_dem_ecef, dem_raster, kwargs={
+            "source_crs": dem_raster.rio.crs}
     )
     dem_ecef = dem_ecef.drop_vars(dem_ecef.rio.grid_mapping)
 
     logger.info("simulate acquisition")
 
     template_raster = dem_raster.drop_vars(dem_raster.rio.grid_mapping) * 0.0
-
+
+    orbit_ecef = product.state_vectors()
+    pos = orbit.OrbitPolyfitIterpolator.from_position(
+        orbit_ecef, poly_type='polynomial', deg=min(14, orbit_ecef.azimuth_time.size - 5))
+
+    pos.coefficients = pos.coefficients.assign_attrs({"referenceTime": pos.epoch, "scaleTime": 1e-9})
+
     acquisition = map_simulate_acquisition(
-        dem_ecef, product.state_vectors(), template_raster, correct_radiometry
+        dem_ecef, pos.coefficients, template_raster, correct_radiometry
     )
 
     simulated_beta_nought = None

sarsen/geocoding.py

@@ -2,103 +2,72 @@
 
 See: https://sentinel.esa.int/documents/247904/0/Guide-to-Sentinel-1-Geocoding.pdf/e0450150-b4e9-4b2d-9b32-dadf989d3bd3
 """
-import functools
-from typing import Any, Callable, Optional, Tuple, TypeVar
 
 import numpy as np
 import numpy.typing as npt
+import numpy.polynomial.polynomial as poly
 import xarray as xr
 
-TimedeltaArrayLike = TypeVar("TimedeltaArrayLike", bound=npt.ArrayLike)
-FloatArrayLike = TypeVar("FloatArrayLike", bound=npt.ArrayLike)
-
-
-def secant_method(
-    ufunc: Callable[[TimedeltaArrayLike], Tuple[FloatArrayLike, FloatArrayLike]],
-    t_prev: TimedeltaArrayLike,
-    t_curr: TimedeltaArrayLike,
-    diff_ufunc: float = 1.0,
-    diff_t: np.timedelta64 = np.timedelta64(0, "ns"),
-) -> Tuple[TimedeltaArrayLike, TimedeltaArrayLike, FloatArrayLike, Any]:
-    """Return the root of ufunc calculated using the secant method."""
-    # implementation modified from https://en.wikipedia.org/wiki/Secant_method
-    f_prev, _ = ufunc(t_prev)
-
-    # strong convergence, all points below one of the two thresholds
-    while True:
-        f_curr, payload_curr = ufunc(t_curr)
-
-        # the `not np.any` construct let us accept `np.nan` as good values
-        if not np.any((np.abs(f_curr) > diff_ufunc)):
-            break
-
-        t_diff: TimedeltaArrayLike
-        p: TimedeltaArrayLike
-        q: FloatArrayLike
-
-        t_diff = t_curr - t_prev  # type: ignore
-        p = f_curr * t_diff  # type: ignore
-        q = f_curr - f_prev  # type: ignore
-
-        # t_prev, t_curr = t_curr, t_curr - f_curr * np.timedelta64(-148_000, "ns")
-        t_prev, t_curr = t_curr, t_curr - np.where(q != 0, p / q, 0)  # type: ignore
-        f_prev = f_curr
-
-        # the `not np.any` construct let us accept `np.nat` as good values
-        if not np.any(np.abs(t_diff) > diff_t):
-            break
-
-    return t_curr, t_prev, f_curr, payload_curr
-
-
-# FIXME: interpolationg the direction decreses the precision, this function should
-#   probably have velocity_ecef_sar in input instead
-def zero_doppler_plane_distance(
-    dem_ecef: xr.DataArray,
-    position_ecef_sar: xr.DataArray,
-    direction_ecef_sar: xr.DataArray,
-    azimuth_time: TimedeltaArrayLike,
-    dim: str = "axis",
-) -> Tuple[xr.DataArray, Tuple[xr.DataArray, xr.DataArray]]:
-    dem_distance = dem_ecef - position_ecef_sar.interp(azimuth_time=azimuth_time)
-    satellite_direction = direction_ecef_sar.interp(azimuth_time=azimuth_time)
-    plane_distance = (dem_distance * satellite_direction).sum(dim, skipna=False)
-    return plane_distance, (dem_distance, satellite_direction)
-
+MAXITER = 10
 
 def backward_geocode(
     dem_ecef: xr.DataArray,
-    position_ecef: xr.DataArray,
-    velocity_ecef: xr.DataArray,
-    azimuth_time: Optional[xr.DataArray] = None,
+    pos: xr.DataArray,
+    t0: xr.DataArray | np.datetime64 | None = None,
     dim: str = "axis",
-    diff_ufunc: float = 1.0,
+    conv_th: float = 1.0e-6,
 ) -> xr.Dataset:
-    direction_ecef = (
-        velocity_ecef / xr.dot(velocity_ecef, velocity_ecef, dims=dim) ** 0.5
-    )
 
-    zero_doppler = functools.partial(
-        zero_doppler_plane_distance, dem_ecef, position_ecef, direction_ecef
-    )
-
-    if azimuth_time is None:
-        azimuth_time = position_ecef.azimuth_time
-    t_template = dem_ecef.isel({dim: 0}).drop_vars(dim)
-    t_prev = xr.full_like(t_template, azimuth_time.values[0], dtype=azimuth_time.dtype)
-    t_curr = xr.full_like(t_template, azimuth_time.values[-1], dtype=azimuth_time.dtype)
+    assert pos.attrs.get("referenceTime", None) is not None, "orbit reference time not defined"
+    assert pos.attrs.get("scaleTime", None) is not None, "orbit scaling time not defined"
+
+    if (t0 is not None):
+        if isinstance(t0, xr.DataArray):
+            assert t0.size == dem_ecef.isel({dim: 0}).size, "number of guess points different from dem_ecef size"
+
+            t = ((t0 - pos.referenceTime) * pos.scaleTime).astype(float)
+        else:
+            t = xr.full_like(dem_ecef.isel({dim: 0}).drop_vars(dim), (t0 - pos.referenceTime)*pos.scaleTime, dtype=float)
+    else:
+        t = xr.full_like(dem_ecef.isel({dim: 0}).drop_vars(dim), 0, dtype=float)
+
+    # compute orbit polynomial derivatives
+    vel = xr.DataArray(poly.polyder(pos, 1), dims=["degree", dim])
+    acc = xr.DataArray(poly.polyder(vel, 1), dims=["degree", dim])
+
+    vel = vel.assign_coords({"degree": np.arange(vel.degree.size)})
+    acc = acc.assign_coords({"degree": np.arange(acc.degree.size)})
+
+    for k in range(MAXITER):        
+        # compute start point
+        p = xr.polyval(t, pos)
+        v = xr.polyval(t, vel)
+        a = xr.polyval(t, acc)
+
+        # compute range vector
+        r = p - dem_ecef
+
+        # update time
+        F = (v*r).sum(dim=dim)
+        F1 = (a*r).sum(dim=dim)+(v*v).sum(dim=dim)
+        delta = (F / F1)
+
+        maxcorr = np.abs(delta).max().values
+        if (maxcorr < conv_th):
+            break
+
+        t = t - delta
 
-    # NOTE: dem_distance has the associated azimuth_time as a coordinate already
-    _, _, _, (dem_distance, satellite_direction) = secant_method(
-        zero_doppler,
-        t_prev,
-        t_curr,
-        diff_ufunc,
+    direction_ecef = (
+        v / xr.dot(v, v, dims=dim) ** 0.5
     )
+
     acquisition = xr.Dataset(
         data_vars={
-            "dem_distance": dem_distance,
-            "satellite_direction": satellite_direction.transpose(*dem_distance.dims),
+            "azimuth_time": (t / pos.scaleTime).astype("timedelta64[ns]")+pos.referenceTime,
+            "dem_distance": -r,
+            "satellite_direction": direction_ecef,
         }
     )
-    return acquisition.reset_coords("azimuth_time")
+
+    return acquisition

sarsen/orbit.py

@@ -1,4 +1,4 @@
-from typing import Any, Optional, Tuple
+from typing import Any, Optional, Tuple, Literal
 
 import attrs
 import numpy as np
@@ -8,19 +8,25 @@
 S_TO_NS = 10**9
 
 
-def polyder(coefficients: xr.DataArray) -> xr.DataArray:
+def polyder(coefficients: xr.DataArray, poly_type: str) -> xr.DataArray:
     # TODO: raise if "degree" coord is not decreasing
     derivative_coefficients = coefficients.isel(degree=slice(1, None)).copy()
-    for degree in coefficients.coords["degree"].values[:-1]:
-        derivative_coefficients.loc[{"degree": degree - 1}] = (
-            coefficients.loc[{"degree": degree}] * degree
-        )
+    if poly_type == 'polynomial':
+        for degree in coefficients.coords["degree"].values[:-1]:
+            derivative_coefficients.loc[{"degree": degree - 1}] = (
+                coefficients.loc[{"degree": degree}] * degree
+            )
+    elif poly_type == 'hermite':
+        v = [np.polynomial.hermite.Hermite(coefficients.isel(
+            axis=i).values).deriv(m=1).coef for i in range(3)]
+        derivative_coefficients.data = np.vstack(v).T
     return derivative_coefficients
 
 
 @attrs.define
 class OrbitPolyfitIterpolator:
     coefficients: xr.DataArray
+    poly_type: str
     epoch: np.datetime64
     interval: Tuple[np.datetime64, np.datetime64]
 
@@ -31,6 +37,7 @@ def from_position(
         dim: str = "azimuth_time",
         deg: int = 5,
         epoch: Optional[np.datetime64] = None,
+        poly_type: Literal['polynomial', 'hermite'] = 'polynomial',
         interval: Optional[Tuple[np.datetime64, np.datetime64]] = None,
     ) -> "OrbitPolyfitIterpolator":
         time = position.coords[dim]
@@ -45,10 +52,17 @@ def from_position(
             interval = (time.values[0], time.values[-1])
 
         data = position.assign_coords({dim: time - epoch})
-        polyfit_results = data.polyfit(dim=dim, deg=deg)
+        if poly_type == 'polynomial':
+            polyfit_results = data.polyfit(dim=dim, deg=deg)
+        elif poly_type == 'hermite':
+            v = [np.polynomial.hermite.hermfit(((time - epoch)/10**9).astype('float64'),
+                                               data.values[i], deg=deg) for i in range(3)]
+            polyfit_results = xr.Dataset(
+                {'axis': [0, 1, 2], 'degree': np.arange(deg, -1, -1)})
+            polyfit_results = polyfit_results.assign({'polyfit_coefficients':
+                                                      (['degree', 'axis'], np.vstack(v).T)})
         # TODO: raise if the fit is not good enough
-
-        return cls(polyfit_results.polyfit_coefficients, epoch, interval)
+        return cls(polyfit_results.polyfit_coefficients, poly_type, epoch, interval)
 
     def azimuth_time_range(self, freq_s: float = 0.02) -> xr.DataArray:
         azimuth_time_values = pd.date_range(
@@ -65,14 +79,22 @@ def azimuth_time_range(self, freq_s: float = 0.02) -> xr.DataArray:
     def position(
         self, time: Optional[xr.DataArray] = None, **kwargs: Any
     ) -> xr.DataArray:
+
         if time is None:
             time = self.azimuth_time_range(**kwargs)
         assert time.dtype.name in ("datetime64[ns]", "timedelta64[ns]")
 
         position: xr.DataArray
-        position = xr.polyval(time - self.epoch, self.coefficients)
-        position = position.assign_coords({time.name: time})
-        return position.rename("position")
+        if self.poly_type == 'polynomial':
+            position = xr.polyval(time - self.epoch, self.coefficients)
+            position = position.assign_coords(
+                {time.name: time}).rename('position')
+        elif self.poly_type == 'hermite':
+            v = [np.polynomial.hermite.hermval(((time - self.epoch)/10**9).astype('float64'),
+                                               self.coefficients.isel(axis=i)) for i in range(3)]
+            position = xr.DataArray(data=np.vstack(v), dims=['axis', time.name],
+                                    coords={'axis': [0, 1, 2], time.name: time}, name='position')
+        return position
 
     def velocity(
         self, time: Optional[xr.DataArray] = None, **kwargs: Any
@@ -81,9 +103,17 @@ def velocity(
             time = self.azimuth_time_range(**kwargs)
         assert time.dtype.name in ("datetime64[ns]", "timedelta64[ns]")
 
-        velocity_coefficients = polyder(self.coefficients) * S_TO_NS
+        velocity_coefficients = polyder(
+            self.coefficients, self.poly_type) * S_TO_NS
 
         velocity: xr.DataArray
-        velocity = xr.polyval(time - self.epoch, velocity_coefficients)
-        velocity = velocity.assign_coords({time.name: time})
-        return velocity.rename("velocity")
+        if self.poly_type == 'polynomial':
+            velocity = xr.polyval(time - self.epoch, velocity_coefficients)
+            velocity = velocity.assign_coords(
+                {time.name: time}).rename('velocity')
+        elif self.poly_type == 'hermite':
+            v = [np.polynomial.hermite.hermval(((time - self.epoch)/10**9).astype('float64'),
+                                               velocity_coefficients.isel(axis=i)) for i in range(3)]
+            velocity = xr.DataArray(data=np.vstack(v), dims=['axis', time.name],
+                                    coords={'axis': [0, 1, 2], time.name: time}, name='velocity')
+        return velocity

sarsen/scene.py

@@ -34,7 +34,8 @@ def convert_to_dem_3d(
 ) -> xr.DataArray:
     _, dem_raster_x = xr.broadcast(dem_raster, dem_raster.coords[x])
     dem_raster_y = dem_raster.coords[y]
-    dem_3d = make_nd_dataarray([dem_raster_x, dem_raster_y, dem_raster], dim=dim)
+    dem_3d = make_nd_dataarray(
+        [dem_raster_x, dem_raster_y, dem_raster], dim=dim)
     return dem_3d.rename("dem_3d")