Changeset 27840 for branches/simtest_nebulous_branches/psModules/src/imcombine/pmSubtractionEquation.c

Timestamp:

May 3, 2010, 8:50:52 AM (16 years ago)

Author:

eugene

Message:

updates from trunk

Location:

branches/simtest_nebulous_branches

Files:

• . (modified) (1 prop)
• psModules (modified) (1 prop)
• psModules/src/imcombine/pmSubtractionEquation.c (modified) (34 diffs)

branches/simtest_nebulous_branches/psModules/src/imcombine/pmSubtractionEquation.c

@@ -7 +7 @@
 #include <pslib.h>
 
+#include "pmErrorCodes.h"
 #include "pmSubtraction.h"
 #include "pmSubtractionKernels.h"
@@ -15 +16 @@
 #include "pmSubtractionVisual.h"
 
-// #define TESTING                         // TESTING output for debugging; may not work with threads!
-
-#define USE_VARIANCE                    // Include variance in equation?
+//#define TESTING                         // TESTING output for debugging; may not work with threads!
+
+//#define USE_WEIGHT                      // Include weight (1/variance) in equation?
+//#define USE_WINDOW                      // Include weight (1/variance) in equation?
+
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -23 +26 @@
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-// Calculate the sum over a stamp product
-static inline double calculateSumProduct(const psKernel *image1, // First image in multiplication
-                                         const psKernel *image2, // Second image in multiplication
-                                         const psKernel *variance, // Variance image
-                                         int footprint // (Half-)Size of stamp
-    )
+// Calculate the least-squares matrix and vector
+static bool calculateMatrixVector(psImage *matrix, // Least-squares matrix, updated
+                                  psVector *vector, // Least-squares vector, updated
+                                  double *norm,     // Normalisation, updated
+                                  const psKernel *input, // Input image (target)
+                                  const psKernel *reference, // Reference image (convolution source)
+                                  const psKernel *weight,  // Weight image
+                                  const psKernel *window,  // Window image
+                                  const psArray *convolutions,         // Convolutions for each kernel
+                                  const pmSubtractionKernels *kernels, // Kernels
+                                  const psImage *polyValues, // Spatial polynomial values
+                                  int footprint, // (Half-)Size of stamp
+                                  int normWindow, // Window (half-)size for normalisation measurement
+                                  const pmSubtractionEquationCalculationMode mode
+                                  )
 {
-    double sum = 0.0;                   // Sum of the image products
+    // (I - R * sum_i a_i k_i - g) (R * k_j) = 0
+    // I C_j = sum_i C_i C_j
+
+    // Background: C_i = 1.0
+    // Normalisation: C_i = R
+
+    int numKernels = kernels->num;                      // Number of kernels
+    int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
+    int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
+    int spatialOrder = kernels->spatialOrder;       // Order of spatial variation
+    int numPoly = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of polynomial terms
+    double poly[numPoly];                                 // Polynomial terms
+    double poly2[numPoly][numPoly];                       // Polynomial-polynomial values
+
+    // Evaluate polynomial-polynomial terms
+    // XXX we can skip this if we are not calculating kernel coeffs
+    for (int iyOrder = 0, iIndex = 0; iyOrder <= spatialOrder; iyOrder++) {
+        for (int ixOrder = 0; ixOrder <= spatialOrder - iyOrder; ixOrder++, iIndex++) {
+            double iPoly = polyValues->data.F64[iyOrder][ixOrder]; // Value of polynomial
+            poly[iIndex] = iPoly;
+            for (int jyOrder = 0, jIndex = 0; jyOrder <= spatialOrder; jyOrder++) {
+                for (int jxOrder = 0; jxOrder <= spatialOrder - jyOrder; jxOrder++, jIndex++) {
+                    double jPoly = polyValues->data.F64[jyOrder][jxOrder];
+                    poly2[iIndex][jIndex] = iPoly * jPoly;
+                }
+            }
+        }
+    }
+
+    // initialize the matrix and vector for NOP on all coeffs.  we only fill in the coeffs we
+    // choose to calculate
+    psImageInit(matrix, 0.0);
+    psVectorInit(vector, 1.0);
+    for (int i = 0; i < matrix->numCols; i++) {
+        matrix->data.F64[i][i] = 1.0;
+    }
+
+    // the order of the elements in the matrix and vector is:
+    // [kernel 0, x^0 y^0][kernel 1 x^0 y^0]...[kernel N, x^0 y^0]
+    // [kernel 0, x^1 y^0][kernel 1 x^1 y^0]...[kernel N, x^1 y^0]
+    // [kernel 0, x^n y^m][kernel 1 x^n y^m]...[kernel N, x^n y^m]
+    // normalization
+    // bg 0, bg 1, bg 2 (only 0 is currently used?)
+
+    for (int i = 0; i < numKernels; i++) {
+        psKernel *iConv = convolutions->data[i]; // Convolution for index i
+        for (int j = i; j < numKernels; j++) {
+            psKernel *jConv = convolutions->data[j]; // Convolution for index j
+
+            double sumCC = 0.0;         // Sum of convolution products
+            for (int y = - footprint; y <= footprint; y++) {
+                for (int x = - footprint; x <= footprint; x++) {
+                    double cc = iConv->kernel[y][x] * jConv->kernel[y][x];
+                    if (weight) {
+                        cc *= weight->kernel[y][x];
+                    }
+                    if (window) {
+                        cc *= window->kernel[y][x];
+                    }
+                    sumCC += cc;
+                }
+            }
+
+            // Spatial variation of kernel coeffs
+            if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
+                for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
+                    for (int jTerm = 0, jIndex = j; jTerm < numPoly; jTerm++, jIndex += numKernels) {
+                        double value = sumCC * poly2[iTerm][jTerm];
+                        matrix->data.F64[iIndex][jIndex] = value;
+                        matrix->data.F64[jIndex][iIndex] = value;
+                    }
+                }
+            }
+        }
+
+        double sumRC = 0.0;             // Sum of the reference-convolution products
+        double sumIC = 0.0;             // Sum of the input-convolution products
+        double sumC = 0.0;              // Sum of the convolution
+        for (int y = - footprint; y <= footprint; y++) {
+            for (int x = - footprint; x <= footprint; x++) {
+                float conv = iConv->kernel[y][x];
+                float in = input->kernel[y][x];
+                float ref = reference->kernel[y][x];
+                double ic = in * conv;
+                double rc = ref * conv;
+                double c = conv;
+                if (weight) {
+                    float wtVal = weight->kernel[y][x];
+                    ic *= wtVal;
+                    rc *= wtVal;
+                    c *= wtVal;
+                }
+                if (window) {
+                    float winVal = window->kernel[y][x];
+                    ic *= winVal;
+                    rc *= winVal;
+                    c  *= winVal;
+                }
+                sumIC += ic;
+                sumRC += rc;
+                sumC += c;
+            }
+        }
+        // Spatial variation
+        for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
+            double normTerm = sumRC * poly[iTerm];
+            double bgTerm = sumC * poly[iTerm];
+            if ((mode & PM_SUBTRACTION_EQUATION_NORM) && (mode & PM_SUBTRACTION_EQUATION_KERNELS)) {
+                matrix->data.F64[iIndex][normIndex] = normTerm;
+                matrix->data.F64[normIndex][iIndex] = normTerm;
+            }
+            if ((mode & PM_SUBTRACTION_EQUATION_BG) && (mode & PM_SUBTRACTION_EQUATION_KERNELS)) {
+                matrix->data.F64[iIndex][bgIndex] = bgTerm;
+                matrix->data.F64[bgIndex][iIndex] = bgTerm;
+            }
+            if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
+                vector->data.F64[iIndex] = sumIC * poly[iTerm];
+                if (!(mode & PM_SUBTRACTION_EQUATION_NORM)) {
+                    // subtract norm * sumRC * poly[iTerm]
+                    psAssert (kernels->solution1, "programming error: define solution first!");
+                    int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
+                    double norm = fabs(kernels->solution1->data.F64[normIndex]);  // Normalisation
+                    vector->data.F64[iIndex] -= norm * normTerm;
+                }
+            }
+        }
+    }
+
+    double sumRR = 0.0;                 // Sum of the reference product
+    double sumIR = 0.0;                 // Sum of the input-reference product
+    double sum1 = 0.0;                  // Sum of the background
+    double sumR = 0.0;                  // Sum of the reference
+    double sumI = 0.0;                  // Sum of the input
+    double normI1 = 0.0, normI2 = 0.0;  // Sum of I_1 and I_2 within the normalisation window
     for (int y = - footprint; y <= footprint; y++) {
         for (int x = - footprint; x <= footprint; x++) {
-            double value = image1->kernel[y][x] * image2->kernel[y][x];
-#ifdef USE_VARIANCE
-            value /= variance->kernel[y][x];
-#endif
-            sum += value;
-        }
-    }
-    return sum;
-}
-
-// Calculate a single element of the least-squares matrix, with the polynomial expansions in one direction
-static inline bool calculateMatrixElement1(psImage *matrix, // Matrix to calculate
-                                           int i, int j, // Coordinates of element
-                                           const psKernel *image1, // First image in multiplication
-                                           const psKernel *image2, // Second image in multiplication
-                                           const psKernel *variance, // Variance image
-                                           const psImage *polyValues, // Spatial polynomial values
-                                           int numKernels, // Number of kernel basis functions
-                                           int footprint, // (Half-)Size of stamp
-                                           int spatialOrder, // Maximum order of spatial variation
-                                           bool symmetric // Is the matrix symmetric?
-    )
+            double in = input->kernel[y][x];
+            double ref = reference->kernel[y][x];
+            double ir = in * ref;
+            double rr = PS_SQR(ref);
+            double one = 1.0;
+
+            if (PS_SQR(x) + PS_SQR(y) <= PS_SQR(normWindow)) {
+                normI1 += ref;
+                normI2 += in;
+            }
+
+            if (weight) {
+                float wtVal = weight->kernel[y][x];
+                rr *= wtVal;
+                ir *= wtVal;
+                in *= wtVal;
+                ref *= wtVal;
+                one *= wtVal;
+            }
+            if (window) {
+                float  winVal = window->kernel[y][x];
+                rr      *= winVal;
+                ir      *= winVal;
+                in      *= winVal;
+                ref *= winVal;
+                one *= winVal;
+            }
+            sumRR += rr;
+            sumIR += ir;
+            sumR += ref;
+            sumI += in;
+            sum1 += one;
+        }
+    }
+
+    *norm = normI2 / normI1;
+
+    if (mode & PM_SUBTRACTION_EQUATION_NORM) {
+        matrix->data.F64[normIndex][normIndex] = sumRR;
+        vector->data.F64[normIndex] = sumIR;
+        // subtract sum over kernels * kernel solution
+    }
+    if (mode & PM_SUBTRACTION_EQUATION_BG) {
+        matrix->data.F64[bgIndex][bgIndex] = sum1;
+        vector->data.F64[bgIndex] = sumI;
+    }
+    if ((mode & PM_SUBTRACTION_EQUATION_NORM) && (mode & PM_SUBTRACTION_EQUATION_BG)) {
+        matrix->data.F64[normIndex][bgIndex] = sumR;
+        matrix->data.F64[bgIndex][normIndex] = sumR;
+    }
+
+    // check for any NAN values in the result, skip if found:
+    for (int iy = 0; iy < matrix->numRows; iy++) {
+        for (int ix = 0; ix < matrix->numCols; ix++) {
+            if (!isfinite(matrix->data.F64[iy][ix])) {
+                fprintf (stderr, "WARNING: NAN in matrix\n");
+                return false;
+            }
+        }
+    }
+    for (int ix = 0; ix < vector->n; ix++) {
+        if (!isfinite(vector->data.F64[ix])) {
+            fprintf (stderr, "WARNING: NAN in vector\n");
+            return false;
+        }
+    }
+
+    return true;
+}
+
+
+// Calculate the least-squares matrix and vector for dual convolution
+static bool calculateDualMatrixVector(psImage *matrix, // Least-squares matrix, updated
+                                      psVector *vector, // Least-squares vector, updated
+                                      double *norm,     // Normalisation, updated
+                                      const psKernel *image1, // Image 1
+                                      const psKernel *image2, // Image 2
+                                      const psKernel *weight,  // Weight image
+                                      const psKernel *window,  // Window image
+                                      const psArray *convolutions1, // Convolutions of image 1 for each kernel
+                                      const psArray *convolutions2, // Convolutions of image 2 for each kernel
+                                      const pmSubtractionKernels *kernels, // Kernels
+                                      const psImage *polyValues, // Spatial polynomial values
+                                      int footprint, // (Half-)Size of stamp
+                                      int normWindow, // Window (half-)size for normalisation measurement
+                                      const pmSubtractionEquationCalculationMode mode
+                                      )
 {
-    double sum = calculateSumProduct(image1, image2, variance, footprint); // Sum of the image products
-    if (!isfinite(sum)) {
-        return false;
-    }
-
-    // Generate the pseudo-convolutions from the spatial polynomial terms
-    for (int iyOrder = 0, iIndex = i; iyOrder <= spatialOrder; iyOrder++) {
-        for (int ixOrder = 0; ixOrder <= spatialOrder - iyOrder; ixOrder++, iIndex += numKernels) {
-            double convPoly = sum * polyValues->data.F64[iyOrder][ixOrder];
-
-            assert(iIndex < matrix->numRows && j < matrix->numCols);
-
-            matrix->data.F64[iIndex][j] = convPoly;
-            if (symmetric) {
-
-                assert(iIndex < matrix->numCols && j < matrix->numRows);
-
-                matrix->data.F64[j][iIndex] = convPoly;
-            }
-        }
-    }
-    return true;
-}
-
-// Calculate a single element of the least-squares matrix, with the polynomial expansions in both directions
-static inline bool calculateMatrixElement2(psImage *matrix, // Matrix to calculate
-                                           int i, int j, // Coordinates of element
-                                           const psKernel *image1, // First image in multiplication
-                                           const psKernel *image2, // Second image in multiplication
-                                           const psKernel *variance, // Variance image
-                                           const psImage *polyValues, // Spatial polynomial values
-                                           int numKernels, // Number of kernel basis functions
-                                           int footprint, // (Half-)Size of stamp
-                                           int spatialOrder, // Maximum order of spatial variation
-                                           bool symmetric // Is the matrix symmetric?
-    )
-{
-    double sum = calculateSumProduct(image1, image2, variance, footprint); // Sum of the image products
-    if (!isfinite(sum)) {
-        return false;
-    }
-
-    // Generate the pseudo-convolutions from the spatial polynomial terms
-    for (int iyOrder = 0, iIndex = i; iyOrder <= spatialOrder; iyOrder++) {
-        for (int ixOrder = 0; ixOrder <= spatialOrder - iyOrder; ixOrder++, iIndex += numKernels) {
+    int numKernels = kernels->num;                      // Number of kernels
+    int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
+    int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
+    int spatialOrder = kernels->spatialOrder;       // Order of spatial variation
+    int numPoly = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of polynomial terms
+    double poly[numPoly];                                 // Polynomial terms
+    double poly2[numPoly][numPoly];                       // Polynomial-polynomial values
+
+    int numBackground = PM_SUBTRACTION_POLYTERMS(kernels->bgOrder); // Number of background terms
+    int numParams = numKernels * numPoly + 1 + numBackground;       // Number of regular parameters
+    int numParams2 = numKernels * numPoly;                          // Number of additional parameters for dual
+    int numDual = numParams + numParams2;                           // Total number of parameters for dual
+
+    psAssert(matrix &&
+             matrix->type.type == PS_TYPE_F64 &&
+             matrix->numCols == numDual &&
+             matrix->numRows == numDual,
+             "Least-squares matrix is bad.");
+    psAssert(vector &&
+             vector->type.type == PS_TYPE_F64 &&
+             vector->n == numDual,
+             "Least-squares vector is bad.");
+
+    // Evaluate polynomial-polynomial terms
+    for (int iyOrder = 0, iIndex = 0; iyOrder <= spatialOrder; iyOrder++) {
+        for (int ixOrder = 0; ixOrder <= spatialOrder - iyOrder; ixOrder++, iIndex++) {
             double iPoly = polyValues->data.F64[iyOrder][ixOrder]; // Value of polynomial
-            for (int jyOrder = 0, jIndex = j; jyOrder <= spatialOrder; jyOrder++) {
-                for (int jxOrder = 0; jxOrder <= spatialOrder - jyOrder; jxOrder++, jIndex += numKernels) {
-                    double convPoly = sum * iPoly * polyValues->data.F64[jyOrder][jxOrder];
-
-                    assert(iIndex < matrix->numRows && jIndex < matrix->numCols);
-
-                    matrix->data.F64[iIndex][jIndex] = convPoly;
-                    if (symmetric) {
-
-                        assert(iIndex < matrix->numCols && jIndex < matrix->numRows);
-
-                        matrix->data.F64[jIndex][iIndex] = convPoly;
-                    }
-                }
-            }
-        }
-    }
-    return true;
-}
-
-// Calculate the square part of the matrix derived from multiplying convolutions
-static bool calculateMatrixSquare(psImage *matrix, // Matrix to calculate
-                                  const psArray *convolutions1, // Convolutions for element 1
-                                  const psArray *convolutions2, // Convolutions for element 2
-                                  const psKernel *variance, // Variance image
-                                  const psImage *polyValues, // Polynomial values
-                                  int numKernels, // Number of kernel basis functions
-                                  int spatialOrder, // Order of spatial variation
-                                  int footprint // Half-size of stamp
-                                  )
-{
-    bool symmetric = (convolutions1 == convolutions2 ? true : false); // Is matrix symmetric?
+            poly[iIndex] = iPoly;
+            for (int jyOrder = 0, jIndex = 0; jyOrder <= spatialOrder; jyOrder++) {
+                for (int jxOrder = 0; jxOrder <= spatialOrder - jyOrder; jxOrder++, jIndex++) {
+                    double jPoly = polyValues->data.F64[jyOrder][jxOrder];
+                    poly2[iIndex][jIndex] = iPoly * jPoly;
+                }
+            }
+        }
+    }
+
+
+    // initialize the matrix and vector for NOP on all coeffs.  we only fill in the coeffs we
+    // choose to calculate
+    psImageInit(matrix, 0.0);
+    psVectorInit(vector, 1.0);
+    for (int i = 0; i < matrix->numCols; i++) {
+        matrix->data.F64[i][i] = 1.0;
+    }
 
     for (int i = 0; i < numKernels; i++) {
-        psKernel *iConv = convolutions1->data[i]; // Convolution for i-th element
-
-        for (int j = (symmetric ? i : 0); j < numKernels; j++) {
-            psKernel *jConv = convolutions2->data[j]; // Convolution for j-th element
-
-            if (!calculateMatrixElement2(matrix, i, j, iConv, jConv, variance, polyValues, numKernels,
-                                         footprint, spatialOrder, symmetric)) {
-                psTrace("psModules.imcombine", 2, "Bad sumCC at %d, %d", i, j);
-                return false;
-            }
-        }
-    }
-
-    return true;
-}
-
-// Calculate least-squares matrix and vector
-static bool calculateMatrix(psImage *matrix, // Matrix to calculate
-                            const pmSubtractionKernels *kernels, // Kernel components
-                            const psArray *convolutions, // Convolutions of source with kernels
-                            const psKernel *input, // Input stamp, or NULL
-                            const psKernel *variance, // Variance stamp
-                            const psImage *polyValues, // Spatial polynomial values
-                            int footprint, // (Half-)Size of stamp
-                            bool normAndBG // Calculate normalisation and background terms?
-    )
-{
-    int numKernels = kernels->num;      // Number of kernel components
-    int spatialOrder = kernels->spatialOrder; // Maximum order of spatial variation
-    int numSpatial = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of spatial variation terms
-    int bgOrder = kernels->bgOrder;     // Maximum order of background fit
-    int numBackground = normAndBG ? PM_SUBTRACTION_POLYTERMS(bgOrder) : 0; // Number of background terms
-    int numTerms = numKernels * numSpatial + (normAndBG ? 1 + numBackground : 0); // Total number of terms
-    assert(matrix);
-    assert(matrix->numCols == matrix->numRows);
-    assert(matrix->numCols == numTerms);
-    assert(convolutions && convolutions->n == numKernels);
-    assert(polyValues);
-    assert(!normAndBG || input);        // If we want the normalisation and BG, then we need the input image
-
-    // Square part of the matrix (convolution-convolution products)
-    if (!calculateMatrixSquare(matrix, convolutions, convolutions, variance, polyValues, numKernels,
-                               spatialOrder, footprint)) {
-        return false;
-    }
-
-    // XXX To support higher-order background model than simply constant, the below code needs to be updated.
-    if (normAndBG) {
-        int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
-        int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
-
-        for (int i = 0; i < numKernels; i++) {
-            psKernel *conv = convolutions->data[i]; // Convolution for i-th element
-
-            // Normalisation-convolution terms
-            if (!calculateMatrixElement1(matrix, i, normIndex, conv, input, variance, polyValues, numKernels,
-                                         footprint, spatialOrder, true)) {
-                psTrace("psModules.imcombine", 2, "Bad sumIC at %d", i);
-                return false;
-            }
-
-            // Background-convolution terms
-            double sumC = 0.0;          // Sum of the convolution
+        psKernel *iConv1 = convolutions1->data[i]; // Convolution 1 for index i
+        psKernel *iConv2 = convolutions2->data[i]; // Convolution 2 for index i
+        for (int j = i; j < numKernels; j++) {
+            psKernel *jConv1 = convolutions1->data[j]; // Convolution 1 for index j
+            psKernel *jConv2 = convolutions2->data[j]; // Convolution 2 for index j
+
+            double sumAA = 0.0;         // Sum of convolution products between image 1
+            double sumBB = 0.0;         // Sum of convolution products between image 2
+            double sumAB = 0.0;         // Sum of convolution products across images 1 and 2
             for (int y = - footprint; y <= footprint; y++) {
                 for (int x = - footprint; x <= footprint; x++) {
-                    double value = conv->kernel[y][x];
-#ifdef USE_VARIANCE
-                    value /= variance->kernel[y][x];
-#endif
-                    sumC += value;
-                }
-            }
-            if (!isfinite(sumC)) {
-                psTrace("psModules.imcombine", 2, "Bad sumC at %d", i);
-                return false;
-            }
-
-            for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) {
-                for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) {
-                    double value = sumC * polyValues->data.F64[yOrder][xOrder];
-                    matrix->data.F64[index][bgIndex] = value;
-                    matrix->data.F64[bgIndex][index] = value;
-                }
-            }
-        }
-
-        // Background only, normalisation only, and background-normalisation terms
-        double sum1 = 0.0;              // Sum of the weighting
-        double sumI = 0.0;              // Sum of the input
-        double sumII = 0.0;             // Sum of the input squared
+                    double aa = iConv1->kernel[y][x] * jConv1->kernel[y][x];
+                    double bb = iConv2->kernel[y][x] * jConv2->kernel[y][x];
+                    double ab = iConv1->kernel[y][x] * jConv2->kernel[y][x];
+                    if (weight) {
+                        float wtVal = weight->kernel[y][x];
+                        aa *= wtVal;
+                        bb *= wtVal;
+                        ab *= wtVal;
+                    }
+                    if (window) {
+                        float wtVal = window->kernel[y][x];
+                        aa *= wtVal;
+                        bb *= wtVal;
+                        ab *= wtVal;
+                    }
+                    sumAA += aa;
+                    sumBB += bb;
+                    sumAB += ab;
+                }
+            }
+
+            // Spatial variation of kernel coeffs
+            if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
+                for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
+                    for (int jTerm = 0, jIndex = j; jTerm < numPoly; jTerm++, jIndex += numKernels) {
+                        double aa = sumAA * poly2[iTerm][jTerm];
+                        double bb = sumBB * poly2[iTerm][jTerm];
+                        double ab = sumAB * poly2[iTerm][jTerm];
+
+                        matrix->data.F64[iIndex][jIndex] = aa;
+                        matrix->data.F64[jIndex][iIndex] = aa;
+
+                        matrix->data.F64[iIndex + numParams][jIndex + numParams] = bb;
+                        matrix->data.F64[jIndex + numParams][iIndex + numParams] = bb;
+
+                        matrix->data.F64[iIndex][jIndex + numParams] = ab;
+                        matrix->data.F64[jIndex + numParams][iIndex] = ab;
+                    }
+                }
+            }
+        }
+        for (int j = 0; j < i; j++) {
+            psKernel *jConv2 = convolutions2->data[j]; // Convolution 2 for index j
+            double sumAB = 0.0;         // Sum of convolution products for matrix C
+            for (int y = - footprint; y <= footprint; y++) {
+                for (int x = - footprint; x <= footprint; x++) {
+                    double ab = iConv1->kernel[y][x] * jConv2->kernel[y][x];
+                    if (weight) {
+                        ab *= weight->kernel[y][x];
+                    }
+                    if (window) {
+                        ab *= window->kernel[y][x];
+                    }
+                    sumAB += ab;
+                }
+            }
+
+            // Spatial variation of kernel coeffs
+            if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
+                for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
+                    for (int jTerm = 0, jIndex = j; jTerm < numPoly; jTerm++, jIndex += numKernels) {
+                        double ab = sumAB * poly2[iTerm][jTerm];
+                        matrix->data.F64[iIndex][jIndex + numParams] = ab;
+                        matrix->data.F64[jIndex + numParams][iIndex] = ab;
+                    }
+                }
+            }
+        }
+
+        double sumAI2 = 0.0;            // Sum of A.I_2 products (for vector)
+        double sumBI2 = 0.0;            // Sum of B.I_2 products (for vector)
+        double sumAI1 = 0.0;            // Sum of A.I_1 products (for matrix, normalisation)
+        double sumA = 0.0;              // Sum of A (for matrix, background)
+        double sumBI1 = 0.0;            // Sum of B.I_1 products (for matrix, normalisation)
+        double sumB = 0.0;              // Sum of B products (for matrix, background)
+        double sumI2 = 0.0;             // Sum of I_2 (for vector, background)
         for (int y = - footprint; y <= footprint; y++) {
             for (int x = - footprint; x <= footprint; x++) {
-                double invNoise2 = 1.0;
-#ifdef USE_VARIANCE
-                invNoise2 /= variance->kernel[y][x];
-#endif
-                double value = input->kernel[y][x] * invNoise2;
-                sumI += value;
-                sumII += value * input->kernel[y][x];
-                sum1 += invNoise2;
-            }
-        }
-        if (!isfinite(sumI)) {
-            psTrace("psModules.imcombine", 2, "Bad sumI detected");
+                double a = iConv1->kernel[y][x];
+                double b = iConv2->kernel[y][x];
+                float i1 = image1->kernel[y][x];
+                float i2 = image2->kernel[y][x];
+
+                double ai2 = a * i2;
+                double bi2 = b * i2;
+                double ai1 = a * i1;
+                double bi1 = b * i1;
+
+                if (weight) {
+                    float wtVal = weight->kernel[y][x];
+                    ai2 *= wtVal;
+                    bi2 *= wtVal;
+                    ai1 *= wtVal;
+                    bi1 *= wtVal;
+                    a *= wtVal;
+                    b *= wtVal;
+                    i2 *= wtVal;
+                }
+                if (window) {
+                    float wtVal = window->kernel[y][x];
+                    ai2 *= wtVal;
+                    bi2 *= wtVal;
+                    ai1 *= wtVal;
+                    bi1 *= wtVal;
+                    a *= wtVal;
+                    b *= wtVal;
+                    i2 *= wtVal;
+                }
+                sumAI2 += ai2;
+                sumBI2 += bi2;
+                sumAI1 += ai1;
+                sumA += a;
+                sumBI1 += bi1;
+                sumB += b;
+                sumI2 += i2;
+            }
+        }
+        // Spatial variation
+        for (int iTerm = 0, iIndex = i; iTerm < numPoly; iTerm++, iIndex += numKernels) {
+            double ai2 = sumAI2 * poly[iTerm];
+            double bi2 = sumBI2 * poly[iTerm];
+            double ai1 = sumAI1 * poly[iTerm];
+            double a   = sumA * poly[iTerm];
+            double bi1 = sumBI1 * poly[iTerm];
+            double b   = sumB * poly[iTerm];
+
+            if ((mode & PM_SUBTRACTION_EQUATION_NORM) && (mode & PM_SUBTRACTION_EQUATION_KERNELS)) {
+                matrix->data.F64[iIndex][normIndex] = ai1;
+                matrix->data.F64[normIndex][iIndex] = ai1;
+                matrix->data.F64[iIndex + numParams][normIndex] = bi1;
+                matrix->data.F64[normIndex][iIndex + numParams] = bi1;
+            }
+            if ((mode & PM_SUBTRACTION_EQUATION_BG) && (mode & PM_SUBTRACTION_EQUATION_KERNELS)) {
+                matrix->data.F64[iIndex][bgIndex] = a;
+                matrix->data.F64[bgIndex][iIndex] = a;
+                matrix->data.F64[iIndex + numParams][bgIndex] = b;
+                matrix->data.F64[bgIndex][iIndex + numParams] = b;
+            }
+            if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
+                vector->data.F64[iIndex] = ai2;
+                vector->data.F64[iIndex + numParams] = bi2;
+                if (!(mode & PM_SUBTRACTION_EQUATION_NORM)) {
+                    // subtract norm * sumRC * poly[iTerm]
+                    psAssert (kernels->solution1, "programming error: define solution first!");
+                    int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
+                    double norm = fabs(kernels->solution1->data.F64[normIndex]);  // Normalisation
+                    vector->data.F64[iIndex] -= norm * ai1;
+                    vector->data.F64[iIndex + numParams] -= norm * bi1;
+                }
+            }
+        }
+    }
+
+    double sumI1 = 0.0;                 // Sum of I_1 (for matrix, background-normalisation)
+    double sumI1I1 = 0.0;               // Sum of I_1^2 (for matrix, normalisation-normalisation)
+    double sum1 = 0.0;                  // Sum of 1 (for matrix, background-background)
+    double sumI2 = 0.0;                 // Sum of I_2 (for vector, background)
+    double sumI1I2 = 0.0;               // Sum of I_1.I_2 (for vector, normalisation)
+    double normI1 = 0.0, normI2 = 0.0;  // Sum of I_1 and I_2 within the normalisation window
+    for (int y = - footprint; y <= footprint; y++) {
+        for (int x = - footprint; x <= footprint; x++) {
+            double i1 = image1->kernel[y][x];
+            double i2 = image2->kernel[y][x];
+
+            double i1i1 = i1 * i1;
+            double one = 1.0;
+            double i1i2 = i1 * i2;
+
+            if (PS_SQR(x) + PS_SQR(y) <= PS_SQR(normWindow)) {
+                normI1 += i1;
+                normI2 += i2;
+            }
+
+            if (weight) {
+                float wtVal = weight->kernel[y][x];
+                i1 *= wtVal;
+                i1i1 *= wtVal;
+                one *= wtVal;
+                i2 *= wtVal;
+                i1i2 *= wtVal;
+            }
+            if (window) {
+                float wtVal = window->kernel[y][x];
+                i1 *= wtVal;
+                i1i1 *= wtVal;
+                one *= wtVal;
+                i2 *= wtVal;
+                i1i2 *= wtVal;
+            }
+            sumI1 += i1;
+            sumI1I1 += i1i1;
+            sum1 += one;
+            sumI2 += i2;
+            sumI1I2 += i1i2;
+        }
+    }
+
+    *norm = normI2 / normI1;
+
+    if (mode & PM_SUBTRACTION_EQUATION_NORM) {
+        matrix->data.F64[normIndex][normIndex] = sumI1I1;
+        vector->data.F64[normIndex] = sumI1I2;
+    }
+    if (mode & PM_SUBTRACTION_EQUATION_BG) {
+        matrix->data.F64[bgIndex][bgIndex] = sum1;
+        vector->data.F64[bgIndex] = sumI2;
+    }
+    if ((mode & PM_SUBTRACTION_EQUATION_NORM) && (mode & PM_SUBTRACTION_EQUATION_BG)) {
+        matrix->data.F64[bgIndex][normIndex] = sumI1;
+        matrix->data.F64[normIndex][bgIndex] = sumI1;
+    }
+
+    // check for any NAN values in the result, skip if found:
+    for (int iy = 0; iy < matrix->numRows; iy++) {
+        for (int ix = 0; ix < matrix->numCols; ix++) {
+            if (!isfinite(matrix->data.F64[iy][ix])) {
+                fprintf (stderr, "WARNING: NAN in matrix\n");
+                return false;
+            }
+        }
+    }
+    for (int ix = 0; ix < vector->n; ix++) {
+        if (!isfinite(vector->data.F64[ix])) {
+            fprintf (stderr, "WARNING: NAN in vector\n");
             return false;
         }
-        if (!isfinite(sumII)) {
-            psTrace("psModules.imcombine", 2, "Bad sumII detected");
-            return false;
-        }
-        if (!isfinite(sum1)) {
-            psTrace("psModules.imcombine", 2, "Bad sum1 detected");
-            return false;
-        }
-        matrix->data.F64[normIndex][normIndex] = sumII;
-        matrix->data.F64[bgIndex][bgIndex] = sum1;
-        matrix->data.F64[normIndex][bgIndex] = sumI;
-        matrix->data.F64[bgIndex][normIndex] = sumI;
-    }
+    }
+
 
     return true;
 }
 
