Merge branch 'master' into tcc_build

13 files changed, 1038 insertions, 233 deletions

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 23e03fc..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, -1);
+
+		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) {
@@ -436,25 +415,20 @@ 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 {
+
+			} 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 };
+					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);
-				if (d->successes_to_reset_cntr > 0)
-					d->successes_to_reset_cntr--;
 			}
 		}
 		return 0;
@@ -466,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 9e750f9..bb1179e 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -106,7 +106,7 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
 	static int did_manual_driver_registration = 0;
 	if (did_manual_driver_registration == 0) {
 #define MANUAL_DRIVER_REGISTRATION(func)                                                                               \
- 	int func(SurviveObject *so, PoserData *pd);                                                                        \
+	int func(SurviveObject *so, PoserData *pd);                                                                        \
 	RegisterDriver(#func, &func);
 
 		MANUAL_DRIVER_REGISTRATION(PoserCharlesSlow)
@@ -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)
@@ -402,6 +404,31 @@ int survive_add_object(SurviveContext *ctx, SurviveObject *obj) {
 	return 0;
 }
 
+void survive_remove_object(SurviveContext *ctx, SurviveObject *obj) {
+	int obj_idx = 0;
+	for (obj_idx = 0; obj_idx < ctx->objs_ct; obj_idx++) {
+		if (ctx->objs[obj_idx] == obj)
+			break;
+	}
+
+	if (obj_idx == ctx->objs_ct) {
+		SV_INFO("Warning: Tried to remove un-added object %p(%s)", obj, obj->codename);
+		return;
+	}
+
+	// Swap the last item into this items slot; this assumes order doesn't matter in this list
+	if (obj_idx != ctx->objs_ct - 1) {
+		ctx->objs[obj_idx] = ctx->objs[ctx->objs_ct - 1];
+	}
+
+	ctx->objs_ct--;
+
+	// Blank out the spot; but this is only really necessary for diagnostic reasons -- presumably no one will ever read
+	// past the end of the list
+	ctx->objs[ctx->objs_ct] = 0;
+
+	free(obj);
+}
 void survive_add_driver(SurviveContext *ctx, void *payload, DeviceDriverCb poll, DeviceDriverCb close,
 						DeviceDriverMagicCb magic) {
 	int oldct = ctx->driver_ct;
diff --git a/src/survive_api.c b/src/survive_api.c
new file mode 100644
index 0000000..a39524b
--- /dev/null
+++ b/src/survive_api.c
@@ -0,0 +1,165 @@
+#include "survive_api.h"
+#include "os_generic.h"
+#include "survive.h"
+#include "stdio.h"
+
+struct SurviveSimpleObject {
+	struct SurviveSimpleContext *actx;
+
+	enum SurviveSimpleObject_type { SurviveSimpleObject_LIGHTHOUSE, SurviveSimpleObject_OBJECT } type;
+
+	union {
+		int lighthouse;
+		struct SurviveObject* so;
+	} data;
+
+	char name[32];
+	bool has_update;
+};
+
+struct SurviveSimpleContext {
+	SurviveContext* ctx; 
+	
+	bool running;
+	og_thread_t thread;
+	og_mutex_t poll_mutex;
+
+	size_t object_ct;
+	struct SurviveSimpleObject objects[];
+};
+
+static void pose_fn(SurviveObject *so, uint32_t timecode, SurvivePose *pose) {
+	struct SurviveSimpleContext *actx = so->ctx->user_ptr;
+	OGLockMutex(actx->poll_mutex);
+	survive_default_raw_pose_process(so, timecode, pose);
+
+	intptr_t idx = (intptr_t)so->user_ptr;
+	actx->objects[idx].has_update = true;
+	OGUnlockMutex(actx->poll_mutex);
+}
+static void lh_fn(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose,
+	SurvivePose *object_pose) {
+	struct SurviveSimpleContext *actx = ctx->user_ptr;
+	OGLockMutex(actx->poll_mutex);
+	survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose);
+
+	actx->objects[lighthouse].has_update = true;
+
+	OGUnlockMutex(actx->poll_mutex);
+}
+
+struct SurviveSimpleContext *survive_simple_init(int argc, char *const *argv) {
+	SurviveContext* ctx = survive_init(argc, argv);
+	if (ctx == 0)
+		return 0;
+
+	survive_startup(ctx);
+
+	int object_ct = ctx->activeLighthouses + ctx->objs_ct;
+	struct SurviveSimpleContext *actx =
+		calloc(1, sizeof(struct SurviveSimpleContext) + sizeof(struct SurviveSimpleObject) * object_ct);
+	actx->object_ct = object_ct;
+	actx->ctx = ctx;
+	actx->poll_mutex = OGCreateMutex();
+	ctx->user_ptr = actx;
+	intptr_t i = 0;
+	for (i = 0; i < ctx->activeLighthouses; i++) {
+		struct SurviveSimpleObject *obj = &actx->objects[i];
+		obj->data.lighthouse = i;
+		obj->type = SurviveSimpleObject_LIGHTHOUSE;
+		obj->actx = actx;
+		obj->has_update = ctx->bsd[i].PositionSet;
+		snprintf(obj->name, 32, "LH%ld", i);
+	}
+	for (; i < object_ct; i++) {
+		struct SurviveSimpleObject *obj = &actx->objects[i];
+		int so_idx = i - ctx->activeLighthouses;
+		obj->data.so = ctx->objs[so_idx];
+		obj->type = SurviveSimpleObject_OBJECT;
+		obj->actx = actx;
+		obj->data.so->user_ptr = (void*)i;
+		snprintf(obj->name, 32, "%s", obj->data.so->codename);
+	}
+
+	survive_install_pose_fn(ctx, pose_fn);
+	survive_install_lighthouse_pose_fn(ctx, lh_fn);
+	return actx;
+}
+
+int survive_simple_stop_thread(struct SurviveSimpleContext *actx) {
+	actx->running = false;
+	intptr_t error = (intptr_t)OGJoinThread(actx->thread);
+	if (error != 0) {
+		SurviveContext *ctx = actx->ctx;
+		SV_INFO("Warning: Loope exited with error %ld", error);
+	}
+	return error;
+}
+
+void survive_simple_close(struct SurviveSimpleContext *actx) {
+	if (actx->running) {
+		survive_simple_stop_thread(actx);
+	}
+
+	survive_close(actx->ctx);
+}
+
+static inline void *__simple_thread(void *_actx) {
+	struct SurviveSimpleContext *actx = _actx;
+	intptr_t error = 0;
+	while (actx->running && error == 0) {
+		error = survive_poll(actx->ctx);
+	}
+	actx->running = false;
+	return (void*)error; 
+}
+bool survive_simple_is_running(struct SurviveSimpleContext *actx) { return actx->running; }
+void survive_simple_start_thread(struct SurviveSimpleContext *actx) {
+	actx->running = true;
+	actx->thread = OGCreateThread(__simple_thread, actx);
+}
+
+const struct SurviveSimpleObject *survive_simple_get_next_object(struct SurviveSimpleContext *actx,
+																 const struct SurviveSimpleObject *curr) {
+	const struct SurviveSimpleObject *next = curr + 1;
+	if (next >= actx->objects + actx->object_ct)
+		return 0;
+	return next;
+}
+
+const struct SurviveSimpleObject *survive_simple_get_first_object(struct SurviveSimpleContext *actx) {
+	return actx->objects;
+}
+
+const struct SurviveSimpleObject *survive_simple_get_next_updated(struct SurviveSimpleContext *actx) {
+	for (int i = 0; i < actx->object_ct; i++) {
+		if (actx->objects[i].has_update) {
+			actx->objects[i].has_update = false;
+			return &actx->objects[i];
+		}
+	}
+	return 0;
+}
+
+uint32_t survive_simple_object_get_latest_pose(const struct SurviveSimpleObject *sao, SurvivePose *pose) {
+	uint32_t timecode = 0;
+	OGLockMutex(sao->actx->poll_mutex);
+
+	switch (sao->type) {
+	case SurviveSimpleObject_LIGHTHOUSE: {
+		if(pose)
+			*pose = sao->actx->ctx->bsd[sao->data.lighthouse].Pose;
+		break;
+	}
+	case SurviveSimpleObject_OBJECT:
+		if(pose) 
+			*pose = sao->data.so->OutPose;
+		timecode = sao->data.so->OutPose_timecode;
+		break;
+	}
+
+	OGUnlockMutex(sao->actx->poll_mutex);
+	return timecode;
+}
+
+const char *survive_simple_object_name(const SurviveSimpleObject *sao) { return sao->name; }
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 4d26bfb..faf2ac7 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -188,7 +188,6 @@ void survive_cal_install( struct SurviveContext * ctx )
 		}
 	}
 
