 #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]);
}


#include <math.h>

// If false, then use the calibration. If true, don't use the calibration.
#define CALIBRATE_FIRST false
#define CALIBRATE_SECOND false

//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

#define USE_MEDIAN_FILT true

