Index: trunk/psModules/src/imcombine/pmSubtractionEquation.c
===================================================================
--- trunk/psModules/src/imcombine/pmSubtractionEquation.c	(revision 17825)
+++ trunk/psModules/src/imcombine/pmSubtractionEquation.c	(revision 18287)
@@ -30,5 +30,5 @@
     for (int y = - footprint; y <= footprint; y++) {
         for (int x = - footprint; x <= footprint; x++) {
-            sum += image1->kernel[y][x] * image2->kernel[y][x] / weight->kernel[y][x];
+            sum += image1->kernel[y][x] * image2->kernel[y][x] / 1.0; // weight->kernel[y][x];
         }
     }
@@ -194,5 +194,5 @@
             for (int y = - footprint; y <= footprint; y++) {
                 for (int x = - footprint; x <= footprint; x++) {
-                    sumC += conv->kernel[y][x] / weight->kernel[y][x];
+                    sumC += conv->kernel[y][x] / 1.0; // weight->kernel[y][x];
                 }
             }
@@ -217,5 +217,5 @@
         for (int y = - footprint; y <= footprint; y++) {
             for (int x = - footprint; x <= footprint; x++) {
-                double invNoise2 = 1.0 / weight->kernel[y][x];
+                double invNoise2 = 1.0 / 1.0; // weight->kernel[y][x];
                 double value = input->kernel[y][x] * invNoise2;
                 sumI += value;
@@ -276,5 +276,5 @@
         for (int y = - footprint; y <= footprint; y++) {
             for (int x = - footprint; x <= footprint; x++) {
-                    sumTC += target->kernel[y][x] * conv->kernel[y][x] / weight->kernel[y][x];
+                sumTC += target->kernel[y][x] * conv->kernel[y][x] / 1.0; // weight->kernel[y][x];
             }
         }
@@ -296,5 +296,5 @@
         for (int y = - footprint; y <= footprint; y++) {
             for (int x = - footprint; x <= footprint; x++) {
-                float value = target->kernel[y][x] / weight->kernel[y][x];
+                float value = target->kernel[y][x] / 1.0; // weight->kernel[y][x];
                 sumIT += value * input->kernel[y][x];
                 sumT += value;
@@ -365,5 +365,5 @@
         for (int y = - footprint; y <= footprint; y++) {
             for (int x = - footprint; x <= footprint; x++) {
-                sumC += conv->kernel[y][x] / weight->kernel[y][x];
+                sumC += conv->kernel[y][x] / 1.0; // weight->kernel[y][x];
             }
         }
@@ -382,7 +382,32 @@
 }
 
+
+// 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
+    )
+{
+    if (kernels->penalty == 0.0) {
+        return true;
+    }
+
+    psVector *penalties = kernels->penalties; // Penalties for each kernel component
+    int spatialOrder = kernels->spatialOrder; // Order of spatial variations
+    int numKernels = kernels->num; // Number of kernel components
+    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];
+            }
+        }
+    }
+
+    return true;
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // Semi-public functions
-// XXX EAM these cannot be inline :: talk to Josh
+// XXX We might like to define these functions as "extern inline" but gcc currently doesn't handle this in c99
+// mode.  See http://gcc.gnu.org/ml/gcc/2006-11/msg00006.html
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
@@ -675,4 +700,5 @@
         psVectorInit(sumVector, 0.0);
         psImageInit(sumMatrix, 0.0);
+        int numStamps = 0;              // Number of good stamps
         for (int i = 0; i < stamps->num; i++) {
             pmSubtractionStamp *stamp = stamps->stamps->data[i]; // Stamp of interest
@@ -680,6 +706,8 @@
                 (void)psBinaryOp(sumMatrix, sumMatrix, "+", stamp->matrix1);
                 (void)psBinaryOp(sumVector, sumVector, "+", stamp->vector1);
-            }
-        }
+                numStamps++;
+            }
+        }
+        calculatePenalty(sumVector, kernels);
 
         psVector *permutation = NULL;       // Permutation vector, required for LU decomposition
@@ -716,4 +744,5 @@
         psVectorInit(sumVector2, 0.0);
 
+        int numStamps = 0;              // Number of good stamps
         for (int i = 0; i < stamps->num; i++) {
             pmSubtractionStamp *stamp = stamps->stamps->data[i]; // Stamp of interest
@@ -724,6 +753,9 @@
                 (void)psBinaryOp(sumVector1, sumVector1, "+", stamp->vector1);
                 (void)psBinaryOp(sumVector2, sumVector2, "+", stamp->vector2);
-            }
-        }
+                numStamps++;
+            }
+        }
+        calculatePenalty(sumVector1, kernels);
+        calculatePenalty(sumVector2, kernels);
 
 #if 0
@@ -778,5 +810,5 @@
         psImage *F = (psImage*)psBinaryOp(NULL, CtBiC, "-", A);
         assert(F->numRows == numParams && F->numCols == numParams);
-        float det = NAN;
+        float det = 0.0;
         psImage *Fi = psMatrixInvert(NULL, F, &det);
         assert(Fi->numRows == numParams && Fi->numCols == numParams);