Merge branch 'master' into simple_api

12 files changed, 1004 insertions, 285 deletions

diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c
index 52a5f54..1749d8e 100644
--- a/src/poser_charlesrefine.c
+++ b/src/poser_charlesrefine.c
@@ -13,7 +13,7 @@
 #include <string.h>
 #include <survive_imu.h>
 
-#define MAX_PT_PER_SWEEP 32
+#define MAX_PT_PER_SWEEP SENSORS_PER_OBJECT
 
 typedef struct {
 	int sweepaxis;
@@ -23,6 +23,11 @@ typedef struct {
 	FLT angles_at_pts[MAX_PT_PER_SWEEP];
 	SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP];
 	uint8_t sensor_ids[MAX_PT_PER_SWEEP];
+
+	LinmathPoint3d MixingPositions[NUM_LIGHTHOUSES][2];
+	SurvivePose InteralPoseUsedForCalc; // Super high speed vibratey and terrible.
+	float MixingConfidence[NUM_LIGHTHOUSES][2];
+
 	int ptsweep;
 
 	SurviveIMUTracker tracker;
@@ -30,17 +35,58 @@ typedef struct {
 
 int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 	CharlesPoserData *dd = so->PoserData;
-	if (!dd)
+	if (!dd) {
 		so->PoserData = dd = calloc(sizeof(CharlesPoserData), 1);
+		SurvivePose object_pose_out;
+		memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity));
+		memcpy(&dd->InteralPoseUsedForCalc, &LinmathPose_Identity, sizeof(LinmathPose_Identity));
+		so->PoseConfidence = 1.0;
+		PoserData_poser_pose_func(pd, so, &object_pose_out);
+	}
 
 	SurviveSensorActivations *scene = &so->activations;
 	switch (pd->pt) {
 	case POSERDATA_IMU: {
 		// Really should use this...
-		PoserDataIMU *imu = (PoserDataIMU *)pd;
+		PoserDataIMU *imuData = (PoserDataIMU *)pd;
 
-		survive_imu_tracker_integrate(so, &dd->tracker, imu);
-		PoserData_poser_pose_func(pd, so, &dd->tracker.pose);
+		// TODO: Actually do Madgwick's algorithm
+		LinmathQuat applymotion;
+		const SurvivePose *object_pose = &so->OutPose;
+		imuData->gyro[0] *= 1. / 240.;
+		imuData->gyro[1] *= 1. / 240.;
+		imuData->gyro[2] *= 1. / 240.;
+		quatfromeuler(applymotion, imuData->gyro);
+		// printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] );
+
+		LinmathQuat InvertQuat;
+		quatgetreciprocal(InvertQuat, object_pose->Rot);
+
+		// Apply a tiny tug to re-right headset based on the gravity vector.
+		LinmathVec3d reright = {0, 0, 1};
+		LinmathVec3d normup;
+		normalize3d(normup, imuData->accel);
+		LinmathVec3d correct_diff;
+		quatrotatevector(reright, InvertQuat, reright);
+		sub3d(correct_diff, normup, reright);
+		scale3d(correct_diff, correct_diff, -0.001); // This is the coefficient applying the drag.
+		add3d(correct_diff, correct_diff, reright);
+		LinmathQuat reright_quat;
+		normalize3d(correct_diff, correct_diff);
+		quatfrom2vectors(reright_quat, reright, correct_diff);
+
+		SurvivePose object_pose_out;
+		quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion);	 // Contribution from Gyro
+		quatrotateabout(object_pose_out.Rot, object_pose_out.Rot, reright_quat); // Contribution from Accelerometer
+		quatnormalize(object_pose_out.Rot, object_pose_out.Rot);
+
+		copy3d(object_pose_out.Pos, object_pose->Pos);
+		PoserData_poser_pose_func(pd, so, &object_pose_out);
+		quatcopy(dd->InteralPoseUsedForCalc.Rot, object_pose_out.Rot);
+
+		// PoserDataIMU *imu = (PoserDataIMU *)pd;
+		// survive_imu_tracker_integrate(so, &dd->tracker, imu);
+		// PoserData_poser_pose_func(pd, so, &dd->tracker.pose);
 
 		return 0;
 	}
@@ -56,7 +102,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 		FLT angle = ld->angle;
 		int sensor_id = ld->sensor_id;
 		int axis = dd->sweepaxis;
-		const SurvivePose *object_pose = &so->OutPose;
+		// const SurvivePose *object_pose = &so->OutPose;
+
 		dd->sweeplh = lhid;
 
 		// FOR NOW, drop LH1.
@@ -70,7 +117,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 		FLT sensor_position_worldspace[3];
 		// XXX Once I saw this get pretty wild (When in playback)
 		// I had to invert the values of sensor_inpos.  Not sure why.
-		ApplyPoseToPoint(sensor_position_worldspace, object_pose, sensor_inpos);
+		ApplyPoseToPoint(sensor_position_worldspace, &dd->InteralPoseUsedForCalc, sensor_inpos);
 
 		// printf( "%f %f %f  == > %f %f %f\n", sensor_inpos[0], sensor_inpos[1], sensor_inpos[2],
 		// sensor_position_worldspace[0], sensor_position_worldspace[1], sensor_position_worldspace[2] );
@@ -109,7 +156,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 			dd->quantity_errors[i] = dist;
 			dd->angles_at_pts[i] = angle;
 			dd->sensor_ids[i] = sensor_id;
-			memcpy(&dd->object_pose_at_hit[i], object_pose, sizeof(SurvivePose));
+			memcpy(&dd->object_pose_at_hit[i], &dd->InteralPoseUsedForCalc, sizeof(SurvivePose));
 			dd->ptsweep++;
 		}
 
@@ -126,8 +173,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 			int lhid = dd->sweeplh;
 			int axis = dd->sweepaxis;
 			int pts = dd->ptsweep;
-			const SurvivePose *object_pose =
-				&so->OutPose; // XXX TODO Should pull pose from approximate time when LHs were scanning it.
+			// const SurvivePose *object_pose =
+			//	&so->OutPose; // XXX TODO Should pull pose from approximate time when LHs were scanning it.
 
 			BaseStationData *bsd = &so->ctx->bsd[lhid];
 			SurvivePose *lh_pose = &bsd->Pose;
@@ -143,11 +190,11 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 #define HIT_QUALITY_BASELINE                                                                                           \
 	0.0001 // Determines which hits to cull.  Actually SQRT(baseline) if 0.0001, it is really 1cm
 
-#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.8 // Explodes if you exceed 1.0
-#define CORRECT_TELESCOPTION_COEFFICIENT 8.0	 // Converges even as high as 10.0 and doesn't explode.
+#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.7 // Explodes if you exceed 1.0
+#define CORRECT_TELESCOPTION_COEFFICIENT 7.00	// Converges even as high as 10.0 and doesn't explode.
 #define CORRECT_ROTATION_COEFFICIENT                                                                                   \
-	1.0 // This starts to fall apart above 5.0, but for good reason.  It is amplified by the number of points seen.
-#define ROTATIONAL_CORRECTION_MAXFORCE 0.10
+	0.2 // This starts to fall apart above 5.0, but for good reason.  It is amplified by the number of points seen.
+#define ROTATIONAL_CORRECTION_MAXFORCE 0.01
 
 			// Step 1: Determine standard of deviation, and average in order to
 			// drop points that are likely in error.
@@ -238,16 +285,33 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 				zoom *= CORRECT_TELESCOPTION_COEFFICIENT;
 
 				FLT veccamalong[3];
-				sub3d(veccamalong, lh_pose->Pos, object_pose->Pos);
+				sub3d(veccamalong, lh_pose->Pos, dd->InteralPoseUsedForCalc.Pos);
 				normalize3d(veccamalong, veccamalong);
 				scale3d(veccamalong, veccamalong, zoom);
 				add3d(vec_correct, veccamalong, vec_correct);
 			}
 
-			SurvivePose object_pose_out;
-			add3d(object_pose_out.Pos, vec_correct, object_pose->Pos);
+#if 0
+			LinmathPoint3d LastDelta;
+			sub3d( LastDelta, dd->MixingPositions[lhid][axis], dd->InteralPoseUsedForCalc.Pos );
+			//Compare with "vec_correct"
+			
+			LinmathPoint3d DeltaDelta;
+			sub3d( DeltaDelta, vec_correct, LastDelta );
+
+
+			//SurvivePose object_pose_out;
 
-			quatcopy(object_pose_out.Rot, object_pose->Rot);
+			memcpy( dd->MixingPositions[lhid][axis], vec_correct, sizeof( vec_correct ) );
+
+			LinmathPoint3d system_average_adjust = { 0, 0, 0 };
+			
+			printf( "%f %f %f + %f %f %f\n", vec_correct[0], vec_correct[1], vec_correct[2], dd->InteralPoseUsedForCalc.Pos[0], dd->InteralPoseUsedForCalc.Pos[1], dd->InteralPoseUsedForCalc.Pos[2] );
+
+#endif
+			add3d(dd->InteralPoseUsedForCalc.Pos, vec_correct, dd->InteralPoseUsedForCalc.Pos);
+
+			// quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot);
 
 			// Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov.
 			// If we were able to determine likliehood of a hit in the sweep instead of afterward
