Finished Levenburg-Marquardt for Stokes lines.

1 files changed, 77 insertions, 62 deletions

diff --git a/stokes.hpp b/stokes.hpp
index 948a484..e14db52 100644
--- a/stokes.hpp
+++ b/stokes.hpp
@@ -344,6 +344,7 @@ public:
     Matrix<Scalar> dż = dzDot(z, dH);
     Matrix<Scalar> dżc = dzDotConjugate(z, dH, ddH);
     Tensor<Scalar, 3> ddż = ddzDot(z, dH);
+    Tensor<Scalar, 3> dcdż = dcdzDot(z, dH, ddH);
     Tensor<Scalar, 3> ddżc = ddzDotConjugate(z, dH, ddH, dddH);
 
     Eigen::array<Eigen::IndexPair<int>, 1> ip20 = {Eigen::IndexPair<int>(2, 0)};
@@ -352,7 +353,7 @@ public:
     Matrix<Scalar> ddżcdz = Eigen::Map<Matrix<Scalar>>(ddżcdzT.data(), z.size(), z.size());
 
     Tensor<Scalar, 2> ddżdzT = ddż.contract(dzT.conjugate(), ip20);
-    Matrix<Scalar> ddżdz = Eigen::Map<Matrix<Scalar>>(ddżcdzT.data(), z.size(), z.size());
+    Matrix<Scalar> ddżdz = Eigen::Map<Matrix<Scalar>>(ddżdzT.data(), z.size(), z.size());
 
     Tensor<Scalar, 2> ddżcżT = ddżc.contract(żT, ip20);
     Matrix<Scalar> ddżcż = Eigen::Map<Matrix<Scalar>>(ddżcżT.data(), z.size(), z.size());
@@ -360,27 +361,35 @@ public:
     Tensor<Scalar, 2> ddżżcT = ddż.contract(żT.conjugate(), ip20);
     Matrix<Scalar> ddżżc = Eigen::Map<Matrix<Scalar>>(ddżżcT.data(), z.size(), z.size());
 
