Index: trunk/Ohana/src/opihi/cmd.astro/fitplx_irls.c
===================================================================
--- trunk/Ohana/src/opihi/cmd.astro/fitplx_irls.c	(revision 39167)
+++ trunk/Ohana/src/opihi/cmd.astro/fitplx_irls.c	(revision 39167)
@@ -0,0 +1,999 @@
+# include "astro.h"
+# define J2000 51544.5       /* Modified Julian date at standard epoch J2000 */
+
+# define ESCAPE(MSG,...) {			\
+    gprint (GP_ERR, MSG, __VA_ARGS__);		\
+    return FALSE; }
+
+typedef struct {
+  double *X;
+  double *Y;
+  double *t;
+  double *pX;
+  double *pY;
+  double *dX;
+  double *dY;
+
+  double *IRLS_u;
+  int *index;
+  int Npts;
+} PlxFitData;
+
+typedef struct {
+  double Ro, dRo;
+  double Do, dDo;
+
+  double uR, duR;
+  double uD, duD;
+
+  double p, dp;
+
+  double chisq;
+  int Nfit;
+  int getChisq;
+} PlxFit_IRLS;
+
+int VectorRobustStats_IRLS (Vector *vector, double *median, double *sigma);
+double VectorFractionInterpolate_IRLS (double *values, float fraction, int Npts);
+
+int PlxSetMeanEpoch_IRLS (double *R, double *D, double *T, double *Rmean, double *Dmean, double *Tmean, int *mask, int Ntotal);
+int PlxSetEpochPosition_IRLS (PlxFitData *fitdata, double *R, double *D, double *dR, double *dD, double *T, int *mask, int Ntotal, Coords *coords, double Tmean);
+int PlxOutlierClip_IRLS (PlxFitData *fitdata, int *mask, int Noutlier, float dPsigMax, Vector *dPvec, int VERBOSE);
+
+int PlxFitDataAlloc_IRLS (PlxFitData *data, int N);
+void PlxFitDataFree_IRLS (PlxFitData *data);
+int PlxBootstrapResample_IRLS (PlxFitData *src, PlxFitData *tgt);
+
+int FitPMandPar_IRLS (PlxFit_IRLS *fit, double *X, double *dX, double *Y, double *dY, double *T, double *pR, double *pD, double *IRLS_u, int Npts, int max_iterations, int VERBOSE);
+int sun_ecliptic_IRLS (double mjd, double *lambda, double *beta, double *epsilon, double *Radius);
+int ParFactor_IRLS (double *pR, double *pD, double RA, double DEC, double Time);
+
+int IRLS_converged (PlxFit_IRLS *fit);
+int Plx_weighted_LS (double *T, double *pR, double *pD, double *X, double *WX, double *Y, double *WY, int Npts,
+		     double **A, double **B, int VERBOSE);
+double Plx_weight_cauchy (double x);
+double Plx_dpsi_cauchy (double x);
+double Plx_MAD(double *in, int N);
+
+
+
+
+int fitplx_irls (int argc, char **argv) {
+  
+  int i, N;
+
+  Vector *rvec, *dvec, *tvec, *dRvec, *dDvec;
+
+  Vector *mvec = NULL; // mask vector
+  if ((N = get_argument (argc, argv, "-mask"))) {
+    remove_argument (N, &argc, argv);
+    if ((mvec = SelectVector (argv[N], ANYVECTOR, TRUE)) == NULL) return (FALSE);
+    remove_argument (N, &argc, argv);
+    CastVector (mvec, OPIHI_INT);
+  }
+
+  int VERBOSE = FALSE;
+  if ((N = get_argument (argc, argv, "-v"))) {
+    remove_argument (N, &argc, argv);
+    VERBOSE = TRUE;
+  }
+  if ((N = get_argument (argc, argv, "-vv"))) {
+    remove_argument (N, &argc, argv);
+    VERBOSE = 2;
+  }
+
+  int Noutlier = 0;
+  float dPsigMax = FLT_MAX;
+  if ((N = get_argument (argc, argv, "-outlier-tests"))) {
+    remove_argument (N, &argc, argv);
+    Noutlier = atoi(argv[N]);
+    remove_argument (N, &argc, argv);
+    dPsigMax = atof(argv[N]);
+    remove_argument (N, &argc, argv);
+  }
+
+  int Nresample = 0;
+  if ((N = get_argument (argc, argv, "-bootstrap-resample"))) {
+    remove_argument (N, &argc, argv);
+    Nresample = atoi(argv[N]);
+    remove_argument (N, &argc, argv);
+  }
+
+  Vector *dPvec = NULL;
+  if ((N = get_argument (argc, argv, "-dPsig"))) {
+    if (!Noutlier) { gprint (GP_ERR, "-dPsig requires -outlier-tests to be non-zero\n"); return FALSE; }
+    remove_argument (N, &argc, argv);
+    if (!(dPvec = SelectVector (argv[N], ANYVECTOR, TRUE))) return FALSE;
+    remove_argument (N, &argc, argv);
+  }
+
+  int max_iterations = 10;
+  if ((N = get_argument (argc, argv, "-max-iterations"))) {
+    remove_argument (N, &argc, argv);
+    max_iterations = atoi(argv[N]);
+    remove_argument (N, &argc, argv);
+  }
+  
+  if (argc != 6) {
+    gprint (GP_ERR, "USAGE: fitplx (ra) (dR) (dec) (dD) (mjd) [-mask mask] [-v] [-vv]\n");
+    gprint (GP_ERR, "  -outlier-tests Nsamples dPsigMax : run Nsample bootstrap-resamples to define the path deviations and reject based on dPsigMax\n");
+    gprint (GP_ERR, "  -dPsig vec : save path deviations in vec\n");
+    gprint (GP_ERR, "  -max_iterations : maximum number of IRLS iterations to run\n");
+    return (FALSE);
+  }
+
+  /* select input / output buffers */
+  // if ((buf = SelectBuffer (argv[1], OLDBUFFER, TRUE)) == NULL) return (FALSE);
+  // Nx = buf[0].header.Naxis[0];
+  // Ny = buf[0].header.Naxis[1];
+  
+  if ((rvec = SelectVector (argv[1], ANYVECTOR, TRUE)) == NULL) ESCAPE ("missing vector %s\n", argv[1]);
+  if ((dvec = SelectVector (argv[3], ANYVECTOR, TRUE)) == NULL) ESCAPE ("missing vector %s\n", argv[3]);
+  if ((tvec = SelectVector (argv[5], ANYVECTOR, TRUE)) == NULL) ESCAPE ("missing vector %s\n", argv[5]);
+
+  if ((dRvec = SelectVector (argv[2], ANYVECTOR, TRUE)) == NULL) ESCAPE ("missing vector %s\n", argv[2]);
+  if ((dDvec = SelectVector (argv[4], ANYVECTOR, TRUE)) == NULL) ESCAPE ("missing vector %s\n", argv[4]);
+
+  double *R = rvec->elements.Flt;
+  double *D = dvec->elements.Flt;
+  double *T = tvec->elements.Flt;
+  
+  double *dR = dRvec->elements.Flt;
+  double *dD = dDvec->elements.Flt;
+
+  int *mask = NULL;
+  if (mvec) {
+    mask = mvec->elements.Int;
+  }
+
+  // Ntotal : all points supplied by user
+  // Nsubset : unmasked points
+  int Ntotal = tvec->Nelements; // XXX check other lengths
+  if (dPvec) ResetVector (dPvec, OPIHI_FLT, Ntotal);
+
+  double Rmean, Dmean, Tmean;
+  PlxSetMeanEpoch_IRLS (R, D, T, &Rmean, &Dmean, &Tmean, mask, Ntotal);
+
+  /* project coordinates to a plane centered on the object with units of arcsec */
+  Coords coords;
+  InitCoords (&coords, "DEC--SIN");
+  coords.crval1 = Rmean;
+  coords.crval2 = Dmean;
+  coords.cdelt1 = coords.cdelt2 = 1.0 / 3600.0;
+
+  PlxFitData fitdata;
+  PlxFitDataAlloc_IRLS (&fitdata, Ntotal);
+  PlxSetEpochPosition_IRLS (&fitdata, R, D, dR, dD, T, mask, Ntotal, &coords, Tmean);
+
+  PlxFit_IRLS fit; memset (&fit, 0, sizeof(PlxFit_IRLS));
+
+  if (0) {
+    // This should be accomplished via the IRLS call
+  // determine dPsig for detections based on Noutlier attempts
+    if (Noutlier) {
+      int clipRetry = TRUE;
+      for (i = 0; clipRetry && (i < 3); i++) {
+	clipRetry = !PlxOutlierClip_IRLS (&fitdata, mask, Noutlier, dPsigMax, dPvec, VERBOSE);
+	
+	// using the new mask values, reset fitdata
+	PlxSetMeanEpoch_IRLS (R, D, T, &Rmean, &Dmean, &Tmean, mask, Ntotal);
+	PlxSetEpochPosition_IRLS (&fitdata, R, D, dR, dD, T, mask, Ntotal, &coords, Tmean);
+	if (VERBOSE) fprintf (stderr, "keep %d of %d\n", fitdata.Npts, Ntotal);
+      }
+    }
+  }
+  
+  for (i = 0; (VERBOSE == 2) && (i < fitdata.Npts); i++) {
+    int n = fitdata.index[i];
+    int maskValue = mask ? mask[n] : 0;
+    fprintf (stderr, "%f %f : %f %d : %f %f %f\n", R[n], D[n], T[n], maskValue, fitdata.t[i], fitdata.X[i], fitdata.Y[i]);
+  }
+
+  fit.getChisq = TRUE;
+  if (!FitPMandPar_IRLS (&fit, 
+			 fitdata.X, fitdata.dX, 
+			 fitdata.Y, fitdata.dY, 
+			 fitdata.t, fitdata.pX, fitdata.pY,
+			 fitdata.IRLS_u,
+			 fitdata.Npts, max_iterations, VERBOSE)) {
+    return FALSE;
+  }
+
+  if (Nresample){
+    PlxFitData sample;
+    PlxFitDataAlloc_IRLS (&sample, fitdata.Npts);
+
+    PlxFit_IRLS *testfit = NULL;
+    ALLOCATE (testfit, PlxFit_IRLS, Nresample);
+
+    int Ngood = 0;
+    for (i = 0; i < Nresample; i++) {
+      PlxBootstrapResample_IRLS (&fitdata, &sample);
+      
+      if (i % 100000 == 99999) fprintf (stderr, ".");
+
+      // fit the sample
+      testfit[Ngood].getChisq = FALSE;
+      if (!FitPMandPar_IRLS (&testfit[Ngood], 
+			     sample.X, sample.dX, 
+			     sample.Y, sample.dY, sample.t, 
+			     sample.pX, sample.pY,
+			     sample.IRLS_u, sample.Npts, 1, VERBOSE)) continue;
+      Ngood ++;
+    }
+
+    Vector *pvec, *uRvec, *uDvec, *Rvec, *Dvec;
+
+    // save the Nresample histograms
+    if ((pvec  = SelectVector ("plxVector", ANYVECTOR, TRUE)) == NULL) ESCAPE ("missing vector %s\n", "plxVector");
+    if ((uRvec = SelectVector ("uRVector",  ANYVECTOR, TRUE)) == NULL) ESCAPE ("missing vector %s\n", "uDVector");
+    if ((uDvec = SelectVector ("uDVector",  ANYVECTOR, TRUE)) == NULL) ESCAPE ("missing vector %s\n", "uRVector");
+    if ((Rvec  = SelectVector ("RoVector",  ANYVECTOR, TRUE)) == NULL) ESCAPE ("missing vector %s\n", "RoVector");
+    if ((Dvec  = SelectVector ("DoVector",  ANYVECTOR, TRUE)) == NULL) ESCAPE ("missing vector %s\n", "DoVector");
+    
+    ResetVector ( pvec, OPIHI_FLT, Ngood);
+    ResetVector (uRvec, OPIHI_FLT, Ngood);
+    ResetVector (uDvec, OPIHI_FLT, Ngood);
+    ResetVector ( Rvec, OPIHI_FLT, Ngood);
+    ResetVector ( Dvec, OPIHI_FLT, Ngood);
+    
+    for (i = 0; i < Ngood; i++) {
+      pvec->elements.Flt[i]  = testfit[i].p;
+      uRvec->elements.Flt[i] = testfit[i].uR;
+      uDvec->elements.Flt[i] = testfit[i].uD;
+      Rvec->elements.Flt[i]  = testfit[i].Ro;
+      Dvec->elements.Flt[i]  = testfit[i].Do;
+    }
+
+    // now calculate median and sigma for each vector
+    VectorRobustStats_IRLS (pvec,  &fit.p,  &fit.dp);
+    VectorRobustStats_IRLS (uRvec, &fit.uR, &fit.duR);
+    VectorRobustStats_IRLS (uDvec, &fit.uD, &fit.duD);
+    VectorRobustStats_IRLS (Rvec,  &fit.Ro, &fit.dRo);
+    VectorRobustStats_IRLS (Dvec,  &fit.Do, &fit.dDo);
+  }
+
+  // fprintf (stderr, "%f +/- %f | %f %f\n", fit.p, fit.dp, fit.uR, fit.uD);
+
+/*
+  FILE *f = fopen ("test.pf.dat", "w");
+  for (i = 0; i < Ntotal; i++) {
+    double Xf = fit.Ro + fit.uR*fitdata.t[i] + fit.p*fitdata.pX[i];
+    double Yf = fit.Do + fit.uD*fitdata.t[i] + fit.p*fitdata.pY[i];
+    fprintf (f, "%f : %f %f : %f %f : %f : %f %f : %f %f\n", T[i], R[i], D[i], Xf, Yf, fitdata.t[i], fitdata.X[i], fitdata.Y[i], fitdata.pX[i], fitdata.pY[i]);
+  }
+  fclose (f);
+*/
+
+  // fprintf (stderr, "Roff, Doff: %f, %f; dRo, dDo: %f, %f\n", fit.Ro, fit.Do, fit.dRo, fit.dDo);
+  
+  XY_to_RD (&Rmean, &Dmean, fit.Ro, fit.Do, &coords);
+  if (VERBOSE) {
+    fprintf (stderr, "Ro, Do: %f, %f +/- %f, %f (%f, %f)\n", Rmean, Dmean, fit.dRo, fit.dDo, fit.Ro, fit.Do);
+    fprintf (stderr, "uR, uD: %f, %f; duR, duD: %f, %f\n", fit.uR, fit.uD, fit.duR, fit.duD);
+    fprintf (stderr, "par: %f +/- %f\n", fit.p, fit.dp);
+    fprintf (stderr, "chisq: %f Nfit %d\n", fit.chisq, fit.Nfit);
+  }
+
+  set_variable ("RA",   Rmean);
+  set_variable ("DEC",  Dmean);
+  set_variable ("dR",   fit.dRo);
+  set_variable ("dD",   fit.dDo);
+  set_variable ("uR",   fit.uR);
+  set_variable ("uD",   fit.uD);
+  set_variable ("duR",  fit.duR);
+  set_variable ("duD",  fit.duD);
+  set_variable ("plx",  fit.p);
+  set_variable ("dplx", fit.dp);
+  
+  set_variable ("Tmean",  Tmean);
+
+  set_variable ("chisq", fit.chisq);
+  set_variable ("Nfit",  fit.Nfit);
+
+  return (TRUE);
+}
+
+/* do we want an init function which does the alloc and a clear function to free? */
+int FitPMandPar_IRLS (PlxFit_IRLS *fit, double *X, double *dX, double *Y, double *dY, double *T, double *pR, double *pD,
+		      double *IRLS_u,
+		      int Npts, int max_iterations, int VERBOSE) {
+
+  int i,j;
+
+  static double **A = NULL;
+  static double **B = NULL;
+  double chisq, Xf, Yf;
+
+  double **Cov;                // Ordinary least squares Covariance matrix
+  double *Beta, *Beta_prev;    // Current parameter vector, previous parameter vector.
+
+  double sigma_ols, sigma_hat; // Sigma estimates.
+  double *Wx, *Wy;             // Weight vectors.  Not errors.
+  double *rx, *ry;             // Deviation from model
+  //  double *u;                   // Deviation magnitude
+  int converged;
+  int iterations;
+  double tolerance = 1e-4;
+  int p = 5;                  // Number of fit parameters
+  
+  // Allocate what will be the Ax=B matrices
+  if (A == NULL) {
+    ALLOCATE (A, double *, 5);
+    ALLOCATE (B, double *, 5);
+    for (i = 0; i < 5; i++) {
+      ALLOCATE (A[i], double, 5);
+      ALLOCATE (B[i], double, 1);
+      memset (A[i], 0, 5*sizeof(double));
+      memset (B[i], 0, 1*sizeof(double));
+    }
+  }
+
+  // Allocate the IRLS specific objects
+  ALLOCATE (Cov, double *, 5);
+  for (i = 0; i < 5; i++) {
+    ALLOCATE ( Cov[i], double, 5);
+  }
+
+  ALLOCATE(Beta, double, 5);
+  ALLOCATE(Beta_prev, double, 5);
+  ALLOCATE(Wx, double, Npts);
+  ALLOCATE(Wy, double, Npts);
+  ALLOCATE(rx,  double, Npts);
+  ALLOCATE(ry,  double, Npts);
+  
+  // Convert the measurement errors into the initial weights.
+  for (i = 0; i < Npts; i++) {
+    Wx[i] = (fabs(dX[i]) < 0.0001) ? 1.0 : 1.0 / SQ(dX[i]);
+    Wy[i] = (fabs(dY[i]) < 0.0001) ? 1.0 : 1.0 / SQ(dY[i]);
+  }
+  
+  // Solve OLS equation
+  if (!Plx_weighted_LS(T,pR,pD,X,Wx,Y,Wy,Npts,
+		       A,B,VERBOSE)) {
+    // Handle fail cases gracefully.
+    return(FALSE);
+  }
+
+  // Calculate r vector of residuals and least squares sigma
+  sigma_ols = 0.0;
+  for (i = 0; i < Npts; i++) {
+    rx[i] = X[i] - (T[i] * B[0][0] + B[1][0] + B[4][0] * pR[i]);
+    ry[i] = Y[i] - (T[i] * B[2][0] + B[3][0] + B[4][0] * pD[i]);
+    //    u[i] = r[i] /
+    sigma_ols += SQ(rx[i]) + SQ(ry[i]);
+
+  }
+  sigma_ols = sqrt(sigma_ols / (Npts - 5.0));
+  
+  // Save OLS covariance;
+  for (i = 0; i < 5; i++) {
+    for (j = 0; j < 5; j++) {
+      Cov[i][j] = A[i][j];
+    }
+  }
+
+  // Save Beta
+  for (i = 0; i < 5; i++) {
+    Beta[i] = B[i][0];
+  }
+
+  // Iteratively reweight and solve
+  converged = FALSE;
+  iterations = 0;
+  do {
+    // Save Beta
+    for (i = 0; i < 5; i++) {
+      Beta_prev[i] = Beta[i];
+    }
+
+    // Assign W
+    for (i = 0; i < Npts; i++) {
+      Wx[i] = Plx_weight_cauchy(rx[i] / dX[i]);
+      Wy[i] = Plx_weight_cauchy(ry[i] / dY[i]);
+    }
+
+    // Solve
+    if (!Plx_weighted_LS(T,pD,pR,X,Wx,Y,Wy,Npts,
+			 A,B,VERBOSE)) {
+      // Handle fail case
+      return(FALSE);
+    }
+
+    // Save Beta    
+    for (i = 0; i < 5; i++) {
+      Beta[i] = B[i][0];
+    }
+
+    // Calculate r vector of residuals
+    for (i = 0; i < Npts; i++) {
+      rx[i] = X[i] - (T[i] * B[0][0] + B[1][0] + B[4][0] * pR[i]);
+      ry[i] = Y[i] - (T[i] * B[2][0] + B[3][0] + B[4][0] * pD[i]);
+      IRLS_u[i] = sqrt(SQ(rx[i] / dX[i]) + SQ(ry[i] / dY[i]));
+    }
+
+    // Calculate sigma_hat from distribution of residual magnitudes
+    sigma_hat = Plx_MAD(IRLS_u,Npts) / 0.6745;
+
+    // Check convergence
+    converged = TRUE;
+
+    for (i = 0; i < 5; i++) {
+      if (fabs(Beta[i] - Beta_prev[i]) > tolerance * abs(Beta[i])) {
+	converged = FALSE;
+      }
+    }
+
+    iterations++;
+    if (iterations > max_iterations) {
+      converged = TRUE;
+      // Throw a warning or something here.
+    }
+
+  } while (!converged);
+
+  double ax, ay;
+  double bx, by;
+  double lambda;
+  double sigma_robust_x, sigma_robust_y;
+  double sigma_final_x,  sigma_final_y;
+  double Sum_Wx, Sum_Wy;
+
+  ax = 0.0; ay = 0.0;
+  bx = 0.0; by = 0.0;
+  lambda = 0.0;
+  for (i = 0; i < Npts; i++) {
+    Wx[i] = Plx_weight_cauchy(rx[i] / dX[i]);
+    Wy[i] = Plx_weight_cauchy(ry[i] / dY[i]);
+
+    ax += Plx_dpsi_cauchy(rx[i] / dX[i]);
+    ay += Plx_dpsi_cauchy(ry[i] / dY[i]);
+
+    bx += SQ(Wx[i]);
+    by += SQ(Wy[i]);
+
+    Sum_Wx += Wx[i];
+    Sum_Wy += Wy[i];
+  }
+  ax /= 1.0 * Npts;  // mean(psi_dot(r))
+  ay /= 1.0 * Npts;
+  bx /= 1.0 * (Npts - p); // mean(psi^2(r)) * (N / (N-p))
+  by /= 1.0 * (Npts - p);
+
+  sigma_robust_x = lambda * sqrt(bx) * sigma_hat * 2.385 / ax;
+  sigma_robust_y = lambda * sqrt(by) * sigma_hat * 2.385 / ay;
+
+  // This is actually sigma^2, as that's the factor in the covariance (dumouchel 4.1)
+  sigma_final_x  = MAX(SQ(sigma_robust_x), (Npts * SQ(sigma_robust_x) + SQ(p * sigma_ols)) / (Npts + SQ(p)));
+  sigma_final_y  = MAX(SQ(sigma_robust_y), (Npts * SQ(sigma_robust_y) + SQ(p * sigma_ols)) / (Npts + SQ(p)));
+
+  for (i = 0; i < 5; i++) {
+    for (j = 0; j < 5; j++) {
+      // This uses the original OLS covariance.
+      if ((i < 2)&&(j < 2)) { // Upper portion
+	Cov[i][j] *= sigma_final_x;
+      }
+      else if ((i < 4)&&(j < 4)) { // Lower portion
+	Cov[i][j] *= sigma_final_y;
+      }
+      else { // Cross term
+	Cov[i][j] *= sqrt(sigma_final_x * sigma_final_y);
+      }
+    }
+  }
+  
+  
+
+  // Finish
+  fit[0].Ro = B[0][0];
+  fit[0].uR = B[1][0];
+  fit[0].Do = B[2][0];
+  fit[0].uD = B[3][0];
+  fit[0].p  = B[4][0];
+  
+  fit[0].dRo = sqrt(Cov[0][0]);
+  fit[0].duR = sqrt(Cov[1][1]);
+  fit[0].dDo = sqrt(Cov[2][2]);
+  fit[0].duD = sqrt(Cov[3][3]);
+  fit[0].dp  = sqrt(Cov[4][4]);
+  
+  // (optionally) add up the chi square for the fit
+  if (fit->getChisq) {
+    chisq = 0.0;
+    fit[0].Nfit = 0;
+
+    double wx,wy;
+    for (i = 0; i < Npts; i++) {
+      if ((Wx[i] > 0.1 * Sum_Wx / (1.0 * Npts))||
+	  (Wy[i] > 0.1 * Sum_Wy / (1.0 * Npts))) {
+	
+	Xf = fit[0].Ro + fit[0].uR*T[i] + fit[0].p*pR[i];
+	Yf = fit[0].Do + fit[0].uD*T[i] + fit[0].p*pD[i];
+	wx = (fabs(dX[i]) < 0.0001) ? 1.0 : 1.0 / SQ(dX[i]);
+	wy = (fabs(dY[i]) < 0.0001) ? 1.0 : 1.0 / SQ(dY[i]);
+	chisq += SQ(X[i] - Xf) * wx;
+	chisq += SQ(Y[i] - Yf) * wy;
+	fit[0].Nfit += 1;
+      // if (VERBOSE) fprintf (stderr, "chisq contrib : %f %f : %f %f : %f %f : %f %f : %f\n", Xf, Yf, X[i] - Xf, Y[i] - Yf, dX[i], dY[i], (X[i] - Xf) / dX[i], (Y[i] - Yf) / dY[i], chisq);
+      }
+    }
+    // the reduced chisq is divided by (Ndof = 2*Npts - 5)
+    fit[0].chisq = chisq / (2.0*Npts - 5.0);
+  }
+  else {
+    fit[0].Nfit = Npts;
+  }
+  
+  return (TRUE);
+}
+
+/* Low precision formulae for the sun, from Astro. Almanac p. C5 (2012) */
+int sun_ecliptic_IRLS (double mjd, double *lambda, double *beta, double *epsilon, double *Radius) {
+
+  double n = mjd - J2000;                          // day number relative to standard epoch
+  double L = 280.460 + 0.9856474 * n;	           // mean solar longitute (corr. for aberration)
+  double g = (357.528 + 0.9856003 * n)*RAD_DEG;    // Mean anomaly
+
+  *lambda = L + 1.915 * sin(g) + 0.020 * sin(2*g); // solar longitude in degrees
+  *beta = 0.0;					   // approx latitude
+  *epsilon = (23.439 - 0.0000004 * n);		   // obliquity of ecliptic in degrees
+  *Radius = 1.00014 - 0.01671*cos(g) - 0.00014*cos(2*g); // earth-to-sun dist in AU
+  return TRUE;
+}
+
+/* given RA, DEC, Time, calculate the parallax factor */
+// RA,DEC are decimal degrees
+// Time is MJD
+int ParFactor_IRLS (double *pR, double *pD, double RA, double DEC, double Time) {
+
+  double lambda, beta, epsilon, Radius;
+
+  // Time must be mjd
+  sun_ecliptic_IRLS (Time, &lambda, &beta, &epsilon, &Radius);
+
+  double lambda_rad = lambda*RAD_DEG;
+  double epsilon_rad = epsilon*RAD_DEG;
+  double RA_rad = RA*RAD_DEG;
+  double DEC_rad = DEC*RAD_DEG;
+
+  double x = Radius*cos(lambda_rad);
+  double y = Radius*cos(epsilon_rad)*sin(lambda_rad);
+  double z = Radius*sin(epsilon_rad)*sin(lambda_rad);
+
+  *pR = +(y*cos(RA_rad) - x*sin(RA_rad));
+  *pD = -(y*sin(RA_rad) + x*cos(RA_rad))*sin(DEC_rad) + z*cos(DEC_rad);
+
+  return TRUE;
+}
+
+// allocate arrays but not the container
+int PlxFitDataAlloc_IRLS (PlxFitData *data, int N) {
+
+  data->Npts = N;
+  ALLOCATE (data->X, double, N);
+  ALLOCATE (data->Y, double, N);
+  ALLOCATE (data->dX, double, N);
+  ALLOCATE (data->dY, double, N);
+  ALLOCATE (data->t, double, N);
+  ALLOCATE (data->pX, double, N);
+  ALLOCATE (data->pY, double, N);
+  ALLOCATE (data->IRLS_u, double, N);
+  ALLOCATE (data->index, int, N);
+  return TRUE;
+}
+
+void PlxFitDataFree_IRLS (PlxFitData *data) {
+  FREE (data->X);
+  FREE (data->Y);
+  FREE (data->dX);
+  FREE (data->dY);
+  FREE (data->t);
+  FREE (data->pX);
+  FREE (data->pY);
+  FREE (data->IRLS_u);
+  FREE (data->index);
+}
+
+int PlxBootstrapResample_IRLS (PlxFitData *src, PlxFitData *tgt) {
+  int i;
+  tgt->Npts = src->Npts;
+  for (i = 0; i < src->Npts; i++) {
+    int N = tgt->Npts * drand48();
+    // int N = i;
+    tgt->X [i] = src->X [N];
+    tgt->Y [i] = src->Y [N];
+    tgt->dX[i] = src->dX[N];
+    tgt->dY[i] = src->dY[N];
+    tgt->t [i] = src->t [N];
+    tgt->pX[i] = src->pX[N];
+    tgt->pY[i] = src->pY[N];
+    tgt->IRLS_u[i] = src->IRLS_u[N];
+  }
+  return TRUE;
+}
+
+int PlxSetMeanEpoch_IRLS (double *R, double *D, double *T, double *Rmean, double *Dmean, double *Tmean, int *mask, int Ntotal) {
+
+  int i;
+
+  // find mean values to remove
+  double Nmean = 0;
+  *Tmean = 0;
+  *Rmean = 0;
+  *Dmean = 0;
+  double Tmin = +1000000;
+  double Tmax = -1000000;
+  for (i = 0; i < Ntotal; i++) {
+    if (mask && !mask[i]) continue;
+    *Rmean += R[i];
+    *Dmean += D[i];
+    *Tmean += T[i];
+    Tmin = MIN(Tmin, T[i]);
+    Tmax = MAX(Tmax, T[i]);
+    Nmean += 1.0;
+  }
+  *Rmean /= Nmean;
+  *Dmean /= Nmean;
+  *Tmean /= Nmean;
+  
+  double Trange = Tmax - Tmin;
+
+  // fprintf (stderr, "R,D : %f,%f, T: %f, Trange: %f, Tmin: %f, Tmax: %f\n", *Rmean, *Dmean, *Tmean, Trange, Tmin, Tmax);
+
+  set_variable ("Trange", Trange);
+  return TRUE;
+}
+
+// generate the fit values (projected X,Y; parallax factors; 
+int PlxSetEpochPosition_IRLS (PlxFitData *fitdata, double *R, double *D, double *dR, double *dD, double *T, int *mask, int Ntotal, Coords *coords, double Tmean) {
+
+  int i;
+
+  float pXmin = +2.0;
+  float pXmax = -2.0;
+  float pYmin = +2.0;
+  float pYmax = -2.0;
+
+  int Nsubset = 0;
+  for (i = 0; i < Ntotal; i++) {
+    if (mask && !mask[i]) continue;
+    RD_to_XY (&fitdata->X[Nsubset], &fitdata->Y[Nsubset], R[i], D[i], coords);
+    fitdata->dX[Nsubset] = dR[i];
+    fitdata->dY[Nsubset] = dD[i];
+    fitdata->t[Nsubset] = (T[i] - Tmean) / 365.25;
+    ParFactor_IRLS (&fitdata->pX[Nsubset], &fitdata->pY[Nsubset], R[i], D[i], T[i]);
+    pXmin = MIN (pXmin, fitdata->pX[Nsubset]);
+    pXmax = MAX (pXmax, fitdata->pX[Nsubset]);
+    pYmin = MIN (pYmin, fitdata->pY[Nsubset]);
+    pYmax = MAX (pYmax, fitdata->pY[Nsubset]);
+
+    fitdata->IRLS_u[Nsubset] = 0.0;
+    fitdata->index[Nsubset] = i;
+    Nsubset++;
+  }
+  fitdata->Npts = Nsubset;
+  float dXRange = pXmax - pXmin;
+  float dYRange = pYmax - pYmin;
+  float parRange = hypot (dXRange, dYRange);
+
+  set_variable ("Prange", parRange);
+  // fprintf (stderr, "par factor range: %f\n", parRange);
+
+  return TRUE;
+}
+
+/* Outlier clipping based on bootstrap-resampling tests of the plx path
+ * generate Noutlier resampled datasets
+ * fit the Noutlier plx paths
+ * determine and save the distribution of dXsig and dYsig for each point
+ * sort the resulting distributions and find dPsig (median point) for each measurement
+ * find the 90% point of dPsig : if > dPsigMax, only clip the 10% most deviant points
+ * set the dPvec values if desired
+ * -- mask is modified, dPvec values are set
+ * -- fitdata is unchanged
+ */
+
+# define MAX_REJECT 0.1
+
+int PlxOutlierClip_IRLS (PlxFitData *fitdata, int *mask, int Noutlier, float dPsigMax, Vector *dPvec, int VERBOSE) {
+
+  int i, n;
+
+  PlxFit_IRLS testfit;
+  testfit.getChisq = FALSE;
+
+  PlxFitData sample;
+  PlxFitDataAlloc_IRLS (&sample, fitdata->Npts);
+
+  double **dXsig, **dYsig;
+  ALLOCATE (dXsig, double *, fitdata->Npts);
+  ALLOCATE (dYsig, double *, fitdata->Npts);
+  for (i = 0; i < fitdata->Npts; i++) {
+    ALLOCATE (dXsig[i], double, Noutlier);
+    ALLOCATE (dYsig[i], double, Noutlier);
+  }
+
+  int Nsamples = 0;
+  for (n = 0; n < Noutlier; n++) {
+    // bootstrap resample (fitdata -> sample)
+    PlxBootstrapResample_IRLS (fitdata, &sample);
+      
+    if (n % 100000 == 99999) fprintf (stderr, ".");
+
+    // fit the sample
+    if (!FitPMandPar_IRLS (&testfit, 
+			   sample.X, sample.dX, 
+			   sample.Y, sample.dY, sample.t, 
+			   sample.pX, sample.pY, sample.IRLS_u, sample.Npts, 1, VERBOSE)) continue;
+
+    // fprintf (stderr, "%f +/- %f | %f %f\n", testfit.p, testfit.dp, testfit.uR, testfit.uD);
+
+    // find the distances to the path
+    for (i = 0; i < fitdata->Npts; i++) {
+      double Xf = testfit.Ro + testfit.uR*fitdata->t[i] + testfit.p*fitdata->pX[i];
+      double Yf = testfit.Do + testfit.uD*fitdata->t[i] + testfit.p*fitdata->pY[i];
+      dXsig[i][Nsamples] = fabs(fitdata->X[i] - Xf) / fitdata->dX[i];
+      dYsig[i][Nsamples] = fabs(fitdata->Y[i] - Yf) / fitdata->dY[i];
+      // fprintf (stderr, "%f : %f %f : %f %f : %f %f : %f %f %f\n", T[i], Xf, Yf, fitdata->X[i], fitdata->Y[i], fitdata->dX[i], fitdata->dY[i], fitdata->t[i], fitdata->pX[i], fitdata->pY[i]);
+    }
+    Nsamples ++;
+  }
+
+  double *dPsig;
+  ALLOCATE (dPsig, double, fitdata->Npts);
+    
+  for (i = 0; i < fitdata->Npts; i++) {
+    dsort (dXsig[i], Nsamples);
+    dsort (dYsig[i], Nsamples);
+
+    // choose the median values
+    double dXsigMedian, dYsigMedian;
+    if (Nsamples % 2) {
+      int Ncenter = Nsamples / 2;
+      dXsigMedian = dXsig[i][Ncenter];
+      dYsigMedian = dYsig[i][Ncenter];
+    } else {
+      int Ncenter = Nsamples / 2 - 1;
+      dXsigMedian = 0.5*(dXsig[i][Ncenter] + dXsig[i][Ncenter + 1]);
+      dYsigMedian = 0.5*(dYsig[i][Ncenter] + dYsig[i][Ncenter + 1]);
+    }
+    // XXX replace with hypotenuse?
+    dPsig[i] = 0.5*(dXsigMedian + dYsigMedian);
+    // fprintf (stderr, "%d %10.6f %10.6f %10.6f  %f %f : %f\n", i, R[i], D[i], T[i], dXsig[i][Ncenter], dYsig[i][Ncenter], dPsig[i]);
+  }
+
+  // make a copy of dPsig[] and check if > 10% are > dPsigMax
+  double *dPsigSort;
+  ALLOCATE (dPsigSort, double, fitdata->Npts);
+  for (i = 0; i < fitdata->Npts; i++) {
+    dPsigSort[i] = dPsig[i];
+  }
+  dsort (dPsigSort, fitdata->Npts);
+  int Nmax = (1.0 - MAX_REJECT)*fitdata->Npts;
+
+  int completeClip = TRUE;
+  if (dPsigSort[Nmax] > dPsigMax) {
+    if (VERBOSE) fprintf (stderr, "too many outliers: %f at 90\n", dPsigSort[Nmax]);
+    dPsigMax = dPsigSort[Nmax];
+    completeClip = FALSE;
+  }
+
+  for (i = 0; i < fitdata->Npts; i++) {
+    if (dPsig[i] < dPsigMax) continue;
+    int n = fitdata->index[i];
+    // fprintf (stderr, "clip %d: %f : %f\n", i, fitdata->t[i], dPsig[i]);
+    mask[n] = 0; // mask these points
+  }
+
+  // only set dPvec if we have completed the clipping?
+  if (dPvec) {
+    for (i = 0; i < dPvec->Nelements; i++) {
+      dPvec->elements.Flt[i] = NAN;
+    }
+    for (i = 0; i < fitdata->Npts; i++) {
+      int n = fitdata->index[i];
+      dPvec->elements.Flt[n] = dPsig[i];
+    }
+  }
+
+  free (dPsig);
+  free (dPsigSort);
+  
+  for (i = 0; i < fitdata->Npts; i++) {
+    free (dXsig[i]);
+    free (dYsig[i]);
+  }
+  free (dXsig);
+  free (dYsig);
+
+  return completeClip;
+}
+
+int VectorRobustStats_IRLS (Vector *vector, double *median, double *sigma) {
+
+  // warn if vector->Nelements > 1000? 10000?)
+  // warn if vector is not float
+
+  // we need to copy the vector to avoid changing the sort order
+  double *values = NULL;
+  ALLOCATE (values, double, vector->Nelements);
+
+  int i;
+  int Npts = 0;
+  for (i = 0; i < vector->Nelements; i++) {
+    if (!isfinite(vector->elements.Flt[i])) continue;
+    values[Npts] = vector->elements.Flt[i];
+    Npts++;
+  }
+
+  dsort (values, Npts);
+
+  if (Npts % 2) {
+    int Ncenter = Npts / 2;
+    *median = values[Ncenter];
+  } else {
+    int Ncenter = Npts / 2 - 1;
+    *median = 0.5*(values[Ncenter] + values[Ncenter + 1]);
+  }
+
+  double Slo = VectorFractionInterpolate_IRLS (values, 0.158655, Npts);
+  double Shi = VectorFractionInterpolate_IRLS (values, 0.841345, Npts);
+
+  *sigma = (Shi - Slo) / 2.0;
+
+  return TRUE;
+}
+
+double VectorFractionInterpolate_IRLS (double *values, float fraction, int Npts) {
+
+  float F = fraction * Npts;
+  int   N = fraction * Npts;
+
+  if (N < 0        ) return NAN;
+  if (N >= Npts - 2) return NAN;
+
+  // interpolate between N,N+1
+    
+  double S = (F - N) * (values[N+1] - values[N]) + values[N];
+  return S;
+}
+
+
+double Plx_weight_cauchy (double x) {
+  double r = x / 2.385;
+  return (1.0 / (1.0 + SQ(r)));
+}
+
+// dpsi = (d/dx) (x * weight(x))
+double Plx_dpsi_cauchy (double x) {
+  double r2 = SQ(x / 2.385);
+  return ((1.0 - r2) / (SQ(1 + r2)));
+}
+
+
+// median absolute deviation
+// MAD = median(abs(x - median(x)))
+double Plx_MAD(double *in, int N) {
+  double *x;
+  double median = 0.0;
+  int i;
+
+  ALLOCATE(x,double,N);
+  for (i = 0; i < N; i++) {
+    x[i] = in[i];
+  }
+
+  dsort(x,N);
+
+  if (N % 2) {
+    median = 0.5*(x[(int)(0.5*N)] + x[(int)(0.5*N) - 1]);
+  } else {
+    median = x[(int)(0.5*N)];
+  }
+
+  for (i = 0; i < N; i++ ) {
+    x[i] = fabs(x[i] - median);
+  }
+
+  dsort(x,N);
+
+  if (N % 2) {
+    median = 0.5*(x[(int)(0.5*N)] + x[(int)(0.5*N) - 1]);
+  } else {
+    median = x[(int)(0.5*N)];
+  }
+
+  return(median);
+}
+
+int Plx_weighted_LS (double *T, double *pR, double *pD, double *X, double *WX, double *Y, double *WY, int Npts,
+		     double **A, double **B, int VERBOSE) {
+
+  int i;
+  double wx, wy, Wx, Wy, Tx, Ty, Tx2, Ty2, Xs, Ys, XT, YT;
+  double PR, PD, PRT, PDT, PRX, PDY, PR2, PD2;
+
+  PR = PD = PRT = PDT = PRX = PDY = PR2 = PD2 = 0.0;
+  Wx = Wy = Tx = Ty = Tx2 = Ty2 = Xs = Ys = XT = YT = 0.0;
+  for (i = 0; i < Npts; i++) {
+
+    if (VERBOSE == 2) fprintf (stderr, "%f %f : %f %f : %f : %f %f\n", X[i], WX[i], Y[i], WY[i], T[i], pR[i], pD[i]);
+
+    wx = WX[i];
+    wy = WY[i];
+
+    Wx += wx;
+    Wy += wy;
+
+    Tx += T[i]*wx;
+    Ty += T[i]*wy;
+    
+    Tx2 += SQ(T[i])*wx;
+    Ty2 += SQ(T[i])*wy;
+    
+    PR += pR[i]*wx;
+    PD += pD[i]*wy;
+    
+    PRT += pR[i]*T[i]*wx;
+    PDT += pD[i]*T[i]*wy;
+    
+    PRX += pR[i]*X[i]*wx;
+    PDY += pD[i]*Y[i]*wy;
+    
+    PR2 += SQ(pR[i])*wx;
+    PD2 += SQ(pD[i])*wy;
+
+    Xs += X[i]*wx;
+    Ys += Y[i]*wy;
+
+    XT += X[i]*T[i]*wx;
+    YT += Y[i]*T[i]*wy;
+  }
+
+  A[0][0] = Wx;
+  A[0][1] = Tx;
+  A[0][4] = PR;
+
+  A[1][0] = Tx;
+  A[1][1] = Tx2;
+  A[1][4] = PRT;
+
+  A[2][2] = Wy;
+  A[2][3] = Ty;
+  A[2][4] = PD;
+
+  A[3][2] = Ty;
+  A[3][3] = Ty2;
+  A[3][4] = PDT;
+
+  A[4][0] = PR;
+  A[4][1] = PRT;
+  A[4][2] = PD;
+  A[4][3] = PDT;
+  A[4][4] = PR2 + PD2;
+
+  B[0][0] = Xs;
+  B[1][0] = XT;
+  B[2][0] = Ys;
+  B[3][0] = YT;
+  B[4][0] = PRX + PDY;
+
+  if (!dgaussjordan ((double **)A, (double **)B, 5, 1)) {
+    if (VERBOSE) fprintf (stderr, "error in fit\n");
+    if (VERBOSE == 2) {
+      int j;
+      for (i = 0; i < 5; i++) {
+	for (j = 0; j < 5; j++) {
+	  fprintf (stderr, "%e ", A[i][j]);
+	}
+	fprintf (stderr, " : %e\n", A[i][0]);
+      }
+    }
+    return FALSE;
+  }
+
+  // A => (X^T W X)^{-1}
+  // B => beta
+
+  return TRUE;
+}