Changeset 5818

Timestamp:

Dec 20, 2005, 12:41:06 PM (21 years ago)

Author:

gusciora

Message:

Added some of EAM's 3D and 4D fitting code.

File:

• trunk/psLib/src/math/psMinimize.c (modified) (20 diffs)

trunk/psLib/src/math/psMinimize.c

@@ -10 +10 @@
  *  @author EAM, IfA
  *
- *  @version $Revision: 1.147 $ $Name: not supported by cvs2svn $
- *  @date $Date: 2005-12-01 23:36:23 $
+ *  @version $Revision: 1.148 $ $Name: not supported by cvs2svn $
+ *  @date $Date: 2005-12-20 22:41:06 $
  *
  *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
@@ -113 +113 @@
     }
 
+    // XXX: Check error codes!
     psGaussJordan(Alpha, Beta);
     psFree(A);
@@ -1441 +1442 @@
 }
 
-// here is the definition for BuildSums4D.  ifdef'ed away until it is used
-// by psPolynomial4DFit..
-# if (0)
-    /******************************************************************************
+/******************************************************************************
+BuildSums3D(sums, x, y, z, nXterm, nYterm, nZterm): this routine calculates the powers of
+input parameter "x", "y", and "z" between 0 and input parameter nXterms*2,
+nYterm*2, and nZterm*2.  The result is returned as a psImage sums.
+ *****************************************************************************/
+static psF64 ***BuildSums3D(
+    psF64 ***sums,
+    psF64 x,
+    psF64 y,
+    psF64 z,
+    psS32 nXterm,
+    psS32 nYterm,
+    psS32 nZterm)
+{
+    psS32 nXsum = 0;
+    psS32 nYsum = 0;
+    psS32 nZsum = 0;
+    psF64 xSum = 1.0;
+    psF64 ySum = 1.0;
+    psF64 zSum = 1.0;
+
+    nXsum = 2*nXterm;
+    nYsum = 2*nYterm;
+    nZsum = 2*nZterm;
+    if (sums == NULL) {
+        sums = (psF64 ***) psAlloc (nXsum*sizeof(psF64));
+        for (int i = 0; i < nXsum; i++) {
+            sums[i] = (psF64 **) psAlloc (nYsum*sizeof(psF64));
+            for (int j = 0; j < nYsum; j++) {
+                sums[i][j] = (psF64 *) psAlloc (nZsum*sizeof(psF64));
+            }
+        }
+    }
+    // careful with this function: there is no size checking and realloc for reuse
+
+    zSum = 1.0;
+    for (int k = 0; k < nZsum; k++) {
+        ySum = zSum;
+        for (int j = 0; j < nYsum; j++) {
+            xSum = ySum;
+            for (int i = 0; i < nXsum; i++) {
+                sums[i][j][k] = xSum;
+                xSum *= x;
+            }
+            ySum *= y;
+        }
+        zSum *= z;
+    }
+    return (sums);
+}
+
+/******************************************************************************
     BuildSums4D(sums, x, y, z, t, nXterm, nYterm, nZterm, nTterm). equiv to
     BuildSums2D(). The result is returned as a double ****
-     *****************************************************************************/
-    static double ****BuildSums4D(
-        psF64 ****sums,
-        psF64 x,
-        psF64 y,
-        psF64 z,
-        psF64 t,
-        psS32 nXterm,
-        psS32 nYterm,
-        psS32 nZterm,
-        psS32 nTterm)
+*****************************************************************************/
+static psF64 ****BuildSums4D(
+    psF64 ****sums,
+    psF64 x,
+    psF64 y,
+    psF64 z,
+    psF64 t,
+    psS32 nXterm,
+    psS32 nYterm,
+    psS32 nZterm,
+    psS32 nTterm)
 {
     psS32 nXsum = 0;
@@ -1505 +1554 @@
     return (sums);
 }