-    Vector<Scalar> x(z.size() * gs.size());
+    Tensor<Scalar, 2> dcdżcdzT = dcdż.conjugate().contract(dzT, ip20);
+    Matrix<Scalar> dcdżcdz = Eigen::Map<Matrix<Scalar>>(dcdżcdzT.data(), z.size(), z.size());
+
+    Tensor<Scalar, 2> dcdżcżT = dcdż.conjugate().contract(żT, ip20);
+    Matrix<Scalar> dcdżcż = Eigen::Map<Matrix<Scalar>>(dcdżcżT.data(), z.size(), z.size());
+
+    Vector<Scalar> x(2 * z.size() * gs.size());
     Matrix<Scalar> M(x.size(), x.size());
 
     for (unsigned i = 0; i < gs.size(); i++) {
-      Real fdg = gCoeff(i, t);
-      Real dfdg = dgCoeff(i, t);
+      Real fdgn = gCoeff(i, t);
+      Real dfdgn = dgCoeff(i, t);
       Vector<Scalar> dC = - 0.5 / ż.norm() / dz.norm() * (
-          dfdg * ż.conjugate() + fdg * dżc * dz + fdg * dż * dz.conjugate()
+          dfdgn * ż.conjugate() + fdgn * dżc * dz + fdgn * dż * dz.conjugate()
           - real(dz.dot(ż)) * (
-              dfdg * dz.conjugate() / dz.squaredNorm() +
-              fdg * (dżc * ż + dż * ż.conjugate()) / ż.squaredNorm()
+              dfdgn * dz.conjugate() / dz.squaredNorm() +
+              fdgn * (dżc * ż + dż * ż.conjugate()) / ż.squaredNorm()
             )
         );
 
       for (unsigned j = 0; j < dC.size(); j++) {
         x(z.size() * i + j) = dC(j);
+        x(z.size() * gs.size() + z.size() * i + j) = conj(dC(j));
       }
 
       for (unsigned j = 0; j < gs.size(); j++) {
         Real fdgm = gCoeff(j, t);
         Real dfdgm = dgCoeff(j, t);
+        Scalar CC = - real(dz.dot(ż)) / ż.norm() / dz.norm();
         Vector<Scalar> dCm = - 0.5 / ż.norm() / dz.norm() * (
             dfdgm * ż.conjugate() + fdgm * dżc * dz + fdgm * dż * dz.conjugate()
             - real(dz.dot(ż)) * (
@@ -388,66 +397,80 @@ public:
                 fdgm * (dżc * ż + dż * ż.conjugate()) / ż.squaredNorm()
               )
           );
-        Matrix<Scalar> ddC =
-          - 0.5 / ż.norm() / dz.norm() * (
-                dfdg * fdgm * dżc.transpose() + dfdgm * fdg * dżc +
-                fdg * fdgm * ddżcdz + fdg * fdgm * ddżdz
+
+        Matrix<Scalar> ddC = 0.5 / ż.norm() / dz.norm() * (
+          -  (
+                dfdgn * fdgm * dżc.transpose() + dfdgm * fdgn * dżc +
+                fdgn * fdgm * ddżcdz + fdgn * fdgm * ddżdz
             )
-          + 0.5 / dz.squaredNorm() * (
-              dfdg * dz.conjugate() * dCm.transpose() +
+          - ż.norm() / dz.norm() * (
+              dfdgn * dz.conjugate() * dCm.transpose() +
               dfdgm * dC * dz.adjoint()
             )
-          + 0.5 / ż.squaredNorm() * (
-              fdg * (dżc * ż + dż * ż.conjugate()) * dCm.transpose() +
+          - dz.norm() / ż.norm() * (
+              fdgn * (dżc * ż + dż * ż.conjugate()) * dCm.transpose() +
               fdgm * dC * (dżc * ż + dż * ż.conjugate()).transpose()
             )
-          + 0.5 / ż.norm() / dz.norm() * real(dz.dot(ż)) * fdg * fdgm * (
+          - CC * dz.norm() / ż.norm() * fdgn * fdgm * (
                   ddżżc + ddżcż + dżc * dż.transpose() + dż * dżc.transpose()
-                ) / ż.squaredNorm()
-          - 0.5 / ż.norm() / dz.norm() * real(dz.dot(ż)) * dfdg * dfdgm * dz.conjugate() * dz.adjoint() / pow(dz.squaredNorm(), 2)
-          - 0.5 / ż.norm() / dz.norm() * real(dz.dot(ż)) / pow(ż.squaredNorm(), 2)
-                * fdg * (dżc * ż + dż * ż.conjugate())
+                )
+          - 0.5 * CC / dz.norm() / ż.norm() * (
+            dfdgn * fdgm * dz.conjugate() * (dżc * ż + dż * ż.conjugate()).transpose() +
+            dfdgm * fdgn * (dżc * ż + dż * ż.conjugate()) * dz.adjoint()
+            )
+          + 0.5 * CC * ż.norm() / pow(dz.norm(), 3) * dfdgn * dfdgm * dz.conjugate() * dz.adjoint()
+          + 0.5 * CC * dz.norm() / pow(ż.norm(), 3)
+                * fdgn * (dżc * ż + dż * ż.conjugate())
                 * fdgm * (dżc * ż + dż * ż.conjugate()).transpose()
+              )
+            ;
+
+        Matrix<Scalar> dcdC = 0.5 / ż.norm() / dz.norm() * (
+          - (
+                dfdgn * fdgm * dż + dfdgm * fdgn * dż.adjoint() +
+                fdgn * fdgm * (dcdżcdz + dcdżcdz.adjoint())
+            )
+          - ż.norm() / dz.norm() * (
+              dfdgn * dz.conjugate() * dCm.adjoint() +
+              dfdgm * dC * dz.transpose()
+            )
+          - dz.norm() / ż.norm() * (
+              fdgn * (dżc * ż + dż * ż.conjugate()) * dCm.adjoint() +
+              fdgm * dC * (dżc * ż + dż * ż.conjugate()).adjoint()
+            )
+          - CC * ż.norm() / dz.norm() * dfdgn * dfdgm * Matrix<Scalar>::Identity(z.size(), z.size())
+          - CC * dz.norm() / ż.norm() * fdgn * fdgm * (
+                  dcdżcż + dcdżcż.adjoint() + dżc * dżc.adjoint() + dż * dż.adjoint()
+                )
+          - 0.5 * CC / dz.norm() / ż.norm() * (
+            dfdgn * fdgm * dz.conjugate() * (dżc * ż + dż * ż.conjugate()).adjoint() +
+            dfdgm * fdgn * (dżc * ż + dż * ż.conjugate()) * dz.transpose()
+            )
+          + 0.5 * CC * ż.norm() / pow(dz.norm(), 3) * dfdgn * dfdgm * dz.conjugate() * dz.transpose()
+          + 0.5 * CC * dz.norm() / pow(ż.norm(), 3) * fdgn * fdgm
+             * (dżc * ż + dż * ż.conjugate())
+             * (dżc * ż + dż * ż.conjugate()).adjoint()
+             )
             ;
 
         for (unsigned k = 0; k < z.size(); k++) {
           for (unsigned l = 0; l < z.size(); l++) {
-            M(z.size() * i + k, z.size() * j + l) = ddC(k, l);
+            M(z.size() * i + k, z.size() * j + l) = dcdC(k, l);
+            M(z.size() * gs.size() + z.size() * i + k, z.size() * j + l) = conj(ddC(k, l));
+            M(z.size() * i + k, z.size() * gs.size() + z.size() * j + l) = ddC(k, l);
+            M(z.size() * gs.size() + z.size() * i + k, z.size() * gs.size() + z.size() * j + l) = conj(dcdC(k, l));
           }
         }
       }
     }
 
-    Cord cNew(*this);
-    Matrix<Scalar> Mf = Matrix<Scalar>::Zero(x.size(), x.size());
-    for (unsigned i = 0; i < x.size(); i++) {
-      for (unsigned j = 0; j < x.size(); j++) {
-        Scalar hi = std::complex<Real>(0.03145, 0.08452);
-        Scalar hj = std::complex<Real>(0.09483, 0.02453);
-        Scalar c0 = cost(J, t);
-        cNew.gs[i / z.size()](i % z.size()) += hi;
-        Scalar ci = cNew.cost(J, t);
-        cNew.gs[j / z.size()](j % z.size()) += hj;
-        Scalar cij = cNew.cost(J, t);
-        cNew.gs[i / z.size()](i % z.size()) -= hi;
-        Scalar cj = cNew.cost(J, t);
-        cNew.gs[j / z.size()](j % z.size()) -= hj;
-
-        Mf(i, j) = (cij - ci - cj + c0) / (hi * hj);
-      }
-    }
-
-    std::cout << M << std::endl;
-    std::cout << Mf << std::endl;
-    getchar();
-
     return {x, M};
   }
 
   template <int p>
   std::pair<Real, Real> relaxStepNewton(const Tensor<Scalar, p>& J, unsigned nt, Real δ₀) {
-    Vector<Scalar> dgsTot = Vector<Scalar>::Zero(z0.size() * gs.size());
-    Matrix<Scalar> HessTot = Matrix<Scalar>::Zero(z0.size() * gs.size(), z0.size() * gs.size());
+    Vector<Scalar> dgsTot = Vector<Scalar>::Zero(2 * z0.size() * gs.size());
+    Matrix<Scalar> HessTot = Matrix<Scalar>::Zero(2 * z0.size() * gs.size(), 2 * z0.size() * gs.size());
 
     for (unsigned i = 0; i < nt; i++) {
       Real t = (i + 1.0) / (nt + 1.0);
@@ -463,35 +486,27 @@ public:
 
     Real δ = δ₀;
     Real oldCost = totalCost(J, nt);
+    Real newCost = std::numeric_limits<Real>::infinity();
 
-    Vector<Scalar> δg = HessTot.partialPivLu().solve(dgsTot);
-
-    for (unsigned i = 0; i < gs.size(); i++) {
-      for (unsigned j = 0; j < z0.size(); j++) {
-        cNew.gs[i](j) = gs[i](j) - δg[z0.size() * i + j];
-      }
-    }
-
-    Real newCost = cNew.totalCost(J, nt);
-    if (newCost < oldCost) {
-      std::cout << "Newton step accepted!" << std::endl;
-    }
+    Matrix<Scalar> I = abs(HessTot.diagonal().array()).matrix().asDiagonal();
 
     while (newCost > oldCost) {
+      Vector<Scalar> δg = (HessTot + δ * I).partialPivLu().solve(dgsTot);
+
       for (unsigned i = 0; i < gs.size(); i++) {
         for (unsigned j = 0; j < z0.size(); j++) {
-          cNew.gs[i](j) = gs[i](j) - δ * conj(dgsTot[z0.size() * i + j]);
+          cNew.gs[i](j) = gs[i](j) - δg[z0.size() * gs.size() + z0.size() * i + j];
         }
       }
 
       newCost = cNew.totalCost(J, nt);
 
-      δ /= 2;
+      δ *= 2;
     }
 
     gs = cNew.gs;
 
-    return {2*δ, stepSize};
+    return {δ / 2, stepSize};
   }
 
   template <int p>
@@ -555,7 +570,7 @@ public:
     while (stepSize > 1e-7 && steps < maxSteps) {
       std::tie(δ, stepSize) = relaxStepNewton(J, nt, δ);
       std::cout << totalCost(J, nt) << " " << δ << " " << stepSize << std::endl;
-      δ *= 2;
+      δ /= 3;
       steps++;
     }
   }