ootk 3.0.0 → 4.0.0-1

package/src/orbit_determination/BatchLeastSquaresOD.ts

@@ -0,0 +1,174 @@
+/**
+ * @author @thkruz Theodore Kruczek
+ * @license AGPL-3.0-or-later
+ * @copyright (c) 2020-2024 Theodore Kruczek
+ *
+ * Orbital Object ToolKit is free software: you can redistribute it and/or modify it under the
+ * terms of the GNU Affero General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later version.
+ *
+ * Orbital Object ToolKit is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
+ * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License along with
+ * Orbital Object ToolKit. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+import { concat, EpochUTC, J2000, Kilometers, KilometersPerSecond, Matrix, Vector, Vector3D } from 'ootk-core';
+import { Observation } from '../observation/Observation';
+import { PropagatorPairs } from '../observation/PropagatorPairs';
+import { Propagator } from '../propagator/Propagator';
+import { CovarianceFrame, StateCovariance } from './../covariance/StateCovariance';
+import { ForceModel } from './../force/ForceModel';
+import { KeplerPropagator } from './../propagator/KeplerPropagator';
+import { RungeKutta89Propagator } from './../propagator/RungeKutta89Propagator';
+import { BatchLeastSquaresResult } from './BatchLeastSquaresResult';
+
+/**
+ * Batch least squares orbit determination.
+ */
+export class BatchLeastSquaresOD {
+  // / Propagator pair cache, for generating observation Jacobians.
+  private propPairs_: PropagatorPairs;
+  // / Nominal state propagator.
+  private propagator_: Propagator;
+  // / State estimate during solve.
+  private nominal_: J2000;
+  // / Solve start epoch.
+  private start_: EpochUTC;
+
+  /**
+   * Create a new [BatchLeastSquaresOD] object from a list of [Observation]
+   * objects, an [apriori] state estimate, and an optional
+   * spacecraft [forceModel].
+   * @param observations_ List of observations.
+   * @param apriori_ Apriori state estimate.
+   * @param forceModel_ Spacecraft force model.
+   * @param posStep_ Position step size.
+   * @param velStep_ Velocity step size.
+   * @param fastDerivatives_ Use fast derivatives.
+   * @returns [BatchLeastSquaresOD] object.
+   */
+  constructor(
+    private observations_: Observation[],
+    private apriori_: J2000,
+    private forceModel_?: ForceModel,
+    private posStep_: number = 1e-5,
+    private velStep_: number = 1e-5,
+    private fastDerivatives_: boolean = false,
+  ) {
+    this.observations_.sort((a, b) => a.epoch.posix - b.epoch.posix);
+    this.start_ = this.observations_[0].epoch;
+    this.propPairs_ = new PropagatorPairs(this.posStep_, this.velStep_);
+    this.forceModel_ ??= new ForceModel().setGravity();
+    this.propagator_ = new RungeKutta89Propagator(this.apriori_, this.forceModel_);
+    this.nominal_ = this.propagator_.propagate(this.start_);
+  }
+
+  private buildPropagator_(x0: Float64Array, simple: boolean): Propagator {
+    const state = new J2000(
+      this.nominal_.epoch,
+      new Vector3D(x0[0] as Kilometers, x0[1] as Kilometers, x0[2] as Kilometers),
+      new Vector3D(x0[3] as KilometersPerSecond, x0[4] as KilometersPerSecond, x0[5] as KilometersPerSecond),
+    );
+
+    if (simple) {
+      return new KeplerPropagator(state.toClassicalElements());
+    }
+
+    return new RungeKutta89Propagator(state, this.forceModel_);
+  }
+
+  private static stateToX0_(state: J2000): Float64Array {
+    return concat(state.position.toArray(), state.velocity.toArray());
+  }
+
+  private setPropagatorPairs_(x0: Float64Array): void {
+    const pl = this.buildPropagator_(x0, this.fastDerivatives_);
+
+    for (let i = 0; i < 6; i++) {
+      const step = this.propPairs_.step(i);
+      const xh = x0.slice();
+
+      xh[i] += step;
+      const ph = this.buildPropagator_(xh, this.fastDerivatives_);
+
+      this.propPairs_.set(i, ph, pl);
+    }
+  }
+
+  /**
+   * Attempt to solve a state estimate with the given root-mean-squared delta
+   * [tolerance].
+   * @param root0 Root initial guess.
+   * @param root0.tolerance Root-mean-squared delta tolerance.
+   * @param root0.maxIter Maximum number of iterations.
+   * @param root0.printIter Print iterations.
+   * @returns [BatchLeastSquaresResult] object.
+   */
+  solve({
+    tolerance = 1e-3,
+    maxIter = 250,
+    printIter = false,
+  }: {
+    tolerance?: number;
+    maxIter?: number;
+    printIter?: boolean;
+  } = {}): BatchLeastSquaresResult {
+    let breakFlag = false;
+    const xNom = BatchLeastSquaresOD.stateToX0_(this.nominal_);
+    let weightedRms = Infinity;
+    const atwaMatInit = Matrix.zero(6, 6);
+    const atwbMatInit = Matrix.zero(6, 1);
+    let atwaMat = atwaMatInit;
+
+    for (let iter = 0; iter < maxIter; iter++) {
+      atwaMat = atwaMatInit;
+      let atwbMat = atwbMatInit;
+
+      this.propagator_ = this.buildPropagator_(xNom, false);
+      this.setPropagatorPairs_(xNom);
+      let rmsTotal = 0.0;
+      let measCount = 0;
+
+      for (const ob of this.observations_) {
+        const noise = ob.noise;
+        const aMat = ob.jacobian(this.propPairs_);
+        const aMatTN = aMat.transpose().multiply(noise);
+        const bMat = ob.residual(this.propagator_);
+
+        atwaMat = atwaMat.add(aMatTN.multiply(aMat));
+        atwbMat = atwbMat.add(aMatTN.multiply(bMat));
+        rmsTotal += bMat.transpose().multiply(noise).multiply(bMat).elements[0][0];
+        measCount += noise.rows;
+      }
+      const newWeightedRms = Math.sqrt(rmsTotal / measCount);
+
+      if (printIter) {
+        // eslint-disable-next-line no-console
+        console.log(`${iter + 1}: rms=${newWeightedRms} x=${new Vector(xNom)}`);
+      }
+      if (Math.abs((weightedRms - newWeightedRms) / weightedRms) <= tolerance) {
+        breakFlag = true;
+      }
+      weightedRms = newWeightedRms;
+      const dX = atwaMat.inverse().multiply(atwbMat);
+
+      for (let i = 0; i < 6; i++) {
+        xNom[i] += dX.elements[i][0];
+      }
+      if (breakFlag) {
+        break;
+      }
+    }
+    const p = atwaMat.inverse();
+    const covariance = new StateCovariance(p, CovarianceFrame.ECI);
+
+    return new BatchLeastSquaresResult(
+      this.buildPropagator_(xNom, false).propagate(this.start_),
+      covariance,
+      weightedRms,
+    );
+  }
+}

