Index: branches/pap/psModules/src/imcombine/pmSubtractionEquation.c
===================================================================
--- branches/pap/psModules/src/imcombine/pmSubtractionEquation.c	(revision 25858)
+++ branches/pap/psModules/src/imcombine/pmSubtractionEquation.c	(revision 25860)
@@ -379,6 +379,8 @@
 
 // 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
+static 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
     )
 {
@@ -393,5 +395,5 @@
         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];
+                matrix->data.F64[index][index] -= norm * penalties->data.F32[i];
             }
         }
@@ -786,5 +788,6 @@
 #endif
 
-        calculatePenalty(sumVector, kernels);
+        int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background
+        calculatePenalty(sumMatrix, sumVector, kernels, sumMatrix->data.F64[bgIndex][bgIndex]);
 
 #ifdef TESTING
@@ -870,6 +873,8 @@
             }
         }
-        calculatePenalty(sumVector1, kernels);
-        calculatePenalty(sumVector2, kernels);
+
+        int bgIndex = PM_SUBTRACTION_INDEX_BG(kernels); // Index for background
+        calculatePenalty(sumMatrix1, sumVector1, kernels, sumMatrix1->data.F64[bgIndex][bgIndex]);
+        calculatePenalty(sumMatrix2, sumVector2, kernels, -sumMatrix1->data.F64[bgIndex][bgIndex]);
 
         // Pure matrix operations