Index: /branches/pap/psModules/src/imcombine/pmSubtractionEquation.c
===================================================================
--- /branches/pap/psModules/src/imcombine/pmSubtractionEquation.c	(revision 25868)
+++ /branches/pap/psModules/src/imcombine/pmSubtractionEquation.c	(revision 25869)
@@ -15,5 +15,5 @@
 #include "pmSubtractionVisual.h"
 
-//#define TESTING                         // TESTING output for debugging; may not work with threads!
+#define TESTING                         // TESTING output for debugging; may not work with threads!
 
 #define USE_VARIANCE                    // Include variance in equation?
@@ -395,4 +395,5 @@
         for (int yOrder = 0, index = i; yOrder <= spatialOrder; yOrder++) {
             for (int xOrder = 0; xOrder <= spatialOrder - yOrder; xOrder++, index += numKernels) {
+                // Contribution to chi^2: a_i^2 P_i
                 matrix->data.F64[index][index] -= norm * penalties->data.F32[i];
             }
@@ -788,6 +789,8 @@
 #endif
 
+#if 0
         int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background
         calculatePenalty(sumMatrix, sumVector, kernels, sumMatrix->data.F64[bgIndex][bgIndex]);
+#endif
 
 #ifdef TESTING
@@ -933,27 +936,147 @@
 #endif
 
-        psVector *permutation = NULL;       // Permutation vector, required for LU decomposition
-        psImage *luMatrix = psMatrixLUDecomposition(NULL, &permutation, F);
-        if (!luMatrix) {
-            psError(PS_ERR_UNKNOWN, true, "LU Decomposition of least-squares matrix failed.\n");
+        psVector *g = NULL;             // Solution!
+        psVector *mask = psVectorAlloc(num, PS_TYPE_U8); // Mask of parameters
+        psVectorInit(mask, 0);
+        {
+            g = psMatrixSolveSVD(g, F, h);
+            if (!g) {
+                psError(PS_ERR_UNKNOWN, false, "SVD solution of least-squares equation failed.\n");
+                psFree(F);
+                psFree(h);
+                psFree(mask);
+                return NULL;
+            }
+
+            #define TOL 1.0e-5
+            int normIndex = PM_SUBTRACTION_INDEX_NORM(kernels); // Index for normalisation
+            double norm = g->data.F64[normIndex];               // Normalisation
+            double thresh = norm * TOL;                         // Threshold for low parameters
+            for (int j = 0; j < numParams2; j++) {
+                double param1 = g->data.F64[j], param2 = g->data.F64[numParams + j]; // Parameters of interest
+                fprintf(stderr, "%lf %lf   ", param1, param2);
+                if (fabs(param1) < thresh) {
+                    fprintf(stderr, "Parameter %d 1 below threshold\n", j);
+                    for (int k = 0; k < numParams2; k++) {
+                        // Matrix 1
+                        F->data.F64[k][j] = 0.0;
+                        F->data.F64[j][k] = 0.0;
+#if 1
+                        // Cross matrices
+                        F->data.F64[numParams + j][k] = 0.0;
+                        F->data.F64[j][numParams + k] = 0.0;
+                        F->data.F64[k][numParams + j] = 0.0;
+                        F->data.F64[numParams + k][j] = 0.0;
+#endif
+                    }
+                    F->data.F64[bgIndex][j] = 0.0;
+                    F->data.F64[j][bgIndex] = 0.0;
+                    F->data.F64[normIndex][j] = 0.0;
+                    F->data.F64[j][normIndex] = 0.0;
+                    F->data.F64[j][j] = 1.0;
+                    h->data.F64[j] = 0.0;
+                    mask->data.U8[j] = 0xFF;
+                }
+                if (fabs(param2) < thresh) {
+                    fprintf(stderr, "Parameter %d 2 below threshold\n", j);
+                    for (int k = 0; k < numParams2; k++) {
+                        // Matrix 2
+                        F->data.F64[k][numParams + j] = 0.0;
+                        F->data.F64[numParams + j][k] = 0.0;
+#if 1
+                        // Cross matrices
+                        F->data.F64[numParams + j][k] = 0.0;
+                        F->data.F64[j][numParams + k] = 0.0;
+                        F->data.F64[k][numParams + j] = 0.0;
+                        F->data.F64[numParams + k][j] = 0.0;
+#endif
+                    }
+                    F->data.F64[bgIndex][numParams + j] = 0.0;
+                    F->data.F64[numParams + j][bgIndex] = 0.0;
+                    F->data.F64[normIndex][numParams + j] = 0.0;
+                    F->data.F64[numParams + j][normIndex] = 0.0;
+                    F->data.F64[numParams + j][numParams + j] = 1.0;
+                    h->data.F64[numParams + j] = 0.0;
+                    mask->data.U8[numParams + j] = 0xFF;
+                }
+
+                if (!mask->data.U8[j] && !mask->data.U8[numParams + j]) {
+                    if (fabs(param1) < fabs(param2)) {
+                        fprintf(stderr, "Parameter %d 1 < 2\n", j);
+                        for (int k = 0; k < numParams2; k++) {
+                            // Matrix 1
+                            F->data.F64[k][j] = 0.0;
+                            F->data.F64[j][k] = 0.0;
+#if 0
+                            // Cross matrices
+                            F->data.F64[numParams + j][k] = 0.0;
+                            F->data.F64[j][numParams + k] = 0.0;
+                            F->data.F64[k][numParams + j] = 0.0;
+                            F->data.F64[numParams + k][j] = 0.0;
+#endif
+                        }
+                        F->data.F64[bgIndex][j] = 0.0;
+                        F->data.F64[j][bgIndex] = 0.0;
+                        F->data.F64[normIndex][j] = 0.0;
+                        F->data.F64[j][normIndex] = 0.0;
+                        F->data.F64[j][j] = 1.0;
+                        h->data.F64[j] = 0.0;
+                        mask->data.U8[j] = 0xFF;
+                    } else {
+                        fprintf(stderr, "Parameter %d 2 < 1\n", j);
+                        for (int k = 0; k < numParams2; k++) {
+                            // Matrix 1
+                            F->data.F64[k][numParams + j] = 0.0;
+                            F->data.F64[numParams + j][k] = 0.0;
+#if 0
+                            // Cross matrices
+                            F->data.F64[numParams + j][k] = 0.0;
+                            F->data.F64[j][numParams + k] = 0.0;
+                            F->data.F64[k][numParams + j] = 0.0;
+                            F->data.F64[numParams + k][j] = 0.0;
+#endif
+                        }
+                        F->data.F64[bgIndex][numParams + j] = 0.0;
+                        F->data.F64[numParams + j][bgIndex] = 0.0;
+                        F->data.F64[normIndex][numParams + j] = 0.0;
+                        F->data.F64[numParams + j][normIndex] = 0.0;
+                        F->data.F64[numParams + j][numParams + j] = 1.0;
+                        h->data.F64[numParams + j] = 0.0;
+                        mask->data.U8[numParams + j] = 0xFF;
+                    }
+                }
+            }
+        }
+
+        g = psMatrixSolveSVD(g, F, h);
+        if (!g) {
+            psError(PS_ERR_UNKNOWN, false, "SVD solution of least-squares equation failed.\n");
             psFree(F);
             psFree(h);
-            psFree(luMatrix);
-            psFree(permutation);
+            psFree(mask);
             return NULL;
         }
+        for (int i = 0; i < num; i++) {
+            if (mask->data.U8[i]) {
+                g->data.F64[i] = 0.0;
+            }
+        }
+        psFree(mask);
+
         psFree(F);
-
-        psVector *g = psMatrixLUSolution(NULL, luMatrix, h, permutation); // Solution!
-        if (!g) {
-            psError(PS_ERR_UNKNOWN, true, "LU Decomposition of least-squares matrix failed.\n");
-            psFree(h);
-            psFree(luMatrix);
-            psFree(permutation);
-            return NULL;
-        }
-        psFree(permutation);
-        psFree(luMatrix);
         psFree(h);
+
+#ifdef TESTING
+        {
+            psImage *image = psImageAlloc(1, num, PS_TYPE_F64);
+            psFits *fits = psFitsOpen("solnVector.fits", "w");
+            for (int i = 0; i < num; i++) {
+                image->data.F64[0][i] = g->data.F64[i];
+            }
+            psFitsWriteImage(fits, NULL, image, 0, NULL);
+            psFree(image);
+            psFitsClose(fits);
+        }
+#endif
 
         if (!kernels->solution1) {