-
-// Calculate least-squares matrix and vector
-static bool calculateVector(psVector *vector, // Vector to calculate, or NULL
-                            const pmSubtractionKernels *kernels, // Kernel components
-                            const psArray *convolutions, // Convolutions of source with kernels
-                            const psKernel *input, // Input stamp, or NULL if !normAndBG
-                            const psKernel *target, // Target stamp
-                            const psKernel *variance, // Variance stamp
-                            const psImage *polyValues, // Spatial polynomial values
-                            int footprint, // (Half-)Size of stamp
-                            bool normAndBG // Calculate normalisation and background terms?
-    )
-{
-    int numKernels = kernels->num;      // Number of kernel components
-    int spatialOrder = kernels->spatialOrder; // Maximum order of spatial variation
-    int numSpatial = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of spatial variation terms
-    int bgOrder = kernels->bgOrder;     // Maximum order of background fit
-    int numBackground = normAndBG ? PM_SUBTRACTION_POLYTERMS(bgOrder) : 0; // Number of background terms
-    int numTerms = numKernels * numSpatial + (normAndBG ? 1 + numBackground : 0); // Total number of terms
-    assert(vector && vector->n == numTerms);
-    assert(convolutions && convolutions->n == numKernels);
-    assert(target);
-    assert(polyValues);
-    assert(!normAndBG || input);       // If we want the normalisation and BG, then we need the input image
-
-    // Convolution terms
-    for (int i = 0; i < numKernels; i++) {
-        psKernel *conv = convolutions->data[i]; // Convolution for i-th element
-        double sumTC = 0.0;          // Sum of the target and convolution
-        for (int y = - footprint; y <= footprint; y++) {
-            for (int x = - footprint; x <= footprint; x++) {
-                double value = target->kernel[y][x] * conv->kernel[y][x];
-#ifdef USE_VARIANCE
-                value /= variance->kernel[y][x];
-#endif
-                sumTC += value;
-            }
-        }
-        if (!isfinite(sumTC)) {
-            psTrace("psModules.imcombine", 2, "Bad sumTC at %d", i);
-            return false;
-        }
-        for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) {
-            for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) {
-                vector->data.F64[index] = sumTC * polyValues->data.F64[yOrder][xOrder];
-            }
-        }
-    }
-
-    if (normAndBG) {
-        // Background terms
-        double sumT = 0.0;              // Sum of the target
-        double sumIT = 0.0;             // Sum of the input-target product
-        for (int y = - footprint; y <= footprint; y++) {
-            for (int x = - footprint; x <= footprint; x++) {
-                double value = target->kernel[y][x];
-#ifdef USE_VARIANCE
-                value /= variance->kernel[y][x];
-#endif
-                sumIT += value * input->kernel[y][x];
-                sumT += value;
-            }
-        }
-        if (!isfinite(sumT)) {
-            psTrace("psModules.imcombine", 2, "Bad sumI detected");
-            return false;
-        }
-        if (!isfinite(sumIT)) {
-            psTrace("psModules.imcombine", 2, "Bad sumIT detected");
-            return false;
-        }
-
-        int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation term
-        vector->data.F64[normIndex] = sumIT;
-        int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background term
-        vector->data.F64[bgIndex] = sumT;
-    }
-
-    return true;
-}
-
-
-
-// Calculate the cross-matrix, composed of convolutions of each image
-// Note that the cross-matrix is NOT square
-static bool calculateMatrixCross(psImage *matrix, // Matrix to calculate
-                                 const pmSubtractionKernels *kernels, // Kernel components
-                                 const psArray *convolutions1, // Convolutions of image 1
-                                 const psArray *convolutions2, // Convolutions of image 2
-                                 const psKernel *image1, // Image 1 stamp
-                                 const psKernel *variance, // Variance stamp
-                                 const psImage *polyValues, // Spatial polynomial values
-                                 int footprint // (Half-)Size of stamp
-                                 )
-{
-    assert(matrix);
-    int numKernels = kernels->num;      // Number of kernel components
-    int spatialOrder = kernels->spatialOrder; // Maximum order of spatial variation
-    int numSpatial = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of spatial polynomial terms
-    int numBackground = PM_SUBTRACTION_POLYTERMS(kernels->bgOrder); // Number of background terms
-    int numCols = numKernels * numSpatial + 1 + numBackground; // Number of columns
-    int numRows = numKernels * numSpatial; // Number of rows
-    assert(matrix->numCols == numCols && matrix->numRows == numRows);
-    assert(convolutions1 && convolutions1->n == numKernels);
-    assert(convolutions2 && convolutions2->n == numKernels);
-
-    int normIndex, bgIndex;             // Indices in matrix for normalisation and background terms
-    PM_SUBTRACTION_INDICES(normIndex, bgIndex, kernels);
-
-    if (!calculateMatrixSquare(matrix, convolutions1, convolutions2, variance, polyValues, numKernels,
-                               spatialOrder, footprint)) {
-        return false;
-    }
-
-    for (int i = 0; i < numKernels; i++) {
-        // Normalisation
-        psKernel *conv = convolutions2->data[i]; // Convolution
-        if (!calculateMatrixElement1(matrix, i, normIndex, conv, image1, variance, polyValues, numKernels,
-                                     footprint, spatialOrder, false)) {
-            psTrace("psModules.imcombine", 2, "Bad sumIC at %d", i);
-            return false;
-        }
-
-        // Background
-        double sumC = 0.0;              // Sum of the weighting
-        for (int y = - footprint; y <= footprint; y++) {
-            for (int x = - footprint; x <= footprint; x++) {
-                double value = conv->kernel[y][x];
-#ifdef USE_VARIANCE
-                value /= variance->kernel[y][x];
-#endif
-                sumC += value;
-            }
-        }
-        if (!isfinite(sumC)) {
-            psTrace("psModules.imcombine", 2, "Bad sumC detected at %d", i);
-            return false;
-        }
-        for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) {
-            for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) {
-                matrix->data.F64[index][bgIndex] = sumC * polyValues->data.F64[yOrder][xOrder];
-            }
-        }
-    }
-
-    return true;
-}
-
-
+#if 1
 // Add in penalty term to least-squares vector
