BUG: Correct legacy_select conversion

Also added small-matrix exception for condition checking in
parameter_search2d

adcp/matbuilder.py

@@ -782,7 +782,9 @@ def nc_select(m, n, vehicle_order=2, current_order=2, vehicle_vel="otg"):
     return scipy.sparse.eye(n_rows, n_cols, diag)
 
 
-def legacy_select(m, n, vehicle_order=2, current_order=2, vehicle_vel="otg"):
+def legacy_select(
+    m, n, vehicle_order=2, current_order=2, vehicle_vel="otg", prob=None
+):
     """Creates a selection matrix for choosing indexes of X
     that align with the original modeling variable (smoothing order=2,
     vehicle_vel="otg").
@@ -793,6 +795,7 @@ def legacy_select(m, n, vehicle_order=2, current_order=2, vehicle_vel="otg"):
         vehicle_order (int) : order of vehicle smoothing in matrix Q
         current_order (int) : order of current smoothing in matrix Q
         vehicle_vel (str) : if "otg", then current skips modeling 1st order
+        prob (GliderProblem) : The problem that is redundant with all that info
     """
 
     EC = ec_select(m, n, vehicle_order, current_order, vehicle_vel)
@@ -808,12 +811,28 @@ def legacy_select(m, n, vehicle_order=2, current_order=2, vehicle_vel="otg"):
     if vehicle_vel == "otg":
         vehicle_c_mat = scipy.sparse.csr_matrix((2 * m, current_order * n))
     else:
+        vehicle_depths = dp._depth_interpolator(prob.times, prob.ddat).loc[
+            prob.times, "depth"
+        ]
+        vehicle_depths = vehicle_depths.to_numpy().flatten()
+        d_list = list(prob.depths)
+        idxdepth = [d_list.index(d) for d in vehicle_depths]
+
         vehicle_c_block = scipy.sparse.diags(
             np.ones(2), current_order - 2, (2, current_order)
         )
