Index: trunk/Ohana/src/relastro/src/FitPM_IRLS.c
===================================================================
--- trunk/Ohana/src/relastro/src/FitPM_IRLS.c	(revision 39237)
+++ trunk/Ohana/src/relastro/src/FitPM_IRLS.c	(revision 39237)
@@ -0,0 +1,332 @@
+# include "relastro.h"
+
+/* do we want an init function which does the alloc and a clear function to free? */
+int FitPMonly_IRLS (FitAstromResult *fit, FitAstromData *data, FitAstromPoint *points, int Npoints, int VERBOSE) {
+
+  int i,j;
+
+  static double **A, **B;
+
+  double chisq, Xf, Yf;
+
+  double **Cov;
+  double *Beta, *Beta_prev;
+  
+  double sigma_ols, sigma_hat;
+  double *Wx, *Wy;
+  double *rx, *ry;
+  //  double *ux, *uy;
+  double *u;
+  int dof = 2 * Npoints - 4;
+  int p   = 4;
+  int n   = 2 * Npoints;
+  double tolerance;
+  int converged;
+  int iterations;
+  
+  /* do I need to do this as 2 2x2 matrix equations? */
+  if (A == NULL) {
+    ALLOCATE (A, double *, 4);
+    ALLOCATE (B, double *, 4);
+    for (i = 0; i < 4; i++) {
+      ALLOCATE (A[i], double, 4);
+      ALLOCATE (B[i], double, 1);
+      memset (A[i], 0, 4*sizeof(double));
+      memset (B[i], 0, 1*sizeof(double));
+    }
+  }
+
+  // things we need
+  ALLOCATE (Cov, double *, 4);
+  for (i = 0; i < 4; i++) {
+    ALLOCATE ( Cov[i], double, 4);
+  }
+
+  ALLOCATE(Beta, double, 4);
+  ALLOCATE(Beta_prev, double, 4);
+  ALLOCATE(Wx, double, Npoints);
+  ALLOCATE(Wy, double, Npoints);
+  ALLOCATE(rx,  double, Npoints);
+  ALLOCATE(ry,  double, Npoints);
+  ALLOCATE(u,  double, Npoints);
+  
+  // Convert the measurement errors into initial weights.
+  for (i = 0; i < Npoints; i++) {
+    Wx[i] = 1 / dX[i];
+    Wy[i] = 1 / dY[i];
+  }
+  
+  // Solve OLS equation  
+  if (!weighted_LS_PM(T,X,Wx,Y,Wy,Npoints,
+		   A,B,VERBOSE)) {
+    // Handle fail case
+    return(FALSE);
+  }
+
+  // Calculate r vector of residuals and least squares sigma
+  sigma_ols = 0.0;
+  for (i = 0; i < Npoints; i++) {
+    rx[i] = X[i] - (T[i] * B[1][0] + B[0][0]);
+    ry[i] = Y[i] - (T[i] * B[3][0] + B[2][0]);
+    //    u[i] = r[i] /
+    sigma_ols += SQ(rx[i]) + SQ(ry[i]);
+
+  }
+  sigma_ols = sqrt(sigma_ols / dof);
+
+  // Save OLS covariance;
+  for (i = 0; i < 4; i++) {
+    for (j = 0; j < 4; j++) {
+      Cov[i][j] = A[i][j];
+    }
+  }
+
+  // Save Beta
+  for (i = 0; i < 4; i++) {
+    Beta[i] = B[i][0];
+  }
+
+  // Iterately reweight and solve
+  converged = FALSE;
+  iterations = 0;
+  do {
+    // Save Beta.
+    for (i = 0; i < 4; i ++) {
+      Beta_prev[i] = Beta[i];
+    }
+
+    // Assign W
+    for (i = 0; i < Npoints; i++) {
+      Wx[i] = weight_cauchy(rx[i] / dX[i]);
+      Wy[i] = weight_cauchy(ry[i] / dY[i]);
+    }    
+
+    // Solve
+    if (!weighted_LS_PM(T,X,Wx,Y,Wy,Npoints,
+		     A,B,VERBOSE)) {
+      // Handle fail case
+      return(FALSE);
+    }
+
+    for (i = 0; i < 4; i++) {
+      Beta[i] = B[i][0];
+    }
+
+    // r
+    sigma_hat = 0.0;
+    for (i = 0; i < Npoints; i++) {
+      rx[i] = X[i] - (T[i] * B[1][0] + B[0][0]);
+      ry[i] = Y[i] - (T[i] * B[3][0] + B[2][0]);
+      u[i] = sqrt(SQ(rx[i] / dX[i]) + SQ(ry[i] / dY[i]));
+    }
+    sigma_hat = MedianAbsDeviation(u,Npoints) / 0.6745;
+    
+    // Check convergence
+    converged = TRUE;
+    tolerance = 1e-4;  // This should probably be tunable.
+    for (i = 0; i < 4; i++) {
+      if (fabs(Beta[i] - Beta_prev[i]) > tolerance * abs(Beta[i])) {
+	converged = FALSE;
+      }
+    }
+
+    iterations++;
+    if (iterations >= 10) {
+      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;
+  
+  Sum_Wx = 0.0;
+  Sum_Wy = 0.0;
+  ax = 0.0; ay = 0.0;
+  bx = 0.0; by = 0.0;
+  lambda = 0.0;
+  for (i = 0; i < Npoints; i++) {
+    Wx[i] = weight_cauchy(rx[i] / dX[i]);
+    Wy[i] = weight_cauchy(ry[i] / dY[i]);
+    
+    ax += dpsi_cauchy(rx[i] / dX[i]);
+    ay += 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 * Npoints;  // mean(psi_dot(r))
+  ay /= 1.0 * Npoints; 
+  bx /= 1.0 * (Npoints - p); // mean(psi^2(r)) * (N / (N-p))
+  by /= 1.0 * (Npoints - 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), (n * SQ(sigma_robust_x) + SQ(p * sigma_ols)) / (n + SQ(p)));
+  sigma_final_y  = MAX(SQ(sigma_robust_y), (n * SQ(sigma_robust_y) + SQ(p * sigma_ols)) / (n + SQ(p)));
+
+  for (i = 0; i < 4; i++) {
+    for (j = 0; j < 4; j++) {
+      // This uses the original OLS covariance.
+      if ((i < 2)&&(j < 2)) { // Upper portion
+	Cov[i][j] *= sigma_final_x;
+      }
+      else if ((i > 1)&&(j > 1)) { // 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 = Beta[0];
+  fit[0].uR = Beta[1];
+  fit[0].Do = Beta[2];
+  fit[0].uD = Beta[3];
+  
+  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]);
+
+  // Sort out the final weight threshold.
+
+  // add up the chi square for the fit
+  chisq = 0.0;
+  fit[0].Nfit = 0;
+  for (i = 0; i < Npoints; i++) {
+    if ((Wx[i] > 0.1 * Sum_Wx / (1.0 * Npoints))||
+	(Wy[i] > 0.1 * Sum_Wy / (1.0 * Npoints))) {
+      Xf = fit[0].Ro + fit[0].uR*T[i];
+      Yf = fit[0].Do + fit[0].uD*T[i];
+      chisq += SQ(X[i] - Xf) / SQ(dX[i]);
+      chisq += SQ(Y[i] - Yf) / SQ(dY[i]);
+      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);
+  }
+  //  fit[0].Nfit = Npoints;
+
+  // the reduced chisq is divided by (Ndof = 2*Npoints - 4)
+  fit[0].chisq = chisq / (2.0*Npoints - 4.0);
+  return (TRUE);
+}
+
+int weighted_LS_PM (FitAstromResult *fit, FitAstromData *data, FitAstromPoint *points, int Npoints, double **A, double **B, int VERBOSE) {
+
+  int i,j;
+  double Wx, Wy, Tx, Ty, Tx2, Ty2, Xs, Ys, XT, YT;
+  Wx = Wy = Tx = Ty = Tx2 = Ty2 = Xs = Ys = XT = YT = 0.0;
+  for (i = 0; i < Npoints; i++) {
+    Wx += WX[i];
+    Wy += WY[i];
+
+    Tx += T[i]*WX[i];
+    Ty += T[i]*WY[i];
+    
+    Tx2 += SQ(T[i])*WX[i];
+    Ty2 += SQ(T[i])*WY[i];
+    
+    Xs += X[i]*WX[i];
+    Ys += Y[i]*WY[i];
+
+    XT += X[i]*T[i]*WX[i];
+    YT += Y[i]*T[i]*WY[i];
+  }
+
+  // X^T W X
+  A[0][0] = Wx;
+  A[0][1] = Tx;
+
+  A[1][0] = Tx;
+  A[1][1] = Tx2;
+
+  A[2][2] = Wy;
+  A[2][3] = Ty;
+
+  A[3][2] = Ty;
+  A[3][3] = Ty2;
+
+  // X^T W Y
+  B[0][0] = Xs;
+  B[1][0] = XT;
+  B[2][0] = Ys;
+  B[3][0] = YT;
+
+  if (!dgaussjordan ((double **)A, (double **)B, 4, 1)) {
+    if (VERBOSE) fprintf (stderr, "error in fit\n");
+    if (VERBOSE == 2) {
+      for (i = 0; i < 4; i++) {
+	for (j = 0; j < 4; 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;
+}
+
+double weight_cauchy (double x) {
+  double r = x / 2.385;
+  return (1.0 / (1.0 + SQ(r)));
+}
+
+// dpsi = (d/dx) (x * weight(x))
+double 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 MedianAbsDeviation(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);
+}
Index: trunk/Ohana/src/relastro/src/UpdateObjects.c
===================================================================
--- trunk/Ohana/src/relastro/src/UpdateObjects.c	(revision 39236)
+++ trunk/Ohana/src/relastro/src/UpdateObjects.c	(revision 39237)
@@ -154,8 +154,10 @@
       // if N_BOOTSTRAP_SAMPLES = 1, no bootstrap resampling:
       FitPM (&fitPM, fitStats->fitdataPM, fitStats->points, fitStats->Npoints);
+      // FitPM_IRLS (&fitPM, fitStats->fitdataPM, fitStats->points, fitStats->Npoints);
     } else {
       fitStats->Nfit = 0;
       for (k = 0; k < fitStats->NfitAlloc; k++) {
 	BootstrapResample (fitStats->sample, fitStats->points, fitStats->Npoints);
+	// if (!FitPM_IRLS (&fitStats->fit[k], fitStats->fitdataPM, fitStats->sample, fitStats->Npoints)) continue;
 	if (!FitPM (&fitStats->fit[k], fitStats->fitdataPM, fitStats->sample, fitStats->Npoints)) continue;
 	fitStats->Nfit ++;
@@ -198,4 +200,5 @@
     if (fitStats->NfitAlloc == 1) {
       // if N_BOOTSTRAP_SAMPLES = 1, no bootstrap resampling:
+      // FitPMandPar_IRLS (&fitPar, fitStats->fitdataPar, fitStats->points, fitStats->Npoints);
       FitPMandPar (&fitPar, fitStats->fitdataPar, fitStats->points, fitStats->Npoints);
     } else {
@@ -203,5 +206,6 @@
       for (k = 0; k < fitStats->NfitAlloc; k++) {
 	BootstrapResample (fitStats->sample, fitStats->points, fitStats->Npoints);
-	FitPMandPar (&fitStats->fit[k], fitStats->fitdataPar, fitStats->sample, fitStats->Npoints);
+	// FitPMandPar_IRLS (&fitStats->fit[k], fitStats->fitdataPar, fitStats->sample, fitStats->Npoints);
+	FitPMandPar_IRLS (&fitStats->fit[k], fitStats->fitdataPar, fitStats->sample, fitStats->Npoints);
 	fitStats->Nfit ++;
       }