-static bool calculatePenalty(psVector *vector, // Vector to which to add in penalty term
-                             const pmSubtractionKernels *kernels // Kernel parameters
+bool calculatePenalty(psImage *matrix,                     // Matrix to which to add in penalty term
+                             psVector *vector,                    // Vector to which to add in penalty term
+                             const pmSubtractionKernels *kernels, // Kernel parameters
+                             float norm                           // Normalisation
     )
 {
@@ -420 +571 @@
     int spatialOrder = kernels->spatialOrder; // Order of spatial variations
     int numKernels = kernels->num; // Number of kernel components
+    int numSpatial = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of spatial variations
+    int numParams = numKernels * numSpatial;                 // Number of kernel parameters
+
+    // order is :
+    // [p_0,x_0,y_0 p_1,x_0,y_0, p_2,x_0,y_0]
+    // [p_0,x_1,y_0 p_1,x_1,y_0, p_2,x_1,y_0]
+    // [p_0,x_0,y_1 p_1,x_0,y_1, p_2,x_0,y_1]
+    // [norm]
+    // [bg]
+    // [q_0,x_0,y_0 q_1,x_0,y_0, q_2,x_0,y_0]
+    // [q_0,x_1,y_0 q_1,x_1,y_0, q_2,x_1,y_0]
+    // [q_0,x_0,y_1 q_1,x_0,y_1, q_2,x_0,y_1]
+
     for (int i = 0; i < numKernels; i++) {
         for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) {
             for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) {
-                vector->data.F64[index] -= penalties->data.F32[i];
+                // Contribution to chi^2: a_i^2 P_i
+                psAssert(isfinite(penalties->data.F32[i]), "Invalid penalty");
+                matrix->data.F64[index][index] += norm * penalties->data.F32[i];
+                if (kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
+                    matrix->data.F64[index + numParams + 2][index + numParams + 2] += norm * penalties->data.F32[i];
+                    // matrix[i][i] is ~ (k_i * I_1)(k_i * I_1)
+                    // penalties scale with second moments
+                    //
+                }
             }
         }
@@ -430 +602 @@
     return true;
 }