@@ -308,25 +372,50 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 				}
 				// printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] );
 				// Apply our corrective quaternion to the output.
-				quatrotateabout(object_pose_out.Rot, object_pose_out.Rot, correction);
-				quatnormalize(object_pose_out.Rot, object_pose_out.Rot);
-			}
-
-			// Janky need to do this somewhere else...  This initializes the pose estimator.
-			if (so->PoseConfidence < .01) {
-				memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity));
-				so->PoseConfidence = 1.0;
+				quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, correction);
+				quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot);
 			}
 
-			// PoserData_poser_pose_func(pd, so, &object_pose_out);
-			FLT var_meters = 0.5;
-			FLT error = 0.0001;
-			FLT var_quat = error + .05;
-			FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat,
-						  error * var_quat,   error * var_quat,   error * var_quat};
+			memcpy(dd->MixingPositions[lhid][axis], dd->InteralPoseUsedForCalc.Pos,
+				   sizeof(dd->InteralPoseUsedForCalc.Pos));
+			dd->MixingConfidence[lhid][axis] = (validpoints) ? ((validpoints > 1) ? 1.0 : 0.5) : 0;
 
-			survive_imu_tracker_integrate_observation(so, l->timecode, &dd->tracker, &object_pose_out, var);
-			PoserData_poser_pose_func(pd, so, &dd->tracker.pose);
+			// Box filter all of the guesstimations, and emit the new guess.
+			{
+				FLT MixedAmount = 0;
+				LinmathPoint3d MixedPosition = {0, 0, 0};
+				int l = 0, a = 0;
+				if (lhid == 0 && axis == 0)
+					for (l = 0; l < NUM_LIGHTHOUSES; l++)
+						for (a = 0; a < 2; a++)
+							dd->MixingConfidence[l][a] -= 0.1;
+
+				for (l = 0; l < NUM_LIGHTHOUSES; l++)
+					for (a = 0; a < 2; a++) {
+						LinmathPoint3d MixThis = {0, 0, 0};
+						FLT Confidence = dd->MixingConfidence[l][a];
+						if (Confidence < 0)
+							Confidence = 0;
+						scale3d(MixThis, dd->MixingPositions[l][a], Confidence);
+						add3d(MixedPosition, MixedPosition, MixThis);
+						MixedAmount += Confidence;
+						// printf( "%f ", Confidence );
+					}
+				scale3d(MixedPosition, MixedPosition, 1. / MixedAmount);
+				// printf( "%f\n", MixedAmount );
+				SurvivePose object_pose_out;
+				quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot);
+				copy3d(object_pose_out.Pos, MixedPosition);
+				PoserData_poser_pose_func(pd, so, &object_pose_out);
+			}
+			//	FLT var_meters = 0.5;
+			//	FLT error = 0.00001;
+			//	FLT var_quat = error + .05;
+			//	FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat,
+			//				  error * var_quat,   error * var_quat,   error * var_quat};
+			//
+			//	survive_imu_tracker_integrate_observation(so, l->timecode, &dd->tracker, &object_pose_out, var);
+			//	PoserData_poser_pose_func(pd, so, &dd->tracker.pose);
 
 			dd->ptsweep = 0;
 		}
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index 7e922e7..90fab65 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -129,7 +129,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
 	case POSERDATA_LIGHT: {
 		PoserDataLight *lightData = (PoserDataLight *)pd;
 
-		SurvivePose posers[2];
+		SurvivePose posers[2] = {0};
 		int meas[2] = {0, 0};
 		for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) {
 			if (so->ctx->bsd[lh].PositionSet) {
diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c
new file mode 100644
index 0000000..ff82fd4
--- /dev/null
+++ b/src/poser_general_optimizer.c
@@ -0,0 +1,101 @@
+#include "poser_general_optimizer.h"
+#include "string.h"
+
+#include <assert.h>
+#include <malloc.h>
+#include <stdio.h>
+
+void *GetDriver(const char *name);
+void general_optimizer_data_init(GeneralOptimizerData *d, SurviveObject *so) {
+	memset(d, 0, sizeof(*d));
+	d->so = so;
+
+	SurviveContext *ctx = so->ctx;
+
+	d->failures_to_reset = survive_configi(ctx, "failures-to-reset", SC_GET, 1);
+	d->successes_to_reset = survive_configi(ctx, "successes-to-reset", SC_GET, -1);
+	d->max_error = survive_configf(ctx, "max-error", SC_GET, .0001);
+
+	const char *subposer = survive_configs(ctx, "seed-poser", SC_GET, "PoserEPNP");
+	d->seed_poser = (PoserCB)GetDriver(subposer);
+
+	SV_INFO("Initializing general optimizer:");
+	SV_INFO("\tmax-error: %f", d->max_error);
+	SV_INFO("\tsuccesses-to-reset: %d", d->successes_to_reset);
+	SV_INFO("\tfailures-to-reset: %d", d->failures_to_reset);
+	SV_INFO("\tseed-poser: %s(%p)", subposer, d->seed_poser);
+}
+void general_optimizer_data_record_failure(GeneralOptimizerData *d) {
+	if (d->failures_to_reset_cntr > 0)
+		d->failures_to_reset_cntr--;
+}
+bool general_optimizer_data_record_success(GeneralOptimizerData *d, FLT error) {
+	d->stats.runs++;
+	if (d->max_error > error) {
+		if (d->successes_to_reset_cntr > 0)
+			d->successes_to_reset_cntr--;
+		d->failures_to_reset_cntr = d->failures_to_reset;
+		return true;
+	}
+	return false;
+}
+
+typedef struct {
+	bool hasInfo;
+	SurvivePose pose;
+} set_position_t;
+
+static void set_position(SurviveObject *so, uint32_t timecode, SurvivePose *new_pose, void *_user) {
+	set_position_t *user = _user;
+	assert(user->hasInfo == false);
+	user->hasInfo = true;
+	user->pose = *new_pose;
+}
+
+bool general_optimizer_data_record_current_pose(GeneralOptimizerData *d, PoserData *_hdr, size_t len_hdr,
+												SurvivePose *soLocation) {
+	*soLocation = d->so->OutPose;
+	bool currentPositionValid = quatmagnitude(soLocation->Rot) != 0;
+	static bool seed_warning = false;
+	if (d->successes_to_reset_cntr == 0 || d->failures_to_reset_cntr == 0 || currentPositionValid == 0) {
+		PoserCB driver = d->seed_poser;
+		SurviveContext *ctx = d->so->ctx;
+		if (driver) {
+
+			PoserData *hdr = alloca(len_hdr);
+			memcpy(hdr, _hdr, len_hdr);
+			memset(hdr, 0, sizeof(PoserData)); // Clear callback functions
+			hdr->pt = _hdr->pt;
+			hdr->poseproc = set_position;
+
+			set_position_t locations = {0};
+			hdr->userdata = &locations;
+			driver(d->so, hdr);
+			d->stats.poser_seed_runs++;
+
+			if (locations.hasInfo == false) {
+				return false;
+			} else if (locations.hasInfo) {
+				*soLocation = locations.pose;
+			}
+
+			d->successes_to_reset_cntr = d->successes_to_reset;
+		} else if (seed_warning == false) {
+			seed_warning = true;
+			SV_INFO("Not using a seed poser for SBA; results will likely be way off");
+		}
+	}
+	return true;
+}
+
+void general_optimizer_data_record_imu(GeneralOptimizerData *d, PoserDataIMU *imu) {
+	if (d->seed_poser) {
+		d->seed_poser(d->so, &imu->hdr);
+	}
+}
+
+void general_optimizer_data_dtor(GeneralOptimizerData *d) {
+	SurviveContext *ctx = d->so->ctx;
+	SV_INFO("\tseed runs %d / %d", d->stats.poser_seed_runs, d->stats.runs);
+	SV_INFO("\terror failures %d", d->stats.error_failures);
+}
diff --git a/src/poser_general_optimizer.h b/src/poser_general_optimizer.h
new file mode 100644
index 0000000..6d4d906
--- /dev/null
+++ b/src/poser_general_optimizer.h
@@ -0,0 +1,29 @@
+#include <stdlib.h>
+#include <survive.h>
+
+typedef struct GeneralOptimizerData {
+	int failures_to_reset;
+	int failures_to_reset_cntr;
+	int successes_to_reset;
+	int successes_to_reset_cntr;
+
+	FLT max_error;
+
+	struct {
+		int runs;
+		int poser_seed_runs;
+		int error_failures;
+	} stats;
+
+	PoserCB seed_poser;
+	SurviveObject *so;
+} GeneralOptimizerData;
+
+void general_optimizer_data_init(GeneralOptimizerData *d, SurviveObject *so);
+void general_optimizer_data_dtor(GeneralOptimizerData *d);
+
+void general_optimizer_data_record_failure(GeneralOptimizerData *d);
+bool general_optimizer_data_record_success(GeneralOptimizerData *d, FLT error);
+void general_optimizer_data_record_imu(GeneralOptimizerData *d, PoserDataIMU *imu);
+bool general_optimizer_data_record_current_pose(GeneralOptimizerData *d, PoserData *hdr, size_t len_hdr,
+												SurvivePose *p);
diff --git a/src/poser_mpfit.c b/src/poser_mpfit.c
new file mode 100644
index 0000000..45e2c69
--- /dev/null
+++ b/src/poser_mpfit.c
@@ -0,0 +1,283 @@
+#ifndef USE_DOUBLE
+#define FLT double
+#define USE_DOUBLE
+#endif
+
+#include <malloc.h>
+
+#include "mpfit/mpfit.h"
+#include "poser.h"
+#include <survive.h>
+#include <survive_imu.h>
+
+#include "assert.h"
+#include "linmath.h"
+#include "math.h"
+#include "poser_general_optimizer.h"
+#include "string.h"
+#include "survive_cal.h"
+#include "survive_config.h"
+#include "survive_reproject.h"
+
+typedef struct {
+	SurviveObject *so;
+	FLT *pts3d;
+	int *lh;
+	FLT *meas;
+	SurvivePose camera_params[2];
+} mpfit_context;
+
+typedef struct MPFITData {
+	GeneralOptimizerData opt;
+
+	int last_acode;
+	int last_lh;
+
+	int sensor_time_window;
+	int use_jacobian_function;
+	int required_meas;
+
+	SurviveIMUTracker tracker;
+	bool useIMU;
+
+	struct {
+		int meas_failures;
+	} stats;
+} MPFITData;
+
+static size_t construct_input_from_scene(MPFITData *d, PoserDataLight *pdl, SurviveSensorActivations *scene,
+										 double *meas, double *sensors, int *lhs) {
+	size_t rtn = 0;
+	SurviveObject *so = d->opt.so;
+
+	for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) {
+		for (size_t lh = 0; lh < 2; lh++) {
+			if (SurviveSensorActivations_isPairValid(scene, d->sensor_time_window, pdl->timecode, sensor, lh)) {
+				const double *a = scene->angles[sensor][lh];
+
+				sensors[rtn * 3 + 0] = so->sensor_locations[sensor * 3 + 0];
+				sensors[rtn * 3 + 1] = so->sensor_locations[sensor * 3 + 1];
+				sensors[rtn * 3 + 2] = so->sensor_locations[sensor * 3 + 2];
+				meas[rtn * 2 + 0] = a[0];
+				meas[rtn * 2 + 1] = a[1];
+				lhs[rtn] = lh;
+				rtn++;
+			}
+		}
+	}
+	return rtn;
+}
+
+static void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
+	SurvivePose obj = *(SurvivePose *)bi;
+	int sensor_idx = j >> 1;
+	int lh = j & 1;
+
+	mpfit_context *ctx = (mpfit_context *)(adata);
+	SurviveObject *so = ctx->so;
+
+	assert(lh < 2);
+	assert(sensor_idx < so->sensor_ct);
+
+	// quatnormalize(obj.Rot, obj.Rot);
+
+	// std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl;
+	SurvivePose *camera = &so->ctx->bsd[lh].Pose;
+	survive_reproject_full(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh],
+						   &so->ctx->calibration_config);
+}
+
+static void str_metric_function_jac(int j, int i, double *bi, double *xij, void *adata) {
+	SurvivePose obj = *(SurvivePose *)bi;
+	int sensor_idx = j >> 1;
+	int lh = j & 1;
+
+	mpfit_context *ctx = (mpfit_context *)(adata);
+	SurviveObject *so = ctx->so;
+
+	assert(lh < 2);
+	assert(sensor_idx < so->sensor_ct);
+
+	// quatnormalize(obj.Rot, obj.Rot);
+
+	SurvivePose *camera = &so->ctx->bsd[lh].Pose;
+	survive_reproject_full_jac_obj_pose(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh],
+										&so->ctx->calibration_config);
+}
+
+int mpfunc(int m, int n, double *p, double *deviates, double **derivs, void *private) {
+	mpfit_context *mpfunc_ctx = private;
+
+	SurvivePose *pose = (SurvivePose *)p;
+
+	for (int i = 0; i < m / 2; i++) {
+		FLT out[2];
+		survive_reproject_full(out, pose, mpfunc_ctx->pts3d + i * 3, &mpfunc_ctx->camera_params[mpfunc_ctx->lh[i]],
+							   &mpfunc_ctx->so->ctx->bsd[mpfunc_ctx->lh[i]], &mpfunc_ctx->so->ctx->calibration_config);
+		assert(!isnan(out[0]));
+		assert(!isnan(out[1]));
+		deviates[i * 2 + 0] = out[0] - mpfunc_ctx->meas[i * 2 + 0];
+		deviates[i * 2 + 1] = out[1] - mpfunc_ctx->meas[i * 2 + 1];
+
+		if (derivs) {
+			FLT out[7 * 2];
+			survive_reproject_full_jac_obj_pose(
+				out, pose, mpfunc_ctx->pts3d + i * 3, &mpfunc_ctx->camera_params[mpfunc_ctx->lh[i]],
+				&mpfunc_ctx->so->ctx->bsd[mpfunc_ctx->lh[i]], &mpfunc_ctx->so->ctx->calibration_config);
+
+			for (int j = 0; j < n; j++) {
+				derivs[j][i * 2 + 0] = out[j];
+				derivs[j][i * 2 + 1] = out[j + 7];
+			}
+		}
+	}
+
+	return 0;
+}
+
+static double run_mpfit_find_3d_structure(MPFITData *d, PoserDataLight *pdl, SurviveSensorActivations *scene,
+										  int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/,
+										  SurvivePose *out) {
+	double *covx = 0;
+	SurviveObject *so = d->opt.so;
+
+	mpfit_context mpfitctx = {.so = so,
+							  .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose},
+							  .meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES),
+							  .lh = alloca(sizeof(int) * so->sensor_ct * NUM_LIGHTHOUSES),
+							  .pts3d = alloca(sizeof(double) * 3 * so->sensor_ct * NUM_LIGHTHOUSES)};
+
+	size_t meas_size = 2 * construct_input_from_scene(d, pdl, scene, mpfitctx.meas, mpfitctx.pts3d, mpfitctx.lh);
+
+	static int failure_count = 500;
+	bool hasAllBSDs = true;
+	for (int lh = 0; lh < so->ctx->activeLighthouses; lh++)
+		hasAllBSDs &= so->ctx->bsd[lh].PositionSet;
+
+	if (!hasAllBSDs || meas_size < d->required_meas) {
+		if (hasAllBSDs && failure_count++ == 500) {
+			SurviveContext *ctx = so->ctx;
+			SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size);
+			failure_count = 0;
+		}
+		if (meas_size < d->required_meas) {
+			d->stats.meas_failures++;
+		}
+		return -1;
+	}
+	failure_count = 0;
+
+	SurvivePose soLocation = {0};
+
+	if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) {
+		return -1;
+	}
+
+	mp_result result = {0};
+	mp_par pars[7] = {0};
+
+	const bool debug_jacobian = false;
+	if (d->use_jacobian_function) {
+		for (int i = 0; i < 7; i++) {
+			if (debug_jacobian) {
+				pars[i].side = 1;
+				pars[i].deriv_debug = 1;
+			} else {
+				pars[i].side = 3;
+			}
+		}
+	}
+
+	int res = mpfit(mpfunc, meas_size, 7, soLocation.Pos, pars, 0, &mpfitctx, &result);
+
+	double rtn = -1;
+	bool status_failure = res <= 0;
+	bool error_failure = !general_optimizer_data_record_success(&d->opt, result.bestnorm);
+	if (!status_failure && !error_failure) {
+		quatnormalize(soLocation.Rot, soLocation.Rot);
+		*out = soLocation;
+		rtn = result.bestnorm;
+	}
+
+	return rtn;
+}
+
+int PoserMPFIT(SurviveObject *so, PoserData *pd) {
+	SurviveContext *ctx = so->ctx;
+	if (so->PoserData == 0) {
+		so->PoserData = calloc(1, sizeof(MPFITData));
+		MPFITData *d = so->PoserData;
+
+		general_optimizer_data_init(&d->opt, so);
+		d->useIMU = (bool)survive_configi(ctx, "mpfit-use-imu", SC_GET, 1);
+		d->required_meas = survive_configi(ctx, "mpfit-required-meas", SC_GET, 8);
+
+		d->sensor_time_window =
+			survive_configi(ctx, "mpfit-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2);
+		d->use_jacobian_function = survive_configi(ctx, "mpfit-use-jacobian-function", SC_GET, 1.0);
+
+		SV_INFO("Initializing MPFIT:");
+		SV_INFO("\tmpfit-required-meas: %d", d->required_meas);
+		SV_INFO("\tmpfit-time-window: %d", d->sensor_time_window);
+		SV_INFO("\tmpfit-use-imu: %d", d->useIMU);
+		SV_INFO("\tmpfit-use-jacobian-function: %d", d->use_jacobian_function);
+	}
+	MPFITData *d = so->PoserData;
+	switch (pd->pt) {
+	case POSERDATA_LIGHT: {
+		// No poses if calibration is ongoing
+		if (ctx->calptr && ctx->calptr->stage < 5)
+			return 0;
+		SurviveSensorActivations *scene = &so->activations;
+		PoserDataLight *lightData = (PoserDataLight *)pd;
+		SurvivePose estimate;
+
+		// only process sweeps
+		FLT error = -1;
+		if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) {
+			error = run_mpfit_find_3d_structure(d, lightData, scene, 100, .5, &estimate);
+
+			d->last_lh = lightData->lh;
+			d->last_acode = lightData->acode;
+
+			if (error > 0) {
+				if (d->useIMU) {
+					FLT var_meters = 0.5;
+					FLT var_quat = error + .05;
+					FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat,
+								  error * var_quat,   error * var_quat,   error * var_quat};
+
+					survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var);
+					estimate = d->tracker.pose;
+				}
+
+				PoserData_poser_pose_func(&lightData->hdr, so, &estimate);
+			}
+		}
+		return 0;
+	}
+
+	case POSERDATA_DISASSOCIATE: {
+		SV_INFO("MPFIT stats:");
+		SV_INFO("\tmeas failures %d", d->stats.meas_failures);
+		general_optimizer_data_dtor(&d->opt);
+		free(d);
+		so->PoserData = 0;
+		return 0;
+	}
+	case POSERDATA_IMU: {
+
+		PoserDataIMU *imu = (PoserDataIMU *)pd;
+		if (ctx->calptr && ctx->calptr->stage < 5) {
+		} else if (d->useIMU) {
+			survive_imu_tracker_integrate(so, &d->tracker, imu);
+			PoserData_poser_pose_func(pd, so, &d->tracker.pose);
+		}
+
+		general_optimizer_data_record_imu(&d->opt, imu);
+	}
+	}
+	return -1;
+}
+
+REGISTER_LINKTIME(PoserMPFIT);
diff --git a/src/poser_sba.c b/src/poser_sba.c
index fcf4f2e..8c9e328 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -13,6 +13,7 @@
 #include "assert.h"
 #include "linmath.h"
 #include "math.h"
