From 9f903e426a0af9c5e44ce39067a9d0a54c3ceb05 Mon Sep 17 00:00:00 2001
From: Katia Esposito <katia@duck.com>
Date: Fri, 28 Jul 2023 00:12:16 +0200
Subject: [PATCH] Update solver.py

---
 mlat/solver.py | 20 +++++++++++---------
 1 file changed, 11 insertions(+), 9 deletions(-)

diff --git a/mlat/solver.py b/mlat/solver.py
index d0870c7..58791b6 100644
--- a/mlat/solver.py
+++ b/mlat/solver.py
@@ -28,6 +28,7 @@
 
 from mlat import geodesy, constants, profile
 from mlat import config
+import numpy as np
 
 # The core of it all. Not very big, is it?
 # (Admittedly the entire least-squares solver is hidden within scipy..)
@@ -36,7 +37,7 @@
 
 
 def _residuals(x_guess, pseudorange_data, altitude, altitude_error):
-    """Return an array of residuals for a position guess at x_guess versus
+    """Return the sum of squared residuals for a position guess at x_guess versus
     actual measurements pseudorange_data and altitude."""
 
     (*position_guess, offset) = x_guess
@@ -53,7 +54,8 @@ def _residuals(x_guess, pseudorange_data, altitude, altitude_error):
         _, _, altitude_guess = geodesy.ecef2llh(position_guess)
         res.append((altitude - altitude_guess) / altitude_error)
 
-    return res
+    # return the sum of squared residuals
+    return np.sum(np.array(res)**2)
 
 
 @profile.trackcpu
@@ -91,19 +93,19 @@ def solve(measurements, altitude, altitude_error, initial_guess):
                          math.sqrt(variance) * constants.Cair)
                         for receiver, timestamp, variance in measurements]
     x_guess = [initial_guess[0], initial_guess[1], initial_guess[2], 0.0]
-    x_est, cov_x, infodict, mesg, ler = scipy.optimize.leastsq(
+    res = scipy.optimize.minimize(
         _residuals,
         x_guess,
         args=(pseudorange_data, altitude, altitude_error),
-        full_output=True,
-        maxfev=config.SOLVER_MAXFEV)
+        method='BFGS',
+        options={'maxiter': config.SOLVER_MAXFEV})
 
-    if ler in (1, 2, 3, 4):
+    if res.success:
         #glogger.info("solver success: {0} {1}".format(ler, mesg))
 
         # Solver found a result. Validate that it makes
         # some sort of physical sense.
-        (*position_est, offset_est) = x_est
+        (*position_est, offset_est) = res.x
 
         if offset_est < 0 or offset_est > 500e3:
             #glogger.info("solver: bad offset: {0}".formaT(offset_est))
@@ -118,10 +120,10 @@ def solve(measurements, altitude, altitude_error, initial_guess):
         #        #glogger.info("solver: bad range: {0}".format(d))
         #        return None
 
-        if cov_x is None:
+        if res.hess_inv is None:
             return position_est, None
         else:
-            return position_est, cov_x[0:3, 0:3]
+            return position_est, res.hess_inv[0:3, 0:3]
 
     else:
         # Solver failed