+# endif
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -439 +612 @@
 // Calculate the value of a polynomial, specified by coefficients and polynomial values
 double p_pmSubtractionCalculatePolynomial(const psVector *coeff, // Coefficients
-                                                 const psImage *polyValues, // Polynomial values
-                                                 int order, // Order of polynomials
-                                                 int index, // Index at which to begin
-                                                 int step // Step between subsequent indices
-                                                 )
+                                          const psImage *polyValues, // Polynomial values
+                                          int order, // Order of polynomials
+                                          int index, // Index at which to begin
+                                          int step // Step between subsequent indices
+                                          )
 {
     double sum = 0.0;                   // Value of the polynomial sum
@@ -458 +631 @@
 
 double p_pmSubtractionSolutionCoeff(const pmSubtractionKernels *kernels, const psImage *polyValues,
-                                           int index, bool wantDual)
+                                    int index, bool wantDual)
 {
 #if 0
@@ -511 +684 @@
     const pmSubtractionKernels *kernels = job->args->data[1]; // Kernels
     int index = PS_SCALAR_VALUE(job->args->data[2], S32); // Stamp index
-
-    return pmSubtractionCalculateEquationStamp(stamps, kernels, index);
+    pmSubtractionEquationCalculationMode mode  = PS_SCALAR_VALUE(job->args->data[3], S32); // calculation model
+
+    return pmSubtractionCalculateEquationStamp(stamps, kernels, index, mode);
 }
 
 bool pmSubtractionCalculateEquationStamp(pmSubtractionStampList *stamps, const pmSubtractionKernels *kernels,
-                                         int index)
+                                         int index, const pmSubtractionEquationCalculationMode mode)
 {
     PM_ASSERT_SUBTRACTION_STAMP_LIST_NON_NULL(stamps, false);
@@ -529 +703 @@
     int numBackground = PM_SUBTRACTION_POLYTERMS(kernels->bgOrder); // Number of background terms
 
+    // numKernels is the number of unique kernel images (one for each Gaussian modified by a specific polynomial).
+    // = \sum_i^N_Gaussians [(order + 1) * (order + 2) / 2], eg for 1 Gauss and 1st order, numKernels = 3
+
     // Total number of parameters to solve for: coefficient of each kernel basis function, multipled by the
     // number of coefficients for the spatial polynomial, normalisation and a constant background offset.
     int numParams = numKernels * numSpatial + 1 + numBackground;
+    if (kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
+        // An additional image is convolved
+        numParams += numKernels * numSpatial;
+    }
 
     pmSubtractionStamp *stamp = stamps->stamps->data[index]; // Stamp of interest
     psAssert(stamp->status == PM_SUBTRACTION_STAMP_CALCULATE, "We only operate on stamps with this state.");
 
-    // Generate convolutions
+    // Generate convolutions: these are generated once and saved
     if (!pmSubtractionConvolveStamp(stamp, kernels, footprint)) {
-        psError(PS_ERR_UNKNOWN, false, "Unable to convolve stamp %d.", index);
+        psError(psErrorCodeLast(), false, "Unable to convolve stamp %d.", index);
         return NULL;
     }
@@ -566 +747 @@
 #endif
 
+    // XXX visualize the set of convolved stamps
+
     psImage *polyValues = p_pmSubtractionPolynomial(NULL, spatialOrder,
                                                     stamp->xNorm, stamp->yNorm); // Polynomial terms
 
-    bool new = stamp->vector1 ? false : true; // Is this a new run?
+    bool new = stamp->vector ? false : true; // Is this a new run?
     if (new) {
-        stamp->matrix1 = psImageAlloc(numParams, numParams, PS_TYPE_F64);
-        stamp->vector1 = psVectorAlloc(numParams, PS_TYPE_F64);
+        stamp->matrix = psImageAlloc(numParams, numParams, PS_TYPE_F64);
+        stamp->vector = psVectorAlloc(numParams, PS_TYPE_F64);
     }
 #ifdef TESTING
-    psImageInit(stamp->matrix1, NAN);
-    psVectorInit(stamp->vector1, NAN);
+    psImageInit(stamp->matrix, NAN);
+    psVectorInit(stamp->vector, NAN);
 #endif
 
     bool status;                    // Status of least-squares matrix/vector calculation
+
+    psKernel *weight = NULL;
+    psKernel *window = NULL;
+
+#ifdef USE_WEIGHT
+    weight = stamp->weight;
+#endif
+#ifdef USE_WINDOW
+    window = stamps->window;
+#endif
+
     switch (kernels->mode) {
       case PM_SUBTRACTION_MODE_1:
-        status = calculateMatrix(stamp->matrix1, kernels, stamp->convolutions1, stamp->image1,
-                                 stamp->variance, polyValues, footprint, true);
-        status &= calculateVector(stamp->vector1, kernels, stamp->convolutions1, stamp->image1,
-                                  stamp->image2, stamp->variance, polyValues, footprint, true);
+        status = calculateMatrixVector(stamp->matrix, stamp->vector, &stamp->norm, stamp->image2, stamp->image1,
+                                       weight, window, stamp->convolutions1, kernels,
+                                       polyValues, footprint, stamps->normWindow, mode);
         break;
       case PM_SUBTRACTION_MODE_2:
-        status = calculateMatrix(stamp->matrix1, kernels, stamp->convolutions2, stamp->image2,
-                                 stamp->variance, polyValues, footprint, true);
-        status &= calculateVector(stamp->vector1, kernels, stamp->convolutions2, stamp->image2,
-                                  stamp->image1, stamp->variance, polyValues, footprint, true);
+        status = calculateMatrixVector(stamp->matrix, stamp->vector, &stamp->norm, stamp->image1, stamp->image2,
+                                       weight, window, stamp->convolutions2, kernels,
+                                       polyValues, footprint, stamps->normWindow, mode);
         break;
       case PM_SUBTRACTION_MODE_DUAL:
-        if (new) {
-            stamp->matrix2 = psImageAlloc(numKernels * numSpatial, numKernels * numSpatial, PS_TYPE_F64);
-            stamp->matrixX = psImageAlloc(numParams, numKernels * numSpatial, PS_TYPE_F64);
-            stamp->vector2 = psVectorAlloc(numKernels * numSpatial, PS_TYPE_F64);
-        }
-#ifdef TESTING
-        psImageInit(stamp->matrix2, NAN);
-        psImageInit(stamp->matrixX, NAN);
-        psVectorInit(stamp->vector2, NAN);
-#endif
-        status  = calculateMatrix(stamp->matrix1, kernels, stamp->convolutions1, stamp->image1,
-                                  stamp->variance, polyValues, footprint, true);
-        status &= calculateMatrix(stamp->matrix2, kernels, stamp->convolutions2, NULL,
-                                  stamp->variance, polyValues, footprint, false);
-        status &= calculateMatrixCross(stamp->matrixX, kernels, stamp->convolutions1,
-                                       stamp->convolutions2, stamp->image1, stamp->variance, polyValues,
-                                       footprint);
-        status &= calculateVector(stamp->vector1, kernels, stamp->convolutions1, stamp->image1,
-                                  stamp->image2, stamp->variance, polyValues, footprint, true);
-        status &= calculateVector(stamp->vector2, kernels, stamp->convolutions2, NULL,
-                                  stamp->image2, stamp->variance, polyValues, footprint, false);
+        status = calculateDualMatrixVector(stamp->matrix, stamp->vector, &stamp->norm,
+                                           stamp->image1, stamp->image2,
+                                           weight, window, stamp->convolutions1, stamp->convolutions2,
+                                           kernels, polyValues, footprint, stamps->normWindow, mode);
         break;
       default:
@@ -623 +798 @@
         stamp->status = PM_SUBTRACTION_STAMP_REJECTED;
         psWarning("Rejecting stamp %d (%d,%d) because of bad equation",
-                  index, (int)(stamp->x + 0.5), (int)(stamp->y + 0.5));
+                  index, (int)(stamp->x - 0.5), (int)(stamp->y - 0.5));
     } else {
         stamp->status = PM_SUBTRACTION_STAMP_USED;
@@ -629 +804 @@
 
 #ifdef TESTING
-    if (psTraceGetLevel("psModules.imcombine.equation") >= 10) {
+    {
         psString matrixName = NULL;
-        psStringAppend(&matrixName, "matrix1_%d.fits", index);
+        psStringAppend(&matrixName, "matrix_%d.fits", index);
         psFits *matrixFile = psFitsOpen(matrixName, "w");
         psFree(matrixName);
-        psFitsWriteImage(matrixFile, NULL, stamp->matrix1, 0, NULL);
+        psFitsWriteImage(matrixFile, NULL, stamp->matrix, 0, NULL);
         psFitsClose(matrixFile);
 
         matrixName = NULL;
-        psStringAppend(&matrixName, "vector1_%d.fits", index);
-        psImage *dummy = psImageAlloc(stamp->vector1->n, 1, PS_TYPE_F64);
-        memcpy(dummy->data.F64[0], stamp->vector1->data.F64,
-               PSELEMTYPE_SIZEOF(PS_TYPE_F64) * stamp->vector1->n);
+        psStringAppend(&matrixName, "vector_%d.fits", index);
+        psImage *dummy = psImageAlloc(stamp->vector->n, 1, PS_TYPE_F64);
+        memcpy(dummy->data.F64[0], stamp->vector->data.F64,
+               PSELEMTYPE_SIZEOF(PS_TYPE_F64) * stamp->vector->n);
         matrixFile = psFitsOpen(matrixName, "w");
         psFree(matrixName);
@@ -647 +822 @@
         psFree(dummy);
         psFitsClose(matrixFile);
-
-        if (stamp->vector2) {
-            matrixName = NULL;
-            psStringAppend(&matrixName, "vector2_%d.fits", index);
-            dummy = psImageAlloc(stamp->vector2->n, 1, PS_TYPE_F64);
-            memcpy(dummy->data.F64[0], stamp->vector2->data.F64,
-                   PSELEMTYPE_SIZEOF(PS_TYPE_F64) * stamp->vector2->n);
-            matrixFile = psFitsOpen(matrixName, "w");
-            psFree(matrixName);
-            psFitsWriteImage(matrixFile, NULL, dummy, 0, NULL);
-            psFree(dummy);
-            psFitsClose(matrixFile);
-        }
-
-        if (kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
-            matrixName = NULL;
-            psStringAppend(&matrixName, "matrix2_%d.fits", index);
-            matrixFile = psFitsOpen(matrixName, "w");
-            psFree(matrixName);
-            psFitsWriteImage(matrixFile, NULL, stamp->matrix2, 0, NULL);
-            psFitsClose(matrixFile);
-
-            matrixName = NULL;
-            psStringAppend(&matrixName, "matrixX_%d.fits", index);
-            matrixFile = psFitsOpen(matrixName, "w");
-            psFree(matrixName);
-            psFitsWriteImage(matrixFile, NULL, stamp->matrixX, 0, NULL);
-            psFitsClose(matrixFile);
-        }
     }
 #endif
@@ -684 +830 @@
 }
 
-bool pmSubtractionCalculateEquation(pmSubtractionStampList *stamps, const pmSubtractionKernels *kernels)
+bool pmSubtractionCalculateEquation(pmSubtractionStampList *stamps, const pmSubtractionKernels *kernels,
+                                    const pmSubtractionEquationCalculationMode mode)
 {
     PM_ASSERT_SUBTRACTION_STAMP_LIST_NON_NULL(stamps, false);
@@ -699 +846 @@
         }
 
+        if ((stamp->x <= 0.0) && (stamp->y <= 0.0)) {
+            psAbort ("bad stamp");
+        }
+        if (!isfinite(stamp->x) && !isfinite(stamp->y)) {
+            psAbort ("bad stamp");
+        }
+
         if (pmSubtractionThreaded()) {
             psThreadJob *job = psThreadJobAlloc("PSMODULES_SUBTRACTION_CALCULATE_EQUATION");
@@ -704 +858 @@
             psArrayAdd(job->args, 1, (pmSubtractionKernels*)kernels); // Casting away const to put on array
             PS_ARRAY_ADD_SCALAR(job->args, i, PS_TYPE_S32);
+            PS_ARRAY_ADD_SCALAR(job->args, mode, PS_TYPE_S32);
             if (!psThreadJobAddPending(job)) {
                 psFree(job);
@@ -710 +865 @@
             psFree(job);
         } else {
-            pmSubtractionCalculateEquationStamp(stamps, kernels, i);
+            pmSubtractionCalculateEquationStamp(stamps, kernels, i, mode);
         }
     }
 
     if (!psThreadPoolWait(true)) {
-        psError(PS_ERR_UNKNOWN, false, "Error waiting for threads.");
+        psError(psErrorCodeLast(), false, "Error waiting for threads.");
         return false;
     }
 
     pmSubtractionVisualPlotLeastSquares(stamps);
+    pmSubtractionVisualShowKernels((pmSubtractionKernels  *)kernels);
+    pmSubtractionVisualShowBasis(stamps);
 
     psLogMsg("psModules.imcombine", PS_LOG_INFO, "Calculate equation: %f sec",
@@ -728 +885 @@
 }
 
-bool pmSubtractionSolveEquation(pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps)
+// private functions used on pmSubtractionSolveEquation
+bool psVectorWriteFile (char *filename, const psVector *vector);
+bool psFitsWriteImageSimple (char *filename, psImage *image, psMetadata *header);
+
+psImage *p_pmSubSolve_wUt (psVector *w, psImage *U);
+psImage *p_pmSubSolve_VwUt (psImage *V, psImage *wUt);
+
+bool p_pmSubSolve_SetWeights (psVector *wApply, psVector *w, psVector *wMask);
+
+bool p_pmSubSolve_UtB (psVector **UtB, psImage *U, psVector *B);
+bool p_pmSubSolve_wUtB (psVector **wUtB, psVector *w, psVector *UtB);
+bool p_pmSubSolve_VwUtB (psVector **VwUtB, psImage *V, psVector *wUtB);
+
+bool p_pmSubSolve_Ax (psVector **B, psImage *A, psVector *x);
+bool p_pmSubSolve_VdV (double *value, psVector *x, psVector *y);
+bool p_pmSubSolve_y2 (double *y2, pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps);
+
+psImage *p_pmSubSolve_Xvar (psImage *V, psVector *w);
+
+double p_pmSubSolve_ChiSquare (pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps);
+
+bool pmSubtractionSolveEquation(pmSubtractionKernels *kernels,
+                                const pmSubtractionStampList *stamps,
+                                const pmSubtractionEquationCalculationMode mode)
 {
     PM_ASSERT_SUBTRACTION_KERNELS_NON_NULL(kernels, false);
@@ -734 +914 @@
 
     // Check inputs
-    int numParams = -1;                // Number of parameters
-    int numParams2 = 0;                // Number of parameters for part solution (DUAL mode)
+    int numKernels = kernels->num;      // Number of kernel basis functions
+    int numSpatial = PM_SUBTRACTION_POLYTERMS(kernels->spatialOrder); // Number of spatial variations
+    int numBackground = PM_SUBTRACTION_POLYTERMS(kernels->bgOrder); // Number of background terms
+    int numParams = numKernels * numSpatial + 1 + numBackground;    // Number of parameters being solved for
+    int numSolution1 = numParams, numSolution2 = 0;                 // Number of parameters for each solution
+    if (kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
+        // An additional image is convolved
+        numSolution2 = numKernels * numSpatial;
+        numParams += numSolution2;
+    }
+
     for (int i = 0; i < stamps->num; i++) {
         pmSubtractionStamp *stamp = stamps->stamps->data[i]; // Stamp of interest
@@ -743 +932 @@
         }
 
-        PS_ASSERT_VECTOR_NON_NULL(stamp->vector1, false);
-        if (numParams == -1) {
-            numParams = stamp->vector1->n;
-        }
-        PS_ASSERT_VECTOR_SIZE(stamp->vector1, (long)numParams, false);
-        PS_ASSERT_VECTOR_TYPE(stamp->vector1, PS_TYPE_F64, false);
-        PS_ASSERT_IMAGE_NON_NULL(stamp->matrix1, false);
-        PS_ASSERT_IMAGE_SIZE(stamp->matrix1, numParams, numParams, false);
-        PS_ASSERT_IMAGE_TYPE(stamp->matrix1, PS_TYPE_F64, false);
-
-        if (kernels->mode == PM_SUBTRACTION_MODE_DUAL) {
-            PS_ASSERT_IMAGE_NON_NULL(stamp->matrix2, false);
-            PS_ASSERT_IMAGE_NON_NULL(stamp->matrixX, false);
-            if (numParams2 == 0) {
-                numParams2 = stamp->matrix2->numCols;
-            }
-            PS_ASSERT_IMAGE_SIZE(stamp->matrix2, numParams2, numParams2, false);
-            PS_ASSERT_IMAGE_SIZE(stamp->matrixX, numParams, numParams2, false);
-            PS_ASSERT_IMAGE_TYPE(stamp->matrix2, PS_TYPE_F64, false);
-            PS_ASSERT_IMAGE_TYPE(stamp->matrixX, PS_TYPE_F64, false);
-            PS_ASSERT_VECTOR_NON_NULL(stamp->vector2, false);
-            PS_ASSERT_VECTOR_SIZE(stamp->vector2, (long)numParams2, false);
-            PS_ASSERT_VECTOR_TYPE(stamp->vector2, PS_TYPE_F64, false);
-        }
-    }
-    if (numParams == -1) {
-        psError(PS_ERR_BAD_PARAMETER_VALUE, true, "No suitable stamps found.");
-        return NULL;
+        PS_ASSERT_VECTOR_NON_NULL(stamp->vector, false);
+        PS_ASSERT_VECTOR_SIZE(stamp->vector, (long)numParams, false);
+        PS_ASSERT_VECTOR_TYPE(stamp->vector, PS_TYPE_F64, false);
+        PS_ASSERT_IMAGE_NON_NULL(stamp->matrix, false);
+        PS_ASSERT_IMAGE_SIZE(stamp->matrix, numParams, numParams, false);
+        PS_ASSERT_IMAGE_TYPE(stamp->matrix, PS_TYPE_F64, false);
     }
 
@@ -786 +953 @@
         psVectorInit(sumVector, 0.0);
         psImageInit(sumMatrix, 0.0);
+
+        psVector *norms = psVectorAllocEmpty(stamps->num, PS_TYPE_F64); // Normalisations
+
         int numStamps = 0;              // Number of good stamps
         for (int i = 0; i < stamps->num; i++) {
             pmSubtractionStamp *stamp = stamps->stamps->data[i]; // Stamp of interest
-
             if (stamp->status == PM_SUBTRACTION_STAMP_USED) {
-
-#ifdef TESTING
-              // XXX double-check for NAN in data:
-                for (int iy = 0; iy < stamp->matrix1->numRows; iy++) {
-                    for (int ix = 0; ix < stamp->matrix1->numCols; ix++) {
-                        if (!isfinite(stamp->matrix1->data.F64[iy][ix])) {
-                            fprintf (stderr, "WARNING: NAN in matrix1\n");
-                        }
-                    }
-                }
-                for (int ix = 0; ix < stamp->vector1->n; ix++) {
-                    if (!isfinite(stamp->vector1->data.F64[ix])) {
-                        fprintf (stderr, "WARNING: NAN in vector1\n");
-                    }
-                }
-#endif
-
-                (void)psBinaryOp(sumMatrix, sumMatrix, "+", stamp->matrix1);
-                (void)psBinaryOp(sumVector, sumVector, "+", stamp->vector1);
+                (void)psBinaryOp(sumMatrix, sumMatrix, "+", stamp->matrix);
+                (void)psBinaryOp(sumVector, sumVector, "+", stamp->vector);
+                psVectorAppend(norms, stamp->norm);
                 pmSubtractionStampPrint(ds9, stamp->x, stamp->y, stamps->footprint, "green");
                 numStamps++;
@@ -817 +970 @@
         }
 
+#if 0
+        int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background
+        calculatePenalty(sumMatrix, sumVector, kernels, sumMatrix->data.F64[bgIndex][bgIndex]);
+#endif
+
+        psVector *solution = NULL;                       // Solution to equation!
+        solution = psVectorAlloc(numParams, PS_TYPE_F64);
+        psVectorInit(solution, 0);
+
+#if 0
+        // Regular, straight-forward solution
+        solution = psMatrixSolveSVD(solution, sumMatrix, sumVector, NAN);
+#else
+        {
+            // Solve normalisation and background separately
+            int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
+            int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background
+
+            psStats *stats = psStatsAlloc(PS_STAT_ROBUST_MEDIAN); // Statistics for norm
+            if (!psVectorStats(stats, norms, NULL, NULL, 0)) {
+                psError(PM_ERR_DATA, false, "Unable to determine median normalisation");
+                psFree(stats);
+                psFree(sumMatrix);
+                psFree(sumVector);
+                psFree(norms);
+                return false;
+            }
+
+            double normValue = stats->robustMedian;
+            // double bgValue = 0.0;
+
+            psFree(stats);
+
 #ifdef TESTING
-        for (int ix = 0; ix < sumVector->n; ix++) {
-            if (!isfinite(sumVector->data.F64[ix])) {
-                fprintf (stderr, "WARNING: NAN in vector1\n");
-            }
-        }
-#endif
-
-        calculatePenalty(sumVector, kernels);
-
-#ifdef TESTING
-        for (int ix = 0; ix < sumVector->n; ix++) {
-            if (!isfinite(sumVector->data.F64[ix])) {
-                fprintf (stderr, "WARNING: NAN in vector1\n");
-            }
-        }
-        {
-            psImage *inverse = psMatrixInvert(NULL, sumMatrix, NULL);
-            psFits *fits = psFitsOpen("matrixInv.fits", "w");
-            psFitsWriteImage(fits, NULL, inverse, 0, NULL);
-            psFitsClose(fits);
-            psFree(inverse);
-        }
-        {
-            psImage *X = psMatrixInvert(NULL, sumMatrix, NULL);
-            psImage *Xt = psMatrixTranspose(NULL, X);
-            psImage *XtX = psMatrixMultiply(NULL, Xt, X);
-            psFits *fits = psFitsOpen("matrixErr.fits", "w");
-            psFitsWriteImage(fits, NULL, XtX, 0, NULL);
-            psFitsClose(fits);
-            psFree(X);
-            psFree(Xt);
-            psFree(XtX);
-        }
-#endif
-
-        psVector *permutation = NULL;       // Permutation vector, required for LU decomposition
-        psImage *luMatrix = psMatrixLUDecomposition(NULL, &permutation, sumMatrix);
+            fprintf(stderr, "Norm: %lf\n", normValue);
+#endif
+            // Solve kernel components
+            for (int i = 0; i < numSolution1; i++) {
+                sumVector->data.F64[i] -= normValue * sumMatrix->data.F64[normIndex][i];
+
+                sumMatrix->data.F64[i][normIndex] = 0.0;
+                sumMatrix->data.F64[normIndex][i] = 0.0;
+            }
+            sumVector->data.F64[bgIndex] -= normValue * sumMatrix->data.F64[normIndex][bgIndex];
+            sumMatrix->data.F64[bgIndex][normIndex] = 0.0;
+            sumMatrix->data.F64[normIndex][bgIndex] = 0.0;
+
+            sumMatrix->data.F64[normIndex][normIndex] = 1.0;
+            sumVector->data.F64[normIndex] = 0.0;
+
+            solution = psMatrixSolveSVD(solution, sumMatrix, sumVector, NAN);
+
+            solution->data.F64[normIndex] = normValue;
+        }
+# endif
+
+        if (!kernels->solution1) {
+            kernels->solution1 = psVectorAlloc(sumVector->n, PS_TYPE_F64);
+            psVectorInit(kernels->solution1, 0.0);
+        }
+
+        // only update the solutions that we chose to calculate:
+        if (mode & PM_SUBTRACTION_EQUATION_NORM) {
+            int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
+            kernels->solution1->data.F64[normIndex] = solution->data.F64[normIndex];
+        }
+        if (mode & PM_SUBTRACTION_EQUATION_BG) {
+            int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
+            kernels->solution1->data.F64[bgIndex] = solution->data.F64[bgIndex];
+        }
+        if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
+            int numKernels = kernels->num;
+            int spatialOrder = kernels->spatialOrder;       // Order of spatial variation
+            int numPoly = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of polynomial terms
+            for (int i = 0; i < numKernels * numPoly; i++) {
+                kernels->solution1->data.F64[i] = solution->data.F64[i];
+            }
+        }
+
+        psFree(norms);
+        psFree(solution);
+        psFree(sumVector);
         psFree(sumMatrix);
-        if (!luMatrix) {
-            psError(PS_ERR_UNKNOWN, true, "LU Decomposition of least-squares matrix failed.\n");
-            psFree(sumVector);
-            psFree(luMatrix);
-            psFree(permutation);
-            return NULL;
-        }
-        kernels->solution1 = psMatrixLUSolution(kernels->solution1, luMatrix, sumVector, permutation);
 
 #ifdef TESTING
@@ -869 +1058 @@
         for (int ix = 0; ix < kernels->solution1->n; ix++) {
             if (!isfinite(kernels->solution1->data.F64[ix])) {
-                fprintf (stderr, "WARNING: NAN in vector1\n");
-            }
-        }
-#endif
-
-        psFree(sumVector);
-        psFree(luMatrix);
-        psFree(permutation);
-        if (!kernels->solution1) {
-            psError(PS_ERR_UNKNOWN, true, "Failed to solve the least-squares system.\n");
-            return NULL;
-        }
+                fprintf (stderr, "WARNING: NAN in vector\n");
+            }
+        }
+#endif
+
     } else {
         // Dual convolution solution
 
         // Accumulation of stamp matrices/vectors
-        psImage *sumMatrix1 = psImageAlloc(numParams, numParams, PS_TYPE_F64);
-        psImage *sumMatrix2 = psImageAlloc(numParams2, numParams2, PS_TYPE_F64);
-        psImage *sumMatrixX = psImageAlloc(numParams, numParams2, PS_TYPE_F64);
-        psVector *sumVector1 = psVectorAlloc(numParams, PS_TYPE_F64);
-        psVector *sumVector2 = psVectorAlloc(numParams, PS_TYPE_F64);
-        psImageInit(sumMatrix1, 0.0);
-        psImageInit(sumMatrix2, 0.0);
-        psImageInit(sumMatrixX, 0.0);
-        psVectorInit(sumVector1, 0.0);
-        psVectorInit(sumVector2, 0.0);
+        psImage *sumMatrix = psImageAlloc(numParams, numParams, PS_TYPE_F64);
+        psVector *sumVector = psVectorAlloc(numParams, PS_TYPE_F64);
+        psImageInit(sumMatrix, 0.0);
+        psVectorInit(sumVector, 0.0);
+
+        psVector *norms = psVectorAllocEmpty(stamps->num, PS_TYPE_F64); // Normalisations
 
         int numStamps = 0;              // Number of good stamps
@@ -900 +1078 @@
             pmSubtractionStamp *stamp = stamps->stamps->data[i]; // Stamp of interest
             if (stamp->status == PM_SUBTRACTION_STAMP_USED) {
-                (void)psBinaryOp(sumMatrix1, sumMatrix1, "+", stamp->matrix1);
-                (void)psBinaryOp(sumMatrix2, sumMatrix2, "+", stamp->matrix2);
-                (void)psBinaryOp(sumMatrixX, sumMatrixX, "+", stamp->matrixX);
-                (void)psBinaryOp(sumVector1, sumVector1, "+", stamp->vector1);
-                (void)psBinaryOp(sumVector2, sumVector2, "+", stamp->vector2);
+                (void)psBinaryOp(sumMatrix, sumMatrix, "+", stamp->matrix);
+                (void)psBinaryOp(sumVector, sumVector, "+", stamp->vector);
+
+                psVectorAppend(norms, stamp->norm);
+
                 pmSubtractionStampPrint(ds9, stamp->x, stamp->y, stamps->footprint, "green");
                 numStamps++;
             }
         }