+#include "poser_general_optimizer.h"
 #include "string.h"
 #include "survive_cal.h"
 #include "survive_config.h"
@@ -32,26 +33,23 @@ typedef struct {
 } sba_context_single_sweep;
 
 typedef struct SBAData {
+	GeneralOptimizerData opt;
+
 	int last_acode;
 	int last_lh;
 
-	int failures_to_reset;
-	int failures_to_reset_cntr;
-	int successes_to_reset;
-	int successes_to_reset_cntr;
-
-	FLT max_error;
-
 	FLT sensor_variance;
 	FLT sensor_variance_per_second;
 	int sensor_time_window;
-
+	int use_jacobian_function;
 	int required_meas;
 
 	SurviveIMUTracker tracker;
 	bool useIMU;
 
-	SurviveObject *so;
+	struct {
+		int meas_failures;
+	} stats;
 } SBAData;
 
 static void metric_function(int j, int i, double *aj, double *xij, void *adata) {
@@ -89,7 +87,7 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs,
 static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, char *vmask,
 										 double *meas, double *cov) {
 	size_t rtn = 0;
-	SurviveObject *so = d->so;
+	SurviveObject *so = d->opt.so;
 
 	for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) {
 		for (size_t lh = 0; lh < 2; lh++) {
@@ -176,20 +174,37 @@ static void str_metric_function(int j, int i, double *bi, double *xij, void *ada
 	assert(lh < 2);
 	assert(sensor_idx < so->sensor_ct);
 
-	quatnormalize(obj.Rot, obj.Rot);
-	FLT xyz[3];
-	ApplyPoseToPoint(xyz, &obj, &so->sensor_locations[sensor_idx * 3]);
+	// quatnormalize(obj.Rot, obj.Rot);
 
 	// std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl;
 	SurvivePose *camera = &so->ctx->bsd[lh].Pose;
-	survive_reproject_from_pose(so->ctx, lh, camera, xyz, xij);
+	survive_reproject_full(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh],
+						   &so->ctx->calibration_config);
+}
+
+static void str_metric_function_jac(int j, int i, double *bi, double *xij, void *adata) {
+	SurvivePose obj = *(SurvivePose *)bi;
+	int sensor_idx = j >> 1;
+	int lh = j & 1;
+
+	sba_context *ctx = (sba_context *)(adata);
+	SurviveObject *so = ctx->so;
+
+	assert(lh < 2);
+	assert(sensor_idx < so->sensor_ct);
+
+	// quatnormalize(obj.Rot, obj.Rot);
+
+	SurvivePose *camera = &so->ctx->bsd[lh].Pose;
+	survive_reproject_full_jac_obj_pose(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh],
+										&so->ctx->calibration_config);
 }
 
 static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene,
 										int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/,
 										SurvivePose *out) {
 	double *covx = 0;
-	SurviveObject *so = d->so;
+	SurviveObject *so = d->opt.so;
 
 	char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES);
 	double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES);
