Index: /branches/pap/psModules/src/imcombine/pmSubtractionEquation.c
===================================================================
--- /branches/pap/psModules/src/imcombine/pmSubtractionEquation.c	(revision 25859)
+++ /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
Index: /branches/pap/psModules/src/imcombine/pmSubtractionKernels.c
===================================================================
--- /branches/pap/psModules/src/imcombine/pmSubtractionKernels.c	(revision 25859)
+++ /branches/pap/psModules/src/imcombine/pmSubtractionKernels.c	(revision 25860)
@@ -94,5 +94,5 @@
             kernels->v->data.S32[index] = v;
             kernels->preCalc->data[index] = NULL;
-            kernels->penalties->data.F32[index] = kernels->penalty * (PS_SQR(u) + PS_SQR(v));
+            kernels->penalties->data.F32[index] = kernels->penalty * PS_SQR(PS_SQR(u) + PS_SQR(v));
 
             psTrace("psModules.imcombine", 7, "Kernel %d: %d %d\n", index, u, v);
@@ -151,5 +151,5 @@
                         double value = xKernel->data.F32[x] * yKernel->data.F32[y]; // Value of kernel
                         kernel->kernel[v][u] = value;
-                        moment += value * (PS_SQR(u) + PS_SQR(v));
+                        moment += value * PS_SQR((PS_SQR(u) + PS_SQR(v)));
                     }
                 }
@@ -190,4 +190,5 @@
                 kernels->preCalc->data[index] = preCalc;
                 kernels->penalties->data.F32[index] = kernels->penalty * fabsf(moment);
+                fprintf(stderr, "Penalty %d: %f\n", index, kernels->penalty * fabsf(moment));
 
                 psTrace("psModules.imcombine", 7, "Kernel %d: %f %d %d %f\n", index,
@@ -592,5 +593,5 @@
                                     poly->data.F32[j] = polyVal;
                                     norm += polyVal;
-                                    moment += polyVal * (PS_SQR(u) + PS_SQR(v));
+                                    moment += polyVal * PS_SQR(PS_SQR(u) + PS_SQR(v));
 
                                     psVectorExtend(uCoords, RINGS_BUFFER, 1);
@@ -618,5 +619,5 @@
                         psBinaryOp(poly, poly, "*", psScalarAlloc(1.0 / norm, PS_TYPE_F32));
                     }
-//                    moment /= norm;
+                    moment /= norm;
                 }