-        calculatePenalty(sumVector1, kernels);
-        calculatePenalty(sumVector2, kernels);
-
-        // Pure matrix operations
-
-        // A * a = Ct * b + d
-        // C * a = B  * b + e
-        //
-        // a = (Ct * Bi * C - A)i (Ct * Bi * e - d)
-        // b = Bi * (C * a - e)
-        psVector *a = psVectorRecycle(kernels->solution1, numParams, PS_TYPE_F64);
-        psVector *b = psVectorRecycle(kernels->solution2, numParams2, PS_TYPE_F64);
+
 #ifdef TESTING
-        psVectorInit(a, NAN);
-        psVectorInit(b, NAN);
-#endif
-        psImage *A = sumMatrix1;
-        psImage *B = sumMatrix2;
-        psImage *C = sumMatrixX;
-        psVector *d = sumVector1;
-        psVector *e = sumVector2;
-
-        assert(a->n == numParams);
-        assert(b->n == numParams2);
-        assert(A->numRows == numParams && A->numCols == numParams);
-        assert(B->numRows == numParams2 && B->numCols == numParams2);
-        assert(C->numRows == numParams2 && C->numCols == numParams);
-        assert(d->n == numParams);
-        assert(e->n == numParams2);
-
-        psImage *Bi = psMatrixInvert(NULL, B, NULL);
-        assert(Bi->numRows == numParams2 && Bi->numCols == numParams2);
-        psImage *Ct = psMatrixTranspose(NULL, C);
-        assert(Ct->numRows == numParams && Ct->numCols == numParams2);
-
-        psImage *BiC = psMatrixMultiply(NULL, Bi, C);
-        assert(BiC->numRows == numParams2 && BiC->numCols == numParams);
-        psImage *CtBi = psMatrixMultiply(NULL, Ct, Bi);
-        assert(CtBi->numRows == numParams && CtBi->numCols == numParams2);
-
-        psImage *CtBiC = psMatrixMultiply(NULL, Ct, BiC);
-        assert(CtBiC->numRows == numParams && CtBiC->numCols == numParams);
-
-        psImage *F = (psImage*)psBinaryOp(NULL, CtBiC, "-", A);
-        assert(F->numRows == numParams && F->numCols == numParams);
-        float det = 0.0;
-        psImage *Fi = psMatrixInvert(NULL, F, &det);
-        assert(Fi->numRows == numParams && Fi->numCols == numParams);
-        psTrace("psModules.imcombine", 4, "Determinant of F: %f\n", det);
-
-        psVector *g = psVectorAlloc(numParams, PS_TYPE_F64);
+        psFitsWriteImageSimple ("sumMatrix.fits", sumMatrix, NULL);
+        psVectorWriteFile("sumVector.dat", sumVector);
+#endif
+
+#if 1
+        // int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background
+        // calculatePenalty(sumMatrix, sumVector, kernels, sumMatrix->data.F64[bgIndex][bgIndex]);
+
+        int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
+        calculatePenalty(sumMatrix, sumVector, kernels, sumMatrix->data.F64[normIndex][normIndex] / 1000.0);
+#endif
+
+        psVector *solution = NULL;                       // Solution to equation!
+        solution = psVectorAlloc(numParams, PS_TYPE_F64);
+        psVectorInit(solution, 0);
+
+#if 0
+        // Regular, straight-forward solution
+        solution = psMatrixSolveSVD(solution, sumMatrix, sumVector, NAN);
+#else
+        {
+            // Solve normalisation and background separately
+            int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
+            int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background
+
+#if 0
+            psImage *normMatrix = psImageAlloc(2, 2, PS_TYPE_F64);
+            psVector *normVector = psVectorAlloc(2, PS_TYPE_F64);
+
+            normMatrix->data.F64[0][0] = sumMatrix->data.F64[normIndex][normIndex];
+            normMatrix->data.F64[1][1] = sumMatrix->data.F64[bgIndex][bgIndex];
+            normMatrix->data.F64[0][1] = normMatrix->data.F64[1][0] = sumMatrix->data.F64[normIndex][bgIndex];
+
+            normVector->data.F64[0] = sumVector->data.F64[normIndex];
+            normVector->data.F64[1] = sumVector->data.F64[bgIndex];
+
+            psVector *normSolution = psMatrixSolveSVD(NULL, normMatrix, normVector, NAN);
+
+            double normValue = normSolution->data.F64[0];
+            double bgValue = normSolution->data.F64[1];
+
+            psFree(normMatrix);
+            psFree(normVector);
+            psFree(normSolution);
+#endif
+
+            psStats *stats = psStatsAlloc(PS_STAT_ROBUST_MEDIAN); // Statistics for norm
+            if (!psVectorStats(stats, norms, NULL, NULL, 0)) {
+                psError(PM_ERR_DATA, false, "Unable to determine median normalisation");
+                psFree(stats);
+                psFree(sumMatrix);
+                psFree(sumVector);
+                psFree(norms);
+                return false;
+            }
+
+            double normValue = stats->robustMedian;
+
+            psFree(stats);
+
 #ifdef TESTING
-        psVectorInit(g, NAN);
-#endif
-        assert(CtBi->numRows == numParams && CtBi->numCols == numParams2);
-        assert(e->n == numParams2);
-        assert(d->n == numParams);
-        for (int i = 0; i < numParams; i++) {
-            double value = 0.0;
-            for (int j = 0; j < numParams2; j++) {
-                value += CtBi->data.F64[i][j] * e->data.F64[j];
-            }
-            g->data.F64[i] = value - d->data.F64[i];
-        }
-
-        assert(Fi->numRows == numParams && Fi->numCols == numParams);
-        assert(g->n == numParams);
-        for (int i = 0; i < numParams; i++) {
-            double value = 0.0;
-            for (int j = 0; j < numParams; j++) {
-                value += Fi->data.F64[i][j] * g->data.F64[j];
-            }
-            a->data.F64[i] = value;
-        }
-
-        psVector *h = psVectorAlloc(numParams2, PS_TYPE_F64);
+            fprintf(stderr, "Norm: %lf\n", normValue);
+#endif
+
+            // Solve kernel components
+            for (int i = 0; i < numSolution2; i++) {
+                sumVector->data.F64[i] -= normValue * sumMatrix->data.F64[normIndex][i];
+                sumVector->data.F64[i + numSolution1] -= normValue * sumMatrix->data.F64[normIndex][i + numSolution1];
+
+                sumMatrix->data.F64[i][normIndex] = 0.0;
+                sumMatrix->data.F64[normIndex][i] = 0.0;
+
+                sumMatrix->data.F64[i + numSolution1][normIndex] = 0.0;
+                sumMatrix->data.F64[normIndex][i + numSolution1] = 0.0;
+            }
+            sumVector->data.F64[bgIndex] -= normValue * sumMatrix->data.F64[normIndex][bgIndex];
+            sumMatrix->data.F64[bgIndex][normIndex] = 0.0;
+            sumMatrix->data.F64[normIndex][bgIndex] = 0.0;
+
+            sumMatrix->data.F64[normIndex][normIndex] = 1.0;
+
+            sumVector->data.F64[normIndex] = 0.0;
+
+            solution = psMatrixSolveSVD(solution, sumMatrix, sumVector, NAN);
+
+            solution->data.F64[normIndex] = normValue;
+        }
+#endif
+
+
 #ifdef TESTING