-	const char * DriverName;
 	cd->ConfigPoserFn = GetDriverByConfig(ctx, "Poser", "configposer", "SBA", 0);
 
 	ootx_packet_clbk = ootx_packet_clbk_d;
diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c
index 954c47a..4fa3284 100644
--- a/src/survive_default_devices.c
+++ b/src/survive_default_devices.c
@@ -170,49 +170,49 @@ 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;
+	// 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 )
-		{
+		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;
+			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?!
+	} 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.
+	} else // Verified on WW, Need to verify on Tracker.
 	{
-		//1G for accelerometer, from MPU6500 datasheet
-		//this can change if the firmware changes the sensitivity.
+		// 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 (so->acc_scale)
+			scale3d(so->acc_scale, so->acc_scale, 2. / 8192.0);
 
-		//If any other device, we know we at least need this.
+		// 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. );
+		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. );
+		if (so->gyro_scale)
+			scale3d(so->gyro_scale, so->gyro_scale, 3.14159 / 1800.);
 	}
 
 	char fname[64];
diff --git a/src/survive_imu.c b/src/survive_imu.c
index 8f4266a..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;
 }
@@ -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;
 
@@ -237,7 +182,7 @@ void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timec
 		tracker->pose = *pose;
 		return;
 	}
-		
+
 	// Kalman filter assuming:
 	// F -> Identity
 	// H -> Identity
@@ -270,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 d6ba70b..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) {
@@ -99,5 +163,5 @@ void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, F
 
 void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config,
 											 int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out) {
-	return survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], config, pose, point3d, out);
+	survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], config, pose, point3d, 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 83bc977..d9cbc3e 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -1233,10 +1233,10 @@ 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;
-		FLT agm[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+		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));
+		for (j = 0; j < 6; j++)
+			agm[j] = (int16_t)(readdata[j * 2 + 1] | (readdata[j * 2 + 2] << 8));
 		calibrate_acc(w, agm);
 		calibrate_gyro(w, agm+3);
 		w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 );
@@ -1660,6 +1660,7 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int
 
 	if( len < 0 )
 	{
+		survive_remove_object(ctx, so);
 		return len;
 	}