@@ -208,40 +223,17 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 			SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size);
 			failure_count = 0;
 		}
+		if (meas_size < d->required_meas) {
+			d->stats.meas_failures++;
+		}
 		return -1;
 	}
 	failure_count = 0;
 
-	SurvivePose soLocation = so->OutPose;
-	bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0;
+	SurvivePose soLocation = {0};
 
-	if (d->successes_to_reset_cntr == 0 || d->failures_to_reset_cntr == 0 || currentPositionValid == 0) {
-		SurviveContext *ctx = so->ctx;
-		// SV_INFO("Must rerun seed poser");
-		const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP");
-		PoserCB driver = (PoserCB)GetDriver(subposer);
-
-		if (driver) {
-			PoserData hdr = pdl->hdr;
-			memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions
-			pdl->hdr.pt = hdr.pt;
-			pdl->hdr.poseproc = sba_set_position;
-
-			sba_set_position_t locations = {0};
-			pdl->hdr.userdata = &locations;
-			driver(so, &pdl->hdr);
-			pdl->hdr = hdr;
-
-			if (locations.hasInfo == false) {
-				return -1;
-			} else if (locations.hasInfo) {
-				soLocation = locations.poses;
-			}
-
-			d->successes_to_reset_cntr = d->successes_to_reset;
-		} else {
-			SV_INFO("Not using a seed poser for SBA; results will likely be way off");
-		}
+	if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) {
+		return -1;
 	}
 
 	double opts[SBA_OPTSSZ] = {0};
@@ -265,38 +257,28 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 								cov,			// cov data
 								2,				// mnp -- 2 points per image
 								str_metric_function,
-								0,				// jacobia of metric_func
-								&ctx,			// user data
-								max_iterations, // Max iterations
-								0,				// verbosity
-								opts,			// options
-								info);			// info
-
-	if (currentPositionValid) {
-		// FLT distp[3];
-		// sub3d(distp, so->OutPose.Pos, soLocation.Pos);
-		// FLT distance = magnitude3d(distp);
-
-		// if (distance > 1.)
-		//	status = -1;
-	}
+								d->use_jacobian_function ? str_metric_function_jac : 0, // jacobia of metric_func
+								&ctx,													// user data
+								max_iterations,											// Max iterations
+								0,														// verbosity
+								opts,													// options
+								info);													// info
 
 	double rtn = -1;
-	if (status > 0 && (info[1] / meas_size * 2) < d->max_error) {
-		d->failures_to_reset_cntr = d->failures_to_reset;
+	bool status_failure = status <= 0;
+	bool error_failure = !general_optimizer_data_record_success(&d->opt, (info[1] / meas_size * 2));
+	if (!status_failure && !error_failure) {
 		quatnormalize(soLocation.Rot, soLocation.Rot);
 		*out = soLocation;
 		rtn = info[1] / meas_size * 2;
-	}
+	} else
 
 	{
 		SurviveContext *ctx = so->ctx;
 		// Docs say info[0] should be divided by meas; I don't buy it really...
-		static int cnt = 0;
-		if (cnt++ > 1000 || meas_size < d->required_meas || (info[1] / meas_size * 2) > d->max_error) {
-			// SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size);
-			// SV_INFO("%f cur reproj error", (info[1] / meas_size * 2));
-			cnt = 0;
+		if (error_failure) {
+			SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size);
+			SV_INFO("%f cur reproj error", (info[1] / meas_size * 2));
 		}
 	}
 
@@ -316,11 +298,11 @@ static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_itera
 						  .obj_pose = so->OutPose};
 
 	{
-		const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP");
+		const char *subposer = survive_configs(so->ctx, "seed-poser", SC_GET, "PoserEPNP");
+
 		PoserCB driver = (PoserCB)GetDriver(subposer);
 		SurviveContext *ctx = so->ctx;
 		if (driver) {
-			SV_INFO("Using %s seed poser for SBA", subposer);
 			PoserData hdr = pdfs->hdr;
 			memset(&pdfs->hdr, 0, sizeof(pdfs->hdr)); // Clear callback functions
 			pdfs->hdr.pt = hdr.pt;
@@ -396,27 +378,24 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 	if (so->PoserData == 0) {
 		so->PoserData = calloc(1, sizeof(SBAData));
 		SBAData *d = so->PoserData;
-		d->failures_to_reset_cntr = 0;
-		d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1);
-		d->successes_to_reset_cntr = 0;
-		d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 100);
+
+		general_optimizer_data_init(&d->opt, so);
 		d->useIMU = survive_configi(ctx, "sba-use-imu", SC_GET, 1);
 		d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8);
-		d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001);
+
 		d->sensor_time_window =
 			survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2);
 		d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 10.0);
 		d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0);