-        psVectorInit(h, NAN);
-#endif
-        assert(C->numRows == numParams2 && C->numCols == numParams);
-        assert(a->n == numParams);
-        assert(e->n == numParams2);
-        for (int i = 0; i < numParams2; i++) {
-            double value = 0.0;
-            for (int j = 0; j < numParams; j++) {
-                value += C->data.F64[i][j] * a->data.F64[j];
-            }
-            h->data.F64[i] = value - e->data.F64[i];
-        }
-
-        assert(Bi->numRows == numParams2 && Bi->numCols == numParams2);
-        assert(h->n == numParams2);
-        for (int i = 0; i < numParams2; i++) {
-            double value = 0.0;
-            for (int j = 0; j < numParams2; j++) {
-                value += Bi->data.F64[i][j] * h->data.F64[j];
-            }
-            b->data.F64[i] = value;
-        }
-
-
-#if 0
-        for (int i = 0; i < numParams; i++) {
-            double aVal1 = 0.0, bVal1 = 0.0;
-            for (int j = 0; j < numParams2; j++) {
-                aVal1 += A->data.F64[i][j] * a->data.F64[j];
-                bVal1 += Ct->data.F64[i][j] * b->data.F64[j];
-            }
-            bVal1 += d->data.F64[i];
-            for (int j = numParams2; j < numParams; j++) {
-                aVal1 += A->data.F64[i][j] * a->data.F64[j];
-            }
-            printf("%d: %lf\n", i, aVal1 - bVal1);
-        }
-
-        for (int i = 0; i < numParams2; i++) {
-            double aVal2 = 0.0, bVal2 = 0.0;
-            for (int j = 0; j < numParams2; j++) {
-                aVal2 += C->data.F64[i][j] * a->data.F64[j];
-                bVal2 += B->data.F64[i][j] * b->data.F64[j];
-            }
-            bVal2 += e->data.F64[i];
-            for (int j = numParams2; j < numParams; j++) {
-                aVal2 += C->data.F64[i][j] * a->data.F64[j];
-            }
-            printf("%d: %lf\n", i, aVal2 - bVal2);
-        }
-#endif
-
-#ifdef TESTING
-        {
-            psFits *fits = psFitsOpen("sumMatrix1.fits", "w");
-            psFitsWriteImage(fits, NULL, sumMatrix1, 0, NULL);
-            psFitsClose(fits);
-        }
-        {
-            psFits *fits = psFitsOpen("sumMatrix2.fits", "w");
-            psFitsWriteImage(fits, NULL, sumMatrix2, 0, NULL);
-            psFitsClose(fits);
-        }
-        {
-            psFits *fits = psFitsOpen("sumMatrixX.fits", "w");
-            psFitsWriteImage(fits, NULL, sumMatrixX, 0, NULL);
-            psFitsClose(fits);
-        }
-        {
-            psFits *fits = psFitsOpen("sumFinverse.fits", "w");
-            psFitsWriteImage(fits, NULL, Fi, 0, NULL);
-            psFitsClose(fits);
-        }
-#endif
-
-        kernels->solution1 = a;
-        kernels->solution2 = b;
-
-        // XXXXX Free temporary matrices and vectors
+        for (int i = 0; i < solution->n; i++) {
+            fprintf(stderr, "Dual solution %d: %lf\n", i, solution->data.F64[i]);
+        }
+#endif
+
+        psFree(sumMatrix);
+        psFree(sumVector);
+
+        psFree(norms);
+
+        if (!kernels->solution1) {
+            kernels->solution1 = psVectorAlloc(numSolution1, PS_TYPE_F64);
+            psVectorInit (kernels->solution1, 0.0);
+        }
+        if (!kernels->solution2) {
+            kernels->solution2 = psVectorAlloc(numSolution2, PS_TYPE_F64);
+            psVectorInit (kernels->solution2, 0.0);
+        }
+
+        // only update the solutions that we chose to calculate:
+        if (mode & PM_SUBTRACTION_EQUATION_NORM) {
+            int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
+            kernels->solution1->data.F64[normIndex] = solution->data.F64[normIndex];
+        }
+        if (mode & PM_SUBTRACTION_EQUATION_BG) {
+            int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
+            kernels->solution1->data.F64[bgIndex] = solution->data.F64[bgIndex];
+        }
+        if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
+            int numKernels = kernels->num;
+            for (int i = 0; i < numKernels * numSpatial; i++) {
+                // XXX fprintf (stderr, "keep\n");
+                kernels->solution1->data.F64[i] = solution->data.F64[i];
+                kernels->solution2->data.F64[i] = solution->data.F64[i + numSolution1];
+            }
+        }
+
+
+        memcpy(kernels->solution1->data.F64, solution->data.F64,
+               numSolution1 * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
+        memcpy(kernels->solution2->data.F64, &solution->data.F64[numSolution1],
+               numSolution2 * PSELEMTYPE_SIZEOF(PS_TYPE_F64));
+
+        psFree(solution);
 
     }
@@ -1083 +1242 @@
      }
 
-    pmSubtractionVisualPlotLeastSquares((pmSubtractionStampList *) stamps); //casting away const
+    // pmSubtractionVisualPlotLeastSquares((pmSubtractionStampList *) stamps); //casting away const
     return true;
 }
 
+bool pmSubtractionResidualStats(psVector *fSigRes, psVector *fMaxRes, psVector *fMinRes, psKernel *target, psKernel *source, psKernel *residual, double norm, int footprint) {
+
+    // XXX measure some useful stats on the residuals
+    float sum = 0.0;
+    float peak = 0.0;
+    for (int y = - footprint; y <= footprint; y++) {
+        for (int x = - footprint; x <= footprint; x++) {
+            sum += 0.5*(target->kernel[y][x] + source->kernel[y][x] * norm);
+            peak = PS_MAX(peak, 0.5*(target->kernel[y][x] + source->kernel[y][x] * norm));
+        }
+    }
+
+    // only count pixels with more than X% of the source flux
+    // calculate stdev(dflux)
+    float dflux1 = 0.0;
+    float dflux2 = 0.0;
+    int npix = 0;
+
+    float dmax = 0.0;
+    float dmin = 0.0;
+
+    for (int y = - footprint; y <= footprint; y++) {
+        for (int x = - footprint; x <= footprint; x++) {
+            float dflux = 0.5*(target->kernel[y][x] + source->kernel[y][x] * norm);
+            if (dflux < 0.02*sum) continue;
+            dflux1 += residual->kernel[y][x];
+            dflux2 += PS_SQR(residual->kernel[y][x]);
+            dmax = PS_MAX(residual->kernel[y][x], dmax);
+            dmin = PS_MIN(residual->kernel[y][x], dmin);
+            npix ++;
+        }
+    }
+    float sigma = sqrt(dflux2 / npix - PS_SQR(dflux1/npix));
+    if (!isfinite(sum))  return false;
+    if (!isfinite(dmax)) return false;
+    if (!isfinite(dmin)) return false;
+    if (!isfinite(peak)) return false;
+
+    // fprintf (stderr, "sum: %f, peak: %f, sigma: %f, fsigma: %f, fmax: %f, fmin: %f\n", sum, peak, sigma, sigma/sum, dmax/peak, dmin/peak);
+    psVectorAppend(fSigRes, sigma/sum);
+    psVectorAppend(fMaxRes, dmax/peak);
+    psVectorAppend(fMinRes, dmin/peak);
+    return true;
+}
+
 psVector *pmSubtractionCalculateDeviations(pmSubtractionStampList *stamps,
-                                           const pmSubtractionKernels *kernels)
+                                           pmSubtractionKernels *kernels)
 {
     PM_ASSERT_SUBTRACTION_STAMP_LIST_NON_NULL(stamps, NULL);
@@ -1103 +1307 @@
     psKernel *residual = psKernelAlloc(-footprint, footprint, -footprint, footprint); // Residual image
 
+    // set up holding images for the visualization
+    pmSubtractionVisualShowFitInit (stamps);
+
+    psVector *fSigRes = psVectorAllocEmpty(stamps->num, PS_TYPE_F32);
+    psVector *fMinRes = psVectorAllocEmpty(stamps->num, PS_TYPE_F32);
+    psVector *fMaxRes = psVectorAllocEmpty(stamps->num, PS_TYPE_F32);
+
+    // we want to save the residual images for the 9 brightest stamps.
+    // identify the 9 brightest stamps
+    psVector *keepStamps  = psVectorAlloc(stamps->num, PS_TYPE_S32);
+    psVectorInit (keepStamps, 0);
+    {
+        psVector *flux  = psVectorAlloc(stamps->num, PS_TYPE_F32);
+        psVectorInit (flux, 0.0);
+
+        for (int i = 0; i < stamps->num; i++) {
+            pmSubtractionStamp *stamp = stamps->stamps->data[i];
+            if (!isfinite(stamp->flux)) continue;
+            flux->data.F32[i] = stamp->flux;
+        }
+
+        psVector *index = psVectorSortIndex(NULL, flux);
+        for (int i = 0; (i < stamps->num) && (i < 9); i++) {
+            int n = stamps->num - i - 1;
+            keepStamps->data.S32[index->data.S32[n]] = 1;
+        }
+        psFree (flux);
+        psFree (index);
+
+        // this function is called multiple times in the iteration, but
+        // we only know after the interation is done if we will try again.
+        // therefore we must save the sample each time, and blow away the old one
+        // if it exists.
+        psFree (kernels->sampleStamps);
+        kernels->sampleStamps = psArrayAllocEmpty(9);
+    }
+
+    psString log = psStringCopy("Deviations:\n");               // Log message with deviations
     for (int i = 0; i < stamps->num; i++) {
         pmSubtractionStamp *stamp = stamps->stamps->data[i]; // The stamp of interest
@@ -1116 +1358 @@
 
         // Calculate residuals
-        psKernel *variance = stamp->variance; // Variance postage stamp
+        psKernel *weight = stamp->weight; // Weight postage stamp
         psImageInit(residual->image, 0.0);
         if (kernels->mode != PM_SUBTRACTION_MODE_DUAL) {
@@ -1133 +1375 @@
                                                           false); // Kernel image
                 if (!image) {
-                    psError(PS_ERR_UNKNOWN, false, "Unable to generate image of kernel.");
+                    psError(psErrorCodeLast(), false, "Unable to generate image of kernel.");
                     return false;
                 }
@@ -1162 +1404 @@
                 for (int y = - footprint; y <= footprint; y++) {
                     for (int x = - footprint; x <= footprint; x++) {
-                        residual->kernel[y][x] -= convolution->kernel[y][x] * coefficient;
+                        residual->kernel[y][x] += convolution->kernel[y][x] * coefficient;
                     }
                 }
             }
+
+            // XXX visualize the target, source, convolution and residual
+            pmSubtractionVisualShowFitAddStamp (target, source, residual, background, norm, i);
+
             for (int y = - footprint; y <= footprint; y++) {
                 for (int x = - footprint; x <= footprint; x++) {
-                    residual->kernel[y][x] += target->kernel[y][x] - background - source->kernel[y][x] * norm;
-                }
-            }
+                    residual->kernel[y][x] += background + source->kernel[y][x] * norm - target->kernel[y][x];
+                }
+            }
+
+            if (keepStamps->data.S32[i]) {
+                psImage *sample = psImageCopy(NULL, residual->image, PS_TYPE_F32);
+                psArrayAdd (kernels->sampleStamps, 9, sample);
+                psFree (sample);
+            }
+
+            pmSubtractionResidualStats(fSigRes, fMaxRes, fMinRes, target, source, residual, norm, footprint);
+
         } else {
             // Dual convolution
@@ -1186 +1441 @@
                 for (int y = - footprint; y <= footprint; y++) {
                     for (int x = - footprint; x <= footprint; x++) {
-                        residual->kernel[y][x] += conv2->kernel[y][x] * coeff2 - conv1->kernel[y][x] * coeff1;
+                        residual->kernel[y][x] += conv2->kernel[y][x] * coeff2 + conv1->kernel[y][x] * coeff1;
                     }
                 }
             }
+
+            // XXX visualize the target, source, convolution and residual
+            pmSubtractionVisualShowFitAddStamp (image2, image1, residual, background, norm, i);
+
             for (int y = - footprint; y <= footprint; y++) {
                 for (int x = - footprint; x <= footprint; x++) {
-                    residual->kernel[y][x] += image2->kernel[y][x] - background - image1->kernel[y][x] * norm;
-                }
-            }
+                    residual->kernel[y][x] += background + image1->kernel[y][x] * norm - image2->kernel[y][x];
+                }
+            }
+            if (keepStamps->data.S32[i]) {
+                psImage *sample = psImageCopy(NULL, residual->image, PS_TYPE_F32);
+                psArrayAdd (kernels->sampleStamps, 9, sample);
+                psFree (sample);
+            }
+
+            pmSubtractionResidualStats(fSigRes, fMaxRes, fMinRes, image1, image2, residual, norm, footprint);
         }
 