package/src/orbit_determination/BatchLeastSquaresResult.ts

@@ -0,0 +1,33 @@
+/**
+ * @author @thkruz Theodore Kruczek
+ * @license AGPL-3.0-or-later
+ * @copyright (c) 2020-2024 Theodore Kruczek
+ *
+ * Orbital Object ToolKit is free software: you can redistribute it and/or modify it under the
+ * terms of the GNU Affero General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later version.
+ *
+ * Orbital Object ToolKit is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
+ * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License along with
+ * Orbital Object ToolKit. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+import { J2000 } from 'ootk-core';
+import { StateCovariance } from './../covariance/StateCovariance';
+
+// / Batch least squares orbit determination result.
+export class BatchLeastSquaresResult {
+  /**
+   * Create a new [BatchLeastSquaresResult] object, containing the solved
+   * [state], [covariance], and root-mean-squared error [rms].
+   * @param state The solved state.
+   * @param covariance The solved covariance.
+   * @param rms The root-mean-squared error.
+   */
+  constructor(public state: J2000, public covariance: StateCovariance, public rms: number) {
+    // Nothing to do here.
+  }
+}

package/src/orbit_determination/GibbsIOD.ts

@@ -0,0 +1,95 @@
+/**
+ * @author @thkruz Theodore Kruczek
+ * @license AGPL-3.0-or-later
+ * @copyright (c) 2020-2024 Theodore Kruczek
+ *
+ * Orbital Object ToolKit is free software: you can redistribute it and/or modify it under the
+ * terms of the GNU Affero General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later version.
+ *
+ * Orbital Object ToolKit is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
+ * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License along with
+ * Orbital Object ToolKit. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+import { DEG2RAD, Earth, EpochUTC, halfPi, J2000, Kilometers, KilometersPerSecond, Radians, Vector3D } from 'ootk-core';
+import { ForceModel } from './../force/ForceModel';
+import { RungeKutta89Propagator } from './../propagator/RungeKutta89Propagator';
+
+/**
+ * Gibbs 3-position inital orbit determination.
+ */
+export class GibbsIOD {
+  constructor(public mu: number = Earth.mu) {
+    // Nothing to do here.
+  }
+
+  /** Abort solve if position plane exceeds this value. */
+  private static readonly coplanarThreshold_: Radians = (5.0 * DEG2RAD) as Radians;
+
+  /**
+   * Attempt to create a state estimate from three inertial position vectors.
+   *
+   * Throws an error if the positions are not coplanar.
+   * @param r1 Position vector 1.
+   * @param r2 Position vector 2.
+   * @param r3 Position vector 3.
+   * @param t2 Time of position 2.
+   * @param t3 Time of position 3.
+   * @returns State estimate at time t2.
+   */
+  solve(
+    r1: Vector3D<Kilometers>,
+    r2: Vector3D<Kilometers>,
+    r3: Vector3D<Kilometers>,
+    t2: EpochUTC,
+    t3: EpochUTC,
+  ): J2000 {
+    const num = r1.normalize().dot(r2.normalize().cross(r3.normalize()));
+    const alpha = halfPi - Math.acos(num);
+
+    if (Math.abs(alpha) > GibbsIOD.coplanarThreshold_) {
+      throw new Error('Orbits are not coplanar.');
+    }
+
+    const r1m = r1.magnitude();
+    const r2m = r2.magnitude();
+    const r3m = r3.magnitude();
+
+    const d = r1.cross(r2).add(r2.cross(r3).add(r3.cross(r1)));
+    const n = r2.cross(r3).scale(r1m).add(r3.cross(r1).scale(r2m)).add(r1.cross(r2).scale(r3m));
+    const b = d.cross(r2);
+    const s = r1.scale(r2m - r3m).add(r2.scale(r3m - r1m).add(r3.scale(r1m - r2m)));
+
+    const nm = n.magnitude();
+    const dm = d.magnitude();
+
+    const vm = Math.sqrt(this.mu / (nm * dm)) as KilometersPerSecond;
+    const vlEci = b.scale((vm / r2m) as KilometersPerSecond).add(s.scale(vm));
+
+    const pv = new J2000(t2, r2, vlEci);
+
+    const forceModel = new ForceModel().setGravity(this.mu);
+    const orbit = new RungeKutta89Propagator(pv, forceModel);
+
+    const pv2 = new J2000(t2, r2, vlEci.negate());
+    const orbit2 = new RungeKutta89Propagator(pv2, forceModel);
+
+    const estP3 = orbit.propagate(t3).position;
+    const dist = estP3.subtract(r3).magnitude();
+    const estP3b = orbit2.propagate(t3).position;
+    const dist2 = estP3b.subtract(r3).magnitude();
+
+    if (dist <= dist2) {
+      orbit.reset();
+
+      return orbit.propagate(t2);
+    }
+    orbit2.reset();
+
+    return orbit2.propagate(t2);
+  }
+}