-		d->so = so;
+		d->use_jacobian_function = survive_configi(ctx, "sba-use-jacobian-function", SC_GET, 1.0);
 
 		SV_INFO("Initializing SBA:");
 		SV_INFO("\tsba-required-meas: %d", d->required_meas);
 		SV_INFO("\tsba-sensor-variance: %f", d->sensor_variance);
 		SV_INFO("\tsba-sensor-variance-per-sec: %f", d->sensor_variance_per_second);
 		SV_INFO("\tsba-time-window: %d", d->sensor_time_window);
-		SV_INFO("\tsba-max-error: %f", d->max_error);
-		SV_INFO("\tsba-successes-to-reset: %d", d->successes_to_reset);
 		SV_INFO("\tsba-use-imu: %d", d->useIMU);
+		SV_INFO("\tsba-use-jacobian-function: %d", d->use_jacobian_function);
 	}
 	SBAData *d = so->PoserData;
 	switch (pd->pt) {
@@ -435,27 +414,23 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 
 			d->last_lh = lightData->lh;
 			d->last_acode = lightData->acode;
-		}
 
-		if (error < 0) {
-			if (d->failures_to_reset_cntr > 0)
-				d->failures_to_reset_cntr--;
-		} else {
-			if (d->useIMU) {
-				FLT var_meters = 0.5;
-				FLT var_quat = error + .05;
-				FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat,
-							  error * var_quat,   error * var_quat,   error * var_quat};
-
-				survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var);
-				estimate = d->tracker.pose;
-			}
+			if (error < 0) {
 
-			PoserData_poser_pose_func(&lightData->hdr, so, &estimate);
-			if (d->successes_to_reset_cntr > 0)
-				d->successes_to_reset_cntr--;
-		}
+			} else {
+				if (d->useIMU) {
+					FLT var_meters = 0.5;
+					FLT var_quat = error + .05;
+					FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat,
+								  error * var_quat,   error * var_quat,   error * var_quat};
+
+					survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var);
+					estimate = d->tracker.pose;
+				}
 
+				PoserData_poser_pose_func(&lightData->hdr, so, &estimate);
+			}
+		}
 		return 0;
 	}
 	case POSERDATA_FULL_SCENE: {
@@ -465,22 +440,24 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 		// std::cerr << "Average reproj error: " << error << std::endl;
 		return 0;
 	}
+	case POSERDATA_DISASSOCIATE: {
+		SV_INFO("SBA stats:");
+		SV_INFO("\tmeas failures %d", d->stats.meas_failures);
+		general_optimizer_data_dtor(&d->opt);
+		free(d);
+		so->PoserData = 0;
+		return 0;
+	}
 	case POSERDATA_IMU: {
 
 	  PoserDataIMU * imu = (PoserDataIMU*)pd;
 	  if (ctx->calptr && ctx->calptr->stage < 5) {
-	  } else if(d->useIMU){
-	    survive_imu_tracker_integrate(so, &d->tracker, imu);
-		PoserData_poser_pose_func(pd, so, &d->tracker.pose);
+	  } else if (d->useIMU) {
+		  survive_imu_tracker_integrate(so, &d->tracker, imu);
+		  PoserData_poser_pose_func(pd, so, &d->tracker.pose);
 	  }
-	} // INTENTIONAL FALLTHROUGH
-	default: {
-		const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP");
-		PoserCB driver = (PoserCB)GetDriver(subposer);
-		if (driver) {
-			return driver(so, pd);
-		}
-		break;
+
+	  general_optimizer_data_record_imu(&d->opt, imu);
 	}
 	}
 	return -1;
diff --git a/src/survive.c b/src/survive.c
index b024bbb..362adbc 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -114,6 +114,8 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
 		MANUAL_DRIVER_REGISTRATION(PoserDummy)
 		MANUAL_DRIVER_REGISTRATION(PoserEPNP)
 		MANUAL_DRIVER_REGISTRATION(PoserSBA)
+		MANUAL_DRIVER_REGISTRATION(PoserCharlesRefine)
+		MANUAL_DRIVER_REGISTRATION(PoserMPFIT)
 
 		MANUAL_DRIVER_REGISTRATION(DriverRegHTCVive)
 		MANUAL_DRIVER_REGISTRATION(DriverRegPlayback)
diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c
index 2e47b9e..4fa3284 100644
--- a/src/survive_default_devices.c
+++ b/src/survive_default_devices.c
@@ -144,10 +144,6 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) {
 			FLT *values = NULL;
 			if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) {
 				so->acc_bias = values;
-				const FLT bias_units = 1. / 1000.; // I deeply suspect bias is in milligravities -JB
-				so->acc_bias[0] *= bias_units;
-				so->acc_bias[1] *= bias_units;
-				so->acc_bias[2] *= bias_units;
 			}
 		}
 		if (jsoneq(ct0conf, tk, "acc_scale") == 0) {
@@ -174,6 +170,51 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) {
 		}
 	}
 
+	// Handle device-specific sacling.
+	if (strcmp(so->codename, "HMD") == 0) {
+		if (so->acc_scale) {
+			so->acc_scale[0] *= -1. / 8192.0;
+			so->acc_scale[1] *= -1. / 8192.0;
+			so->acc_scale[2] *= 1. / 8192.0;
+		}
+		if (so->acc_bias)
+			scale3d(so->acc_bias, so->acc_bias, 2. / 1000.); // Odd but seems right.
+		if (so->gyro_scale) {
+			so->gyro_scale[0] *= -0.000065665;
+			so->gyro_scale[1] *= -0.000065665;
+			so->gyro_scale[2] *= 0.000065665;
+		}
+	} else if (memcmp(so->codename, "WM", 2) == 0) {
+		if (so->acc_scale)
+			scale3d(so->acc_scale, so->acc_scale, 2. / 8192.0);
+		if (so->acc_bias)
+			scale3d(so->acc_bias, so->acc_bias, 2. / 1000.); // Need to verify.
+		if (so->gyro_scale)
+			scale3d(so->gyro_scale, so->gyro_scale, 3.14159 / 1800. / 1.8); //??! 1.8 feels right but why?!
+		int j;
+		for (j = 0; j < so->sensor_ct; j++) {
+			so->sensor_locations[j * 3 + 0] *= 1.0;
+		}
+
+	} else // Verified on WW, Need to verify on Tracker.
+	{
+		// 1G for accelerometer, from MPU6500 datasheet
+		// this can change if the firmware changes the sensitivity.
+		// When coming off of USB, these values are in units of .5g -JB
+		if (so->acc_scale)
+			scale3d(so->acc_scale, so->acc_scale, 2. / 8192.0);
+
+		// If any other device, we know we at least need this.
+		// I deeply suspect bias is in milligravities -JB
+		if (so->acc_bias)
+			scale3d(so->acc_bias, so->acc_bias, 1. / 1000.);
+
+		// From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits
+		// FLT deg_per_sec = 250;
+		if (so->gyro_scale)
+			scale3d(so->gyro_scale, so->gyro_scale, 3.14159 / 1800.);
+	}
+
 	char fname[64];
 
 	sprintf(fname, "calinfo/%s_points.csv", so->codename);
diff --git a/src/survive_imu.c b/src/survive_imu.c
index 36d1aeb..3622b2d 100644
--- a/src/survive_imu.c
+++ b/src/survive_imu.c
@@ -1,107 +1,56 @@
 #include "survive_imu.h"
 #include "linmath.h"
+#include "math.h"
 #include "survive_internal.h"
+#include <memory.h>
 #include <survive_imu.h>
 
-//---------------------------------------------------------------------------------------------------
-// Definitions
+// Mahoney is due to https://hal.archives-ouvertes.fr/hal-00488376/document
+// See also http://www.olliw.eu/2013/imu-data-fusing/#chapter41 and
+// http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+static void mahony_ahrs(SurviveIMUTracker *tracker, LinmathVec3d _gyro, LinmathVec3d _accel) {
+	LinmathVec3d gyro;
+	memcpy(gyro, _gyro, 3 * sizeof(FLT));
 
-#define sampleFreq 240.0f	  // sample frequency in Hz
-#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
-#define twoKiDef (2.0f * 0.0f) // 2 * integral gain
+	LinmathVec3d accel;
+	memcpy(accel, _accel, 3 * sizeof(FLT));
 
-//---------------------------------------------------------------------------------------------------
-// Function declarations
+	const FLT sample_f = 240;
+	const FLT prop_gain = .5;
+	const FLT int_gain = 0;
 