@@ -1200 +1466 @@
         for (int y = - footprint; y <= footprint; y++) {
             for (int x = - footprint; x <= footprint; x++) {
-                double dev = PS_SQR(residual->kernel[y][x]) / variance->kernel[y][x];
+                double dev = PS_SQR(residual->kernel[y][x]) * weight->kernel[y][x];
                 deviation += dev;
 #ifdef TESTING
@@ -1209 +1475 @@
         deviations->data.F32[i] = devNorm * deviation;
         psTrace("psModules.imcombine", 5, "Deviation for stamp %d (%d,%d): %f\n",
-                i, (int)(stamp->x + 0.5), (int)(stamp->y + 0.5), deviations->data.F32[i]);
+                i, (int)(stamp->x - 0.5), (int)(stamp->y - 0.5), deviations->data.F32[i]);
+        psStringAppend(&log, "Stamp %d (%d,%d): %f\n",
+                       i, (int)(stamp->x - 0.5), (int)(stamp->y - 0.5), deviations->data.F32[i]);
         if (!isfinite(deviations->data.F32[i])) {
             stamp->status = PM_SUBTRACTION_STAMP_REJECTED;
             psTrace("psModules.imcombine", 5,
                     "Rejecting stamp %d (%d,%d) because of non-finite deviation\n",
-                    i, (int)(stamp->x + 0.5), (int)(stamp->y + 0.5));
+                    i, (int)(stamp->x - 0.5), (int)(stamp->y - 0.5));
             continue;
         }
@@ -1243 +1511 @@
             psFitsClose(fits);
         }
-        if (stamp->variance) {
+        if (stamp->weight) {
             psString filename = NULL;
-            psStringAppend(&filename, "stamp_variance_%03d.fits", i);
+            psStringAppend(&filename, "stamp_weight_%03d.fits", i);
             psFits *fits = psFitsOpen(filename, "w");
             psFree(filename);
-            psFitsWriteImage(fits, NULL, stamp->variance->image, 0, NULL);
+            psFitsWriteImage(fits, NULL, stamp->weight->image, 0, NULL);
             psFitsClose(fits);
         }
@@ -1254 +1522 @@
 
     }
+
+    psFree(keepStamps);
+
+    psLogMsg("psModules.imcombine", PS_LOG_DETAIL, "%s", log);
+    psFree(log);
+
+    // calculate and report the normalization and background for the image center
+    {
+        polyValues = p_pmSubtractionPolynomial(polyValues, kernels->spatialOrder, 0.0, 0.0);
+        double norm = p_pmSubtractionSolutionNorm(kernels); // Normalisation
+        double background = p_pmSubtractionSolutionBackground(kernels, polyValues);// Difference in background
+        psLogMsg("psModules.imcombine", PS_LOG_INFO, "normalization: %f, background: %f", norm, background);
+
+        pmSubtractionVisualShowFit(norm);
+        pmSubtractionVisualPlotFit(kernels);
+
+        psStats *stats = psStatsAlloc(PS_STAT_ROBUST_MEDIAN | PS_STAT_ROBUST_STDEV);
+        psVectorStats (stats, fSigRes, NULL, NULL, 0);
+        kernels->fSigResMean = stats->robustMedian;
+        kernels->fSigResStdev = stats->robustStdev;
+
+        psStatsInit (stats);
+        psVectorStats (stats, fMaxRes, NULL, NULL, 0);
+        kernels->fMaxResMean = stats->robustMedian;
+        kernels->fMaxResStdev = stats->robustStdev;
+
+        psStatsInit (stats);
+        psVectorStats (stats, fMinRes, NULL, NULL, 0);
+        kernels->fMinResMean = stats->robustMedian;
+        kernels->fMinResStdev = stats->robustStdev;
+
+        // XXX save these values somewhere
+        psLogMsg("psModules.imcombine", PS_LOG_INFO, "fSigma: %f +/- %f, fMaxRes: %f +/- %f, fMinRes: %f +/- %f",
+                 kernels->fSigResMean, kernels->fSigResStdev,
+                 kernels->fMaxResMean, kernels->fMaxResStdev,
+                 kernels->fMinResMean, kernels->fMinResStdev);
+
+        psFree (fSigRes);
+        psFree (fMaxRes);
+        psFree (fMinRes);
+        psFree (stats);
+    }
+
     psFree(residual);
     psFree(polyValues);
 
+
     return deviations;
 }