-# endif /* BuildSums4D */
 
 /******************************************************************************
@@ -1556 +1604 @@
 
 /******************************************************************************
-vectorFitPolynomial1DCheb():  This routine will fit a Chebyshev polynomial of
-degree myPoly to the data points (x, y) and return the coefficients of that
+vectorFitPolynomial1DChebSlow():  This routine will fit a Chebyshev polynomial
+of degree myPoly to the data points (x, y) and return the coefficients of that
 polynomial.
+ 
+    NOTE: We currently have implemented two algorithms.  This one is
+    non-standard.  It ignores the orthogonal properties of the Chebyshev
+    polys, and their known zero values.  Instead, we do build a system of
+    linear equations based on minimizing the chi-squared for all data points
+    and we then solve those equations.  This method is significantly slower
+    than the other algorithm.  It was explicitly requested that we implement
+    this algorithm.
  
 XXX: mask, maskValue, yErr are currently ignored.
@@ -1564 +1620 @@
 XXX: Change arg order to that of the psLib function.
 *****************************************************************************/
-static psPolynomial1D *vectorFitPolynomial1DCheby(
+static psPolynomial1D *vectorFitPolynomial1DChebySlow(
     psPolynomial1D* myPoly,
     const psVector *mask,
@@ -1572 +1628 @@
     const psVector* x)
 {
-    // XXX: these ASSERTS are redundant.
+    PS_ASSERT_POLY_NON_NULL(myPoly, NULL);
+    PS_ASSERT_INT_LARGER_THAN_OR_EQUAL(myPoly->nX, 0, NULL);
+    PS_ASSERT_VECTOR_NON_NULL(y, NULL);
+    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F64, NULL);
+    if (yErr != NULL) {
+        PS_ASSERT_VECTORS_SIZE_EQUAL(y, yErr, NULL);
+        PS_ASSERT_VECTOR_TYPE(yErr, PS_TYPE_F64, NULL);
+    }
+    if (x != NULL) {
+        PS_ASSERT_VECTORS_SIZE_EQUAL(y, x, NULL);
+        PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F64, NULL);
+    }
+    psS32 NUM_POLY = myPoly->nX+1;
+    psS32 NUM_DATA = x->n;
+    psPolynomial1D **chebPolys = createChebyshevPolys(NUM_POLY);
+    if (0) {
+        for (psS32 j = 0; j < NUM_POLY; j++) {
+            PS_POLY_PRINT_1D(chebPolys[j]);
+        }
+    }
+
+    // Pre-compute the various Chebyshev polys T_i(x[j]) for all x[]
+    psImage *tMatrix = psImageAlloc(NUM_DATA, NUM_POLY, PS_TYPE_F64);
+    for (psS32 p = 0 ; p < NUM_POLY ; p++) {
+        for (psS32 d = 0 ; d < NUM_DATA ; d++) {
+            tMatrix->data.F64[p][d] = psPolynomial1DEval(chebPolys[p], x->data.F64[d]);
+        }
+    }
+
+    // Compute the A matrix
+    psImage *A = psImageAlloc(NUM_POLY, NUM_POLY, PS_TYPE_F64);
+    for (psS32 i = 0 ; i < NUM_POLY ; i++) {
+        for (psS32 j = 0 ; j < NUM_POLY ; j++) {
+            A->data.F64[i][j] = 0.0;
+            for (psS32 d = 0 ; d < NUM_DATA ; d++) {
+                A->data.F64[i][j]+= (tMatrix->data.F64[j][d] * tMatrix->data.F64[i][d]);
+            }
+        }
+        // This is because of the last term in: f(x) = SUM[c_i * T_i(x)]  -  c_0/2
+        for (psS32 d = 0 ; d < NUM_DATA ; d++) {
+            A->data.F64[i][0] -= (tMatrix->data.F64[i][d]/2.0);
+        }
+    }
+
+    // Compute the B vector
+    psVector *B = psVectorAlloc(NUM_POLY, PS_TYPE_F64);
+    for (psS32 i = 0 ; i < NUM_POLY ; i++) {
+        B->data.F64[i] = 0.0;
+        for (psS32 d = 0 ; d < NUM_DATA ; d++) {
+            B->data.F64[i] += (y->data.F64[d] * tMatrix->data.F64[i][d]);
+
+        }
+    }
+
+    // GaussJordan version
+    if (0) {
+        // does the solution in place
+        // XXX: Check error codes!
+        psGaussJordan (A, B);
+
+        // the first nTerm entries in B correspond directly to the desired
+        // polynomial coefficients.  this is only true for the 1D case
+        for (psS32 k = 0; k < NUM_POLY; k++) {
+            myPoly->coeff[k] = B->data.F64[k];
+        }
+    } else {
+        // LUD version of the fit
+        psImage *ALUD = NULL;
+        psVector* outPerm = NULL;
+        psVector* coeffs = NULL;
+
+        ALUD = psImageAlloc(NUM_POLY, NUM_POLY, PS_TYPE_F64);
+        ALUD = psMatrixLUD(ALUD, &outPerm, A);
+        coeffs = psMatrixLUSolve(coeffs, ALUD, B, outPerm);
+        for (psS32 k = 0; k < NUM_POLY; k++) {
+            myPoly->coeff[k] = coeffs->data.F64[k];
+        }
+
+        psFree(ALUD);
+        psFree(coeffs);
+        psFree(outPerm);
+    }
+
+    psFree(A);
+    psFree(B);
+    psFree(tMatrix);
+    for (psS32 i=0;i<NUM_POLY;i++) {
+        psFree(chebPolys[i]);
+    }
+    psFree(chebPolys);
+
+    return(myPoly);
+}
+
+/******************************************************************************
+vectorFitPolynomial1DChebFast():  This routine will fit a Chebyshev polynomial
+of degree myPoly to the data points (x, y) and return the coefficients of that
+polynomial.
+ 
+    NOTE: We currently have two algorithms.  This is standard method which
+    uses the orthogonal properties of the Chebyshev polys, and their known
+    zero values.  This is significantly faster than the chi-squared approach.
+ 
+XXX: mask, maskValue, yErr are currently ignored.
+ 
+XXX: Change arg order to that of the psLib function.
+ 
+XXX: This function will not work properly if the x-vector does not fully span
+the [-1:1] interval.
+*****************************************************************************/
+static psPolynomial1D *vectorFitPolynomial1DChebyFast(
+    psPolynomial1D* myPoly,
+    const psVector *mask,
+    psMaskType maskValue,
+    const psVector* y,
+    const psVector* yErr,
+    const psVector* x)
+{
     PS_ASSERT_POLY_NON_NULL(myPoly, NULL);
     PS_ASSERT_INT_NONNEGATIVE(myPoly->nX, NULL);
@@ -1614 +1787 @@
     for (psS32 i=0;i<n;i++) {
         // NR 5.8.4
+        // NR 5.8.4
         psF64 Y = cos(M_PI * (0.5 + ((psF32) i)) / ((psF32) n));
         psF64 X = (Y + bma + bpa) - 1.0;
@@ -1620 +1794 @@
         // We interpolate against the tabluated x,y vectors to determine the
         // function value at X.
-        fScalar = p_psVectorInterpolate((psVector *) x,
-                                        (psVector *) y,
-                                        3,
-                                        &tmpScalar);
-
-        f->data.F64[i] = fScalar->data.F64;
-        psFree(fScalar);
+        // XXX: This is somewhat of a hack to handle cases where the x vector does
+        // not fully span the [-1.0:1.0] interval.  We set the values of f[] to the
+        // values of y[] at those endpoints.
+        // XXX: This only works if x[] is increasing.
+
+        if (X <= x->data.F64[0]) {
+            f->data.F64[i] = y->data.F64[0];
+        } else if (X >= x->data.F64[x->n-1]) {
+            f->data.F64[i] = y->data.F64[x->n-1];
+        } else {
+            fScalar = p_psVectorInterpolate((psVector *) x, (psVector *) y,
+                                            3, &tmpScalar);
+            f->data.F64[i] = fScalar->data.F64;
+            psFree(fScalar);
+        }
 
         psTrace(".psLib.dataManip.vectorFitPolynomial1DCheby", 6,
@@ -1650 +1832 @@
     return(myPoly);
 }
+
+
 
 /******************************************************************************
@@ -1759 +1943 @@
     if (0) {
         // does the solution in place
+        // XXX: Check error codes!
         psGaussJordan (A, B);
 
@@ -1859 +2044 @@
             psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring mask and maskValue with Chebyshev polynomials.\n");
         }
+        if (fErr != NULL) {
+            psLogMsg(__func__, PS_LOG_WARN, "WARNING: ignoring error vector with Chebyshev polynomials.\n");
+        }
         if (x == NULL) {
             // If x==NULL, create an x64 vector with x values set to (-1:1).
             PS_VECTOR_GEN_CHEBY_INDEX(x64, f64->n);
         }
-        // XXX: Change arg order.
-        // XXX: Must modify this routine so that x64 or fErr64 can be NULL.
-        poly = vectorFitPolynomial1DCheby(poly, NULL, 0, f64, fErr64, x64);
+
+        if (1) {
+            poly = vectorFitPolynomial1DChebySlow(poly, NULL, 0, f64, fErr64, x64);
+        } else {
+            poly = vectorFitPolynomial1DChebyFast(poly, NULL, 0, f64, fErr64, x64);
+        }
         if (x == NULL) {
             psFree(x64);
@@ -2102 +2293 @@
 
     // does the solution in place
+    // XXX: Check error codes!
     psGaussJordan (A, B);
 
@@ -2475 +2667 @@
 }
 
+
 /******************************************************************************
  ******************************************************************************
- 3-D Vector Fitting Code.
+ 3-D Vector Fit Code.
  ******************************************************************************
  *****************************************************************************/
@@ -2526 +2719 @@
     }
 
-    psError(PS_ERR_UNKNOWN, true, "3-D Polynomial Fitting is not Implemented.\n");
-    return (NULL);
+    psImage    *A = NULL;
+    psVector   *B = NULL;
+    psF64 ***Sums = NULL;
+    psF64 wt;
+    psS32 nTerm;
+
+    // XXX:Watch for changes to the psPolys: nTerm != nOrder.
+    psS32 nXterm = 1 + myPoly->nX;
+    psS32 nYterm = 1 + myPoly->nY;
+    psS32 nZterm = 1 + myPoly->nZ;
+    nTerm = nXterm * nYterm * nZterm;
+
+    A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
+    B = psVectorAlloc(nTerm, PS_TYPE_F64);
+
+    // Initialize data structures.
+    psVectorInit (B, 0.0);
+    psImageInit (A, 0.0);
+
+    // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ...
+
+    // Build the B and A data structs.
+    for (int k  = 0; k < x->n; k++) {
+        if ((mask != NULL) && (mask->data.U8[k] & maskValue)) {
+            continue;
+        }
+
+        Sums = BuildSums3D(Sums, x->data.F64[k], y->data.F64[k], z->data.F64[k], nXterm, nYterm, nZterm);
+
+        if (fErr == NULL) {
+            wt = 1.0;
+        } else {
+            // this filters fErr == 0 values
+            wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErr->data.F64[k]);
+        }
+
+        // we could skip half of the array and assign at the end
+        // we must handle masked orders
+        for (int ix = 0; ix < nXterm; ix++) {
+            for (int iy = 0; iy < nYterm; iy++) {
+                for (int iz = 0; iz < nZterm; iz++) {
+                    if (myPoly->mask[ix][iy][iz])
+                        continue;
+                    int nx = ix+iy*nXterm+iz*nXterm*nYterm;
+                    B->data.F64[nx] += f->data.F64[k] * Sums[ix][iy][iz] * wt;
+                }
+            }
+        }
+
+        for (int ix = 0; ix < nXterm; ix++) {
+            for (int iy = 0; iy < nYterm; iy++) {
+                for (int iz = 0; iz < nZterm; iz++) {
+                    if (myPoly->mask[ix][iy][iz])
+                        continue;
+                    int nx = ix+iy*nXterm+iz*nXterm*nYterm;
+                    for (int jx = 0; jx < nXterm; jx++) {
+                        for (int jy = 0; jy < nYterm; jy++) {
+                            for (int jz = 0; jz < nZterm; jz++) {
+                                if (myPoly->mask[jx][jy][jz])
+                                    continue;
+                                int ny = jx+jy*nXterm+jz*nXterm*nYterm;
+                                A->data.F64[nx][ny] += Sums[ix+jx][iy+jy][iz+jz] * wt;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    for (int ix = 0; ix < nXterm; ix++) {
+        for (int iy = 0; iy < nYterm; iy++) {
+            for (int iz = 0; iz < nZterm; iz++) {
+                if (!myPoly->mask[ix][iy][iz])
+                    continue;
+                int nx = ix+iy*nXterm+iz*nXterm*nYterm;
+                B->data.F64[nx] = 0;
+                for (int jx = 0; jx < nXterm; jx++) {
+                    for (int jy = 0; jy < nYterm; jy++) {
+                        for (int jz = 0; jz < nZterm; jz++) {
+                            int ny = jx+jy*nXterm+jz*nXterm*nYterm;
+                            A->data.F64[nx][ny] = (nx == ny) ? 1 : 0;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    //    PS_IMAGE_PRINT_F64(A);
+    //    PS_VECTOR_PRINT_F64(B);
+    // does the solution in place
+    if (false == psGaussJordan (A, B)) {
+        psFree(A);
+        psFree(B);
+
+        for (int ix = 0; ix < 2*nXterm; ix++) {
+            for (int iy = 0; iy < 2*nYterm; iy++) {
+                psFree(Sums[ix][iy]);
+            }
+            psFree(Sums[ix]);
+        }
+        psFree(Sums);
+        psError(PS_ERR_UNKNOWN, false, "Failed to perform GaussJordan elimination.\n");
+        return(NULL);
+    }
+
+    // select the appropriate solution entries
+    for (int ix = 0; ix < nXterm; ix++) {
+        for (int iy = 0; iy < nYterm; iy++) {
+            for (int iz = 0; iz < nZterm; iz++) {
+                int nx = ix+iy*nXterm+iz*nXterm*nYterm;
+                myPoly->coeff[ix][iy][iz] = B->data.F64[nx];
+                myPoly->coeffErr[ix][iy][iz] = sqrt(A->data.F64[nx][nx]);
+            }
+        }
+    }
+
+    psFree(A);
+    psFree(B);
+
+    for (int ix = 0; ix < 2*nXterm; ix++) {
+        for (int iy = 0; iy < 2*nYterm; iy++) {
+            psFree(Sums[ix][iy]);
+        }
+        psFree(Sums[ix]);
+    }
+    psFree(Sums);
+
+    psTrace(".psLib.dataManip.VectorFitPolynomial3DOrd", 4,
+            "---- VectorFitPolynomial3DOrd() begin ----\n");
+    return (myPoly);
 }
 
@@ -2599 +2922 @@
     PS_ASSERT_VECTORS_SIZE_EQUAL(f, z, NULL);
     if (z->type.type != PS_TYPE_F64) {
-        PS_VECTOR_GEN_F64_FROM_F32(y64, z);
+        PS_VECTOR_GEN_F64_FROM_F32(z64, z);
     } else {
         z64 = (psVector *) z;
@@ -2714 +3037 @@
     PS_ASSERT_PTR_NON_NULL(stats, NULL);
     PS_ASSERT_VECTOR_NON_NULL(f, NULL);
-    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F32, NULL);
+    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);
     if (mask != NULL) {
         PS_ASSERT_VECTORS_SIZE_EQUAL(f, mask, NULL);
@@ -2721 +3044 @@
     PS_ASSERT_VECTOR_NON_NULL(x, NULL);
     PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
-    PS_ASSERT_VECTOR_TYPE(x, PS_TYPE_F32, NULL);
+    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(x, NULL);
     PS_ASSERT_VECTOR_NON_NULL(y, NULL);
     PS_ASSERT_VECTORS_SIZE_EQUAL(f, y, NULL);
-    PS_ASSERT_VECTOR_TYPE(y, PS_TYPE_F32, NULL);
+    PS_ASSERT_VECTOR_TYPE_F32_OR_F64(y, NULL);
     PS_ASSERT_VECTOR_NON_NULL(f, NULL);
-    PS_ASSERT_VECTORS_SIZE_EQUAL(f, f, NULL);
-    PS_ASSERT_VECTOR_TYPE(f, PS_TYPE_F32, NULL);
+    // PS_ASSERT_VECTORS_SIZE_EQUAL(f, f, NULL);
+    // PS_ASSERT_VECTOR_TYPE_F32_OR_F64(f, NULL);
     if (fErr != NULL) {
         PS_ASSERT_VECTORS_SIZE_EQUAL(fErr, mask, NULL);
-        PS_ASSERT_VECTOR_TYPE(fErr, PS_TYPE_F32, NULL);
-    }
-
-    psLogMsg(__func__, PS_LOG_WARN, "WARNING: This function has not been implemented.  Returning NULL.\n");
-    return(NULL);
+        PS_ASSERT_VECTOR_TYPE_F32_OR_F64(fErr, NULL);
+    }
+
+    // clipping range defined by min and max and/or clipSigma
+    float minClipSigma;
+    float maxClipSigma;
+    if (isfinite(stats->max)) {
+        maxClipSigma = fabs(stats->max);
+    } else {
+        maxClipSigma = fabs(stats->clipSigma);
+    }
+    if (isfinite(stats->min)) {
+        minClipSigma = fabs(stats->min);
+    } else {
+        minClipSigma = fabs(stats->clipSigma);
+    }
+    psVector *fit   = NULL;
+    psVector *resid = psVectorAlloc (x->n, PS_TYPE_F64);
+
+    // eventual expansion: user supplies one of various stats option pairs,
+    // eg (SAMPLE_MEAN | SAMPLE_STDEV) and the correct pair is used to
+    // evaluate the clipping sigma
+    // for now, for the SAMPLE_MEDIAN and SAMPLE_STDEV to be used
+    stats->options |= (PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_STDEV);
+
+    for (int N = 0; N < stats->clipIter; N++) {
+        int Nkeep = 0;
+
+        poly = psVectorFitPolynomial3D (poly, mask, maskValue, f, fErr, x, y, z);
+        fit = psPolynomial3DEvalVector (poly, x, y, z);
+        resid = (psVector *) psBinaryOp (resid, (void *) f, "-", (void *) fit);
+
+        stats  = psVectorStats (stats, resid, NULL, mask, maskValue);
+        float minClipValue = -minClipSigma*stats->sampleStdev;
+        float maxClipValue = +maxClipSigma*stats->sampleStdev;
+        psTrace (".psphot.VectorClipFit", 5, "resid stats: %f +/- %f\n", stats->sampleMedian, stats->sampleStdev);
+        psTrace (".psphot.VectorClipFit", 5, "min clip: %f, max clip: %f\n", minClipValue, maxClipValue);
+
+        // set mask if pts are not valid
+        // we are masking out any point which is out of range
+        // recovery is not allowed with this scheme
+        for (int i = 0; i < resid->n; i++) {
+            if ((mask != NULL) && (mask->data.U8[i] & maskValue)) {
+                continue;
+            }
+            if (resid->data.F64[i] - stats->sampleMedian > maxClipValue) {
+                if (mask != NULL) {
+                    mask->data.U8[i] |= 0x01;
+                }
+                continue;
+            }
+            if (resid->data.F64[i] - stats->sampleMedian < minClipValue) {
+                if (mask != NULL) {
+                    mask->data.U8[i] |= 0x01;
+                }
+                continue;
+            }
+            Nkeep ++;
+        }
+
+        psTrace (".psphot.VectorClipFit", 4, "keeping %d of %d pts for fit\n",
+                 Nkeep, x->n);
+
+        psFree (fit);
+    }
+    // Free local temporary variables
+    psFree (resid);
+
+    if (poly == NULL) {
+        psError(PS_ERR_UNKNOWN, true, "Could not fit a polynomial to the data.  Returning NULL.\n");
+        return(NULL);
+    }
+    return(poly);
 }
-
 
 /******************************************************************************
@@ -2792 +3182 @@
     }
 
-    psError(PS_ERR_UNKNOWN, true, "4-D Polynomial Fitting is not Implemented.\n");
-    return (NULL);
+    // I think this is 1 dimension down
+    psImage     *A = NULL;
+    psVector    *B = NULL;
+    psF64 ****Sums = NULL;
+    psF64 wt;
+    psS32 nTerm;
+
+    // XXX:Watch for changes to the psPolys: nTerm != nOrder.
+    psS32 nXterm = 1 + myPoly->nX;
+    psS32 nYterm = 1 + myPoly->nY;
+    psS32 nZterm = 1 + myPoly->nZ;
+    psS32 nTterm = 1 + myPoly->nZ;
+    nTerm = nXterm * nYterm * nZterm * nTterm;
+
+    A = psImageAlloc(nTerm, nTerm, PS_TYPE_F64);
+    B = psVectorAlloc(nTerm, PS_TYPE_F64);
+
+    // Initialize data structures.
+    psVectorInit (B, 0.0);
+    psImageInit (A, 0.0);
+
+    // Sums look like: 1, x, x^2, ... x^(2n+1), y, xy, x^2y, ... x^(2n+1)*y, ...
+
+    // Build the B and A data structs.
+    for (int k  = 0; k < x->n; k++) {
+        if ((mask != NULL) && (mask->data.U8[k] & maskValue)) {
+            continue;
+        }
+
+        Sums = BuildSums4D(Sums, x->data.F64[k], y->data.F64[k], z->data.F64[k], t->data.F64[k], nXterm, nYterm, nZterm, nTterm);
+
+        if (fErr == NULL) {
+            wt = 1.0;
+        } else {
+            // this filters fErr == 0 values
+            wt = (fErr->data.F64[k] == 0.0) ? 0.0 : 1.0 / PS_SQR(fErr->data.F64[k]);
+        }
+
+        // we could skip half of the array and assign at the end
+        // we must handle masked orders
+        for (int ix = 0; ix < nXterm; ix++) {
+            for (int iy = 0; iy < nYterm; iy++) {
+                for (int iz = 0; iz < nZterm; iz++) {
+                    for (int it = 0; it < nTterm; it++) {
+                        if (myPoly->mask[ix][iy][iz][it])
+                            continue;
+                        int nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm;
+                        B->data.F64[nx] += f->data.F64[k] * Sums[ix][iy][iz][it] * wt;
+                    }
+                }
+            }
+        }
+
+        for (int ix = 0; ix < nXterm; ix++) {
+            for (int iy = 0; iy < nYterm; iy++) {
+                for (int iz = 0; iz < nZterm; iz++) {
+                    for (int it = 0; it < nTterm; it++) {
+                        if (myPoly->mask[ix][iy][iz][it])
+                            continue;
+                        int nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm;
+                        for (int jx = 0; jx < nXterm; jx++) {
+                            for (int jy = 0; jy < nYterm; jy++) {
+                                for (int jz = 0; jz < nZterm; jz++) {
+                                    for (int jt = 0; jt < nTterm; jt++) {
+                                        if (myPoly->mask[jx][jy][jz][jt])
+                                            continue;
+                                        int ny = jx+jy*nXterm+jz*nXterm*nYterm+jt*nXterm*nYterm*nZterm;
+                                        A->data.F64[nx][ny]+= Sums[ix+jx][iy+jy][iz+jz][it+jt] * wt;
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    for (int ix = 0; ix < nXterm; ix++) {
+        for (int iy = 0; iy < nYterm; iy++) {
+            for (int iz = 0; iz < nZterm; iz++) {
+                for (int it = 0; it < nTterm; it++) {
+                    if (!myPoly->mask[ix][iy][iz][it])
+                        continue;
+                    int nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm;
+                    B->data.F64[nx] = 0;
+                    for (int jx = 0; jx < nXterm; jx++) {
+                        for (int jy = 0; jy < nYterm; jy++) {
+                            for (int jz = 0; jz < nZterm; jz++) {
+                                for (int jt = 0; jt < nTterm; jt++) {
+                                    int ny = jx+jy*nXterm+jz*nXterm*nYterm+jt*nXterm*nYterm*nZterm;
+                                    A->data.F64[nx][ny] = (nx == ny) ? 1 : 0;
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    // does the solution in place
+
+    if (false == psGaussJordan(A, B)) {
+        psFree(A);
+        psFree(B);
+        for (int ix = 0; ix < 2*nXterm; ix++) {
+            for (int iy = 0; iy < 2*nYterm; iy++) {
+                for (int iz = 0; iz < 2*nZterm; iz++) {
+                    psFree(Sums[ix][iy][iz]);
+                }
+                psFree(Sums[ix][iy]);
+            }
+            psFree(Sums[ix]);
+        }
+        psFree(Sums);
+        psError(PS_ERR_UNKNOWN, false, "Failed to perform GaussJordan elimination.\n");
+        return(NULL);
+    }
+
+    // select the appropriate solution entries
+    for (int ix = 0; ix < nXterm; ix++) {
+        for (int iy = 0; iy < nYterm; iy++) {
+            for (int iz = 0; iz < nZterm; iz++) {
+                for (int it = 0; it < nTterm; it++) {
+                    int nx = ix+iy*nXterm+iz*nXterm*nYterm+it*nXterm*nYterm*nZterm;
+                    myPoly->coeff[ix][iy][iz][it] = B->data.F64[nx];
+                    myPoly->coeffErr[ix][iy][iz][it] = sqrt(A->data.F64[nx][nx]);
+                }
+            }
+        }
+    }
+
+    psFree(A);
+    psFree(B);
+
+    for (int ix = 0; ix < 2*nXterm; ix++) {
+        for (int iy = 0; iy < 2*nYterm; iy++) {
+            for (int iz = 0; iz < 2*nZterm; iz++) {
+                psFree(Sums[ix][iy][iz]);
+            }
+            psFree(Sums[ix][iy]);
+        }
+        psFree(Sums[ix]);
+    }
+    psFree(Sums);
+
+    psTrace(".psLib.dataManip.VectorFitPolynomial3DOrd", 4,
+            "---- VectorFitPolynomial3DOrd() begin ----\n");
+    return (myPoly);
 }
 
@@ -3029 +3567 @@
     }
 
-    psLogMsg(__func__, PS_LOG_WARN, "WARNING: This function has not been implemented.  Returning NULL.\n");
-
-    return(NULL);
+    // clipping range defined by min and max and/or clipSigma
+    float minClipSigma;
+    float maxClipSigma;
+    if (isfinite(stats->max)) {
+        maxClipSigma = fabs(stats->max);
+    } else {
+        maxClipSigma = fabs(stats->clipSigma);
+    }
+    if (isfinite(stats->min)) {
+        minClipSigma = fabs(stats->min);
+    } else {
+        minClipSigma = fabs(stats->clipSigma);
+    }
+    psVector *fit   = NULL;
+    psVector *resid = psVectorAlloc (x->n, PS_TYPE_F64);
+
+    // eventual expansion: user supplies one of various stats option pairs,
+    // eg (SAMPLE_MEAN | SAMPLE_STDEV) and the correct pair is used to
+    // evaluate the clipping sigma
+    // for now, for the SAMPLE_MEDIAN and SAMPLE_STDEV to be used
+    stats->options |= (PS_STAT_SAMPLE_MEDIAN | PS_STAT_SAMPLE_STDEV);
+
+    for (int N = 0; N < stats->clipIter; N++) {
+        int Nkeep = 0;
+
+        poly = psVectorFitPolynomial4D (poly, mask, maskValue, f, fErr, x, y, z, t);
+        fit = psPolynomial4DEvalVector (poly, x, y, z, t);
+        resid = (psVector *) psBinaryOp (resid, (void *) f, "-", (void *) fit);
+
+        stats  = psVectorStats (stats, resid, NULL, mask, maskValue);
+        float minClipValue = -minClipSigma*stats->sampleStdev;
+        float maxClipValue = +maxClipSigma*stats->sampleStdev;
+        psTrace (".psphot.VectorClipFit", 5, "resid stats: %f +/- %f\n", stats->sampleMedian, stats->sampleStdev);
+        psTrace (".psphot.VectorClipFit", 5, "min clip: %f, max clip: %f\n", minClipValue, maxClipValue);
+
+        // set mask if pts are not valid
+        // we are masking out any point which is out of range
+        // recovery is not allowed with this scheme
+        for (int i = 0; i < resid->n; i++) {
+            if ((mask != NULL) && (mask->data.U8[i] & maskValue)) {
+                continue;
+            }
+            if (resid->data.F64[i] - stats->sampleMedian > maxClipValue) {
+                if (mask != NULL) {
+                    mask->data.U8[i] |= 0x01;
+                }
+                continue;
+            }
+            if (resid->data.F64[i] - stats->sampleMedian < minClipValue) {
+                if (mask != NULL) {
+                    mask->data.U8[i] |= 0x01;
+                }
+                continue;
+            }
+            Nkeep ++;
+        }
+
+        psTrace (".psphot.VectorClipFit", 4, "keeping %d of %d pts for fit\n",
+                 Nkeep, x->n);
+
+        psFree (fit);
+    }
+    // Free local temporary variables
+    psFree (resid);
+
+    if (poly == NULL) {
+        psError(PS_ERR_UNKNOWN, true, "Could not fit a polynomial to the data.  Returning NULL.\n");
+        return(NULL);
+    }
+    return(poly);
 }
-
-