-float invSqrt(float x) {
-	float halfx = 0.5f * x;
-	float y = x;
-	long i = *(long *)&y;
-	i = 0x5f3759df - (i >> 1);
-	y = *(float *)&i;
-	y = y * (1.5f - (halfx * y * y));
-	return y;
-}
-//---------------------------------------------------------------------------------------------------
-// IMU algorithm update
-// From http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
-static void MahonyAHRSupdateIMU(SurviveIMUTracker *tracker, float gx, float gy, float gz, float ax, float ay,
-								float az) {
-	float recipNorm;
-	float halfvx, halfvy, halfvz;
-	float halfex, halfey, halfez;
-	float qa, qb, qc;
-
-	const float twoKp = twoKpDef; // 2 * proportional gain (Kp)
-	const float twoKi = twoKiDef; // 2 * integral gain (Ki)
-
-	float q0 = tracker->pose.Rot[0];
-	float q1 = tracker->pose.Rot[1];
-	float q2 = tracker->pose.Rot[2];
-	float q3 = tracker->pose.Rot[3];
-
-	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-	if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
-		// Normalise accelerometer measurement
-		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
-		ax *= recipNorm;
-		ay *= recipNorm;
-		az *= recipNorm;
-
-		// Estimated direction of gravity and vector perpendicular to magnetic flux
-		halfvx = q1 * q3 - q0 * q2;
-		halfvy = q0 * q1 + q2 * q3;
-		halfvz = q0 * q0 - 0.5f + q3 * q3;
-
-		// Error is sum of cross product between estimated and measured direction of gravity
-		halfex = (ay * halfvz - az * halfvy);
-		halfey = (az * halfvx - ax * halfvz);
-		halfez = (ax * halfvy - ay * halfvx);
-
-		// Compute and apply integral feedback if enabled
-		if (twoKi > 0.0f) {
-			tracker->integralFBx += twoKi * halfex * (1.0f / sampleFreq); // tracker->integral error scaled by Ki
-			tracker->integralFBy += twoKi * halfey * (1.0f / sampleFreq);
-			tracker->integralFBz += twoKi * halfez * (1.0f / sampleFreq);
-			gx += tracker->integralFBx; // apply tracker->integral feedback
-			gy += tracker->integralFBy;
-			gz += tracker->integralFBz;
-		} else {
-			tracker->integralFBx = 0.0f; // prevent tracker->integral windup
-			tracker->integralFBy = 0.0f;
-			tracker->integralFBz = 0.0f;
+	FLT *q = tracker->pose.Rot;
+
+	FLT mag_accel = magnitude3d(accel);
+
+	if (mag_accel != 0.0) {
+		scale3d(accel, accel, 1. / mag_accel);
+
+		// Equiv of q^-1 * G
+		LinmathVec3d v = {q[1] * q[3] - q[0] * q[2], q[0] * q[1] + q[2] * q[3], q[0] * q[0] - 0.5 + q[3] * q[3]};
+
+		LinmathVec3d error;
+		cross3d(error, accel, v);
+
+		if (int_gain > 0.0f) {
+			LinmathVec3d fb_correction;
+			scale3d(fb_correction, error, int_gain * 2. / sample_f);
+			add3d(tracker->integralFB, tracker->integralFB, fb_correction);
+			add3d(gyro, gyro, tracker->integralFB);
 		}
 
-		// Apply proportional feedback
-		gx += twoKp * halfex;
-		gy += twoKp * halfey;
-		gz += twoKp * halfez;
+		scale3d(error, error, prop_gain * 2.);
+		add3d(gyro, gyro, error);
 	}
 
-	// Integrate rate of change of quaternion
-	gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
-	gy *= (0.5f * (1.0f / sampleFreq));
-	gz *= (0.5f * (1.0f / sampleFreq));
-	qa = q0;
-	qb = q1;
-	qc = q2;
-	q0 += (-qb * gx - qc * gy - q3 * gz);
-	q1 += (qa * gx + qc * gz - q3 * gy);
-	q2 += (qa * gy - qb * gz + q3 * gx);
-	q3 += (qa * gz + qb * gy - qc * gx);
-
-	// Normalise quaternion
-	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
-	q0 *= recipNorm;
-	q1 *= recipNorm;
-	q2 *= recipNorm;
-	q3 *= recipNorm;
-
-	tracker->pose.Rot[0] = q0;
-	tracker->pose.Rot[1] = q1;
-	tracker->pose.Rot[2] = q2;
-	tracker->pose.Rot[3] = q3;
+	scale3d(gyro, gyro, 0.5 / sample_f);
+
+	LinmathQuat correction = {
+		(-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]), (q[0] * gyro[0] + q[2] * gyro[2] - q[3] * gyro[1]),
+		(q[0] * gyro[1] - q[1] * gyro[2] + q[3] * gyro[0]), (q[0] * gyro[2] + q[1] * gyro[1] - q[2] * gyro[0])};
+
+	quatadd(q, q, correction);
+	quatnormalize(q, q);
 }
 
 static inline uint32_t tick_difference(uint32_t most_recent, uint32_t least_recent) {
@@ -125,7 +74,6 @@ void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode,
 	}
 	//(pose->Pos[i] - tracker->lastGT.Pos[i]) / tick_difference(timecode, tracker->lastGTTime) * 48000000.;
 
-	tracker->integralFBx = tracker->integralFBy = tracker->integralFBz = 0.0;
 	tracker->lastGTTime = timecode;
 	tracker->lastGT = *pose;
 }
@@ -174,7 +122,7 @@ static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, do
 }
 
 void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) {
-	if (tracker->last_data.timecode == 0) {
+	if (!tracker->is_initialized) {
 		tracker->pose.Rot[0] = 1.;
 		if (tracker->last_data.datamask == imu_calibration_iterations) {
 			tracker->last_data = *data;
@@ -182,7 +130,7 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker
 			const FLT up[3] = {0, 0, 1};
 			quatfrom2vectors(tracker->pose.Rot, tracker->updir, up);
 			tracker->accel_scale_bias = 1. / magnitude3d(tracker->updir);
-
+			tracker->is_initialized = true;
 			return;
 		}
 
@@ -198,10 +146,7 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker
 		tracker->updir[i] = data->accel[i] * .10 + tracker->updir[i] * .90;
 	}
 
-	float gx = data->gyro[0], gy = data->gyro[1], gz = data->gyro[2];
-	float ax = data->accel[0], ay = data->accel[1], az = data->accel[2];
-
-	MahonyAHRSupdateIMU(tracker, gx, gy, gz, ax, ay, az);
+	mahony_ahrs(tracker, data->gyro, data->accel);
 
 	FLT time_diff = tick_difference(data->timecode, tracker->last_data.timecode) / (FLT)so->timebase_hz;
 
@@ -233,6 +178,11 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker
 
 void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timecode, SurviveIMUTracker *tracker,
 											   SurvivePose *pose, const FLT *R) {
+	if (!tracker->is_initialized) {
+		tracker->pose = *pose;
+		return;
+	}
+
 	// Kalman filter assuming:
 	// F -> Identity
 	// H -> Identity
@@ -265,4 +215,4 @@ void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timec
 
 	tracker->lastGTTime = timecode;
 	tracker->lastGT = tracker->pose;
-}
\ No newline at end of file
+}
diff --git a/src/survive_reproject.c b/src/survive_reproject.c
index 8d8f0ed..ae946fe 100644
--- a/src/survive_reproject.c
+++ b/src/survive_reproject.c
@@ -1,8 +1,72 @@
 #include "survive_reproject.h"
-#include <../redist/linmath.h>
+#include "survive_reproject.generated.h"
 #include <math.h>