+
+// we are supplied U, not Ut; w represents a diagonal matrix (also, we apply 1/w instead of w)
+psImage *p_pmSubSolve_wUt (psVector *w, psImage *U) {
+
+    psAssert (w->n == U->numCols, "w and U dimensions do not match");
+
+    // wUt has dimensions transposed relative to Ut.
+    psImage *wUt = psImageAlloc (U->numRows, U->numCols, PS_TYPE_F64);
+    psImageInit (wUt, 0.0);
+
+    for (int i = 0; i < wUt->numCols; i++) {
+        for (int j = 0; j < wUt->numRows; j++) {
+            if (!isfinite(w->data.F64[j])) continue;
+            if (w->data.F64[j] == 0.0) continue;
+            wUt->data.F64[j][i] = U->data.F64[i][j] / w->data.F64[j];
+        }
+    }
+    return wUt;
+}
+
+// XXX this is just standard matrix multiplication: use psMatrixMultiply?
+psImage *p_pmSubSolve_VwUt (psImage *V, psImage *wUt) {
+
+    psAssert (V->numCols == wUt->numRows, "matrix dimensions do not match");
+
+    psImage *Ainv = psImageAlloc (wUt->numCols, V->numRows, PS_TYPE_F64);
+
+    for (int i = 0; i < Ainv->numCols; i++) {
+        for (int j = 0; j < Ainv->numRows; j++) {
+            double sum = 0.0;
+            for (int k = 0; k < V->numCols; k++) {
+                sum += V->data.F64[j][k] * wUt->data.F64[k][i];
+            }
+            Ainv->data.F64[j][i] = sum;
+        }
+    }
+    return Ainv;
+}
+
+// we are supplied U, not Ut
+bool p_pmSubSolve_UtB (psVector **UtB, psImage *U, psVector *B) {
+
+    psAssert (U->numRows == B->n, "U and B dimensions do not match");
+
+    UtB[0] = psVectorRecycle (UtB[0], U->numCols, PS_TYPE_F64);
+
+    for (int i = 0; i < U->numCols; i++) {
+        double sum = 0.0;
+        for (int j = 0; j < U->numRows; j++) {
+            sum += B->data.F64[j] * U->data.F64[j][i];
+        }
+        UtB[0]->data.F64[i] = sum;
+    }
+    return true;
+}
+
+// w is diagonal
+bool p_pmSubSolve_wUtB (psVector **wUtB, psVector *w, psVector *UtB) {
+
+    psAssert (w->n == UtB->n, "w and UtB dimensions do not match");
+
+    // wUt has dimensions transposed relative to Ut.
+    wUtB[0] = psVectorRecycle (wUtB[0], w->n, PS_TYPE_F64);
+    psVectorInit (wUtB[0], 0.0);
+
+    for (int i = 0; i < w->n; i++) {
+        if (!isfinite(w->data.F64[i])) continue;
+        if (w->data.F64[i] == 0.0) continue;
+        wUtB[0]->data.F64[i] = UtB->data.F64[i] / w->data.F64[i];
+    }
+    return true;
+}
+
+// this is basically matrix * vector
+bool p_pmSubSolve_VwUtB (psVector **VwUtB, psImage *V, psVector *wUtB) {
+
+    psAssert (V->numCols == wUtB->n, "V and wUtB dimensions do not match");
+
+    VwUtB[0] = psVectorRecycle (*VwUtB, V->numRows, PS_TYPE_F64);
+
+    for (int j = 0; j < V->numRows; j++) {
+        double sum = 0.0;
+        for (int i = 0; i < V->numCols; i++) {
+            sum += V->data.F64[j][i] * wUtB->data.F64[i];
+        }
+        VwUtB[0]->data.F64[j] = sum;
+    }
+    return true;
+}
+
+// this is basically matrix * vector
+bool p_pmSubSolve_Ax (psVector **B, psImage *A, psVector *x) {
+
+    psAssert (A->numCols == x->n, "A and x dimensions do not match");
+
+    B[0] = psVectorRecycle (*B, A->numRows, PS_TYPE_F64);
+
+    for (int j = 0; j < A->numRows; j++) {
+        double sum = 0.0;
+        for (int i = 0; i < A->numCols; i++) {
+            sum += A->data.F64[j][i] * x->data.F64[i];
+        }
+        B[0]->data.F64[j] = sum;
+    }
+    return true;
+}
+
+// this is basically Vector * vector
+bool p_pmSubSolve_VdV (double *value, psVector *x, psVector *y) {
+
+    psAssert (x->n == y->n, "x and y dimensions do not match");
+
+    double sum = 0.0;
+    for (int i = 0; i < x->n; i++) {
+        sum += x->data.F64[i] * y->data.F64[i];
+    }
+    *value = sum;
+    return true;
+}
+
+bool p_pmSubSolve_y2 (double *y2, pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps) {
+
+    int footprint = stamps->footprint; // Half-size of stamps
+
+    double sum = 0.0;
+    for (int i = 0; i < stamps->num; i++) {
+
+        pmSubtractionStamp *stamp = stamps->stamps->data[i];
+        if (stamp->status != PM_SUBTRACTION_STAMP_USED) continue;
+
+        psKernel *weight = NULL;
+        psKernel *window = NULL;
+        psKernel *input = NULL;
+
+#ifdef USE_WEIGHT
+        weight = stamp->weight;
+#endif
+#ifdef USE_WINDOW
+        window = stamps->window;
+#endif
+
+        switch (kernels->mode) {
+            // MODE_1 : convolve image 1 to match image 2 (and vice versa)
+          case PM_SUBTRACTION_MODE_1:
+            input = stamp->image2;
+            break;
+          case PM_SUBTRACTION_MODE_2:
+            input = stamp->image1;
+            break;
+          default:
+            psAbort ("programming error");
+        }
+
+        for (int y = - footprint; y <= footprint; y++) {
+            for (int x = - footprint; x <= footprint; x++) {
+                double in = input->kernel[y][x];
+                double value = in*in;
+                if (weight) {
+                    float wtVal = weight->kernel[y][x];
+                    value *= wtVal;
+                }
+                if (window) {
+                    float  winVal = window->kernel[y][x];
+                    value *= winVal;
+                }
+                sum += value;
+            }
+        }
+    }
+    *y2 = sum;
+    return true;
+}
+
+double p_pmSubSolve_ChiSquare (pmSubtractionKernels *kernels, const pmSubtractionStampList *stamps) {
+
+    int footprint = stamps->footprint; // Half-size of stamps
+    int numKernels = kernels->num;      // Number of kernels
+
+    double sum = 0.0;
+
+    psKernel *residual = psKernelAlloc(-footprint, footprint, -footprint, footprint); // Residual image
+    psImageInit(residual->image, 0.0);
+
+    psImage *polyValues = NULL;         // Polynomial values
+
+    for (int i = 0; i < stamps->num; i++) {
+
+        pmSubtractionStamp *stamp = stamps->stamps->data[i];
+        if (stamp->status != PM_SUBTRACTION_STAMP_USED) continue;
+
+        psKernel *weight = NULL;
+        psKernel *window = NULL;
+        psKernel *target = NULL;
+        psKernel *source = NULL;
+
+        psArray *convolutions = NULL;
+
+#ifdef USE_WEIGHT
+        weight = stamp->weight;
+#endif
+#ifdef USE_WINDOW
+        window = stamps->window;
+#endif
+
+        switch (kernels->mode) {
+            // MODE_1 : convolve image 1 to match image 2 (and vice versa)
+          case PM_SUBTRACTION_MODE_1:
+            target = stamp->image2;
+            source = stamp->image1;
+            convolutions = stamp->convolutions1;
+            break;
+          case PM_SUBTRACTION_MODE_2:
+            target = stamp->image1;
+            source = stamp->image2;
+            convolutions = stamp->convolutions2;
+            break;
+          default:
+            psAbort ("programming error");
+        }
+
+        // Calculate coefficients of the kernel basis functions
+        polyValues = p_pmSubtractionPolynomial(polyValues, kernels->spatialOrder, stamp->xNorm, stamp->yNorm);
+        double norm = p_pmSubtractionSolutionNorm(kernels); // Normalisation
+        double background = p_pmSubtractionSolutionBackground(kernels, polyValues);// Difference in background
+
+        psImageInit(residual->image, 0.0);
+        for (int j = 0; j < numKernels; j++) {
+            psKernel *convolution = convolutions->data[j]; // Convolution
+            double coefficient = p_pmSubtractionSolutionCoeff(kernels, polyValues, j, false); // Coefficient
+            for (int y = - footprint; y <= footprint; y++) {
+                for (int x = - footprint; x <= footprint; x++) {
+                    residual->kernel[y][x] -= convolution->kernel[y][x] * coefficient;
+                }
+            }
+        }
+
+        for (int y = - footprint; y <= footprint; y++) {
+            for (int x = - footprint; x <= footprint; x++) {
+                double resid = target->kernel[y][x] - background - source->kernel[y][x] * norm + residual->kernel[y][x];
+                double value = PS_SQR(resid);
+                if (weight) {
+                    float wtVal = weight->kernel[y][x];
+                    value *= wtVal;
+                }
+                if (window) {
+                    float  winVal = window->kernel[y][x];
+                    value *= winVal;
+                }
+                sum += value;
+            }
+        }
+    }
+    psFree (polyValues);
+    psFree (residual);
+
+    return sum;
+}
+
+bool p_pmSubSolve_SetWeights (psVector *wApply, psVector *w, psVector *wMask) {
+
+    for (int i = 0; i < w->n; i++) {
+        wApply->data.F64[i] = wMask->data.U8[i] ? 0.0 : w->data.F64[i];
+    }
+    return true;
+}
+
+// we are supplied V and w; w represents a diagonal matrix (also, we apply 1/w instead of w)
+psImage *p_pmSubSolve_Xvar (psImage *V, psVector *w) {
+
+    psAssert (w->n == V->numCols, "w and U dimensions do not match");
+
+    psImage *Vn = psImageAlloc (V->numCols, V->numRows, PS_TYPE_F64);
+    psImageInit (Vn, 0.0);
+
+    // generate Vn = V * w^{-1}
+    for (int j = 0; j < Vn->numRows; j++) {
+        for (int i = 0; i < Vn->numCols; i++) {
+            if (!isfinite(w->data.F64[i])) continue;
+            if (w->data.F64[i] == 0.0) continue;
+            Vn->data.F64[j][i] = V->data.F64[j][i] / w->data.F64[i];
+        }
+    }
+
+    psImage *Xvar = psImageAlloc (V->numCols, V->numRows, PS_TYPE_F64);
+    psImageInit (Xvar, 0.0);
+
+    // generate Xvar = Vn * Vn^T
+    for (int j = 0; j < Vn->numRows; j++) {
+        for (int i = 0; i < Vn->numCols; i++) {
+            double sum = 0.0;
+            for (int k = 0; k < Vn->numCols; k++) {
+                sum += Vn->data.F64[k][i]*Vn->data.F64[k][j];
+            }
+            Xvar->data.F64[j][i] = sum;
+        }
+    }
+    return Xvar;
+}
+
+// I get confused by the index values between the image vs matrix usage:  In terms
+// of the elements of an image A(x,y) = A->data.F64[y][x] = A_x,y, a matrix
+// multiplication is: A_k,j * B_i,k = C_i,j
+
+
+bool psFitsWriteImageSimple (char *filename, psImage *image, psMetadata *header) {
+
+    psFits *fits = psFitsOpen(filename, "w");
+    psFitsWriteImage(fits, header, image, 0, NULL);
+    psFitsClose(fits);
+
+    return true;
+}
+
+bool psVectorWriteFile (char *filename, const psVector *vector) {
+
+    FILE *f = fopen (filename, "w");
+    int fd = fileno(f);
+    p_psVectorPrint (fd, vector, "unnamed");
+    fclose (f);
+
+    return true;
+}
+
+
+# if 0
+
+#ifdef TESTING
+        psFitsWriteImageSimple("A.fits", sumMatrix, NULL);
+        psVectorWriteFile ("B.dat", sumVector);
+#endif
+
+# define SVD_ANALYSIS 0
+# define COEFF_SIG 0.0
+# define SVD_TOL 0.0
+
+        // Use SVD to determine the kernel coeffs (and validate)
+        if (SVD_ANALYSIS) {
+
+            // We have sumVector and sumMatrix.  we are trying to solve the following equation:
+            // sumMatrix * x = sumVector.
+
+            // we can use any standard matrix inversion to solve this.  However, the basis
+            // functions in general have substantial correlation, so that the solution may be
+            // somewhat poorly determined or unstable.  If not numerically ill-conditioned, the
+            // system of equations may be statistically ill-conditioned.  Noise in the image
+            // will drive insignificant, but correlated, terms in the solution.  To avoid these
+            // problems, we can use SVD to identify numerically unconstrained values and to
+            // avoid statistically badly determined value.
+
+            // A = sumMatrix, B = sumVector
+            // SVD: A = U w V^T  -> A^{-1} = V (1/w) U^T
+            // x = V (1/w) (U^T B)
+            // \sigma_x = sqrt(diag(A^{-1}))
+            // solve for x and A^{-1} to get x & dx
+            // identify the elements of (1/w) that are nan (1/0.0) -> set to 0.0
+            // identify the elements of x that are insignificant (x / dx < 1.0? < 0.5?) -> set to 0.0
+
+            // If I use the SVD trick to re-condition the matrix, I need to break out the
+            // kernel and normalization terms from the background term.
+            // XXX is this true?  or was this due to an error in the analysis?
+
+            int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
+
+            // now pull out the kernel elements into their own square matrix
+            psImage  *kernelMatrix = psImageAlloc  (sumMatrix->numCols - 1, sumMatrix->numRows - 1, PS_TYPE_F64);
+            psVector *kernelVector = psVectorAlloc (sumMatrix->numCols - 1, PS_TYPE_F64);
+
+            for (int ix = 0, kx = 0; ix < sumMatrix->numCols; ix++) {
+                if (ix == bgIndex) continue;
+                for (int iy = 0, ky = 0; iy < sumMatrix->numRows; iy++) {
+                    if (iy == bgIndex) continue;
+                    kernelMatrix->data.F64[ky][kx] = sumMatrix->data.F64[iy][ix];
+                    ky++;
+                }
+                kernelVector->data.F64[kx] = sumVector->data.F64[ix];
+                kx++;
+            }
+
+            psImage *U = NULL;
+            psImage *V = NULL;
+            psVector *w = NULL;
+            if (!psMatrixSVD (&U, &w, &V, kernelMatrix)) {
+                psError(psErrorCodeLast(), false, "failed to perform SVD on sumMatrix\n");
+                return NULL;
+            }
+
+            // calculate A_inverse:
+            // Ainv = V * w * U^T
+            psImage *wUt  = p_pmSubSolve_wUt (w, U);
+            psImage *Ainv = p_pmSubSolve_VwUt (V, wUt);
+            psImage *Xvar = NULL;
+            psFree (wUt);
+
+# ifdef TESTING
+            // kernel terms:
+            for (int i = 0; i < w->n; i++) {
+                fprintf (stderr, "w: %f\n", w->data.F64[i]);
+            }
+# endif
+            // loop over w adding in more and more of the values until chisquare is no longer
+            // dropping significantly.
+            // XXX this does not seem to work very well: we seem to need all terms even for
+            // simple cases...
+
+            psVector *Xsvd = NULL;
+            {
+                psVector *Ax = NULL;
+                psVector *UtB = NULL;
+                psVector *wUtB = NULL;
+
+                psVector *wApply = psVectorAlloc(w->n, PS_TYPE_F64);
+                psVector *wMask = psVectorAlloc(w->n, PS_TYPE_U8);
+                psVectorInit (wMask, 1); // start by masking everything
+
+                double chiSquareLast = NAN;
+                int maxWeight = 0;
+
+                double Axx, Bx, y2;
+
+                // XXX this is an attempt to exclude insignificant modes.
+                // it was not successful with the ISIS kernel set: removing even
+                // the least significant mode leaves additional ringing / noise
+                // because the terms are so coupled.
+                for (int k = 0; false && (k < w->n); k++) {
+
+                    // unmask the k-th weight
+                    wMask->data.U8[k] = 0;
+                    p_pmSubSolve_SetWeights(wApply, w, wMask);
+
+                    // solve for x:
+                    // x = V * w * (U^T * B)
+                    p_pmSubSolve_UtB (&UtB, U, kernelVector);
+                    p_pmSubSolve_wUtB (&wUtB, wApply, UtB);
+                    p_pmSubSolve_VwUtB (&Xsvd, V, wUtB);
+
+                    // chi-square for this system of equations:
+                    // chi-square = sum over terms of: (Ax - B)*x - b*x - y^2
+                    // y^2 = \sum_stamps \sum_pixels input->kernel[y][x]^2
+                    p_pmSubSolve_Ax (&Ax, kernelMatrix, Xsvd);
+                    p_pmSubSolve_VdV (&Axx, Ax, Xsvd);
+                    p_pmSubSolve_VdV (&Bx, kernelVector, Xsvd);
+                    p_pmSubSolve_y2 (&y2, kernels, stamps);
+
+                    // apparently, this works (compare with the brute force value below
+                    double chiSquare = Axx - 2.0*Bx + y2;
+                    double deltaChi = (k == 0) ? chiSquare : chiSquareLast - chiSquare;
+                    chiSquareLast = chiSquare;
+
+                    // fprintf (stderr, "chi square = %f, delta: %f\n", chiSquare, deltaChi);
+                    if (k && !maxWeight && (deltaChi < 1.0)) {
+                        maxWeight = k;
+                    }
+                }
+
+                // keep all terms or we get extra ringing
+                maxWeight = w->n;
+                psVectorInit (wMask, 1);
+                for (int k = 0; k < maxWeight; k++) {
+                    wMask->data.U8[k] = 0;
+                }
+                p_pmSubSolve_SetWeights(wApply, w, wMask);
+
+                // solve for x:
+                // x = V * w * (U^T * B)
+                p_pmSubSolve_UtB (&UtB, U, kernelVector);
+                p_pmSubSolve_wUtB (&wUtB, wApply, UtB);
+                p_pmSubSolve_VwUtB (&Xsvd, V, wUtB);
+
+                // chi-square for this system of equations:
+                // chi-square = sum over terms of: (Ax - B)*x - b*x - y^2
+                // y^2 = \sum_stamps \sum_pixels input->kernel[y][x]^2
+                p_pmSubSolve_Ax (&Ax, kernelMatrix, Xsvd);
+                p_pmSubSolve_VdV (&Axx, Ax, Xsvd);
+                p_pmSubSolve_VdV (&Bx, kernelVector, Xsvd);
+                p_pmSubSolve_y2 (&y2, kernels, stamps);
+
+                // apparently, this works (compare with the brute force value below
+                double chiSquare = Axx - 2.0*Bx + y2;
+                psLogMsg ("psModules.imcombine", PS_LOG_INFO, "model kernel with %d terms; chi square = %f\n", maxWeight, chiSquare);
+
+                // re-calculate A^{-1} to get new variances:
+                // Ainv = V * w * U^T
+                // XXX since we keep all terms, this is identical to Ainv
+                psImage *wUt  = p_pmSubSolve_wUt (wApply, U);
+                Xvar = p_pmSubSolve_VwUt (V, wUt);
+                psFree (wUt);
+
+                psFree (Ax);
+                psFree (UtB);
+                psFree (wUtB);
+                psFree (wApply);
+                psFree (wMask);
+            }
+
+            // copy the kernel solutions to the full solution vector:
+            solution = psVectorAlloc(sumVector->n, PS_TYPE_F64);
+            solutionErr = psVectorAlloc(sumVector->n, PS_TYPE_F64);
+
+            for (int ix = 0, kx = 0; ix < sumVector->n; ix++) {
+                if (ix == bgIndex) {
+                    solution->data.F64[ix] = 0;
+                    solutionErr->data.F64[ix] = 0.001;
+                    continue;
+                }
+                solutionErr->data.F64[ix] = sqrt(Ainv->data.F64[kx][kx]);
+                solution->data.F64[ix] = Xsvd->data.F64[kx];
+                kx++;
+            }
+
+            psFree (kernelMatrix);
+            psFree (kernelVector);
+
+            psFree (U);
+            psFree (V);
+            psFree (w);
+
+            psFree (Ainv);
+            psFree (Xsvd);
+        } else {
+            psVector *permutation = NULL;       // Permutation vector, required for LU decomposition
+            psImage *luMatrix = psMatrixLUDecomposition(NULL, &permutation, sumMatrix);
+            if (!luMatrix) {
+                psError(PM_ERR_DATA, true, "LU Decomposition of least-squares matrix failed.\n");
+                psFree(solution);
+                psFree(sumVector);
+                psFree(sumMatrix);
+                psFree(luMatrix);
+                psFree(permutation);
+                return NULL;
+            }
+
+            solution = psMatrixLUSolution(NULL, luMatrix, sumVector, permutation);
+            psFree(luMatrix);
+            psFree(permutation);
+            if (!solution) {
+                psError(PM_ERR_DATA, true, "Failed to solve the least-squares system.\n");
+                psFree(solution);
+                psFree(sumVector);
+                psFree(sumMatrix);
+                return NULL;
+            }
+
+            // XXX LUD does not provide A^{-1}?  fake the error for now
+            solutionErr = psVectorAlloc(sumVector->n, PS_TYPE_F64);
+            for (int ix = 0; ix < sumVector->n; ix++) {
+                solutionErr->data.F64[ix] = 0.1*solution->data.F64[ix];
+            }
+        }
+
+        if (!kernels->solution1) {
+            kernels->solution1 = psVectorAlloc (sumVector->n, PS_TYPE_F64);
+            psVectorInit (kernels->solution1, 0.0);
+        }
+
+        // only update the solutions that we chose to calculate:
+        if (mode & PM_SUBTRACTION_EQUATION_NORM) {
+            int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
+            kernels->solution1->data.F64[normIndex] = solution->data.F64[normIndex];
+        }
+        if (mode & PM_SUBTRACTION_EQUATION_BG) {
+            int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index in matrix for background
+            kernels->solution1->data.F64[bgIndex] = solution->data.F64[bgIndex];
+        }
+        if (mode & PM_SUBTRACTION_EQUATION_KERNELS) {
+            int numKernels = kernels->num;
+            int spatialOrder = kernels->spatialOrder;       // Order of spatial variation
+            int numPoly = PM_SUBTRACTION_POLYTERMS(spatialOrder); // Number of polynomial terms
+            for (int i = 0; i < numKernels * numPoly; i++) {
+                // XXX fprintf (stderr, "%f +/- %f (%f) -> ", solution->data.F64[i], solutionErr->data.F64[i], fabs(solution->data.F64[i]/solutionErr->data.F64[i]));
+                if (fabs(solution->data.F64[i] / solutionErr->data.F64[i]) < COEFF_SIG) {
+                    // XXX fprintf (stderr, "drop\n");
+                    kernels->solution1->data.F64[i] = 0.0;
+                } else {
+                    // XXX fprintf (stderr, "keep\n");
+                    kernels->solution1->data.F64[i] = solution->data.F64[i];
+                }
+            }
+        }
+        // double chiSquare = p_pmSubSolve_ChiSquare (kernels, stamps);
+        // fprintf (stderr, "chi square Brute = %f\n", chiSquare);
+
+        psFree(solution);
+        psFree(sumVector);
+        psFree(sumMatrix);
+# endif
+
+#ifdef TESTING
+              // XXX double-check for NAN in data:
+                for (int iy = 0; iy < stamp->matrix->numRows; iy++) {
+                    for (int ix = 0; ix < stamp->matrix->numCols; ix++) {
+                        if (!isfinite(stamp->matrix->data.F64[iy][ix])) {
+                            fprintf (stderr, "WARNING: NAN in matrix\n");
+                        }
+                    }
+                }
+                for (int ix = 0; ix < stamp->vector->n; ix++) {
+                    if (!isfinite(stamp->vector->data.F64[ix])) {
+                        fprintf (stderr, "WARNING: NAN in vector\n");
+                    }
+                }
+#endif
+
+#ifdef TESTING
+        for (int ix = 0; ix < sumVector->n; ix++) {
+            if (!isfinite(sumVector->data.F64[ix])) {
+                fprintf (stderr, "WARNING: NAN in vector\n");
+            }
+        }
+#endif
+
+#ifdef TESTING
+        for (int ix = 0; ix < sumVector->n; ix++) {
+            if (!isfinite(sumVector->data.F64[ix])) {
+                fprintf (stderr, "WARNING: NAN in vector\n");
+            }
+        }
+        {
+            psImage *inverse = psMatrixInvert(NULL, sumMatrix, NULL);
+            psFitsWriteImageSimple("matrixInv.fits", inverse, NULL);
+            psFree(inverse);
+        }
+        {
+            psImage *X = psMatrixInvert(NULL, sumMatrix, NULL);
+            psImage *Xt = psMatrixTranspose(NULL, X);
+            psImage *XtX = psMatrixMultiply(NULL, Xt, X);
+            psFitsWriteImageSimple("matrixErr.fits", XtX, NULL);
+            psFree(X);
+            psFree(Xt);
+            psFree(XtX);
+        }
+#endif
+