-        vehicle_c_mat = scipy.sparse.block_diag(
-            list(repeat(vehicle_c_block, m))
-        )
+        idxmin = [-1] + idxdepth[:-1]
+        blocks = []
+        for i, (l, r) in enumerate(zip(idxmin, idxdepth)):
+            block = scipy.sparse.csr_matrix((2 * m, current_order * (r - l)))
+            block[
+                2 * i : 2 * (i + 1), (r - l - 1) * current_order :
+            ] = vehicle_c_block
+            blocks.append(block)
+        vehicle_c_mat = scipy.sparse.hstack(blocks)
+        w = vehicle_c_mat.shape[1]
+        blank_mat = scipy.sparse.csr_matrix((2 * m, current_order * n - w))
+        vehicle_c_mat = scipy.sparse.hstack((vehicle_c_mat, blank_mat))
 
     vehicle_mat_e = scipy.sparse.hstack(
         (
@@ -837,3 +856,6 @@ def legacy_select(m, n, vehicle_order=2, current_order=2, vehicle_vel="otg"):
     return scipy.sparse.vstack(
         (vehicle_mat_e, vehicle_mat_n, current_mat_e, current_mat_n)
     )
+
+
+# %%

adcp/tests/test_integration.py

@@ -84,18 +84,27 @@ def standard_sim(
     # %%  Solve problem
     x_sol = op.backsolve(prob)
 
-    Xs = prob.Xs
-    EV = prob.EV
-    NV = prob.NV
-    EC = prob.EC
-    NC = prob.NC
+    legacy = mb.legacy_select(
+        prob.m,
+        prob.n,
+        prob.vehicle_order,
+        prob.current_order,
+        prob.vehicle_vel,
+        prob=prob,
+    )
+    leg_Xs = mb.x_select(len(prob.times), 2)
+    leg_NV = mb.nv_select(len(prob.times), len(prob.depths), 2, 2, "otg")
+    leg_EV = mb.ev_select(len(prob.times), len(prob.depths), 2, 2, "otg")
+    leg_NC = mb.nc_select(len(prob.times), len(prob.depths), 2, 2, "otg")
+    leg_EC = mb.ec_select(len(prob.times), len(prob.depths), 2, 2, "otg")
 
-    err = x_sol - x
+    err = legacy @ x_sol - x
     path_error = (
-        np.linalg.norm(Xs @ NV @ err) ** 2 + np.linalg.norm(Xs @ EV @ err) ** 2
+        np.linalg.norm(leg_Xs @ leg_NV @ err) ** 2
+        + np.linalg.norm(leg_Xs @ leg_EV @ err) ** 2
     )
     current_error = (
-        np.linalg.norm(EC @ err) ** 2 + np.linalg.norm(NC @ err) ** 2
+        np.linalg.norm(leg_EC @ err) ** 2 + np.linalg.norm(leg_NC @ err) ** 2
     )
     return {
         "path_error": path_error,

test/parameter_search2d.py

@@ -99,6 +99,7 @@
         prob.vehicle_order,
         prob.current_order,
         prob.vehicle_vel,
+        prob=prob,
     )
     Xs = prob.Xs
     EV = prob.EV
@@ -107,18 +108,25 @@
     NC = prob.NC
     CV = prob.CV
 
-    x_sol = legacy @ x_sol
+    x_leg = legacy @ x_sol
 
-    err = x_sol - x
+    leg_Xs = mb.x_select(len(prob.times), 2)
+    leg_NV = mb.nv_select(len(prob.times), len(prob.depths), 2, 2, "otg")
+    leg_EV = mb.ev_select(len(prob.times), len(prob.depths), 2, 2, "otg")
+    leg_NC = mb.nc_select(len(prob.times), len(prob.depths), 2, 2, "otg")
+    leg_EC = mb.ec_select(len(prob.times), len(prob.depths), 2, 2, "otg")
+
+    err = x_leg - x
     path_error = (
-        np.linalg.norm(Xs @ NV @ err) ** 2 + np.linalg.norm(Xs @ EV @ err) ** 2
+        np.linalg.norm(leg_Xs @ leg_NV @ err) ** 2
+        + np.linalg.norm(leg_Xs @ leg_EV @ err) ** 2
     )
     current_error = (
-        np.linalg.norm(EC @ err) ** 2 + np.linalg.norm(NC @ err) ** 2
+        np.linalg.norm(leg_EC @ err) ** 2 + np.linalg.norm(leg_NC @ err) ** 2
     )
     errmap[0, i, j] = path_error
     errmap[1, i, j] = current_error
-    paths[i][j] = x_sol
+    paths[i][j] = x_leg
     # print(f"Scenario: rho_v = {rv} and rho_c = {rc}")
     # print(f"\tPath error: {path_error}")
     # print(f"\tCurrent error: {current_error}\n")
@@ -162,7 +170,9 @@ def plot_bundle(sol_x):
         sol_x, ddat, times, depths, direction="both", x_true=x, ttw=False
     )
     viz.inferred_ttw_error_plot(sol_x, adat, ddat, direction="both", x_true=x)
-    viz.current_depth_plot(sol_x, adat, ddat, direction="both", x_true=x)
+    viz.current_depth_plot(
+        sol_x, adat, ddat, direction="both", x_true=x, prob=prob, adcp=True
+    )
     viz.inferred_adcp_error_plot(sol_x, adat, ddat, direction="both", x_true=x)
     viz.vehicle_posit_plot(
         sol_x, ddat, times, depths, x_true=x, dead_reckon=True
@@ -207,12 +217,14 @@ def check_condition(prob: op.GliderProblem) -> Tuple:
     rho_a=rho_a,
     rho_r=rho_r,
 )
-c1, c2, c3, c4 = check_condition(prob)
-print("100x100 sample of kalman matrix has condition number ", c1)
-print("100x100 sample of backsolve matrix has condition number ", c2)
-print("1000x1000 sample of kalman matrix has condition number ", c3)
-print("1000x1000 sample of backsolve matrix has condition number ", c4)
-
+try:
+    c1, c2, c3, c4 = check_condition(prob)
+    print("100x100 sample of kalman matrix has condition number ", c1)
+    print("100x100 sample of backsolve matrix has condition number ", c2)
+    print("1000x1000 sample of kalman matrix has condition number ", c3)
+    print("1000x1000 sample of backsolve matrix has condition number ", c4)
+except ValueError:
+    print("small matrices")
 
 plot_bundle(nav_x)
 if (i1 != i2) or (j1 != j2):
@@ -228,11 +240,14 @@ def check_condition(prob: op.GliderProblem) -> Tuple:
         rho_a=rho_a,
         rho_r=rho_r,
     )
-    c1, c2, c3, c4 = check_condition(prob)
-    print("100x100 sample of kalman matrix has condition number ", c1)
-    print("100x100 sample of backsolve matrix has condition number ", c2)
-    print("1000x1000 sample of kalman matrix has condition number ", c3)
-    print("1000x1000 sample of backsolve matrix has condition number ", c4)
+    try:
+        c1, c2, c3, c4 = check_condition(prob)
+        print("100x100 sample of kalman matrix has condition number ", c1)
+        print("100x100 sample of backsolve matrix has condition number ", c2)
+        print("1000x1000 sample of kalman matrix has condition number ", c3)
+        print("1000x1000 sample of backsolve matrix has condition number ", c4)
+    except ValueError:
+        print("small matrices")
 
 else:
     print("... and it's also the best current solution")