 #define BEACON_3_X 7.92f
 #define BEACON_3_Y 7.92f
 
-#define BEACON_1_OFFSET 0.0368f
-#define BEACON_1_MULTIPLIER 1.0692f
-
-#define BEACON_2_OFFSET 0.6336f
-#define BEACON_2_MULTIPLIER 1.0440f
-
-#define BEACON_3_OFFSET 0.5575f
-#define BEACON_3_MULTIPLIER 1.0666f
+//First distance from ground truth 2 meters
+//Second from ground truth 5 meters.
+#define BEACON_1_FIRSTDIST 2.368
+#define BEACON_1_SECONDDIST 5.372
+float beacon_1_slope = 3.0f/(BEACON_1_SECONDDIST-BEACON_1_FIRSTDIST);
+float beacon_1_offset = 5.0f-(1.0f/beacon_1_slope*BEACON_1_SECONDDIST);
+
+//First distance from ground truth 2 meters
+//Second from ground truth 5 meters.
+#define BEACON_2_FIRSTDIST 2.368
+#define BEACON_2_SECONDDIST 5.372
+float beacon_2_slope = 3.0f/(BEACON_2_SECONDDIST-BEACON_2_FIRSTDIST);
+float beacon_2_offset = 5.0f-(1.0f/beacon_2_slope*BEACON_2_SECONDDIST);
+
+//First distance from ground truth 2 meters
+//Second from ground truth 5 meters.
+#define BEACON_3_FIRSTDIST 2.368
+#define BEACON_3_SECONDDIST 5.372
+float beacon_3_slope = 3.0f/(BEACON_3_SECONDDIST-BEACON_3_FIRSTDIST);
+float beacon_3_offset = 5.0f-(1.0f/beacon_3_slope*BEACON_3_SECONDDIST);
+//
+//#define BEACON_1_OFFSET 0.0368f
+//#define BEACON_1_MULTIPLIER 1.0692f
+//
+//#define BEACON_2_OFFSET 0.6336f
+//#define BEACON_2_MULTIPLIER 1.0440f
+//
+//#define BEACON_3_OFFSET 0.5575f
+//#define BEACON_3_MULTIPLIER 1.0666f
 
 // connection pins
 const uint8_t PIN_RST = 3; // reset pin


  DW1000Ranging.setLoc(estimate[0], estimate[1]);
}


  range = DW1000Ranging.getDistantDevice()->getRange();
  if(range < 0) address = NULL;
  if(address == BEACON_1_ADDRESS){
    range *= CALIBRATE_FIRST||CALIBRATE_SECOND?1.0f:beacon_1_slope;
    range -= CALIBRATE_FIRST||CALIBRATE_SECOND?0.0f:beacon_1_offset;
    
    cir_enqueue(cir1,range);
    dist[0] = (USE_MEDIAN_FILT)?cir_median(cir1):range;
    
    if(CALIBRATE_FIRST||CALIBRATE_SECOND){
      if(dist[0] < shortestDistanceBeacon1 && dist[0] > 0){
        shortestDistanceBeacon1 = dist[0];
      }
    }
  }
  else if(address == BEACON_2_ADDRESS){
    range *= CALIBRATE_FIRST||CALIBRATE_SECOND?1.0f:beacon_2_slope;
    range -= CALIBRATE_FIRST||CALIBRATE_SECOND?0.0f:beacon_2_offset;
    cir_enqueue(cir2,range);
    dist[1] = (USE_MEDIAN_FILT)?cir_median(cir2):range;
    
    if(CALIBRATE_FIRST||CALIBRATE_SECOND){
      if(dist[1] < shortestDistanceBeacon2 && dist[1] > 0){
        shortestDistanceBeacon2 = dist[1];
      }
    }
  }
  else if(address == BEACON_3_ADDRESS){
    range *= CALIBRATE_FIRST||CALIBRATE_SECOND?1.0f:beacon_3_slope;
    range -= CALIBRATE_FIRST||CALIBRATE_SECOND?0.0f:beacon_3_offset;
    cir_enqueue(cir3,range);
    dist[2] = (USE_MEDIAN_FILT)?cir_median(cir3):range;
    
    if(CALIBRATE_FIRST||CALIBRATE_SECOND){
      if(dist[2] < shortestDistanceBeacon3 && dist[2] > 0){
        shortestDistanceBeacon3 = dist[2];
      }
    }
  }
  Serial.print("1: ");Serial.print(dist[0]);Serial.print(" 2: ");Serial.print(dist[1]);Serial.print(" 3: ");Serial.print(dist[2]);
  
  // ITERATIVE STEPS BELOW