-#include <stdio.h>
-#include <string.h>
+
+void survive_reproject_full_jac_obj_pose(FLT *out, const SurvivePose *obj_pose, const LinmathVec3d obj_pt,
+										 const SurvivePose *lh2world, const BaseStationData *bsd,
+										 const survive_calibration_config *config) {
+	FLT phase_scale = config->use_flag & SVCal_Phase ? config->phase_scale : 0.;
+	FLT phase_0 = bsd->fcal.phase[0];
+	FLT phase_1 = bsd->fcal.phase[1];
+
+	FLT tilt_scale = config->use_flag & SVCal_Tilt ? config->tilt_scale : 0.;
+	FLT tilt_0 = bsd->fcal.tilt[0];
+	FLT tilt_1 = bsd->fcal.tilt[1];
+
+	FLT curve_scale = config->use_flag & SVCal_Curve ? config->curve_scale : 0.;
+	FLT curve_0 = bsd->fcal.curve[0];
+	FLT curve_1 = bsd->fcal.curve[1];
+
+	FLT gib_scale = config->use_flag & SVCal_Gib ? config->gib_scale : 0;
+	FLT gibPhase_0 = bsd->fcal.gibpha[0];
+	FLT gibPhase_1 = bsd->fcal.gibpha[1];
+	FLT gibMag_0 = bsd->fcal.gibmag[0];
+	FLT gibMag_1 = bsd->fcal.gibmag[1];
+
+	gen_reproject_jac_obj_p(out, obj_pose->Pos, obj_pt, lh2world->Pos, phase_scale, phase_0, phase_1, tilt_scale,
+							tilt_0, tilt_1, curve_scale, curve_0, curve_1, gib_scale, gibPhase_0, gibPhase_1, gibMag_0,
+							gibMag_1);
+}
+
+void survive_reproject_full(FLT *out, const SurvivePose *obj_pose, const LinmathVec3d obj_pt,
+							const SurvivePose *lh2world, const BaseStationData *bsd,
+							const survive_calibration_config *config) {
+	LinmathVec3d world_pt;
+	ApplyPoseToPoint(world_pt, obj_pose, obj_pt);
+
+	SurvivePose world2lh;
+	InvertPose(&world2lh, lh2world);
+
+	LinmathPoint3d t_pt;
+	ApplyPoseToPoint(t_pt, &world2lh, world_pt);
+
+	FLT x = -t_pt[0] / -t_pt[2];
+	FLT y = t_pt[1] / -t_pt[2];
+	double xy[] = {x, y};
+	double ang[] = {atan(x), atan(y)};
+
+	const FLT *phase = bsd->fcal.phase;
+	const FLT *curve = bsd->fcal.curve;
+	const FLT *tilt = bsd->fcal.tilt;
+	const FLT *gibPhase = bsd->fcal.gibpha;
+	const FLT *gibMag = bsd->fcal.gibmag;
+	enum SurviveCalFlag f = config->use_flag;
+
+	for (int axis = 0; axis < 2; axis++) {
+		int opp_axis = axis == 0 ? 1 : 0;
+
+		out[axis] = ang[axis];
+
+		if (f & SVCal_Phase)
+			out[axis] -= config->phase_scale * phase[axis];
+		if (f & SVCal_Tilt)
+			out[axis] -= tan(config->tilt_scale * tilt[axis]) * xy[opp_axis];
+		if (f & SVCal_Curve)
+			out[axis] -= config->curve_scale * curve[axis] * xy[opp_axis] * xy[opp_axis];
+		if (f & SVCal_Gib)
+			out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis];
+	}
+}
 
 void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config,
 										  const SurvivePose *pose, const FLT *pt, FLT *out) {
diff --git a/src/survive_reproject.generated.h b/src/survive_reproject.generated.h
new file mode 100644
index 0000000..6d834f7
--- /dev/null
+++ b/src/survive_reproject.generated.h
@@ -0,0 +1,215 @@
+// NOTE: Auto-generated code; see tools/generate_reprojection_functions
+#include <math.h>
+static inline void gen_reproject_jac_obj_p(FLT *out, const FLT *obj, const FLT *sensor, const FLT *lh, FLT phase_scale,
+										   FLT phase_0, FLT phase_1, FLT tilt_scale, FLT tilt_0, FLT tilt_1,
+										   FLT curve_scale, FLT curve_0, FLT curve_1, FLT gib_scale, FLT gibPhase_0,
+										   FLT gibPhase_1, FLT gibMag_0, FLT gibMag_1) {
+	FLT obj_px = *(obj++);
+	FLT obj_py = *(obj++);
+	FLT obj_pz = *(obj++);
+	FLT obj_qw = *(obj++);
+	FLT obj_qi = *(obj++);
+	FLT obj_qj = *(obj++);
+	FLT obj_qk = *(obj++);
+	FLT sensor_x = *(sensor++);
+	FLT sensor_y = *(sensor++);
+	FLT sensor_z = *(sensor++);
+	FLT lh_px = *(lh++);
+	FLT lh_py = *(lh++);
+	FLT lh_pz = *(lh++);
+	FLT lh_qw = *(lh++);
+	FLT lh_qi = *(lh++);
+	FLT lh_qj = *(lh++);
+	FLT lh_qk = *(lh++);
+	FLT x0 = tan(tilt_0 * tilt_scale);
+	FLT x1 = lh_qi * lh_qj;
+	FLT x2 = lh_qk * lh_qw;
+	FLT x3 = x1 - x2;
+	FLT x4 = (lh_qi * lh_qi);
+	FLT x5 = (lh_qj * lh_qj);
+	FLT x6 = x4 + x5;
+	FLT x7 = (lh_qk * lh_qk);
+	FLT x8 = sqrt((lh_qw * lh_qw) + x6 + x7);
+	FLT x9 = lh_qi * lh_qk;
+	FLT x10 = lh_qj * lh_qw;
+	FLT x11 = x10 + x9;
+	FLT x12 = 2 * lh_px * x8;
+	FLT x13 = lh_qj * lh_qk;
+	FLT x14 = lh_qi * lh_qw;
+	FLT x15 = x13 - x14;
+	FLT x16 = 2 * lh_py * x8;
+	FLT x17 = 2 * x8;
+	FLT x18 = x17 * x6 - 1;
+	FLT x19 = obj_qi * obj_qk;
+	FLT x20 = obj_qj * obj_qw;
+	FLT x21 = sensor_z * (x19 + x20);
+	FLT x22 = (obj_qi * obj_qi);
+	FLT x23 = (obj_qj * obj_qj);
+	FLT x24 = x22 + x23;
+	FLT x25 = (obj_qk * obj_qk);
+	FLT x26 = sqrt((obj_qw * obj_qw) + x24 + x25);
+	FLT x27 = 2 * x26;
+	FLT x28 = obj_qi * obj_qj;
+	FLT x29 = obj_qk * obj_qw;
+	FLT x30 = sensor_y * (x28 - x29);
+	FLT x31 = x23 + x25;
+	FLT x32 = obj_px - sensor_x * (x27 * x31 - 1) + x21 * x27 + x27 * x30;
+	FLT x33 = 2 * x32 * x8;
+	FLT x34 = sensor_x * (x28 + x29);
+	FLT x35 = obj_qj * obj_qk;
+	FLT x36 = obj_qi * obj_qw;
+	FLT x37 = sensor_z * (x35 - x36);
+	FLT x38 = x22 + x25;
+	FLT x39 = obj_py - sensor_y * (x27 * x38 - 1) + x27 * x34 + x27 * x37;
+	FLT x40 = 2 * x39 * x8;
+	FLT x41 = sensor_y * (x35 + x36);
+	FLT x42 = sensor_x * (x19 - x20);
+	FLT x43 = obj_pz - sensor_z * (x24 * x27 - 1) + x27 * x41 + x27 * x42;
+	FLT x44 = lh_pz * x18 - x11 * x12 + x11 * x33 - x15 * x16 + x15 * x40 - x18 * x43;
+	FLT x45 = 1.0 / x44;
+	FLT x46 = 2 * x45 * x8;
+	FLT x47 = x13 + x14;
+	FLT x48 = 2 * lh_pz * x8;
+	FLT x49 = x17 * (x4 + x7) - 1;
+	FLT x50 = 2 * x43 * x8;
+	FLT x51 = lh_py * x49 - x12 * x3 + x3 * x33 - x39 * x49 - x47 * x48 + x47 * x50;
+	FLT x52 = 1. / (x44 * x44);
+	FLT x53 = 4 * curve_0 * curve_scale * x51 * x52 * x8;
+	FLT x54 = x11 * x52;
+	FLT x55 = (x51 * x51);
+	FLT x56 = curve_0 * x55;
+	FLT x57 = 1. / (x44 * x44 * x44);
+	FLT x58 = 4 * curve_scale * x11 * x57 * x8;
+	FLT x59 = x1 + x2;
+	FLT x60 = -x10 + x9;
+	FLT x61 = x17 * (x5 + x7) - 1;
+	FLT x62 = lh_px * x61 - x16 * x59 - x32 * x61 + x40 * x59 - x48 * x60 + x50 * x60;
+	FLT x63 = (x62 * x62);
+	FLT x64 = 1.0 / (x52 * x63 + 1);
+	FLT x65 = x45 * x61;
+	FLT x66 = 2 * x54 * x62 * x8;
+	FLT x67 = x64 * (x65 + x66);
+	FLT x68 = gibMag_0 * gib_scale * cos(gibPhase_0 + atan(x45 * x62));
+	FLT x69 = x45 * x49;
+	FLT x70 = 2 * x15 * x8;
+	FLT x71 = x51 * x52;
+	FLT x72 = x70 * x71;
+	FLT x73 = 4 * curve_scale * x15 * x57 * x8;
+	FLT x74 = 2 * x64;
+	FLT x75 = x45 * x8;
+	FLT x76 = x74 * (-x15 * x52 * x62 * x8 + x59 * x75);
+	FLT x77 = x46 * x47;
+	FLT x78 = x18 * x71;
+	FLT x79 = 2 * curve_scale * x18 * x57;
+	FLT x80 = x46 * x60;
+	FLT x81 = x52 * x62;
+	FLT x82 = x18 * x81;
+	FLT x83 = x64 * (x80 + x82);
+	FLT x84 = 2 * x0;
+	FLT x85 = obj_qj * x26;
+	FLT x86 = sensor_x * x85;
+	FLT x87 = obj_qi * x26;
+	FLT x88 = sensor_y * x87;
+	FLT x89 = sensor_z * x24;
+	FLT x90 = 1.0 / x26;
+	FLT x91 = obj_qw * x90;
+	FLT x92 = -x41 * x91 - x42 * x91 + x86 - x88 + x89 * x91;
+	FLT x93 = 2 * x8 * x92;
+	FLT x94 = obj_qk * x26;
+	FLT x95 = sensor_y * x94;
+	FLT x96 = sensor_z * x85;
+	FLT x97 = sensor_x * x31;
+	FLT x98 = -x21 * x91 - x30 * x91 + x91 * x97 + x95 - x96;
+	FLT x99 = 2 * x8 * x98;
+	FLT x100 = sensor_x * x94;
+	FLT x101 = sensor_z * x87;
+	FLT x102 = sensor_y * x38;
+	FLT x103 = x100 - x101 - x102 * x91 + x34 * x91 + x37 * x91;
+	FLT x104 = x103 * x49 + x3 * x99 + x47 * x93;
+	FLT x105 = x104 * x45;
+	FLT x106 = 4 * curve_0 * curve_scale * x51 * x52;
+	FLT x107 = -x103 * x70 + x11 * x99 - x18 * x92;
+	FLT x108 = x107 * x71;
+	FLT x109 = 4 * curve_scale * x107 * x57;
+	FLT x110 = 2 * x59 * x8;
+	FLT x111 = x103 * x110 - x60 * x93 + x61 * x98;
+	FLT x112 = x74 * (x107 * x81 + x111 * x45);
+	FLT x113 = sensor_y * x85;
+	FLT x114 = sensor_z * x94;
+	FLT x115 = obj_qi * x90;
+	FLT x116 = -x113 - x114 - x115 * x21 - x115 * x30 + x115 * x97;
+	FLT x117 = 2 * x116 * x8;
+	FLT x118 = obj_qw * x26;
+	FLT x119 = sensor_y * x118;
+	FLT x120 = obj_qi * x27;
+	FLT x121 = -sensor_z * (x115 * x24 + x120) + x100 + x115 * x41 + x115 * x42 + x119;
+	FLT x122 = 2 * x121 * x8;
+	FLT x123 = sensor_z * x118;
+	FLT x124 = -sensor_y * (x115 * x38 + x120) + x115 * x34 + x115 * x37 - x123 + x86;
+	FLT x125 = x117 * x3 - x122 * x47 + x124 * x49;
+	FLT x126 = x125 * x45;
+	FLT x127 = x11 * x117 + x121 * x18 - x124 * x70;
+	FLT x128 = x127 * x71;
+	FLT x129 = 4 * curve_0 * curve_scale * x55 * x57;
+	FLT x130 = x110 * x124 + x116 * x61 + x122 * x60;
+	FLT x131 = x74 * (x127 * x81 + x130 * x45);
+	FLT x132 = sensor_x * x87;
+	FLT x133 = obj_qj * x90;
+	FLT x134 = -x102 * x133 + x114 + x132 + x133 * x34 + x133 * x37;
+	FLT x135 = obj_qj * x27;
+	FLT x136 = -sensor_x * (x133 * x31 + x135) + x123 + x133 * x21 + x133 * x30 + x88;
+	FLT x137 = 2 * x136 * x8;
+	FLT x138 = sensor_x * x118;
+	FLT x139 = -sensor_z * (x133 * x24 + x135) + x133 * x41 + x133 * x42 - x138 + x95;
+	FLT x140 = 2 * x139 * x8;
+	FLT x141 = -x134 * x49 + x137 * x3 + x140 * x47;
+	FLT x142 = x141 * x45;
+	FLT x143 = x11 * x137 + x134 * x70 - x139 * x18;
+	FLT x144 = x143 * x71;
+	FLT x145 = x110 * x134 - x136 * x61 + x140 * x60;
+	FLT x146 = x74 * (-x143 * x81 + x145 * x45);
+	FLT x147 = obj_qk * x90;
+	FLT x148 = x113 + x132 + x147 * x41 + x147 * x42 - x147 * x89;
+	FLT x149 = 2 * x148 * x8;
+	FLT x150 = obj_qk * x27;
+	FLT x151 = -sensor_x * (x147 * x31 + x150) + x101 - x119 + x147 * x21 + x147 * x30;
+	FLT x152 = 2 * x151 * x8;
+	FLT x153 = -sensor_y * (x147 * x38 + x150) + x138 + x147 * x34 + x147 * x37 + x96;
+	FLT x154 = x149 * x47 + x152 * x3 - x153 * x49;
+	FLT x155 = x154 * x45;
+	FLT x156 = x11 * x152 - x148 * x18 + x153 * x70;
+	FLT x157 = x156 * x71;
+	FLT x158 = x110 * x153 + x149 * x60 - x151 * x61;
+	FLT x159 = x74 * (-x156 * x81 + x158 * x45);
+	FLT x160 = tan(tilt_1 * tilt_scale);
+	FLT x161 = curve_1 * x63;
+	FLT x162 = 1.0 / (x52 * x55 + 1);
+	FLT x163 = 2 * x162;
+	FLT x164 = x163 * (x3 * x75 - x51 * x54 * x8);
+	FLT x165 = gibMag_1 * gib_scale * cos(gibPhase_1 - atan(x45 * x51));
+	FLT x166 = 4 * curve_1 * curve_scale * x52 * x62 * x8;
+	FLT x167 = x162 * (x69 + x72);
+	FLT x168 = x162 * (x77 + x78);
+	FLT x169 = 2 * x160 * x45;
+	FLT x170 = 4 * curve_1 * curve_scale * x52 * x62;
+	FLT x171 = 2 * x160 * x52 * x62;
+	FLT x172 = x163 * (-x105 + x108);
+	FLT x173 = 4 * curve_1 * curve_scale * x57 * x63;
+	FLT x174 = x163 * (-x126 + x128);
+	FLT x175 = x163 * (x142 - x144);
+	FLT x176 = x163 * (x155 - x157);
+	*(out++) = x0 * x3 * x46 - 2 * x0 * x51 * x54 * x8 - x3 * x53 + x56 * x58 + x67 * x68 - x67;
+	*(out++) = 2 * curve_0 * curve_scale * x49 * x51 * x52 - x0 * x69 - x0 * x72 + x56 * x73 - x68 * x76 + x76;
+	*(out++) = x0 * x77 + x0 * x78 - x47 * x53 - x56 * x79 - x68 * x83 + x83;
+	*(out++) = x104 * x106 - x105 * x84 + x108 * x84 - x109 * x56 - x112 * x68 + x112;
+	*(out++) = x106 * x125 - x126 * x84 - x127 * x129 + x128 * x84 - x131 * x68 + x131;
+	*(out++) = -x106 * x141 + x129 * x143 + x142 * x84 - x144 * x84 - x146 * x68 + x146;
+	*(out++) = -x106 * x154 + x129 * x156 + x155 * x84 - x157 * x84 - x159 * x68 + x159;
+	*(out++) = 2 * curve_1 * curve_scale * x52 * x61 * x62 + x160 * x65 + x160 * x66 + x161 * x58 + x164 * x165 - x164;
+	*(out++) = -x110 * x160 * x45 + x160 * x52 * x62 * x70 + x161 * x73 - x165 * x167 - x166 * x59 + x167;
+	*(out++) = -x160 * x80 - x160 * x82 - x161 * x79 + x165 * x168 - x166 * x60 - x168;
+	*(out++) = -x107 * x171 - x109 * x161 - x111 * x169 - x111 * x170 + x165 * x172 - x172;
+	*(out++) = -x127 * x171 - x127 * x173 - x130 * x169 - x130 * x170 + x165 * x174 - x174;
+	*(out++) = x143 * x171 + x143 * x173 - x145 * x169 - x145 * x170 + x165 * x175 - x175;
+	*(out++) = x156 * x171 + x156 * x173 - x158 * x169 - x158 * x170 + x165 * x176 - x176;
+}
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 1ffb737..0fae736 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -1233,31 +1233,15 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 	if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset.
 	{
 		propset |= 2;
-		//XXX XXX BIG TODO!!! Actually recal gyro data.
-		FLT agm[9] = { readdata[1], readdata[2], readdata[3],
-						readdata[4], readdata[5], readdata[6],
-						0,0,0 };
-
-//		if (w->acc_scale) printf("%f %f %f\n",w->acc_scale[0],w->acc_scale[1],w->acc_scale[2]);
+		FLT agm[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
+		int j;
+		for (j = 0; j < 6; j++)
+			agm[j] = (int16_t)(readdata[j * 2 + 1] | (readdata[j * 2 + 2] << 8));
 		calibrate_acc(w, agm);
-
-		//I don't understand where these numbers come from but the data from the WMD seems to max out at 255...
-		agm[0]*=(1.0f/255.0f);
-		agm[1]*=(1.0f/255.0f);
-		agm[2]*=(1.0f/255.0f);
-
 		calibrate_gyro(w, agm+3);
-
-		//I don't understand where these numbers come from but the data from the WMD seems to max out at 255...
-		agm[3]*=(1.0f/255.0f);
-		agm[4]*=(1.0f/255.0f);
-		agm[5]*=(1.0f/255.0f);
-
 		w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 );
-
-		int16_t * k = (int16_t *)readdata+1;
-		//printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] );
 		readdata += 13; qty -= 13;
+
 		type &= ~0xe8;
 		if( qty )
 		{
@@ -1495,24 +1479,8 @@ void survive_data_cb( SurviveUSBInterface * si )
 							  0,
 							  0};
 
-				//1G for accelerometer, from MPU6500 datasheet
-				//this can change if the firmware changes the sensitivity.
-				// When coming off of USB, these values are in units of .5g -JB
-				agm[0] *= (float)(2. / 8192.0);
-				agm[1] *= (float)(2. / 8192.0);
-				agm[2] *= (float)(2. / 8192.0);
 				calibrate_acc(obj, agm);
-
-				// From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits
-				// FLT deg_per_sec = 250;
-				// FLT conv = (float)((1./deg_per_sec)*(3.14159/180.)) / 8192.;
-				FLT DEGREES_TO_RADS = 3.14159 / 180.;
-				FLT conv = 1. / 10. * DEGREES_TO_RADS;
 				calibrate_gyro(obj, agm + 3);
-				agm[3] *= conv;
-				agm[4] *= conv;
-				agm[5] *= conv;
-
 				ctx->imuproc( obj, 3, agm, timecode, code );
 			}
 		}