package/src/orbit_determination/GoodingIOD.ts

@@ -0,0 +1,380 @@
+/**
+ * @author @thkruz Theodore Kruczek
+ * @license AGPL-3.0-or-later
+ * @copyright (c) 2020-2024 Theodore Kruczek
+ *
+ * Orbital Object ToolKit is free software: you can redistribute it and/or modify it under the
+ * terms of the GNU Affero General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later version.
+ *
+ * Orbital Object ToolKit is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
+ * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU Affero General Public License for more details.
+ *
+ * You should have received a copy of the GNU Affero General Public License along with
+ * Orbital Object ToolKit. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+import { Earth, J2000, Kilometers, KilometersPerSecond, Seconds, TAU, Vector3D } from 'ootk-core';
+import { ForceModel } from '../force/ForceModel';
+import { RungeKutta89Propagator } from '../propagator/RungeKutta89Propagator';
+import { ObservationOptical } from './../observation/ObservationOptical';
+import { GibbsIOD } from './GibbsIOD';
+import { LambertIOD } from './LambertIOD';
+
+type SolveRangeProblemParams = {
+  rho1init: Kilometers;
+  rho3init: Kilometers;
+  t13: Seconds;
+  t12: Seconds;
+  nrev: number;
+  direction: boolean;
+  lineOfSight1: Vector3D<Kilometers>;
+  lineOfSight2: Vector3D<Kilometers>;
+  lineOfSight3: Vector3D<Kilometers>;
+  maxIterations: number;
+};
+
+type ComputeDerivativesParams = {
+  x: Kilometers;
+  y: Kilometers;
+  lineOfSight1: Vector3D<Kilometers>;
+  lineOfSight3: Vector3D<Kilometers>;
+  pin: Vector3D<Kilometers>;
+  ein: Vector3D<Kilometers>;
+  t13: Seconds
+  t12: Seconds;
+  nrev: number;
+  direction: boolean;
+  fd: Float64Array;
+  gd: Float64Array;
+};
+
+type GetPositionOnLoS2Params = {
+  e1: Vector3D<Kilometers>;
+  r01: Kilometers;
+  e3: Vector3D<Kilometers>;
+  r03: Kilometers;
+  t13: Seconds;
+  t12: Seconds;
+  nRev: number;
+  posigrade: boolean;
+};
+
+/**
+ * Gooding angles-only initial orbit determination.
+ *
+ * Used for orbit determination from three optical observations.
+ */
+export class GoodingIOD {
+  private readonly _mu: number;
+  private readonly o1_: ObservationOptical;
+  private readonly o2_: ObservationOptical;
+  private readonly o3_: ObservationOptical;
+  private vObserverPosition1_ = Vector3D.origin as Vector3D<Kilometers>;
+  private vObserverPosition2_ = Vector3D.origin as Vector3D<Kilometers>;
+  private vObserverPosition3_ = Vector3D.origin as Vector3D<Kilometers>;
+  private r_ = 0.0 as Kilometers;
+  private v_ = 0.0;
+  private t_ = 0.0;
+  private r1_ = 0.0;
+  private r2_ = 0.0;
+  private r3_ = 0.0;
+  private rho1_ = 0.0 as Kilometers;
+  private rho2_ = 0.0 as Kilometers;
+  private rho3_ = 0.0 as Kilometers;
+  private d1_ = 0.0 as Kilometers;
+  private d3_ = 0.0 as Kilometers;
+  private facFiniteDiff_ = 0.0;
+  private readonly _forceModel: ForceModel = new ForceModel().setGravity(1.0);
+
+  constructor(o1: ObservationOptical, o2: ObservationOptical, o3: ObservationOptical, mu: number = Earth.mu) {
+    this._mu = mu;
+    this.o1_ = o1;
+    this.o2_ = o2;
+    this.o3_ = o3;
+  }
+
+  private _getPositionOnLoS2({
+    e1,
+    r01,
+    e3,
+    r03,
+    t13,
+    t12,
+    nRev,
+    posigrade,
+  }: GetPositionOnLoS2Params): Vector3D<Kilometers> | null {
+    const p1 = this.vObserverPosition1_.add(e1.scale(r01));
+
+    this.r1_ = p1.magnitude();
+
+    const p3 = this.vObserverPosition3_.add(e3.scale(r03));
+
+    this.r3_ = p3.magnitude();
+
+    const p13 = p1.cross(p3);
+
+    let th = Math.atan2(p13.magnitude(), p1.dot(p3));
+
+    if (!posigrade) {
+      th = TAU - th;
+    }
+
+    const v1 = new Float64Array(2);
+    const exitflag = LambertIOD.solve(this.r1_, this.r3_, th, t13, nRev, v1);
+
+    if (exitflag) {
+      const pn = p1.cross(p3);
+      const pt = pn.cross(p1);
+
+      let rt = pt.magnitude();
+
+      if (!posigrade) {
+        rt = -rt as Kilometers;
+      }
+
+      const vel1 = p1.scale(v1[0] / this.r1_).add(pt.scale(v1[1] / rt)) as Vector3D<KilometersPerSecond>;
+
+      const p2 = new RungeKutta89Propagator(new J2000(this.o1_.epoch, p1, vel1), this._forceModel).propagate(
+        this.o1_.epoch.roll(t12),
+      ).position;
+
+      return p2;
+    }
+
+    return null;
+  }
+
+  private _modifyIterate(lineOfSight1: Vector3D, lineOfSight3: Vector3D): void {
+    const r13 = this.vObserverPosition3_.subtract(this.vObserverPosition1_);
+
+    this.d1_ = r13.dot(lineOfSight1) as Kilometers;
+    this.d3_ = r13.dot(lineOfSight3) as Kilometers;
+    const d2 = lineOfSight1.dot(lineOfSight3);
+    const d4 = 1.0 - d2 * d2;
+
+    this.rho1_ = Math.max((this.d1_ - this.d3_ * d2) / d4, 0.0) as Kilometers;
+    this.rho3_ = Math.max((this.d1_ * d2 - this.d3_) / d4, 0.0) as Kilometers;
+  }
+
+  private _computeDerivatives({
+    x,
+    y,
+    lineOfSight1,
+    lineOfSight3,
+    pin,
+    ein,
+    t13,
+    t12,
+    nrev,
+    direction,
+    fd,
+    gd,
+  }: ComputeDerivativesParams): void {
+    const p = pin.normalize();
+    const en = ein.normalize();
+
+    const dx = this.facFiniteDiff_ * x as Kilometers;
+    const dy = this.facFiniteDiff_ * y as Kilometers;
+
+    const cm1 = this._getPositionOnLoS2({
+      e1: lineOfSight1,
+      r01: x - dx as Kilometers,
+      e3: lineOfSight3,
+      r03: y,
+      t13,
+      t12,
+      nRev: nrev,
+      posigrade: direction,
+    })!.subtract(this.vObserverPosition2_);
+
+    const fm1 = p.dot(cm1);
+    const gm1 = en.dot(cm1);
+
+    const cp1 = this._getPositionOnLoS2({
+      e1: lineOfSight1,
+      r01: x + dx as Kilometers,
+      e3: lineOfSight3,
+      r03: y,
+      t13,
+      t12,
+      nRev: nrev,
+      posigrade: direction,
+    })!.subtract(this.vObserverPosition2_);
+
+    const fp1 = p.dot(cp1);
+    const gp1 = en.dot(cp1);
+
+    const fx = (fp1 - fm1) / (2.0 * dx);
+    const gx = (gp1 - gm1) / (2.0 * dx);
+
+    const cm3 = this._getPositionOnLoS2({
+      e1: lineOfSight1,
+      r01: x,
+      e3: lineOfSight3,
+      r03: y - dy as Kilometers,
+      t13,
+      t12,
+      nRev: nrev,
+      posigrade: direction,
+    })!.subtract(this.vObserverPosition2_);
+
+    const fm3 = p.dot(cm3);
+    const gm3 = en.dot(cm3);
+
+    const cp3 = this._getPositionOnLoS2({
+      e1: lineOfSight1,
+      r01: x,
+      e3: lineOfSight3,
+      r03: y + dy as Kilometers,
+      t13,
+      t12,
+      nRev: nrev,
+      posigrade: direction,
+    })!.subtract(this.vObserverPosition2_);
+
+    const fp3 = p.dot(cp3);
+    const gp3 = en.dot(cp3);
+
+    const fy = (fp3 - fm3) / (2.0 * dy);
+    const gy = (gp3 - gm3) / (2.0 * dy);
+
+    fd[0] = fx;
+    fd[1] = fy;
+    gd[0] = gx;
+    gd[1] = gy;
+  }
+
+  solve(r1Init: Kilometers, r3Init: Kilometers, nRev = 0, direction = true): J2000 {
+    const lineOfSight1 = this.o1_.observation.lineOfSight() as Vector3D<Kilometers>;
+    const lineOfSight2 = this.o2_.observation.lineOfSight() as Vector3D<Kilometers>;
+    const lineOfSight3 = this.o3_.observation.lineOfSight() as Vector3D<Kilometers>;
+
+    this.r_ = Math.max(r1Init, r3Init) as Kilometers;
+    this.v_ = Math.sqrt(this._mu / this.r_);
+    this.t_ = this.r_ / this.v_;
+
+    this.vObserverPosition1_ = this.o1_.site.position.scale(1.0 / this.r_ as Kilometers);
+    this.vObserverPosition2_ = this.o2_.site.position.scale(1.0 / this.r_ as Kilometers);
+    this.vObserverPosition3_ = this.o3_.site.position.scale(1.0 / this.r_ as Kilometers);
+
+    const maxiter = 100;
+
+    this._solveRangeProblem({
+      rho1init: r1Init / this.r_ as Kilometers,
+      rho3init: r3Init / this.r_ as Kilometers,
+      t13: this.o3_.epoch.difference(this.o1_.epoch) / this.t_ as Seconds,
+      t12: this.o2_.epoch.difference(this.o1_.epoch) / this.t_ as Seconds,
+      nrev: nRev,
+      direction,
+      lineOfSight1,
+      lineOfSight2,
+      lineOfSight3,
+      maxIterations: maxiter,
+    });
+    const gibbs = new GibbsIOD(this._mu);
+    const p1 = this.vObserverPosition1_.add(lineOfSight1.scale(this.rho1_)).scale(this.r_);
+    const p2 = this.vObserverPosition2_.add(lineOfSight2.scale(this.rho2_)).scale(this.r_);
+    const p3 = this.vObserverPosition3_.add(lineOfSight3.scale(this.rho3_)).scale(this.r_);
+
+    return gibbs.solve(p1, p2, p3, this.o2_.epoch, this.o3_.epoch);
+  }
+
+  private _solveRangeProblem({
+    rho1init,
+    rho3init,
+    t13,
+    t12,
+    nrev,
+    direction,
+    lineOfSight1,
+    lineOfSight2,
+    lineOfSight3,
+    maxIterations,
+  }: SolveRangeProblemParams): void {
+    const arbf = 1e-6;
+    const cvtol = 1e-14;
+
+    this.rho1_ = rho1init;
+    this.rho3_ = rho3init;
+
+    let iter = 0;
+    let stoppingCriterion = 10.0 * cvtol;
+
+    while (iter < maxIterations && Math.abs(stoppingCriterion) > cvtol) {
+      this.facFiniteDiff_ = arbf;
+
+      const p2 = this._getPositionOnLoS2({
+        e1: lineOfSight1,
+        r01: this.rho1_,
+        e3: lineOfSight3,
+        r03: this.rho3_,
+        t13,
+        t12,
+        nRev: nrev,
+        posigrade: direction,
+      });
+
+      if (p2 === null) {
+        this._modifyIterate(lineOfSight1, lineOfSight3);
+      } else {
+        this.r2_ = p2.magnitude();
+        const c = p2.subtract(this.vObserverPosition2_);
+
+        this.rho2_ = c.magnitude();
+        const cr = lineOfSight2.dot(c);
+
+        const u = lineOfSight2.cross(c);
+        const p = u.cross(lineOfSight2).normalize();
+        const ent = lineOfSight2.cross(p);
+
+        const enr = ent.magnitude();
+
+        if (enr === 0.0) {
+          return;
+        }
+
+        const en = ent.normalize();
+
+        const fc = p.dot(c);
+
+        const fd = new Float64Array(2);
+        const gd = new Float64Array(2);
+
+        this._computeDerivatives({
+          x: this.rho1_,
+          y: this.rho3_,
+          lineOfSight1,
+          lineOfSight3,
+          pin: p,
+          ein: en,
+          t13,
+          t12,
+          nrev,
+          direction,
+          fd,
+          gd,
+        });
+
+        const fr1 = fd[0];
+        const fr3 = fd[1];
+        const gr1 = gd[0];
+        const gr3 = gd[1];
+        const detj = fr1 * gr3 - fr3 * gr1;
+
+        this.d3_ = (-gr3 * fc) / detj as Kilometers;
+        this.d1_ = (gr1 * fc) / detj as Kilometers;
+
+        this.rho1_ = this.rho1_ + this.d3_ as Kilometers;
+        this.rho3_ = this.rho3_ + this.d1_ as Kilometers;
+
+        const den = Math.max(cr, this.r2_);
+
+        stoppingCriterion = fc / den;
+      }
+
+      ++iter;
+    }
+  }
+}