23 files changed, 65 insertions, 196 deletions

diff --git a/Makefile b/Makefile
index 7a35b2a..3e460e4 100644
--- a/Makefile
+++ b/Makefile
@@ -8,6 +8,7 @@ all : $(STATIC_LIBRARY) $(LIBRARY) data_recorder test calibrate calibrate_client
 
 OBJDIR:=build
 
+CFLAGS += -Wall -Wno-unused-variable -Wno-switch -Wno-parentheses -Wno-missing-braces
 
 ifdef WINDOWS
 	CFLAGS+=-Iinclude/libsurvive -g -O3 -Iredist -DUSE_DOUBLE -std=gnu99 -MD -DNOZLIB -DWINDOWS -DWIN32 -DHIDAPI
@@ -16,7 +17,7 @@ ifdef WINDOWS
 	LIBSURVIVE_CORE:=redist/puff.c redist/crc32.c redist/hid-windows.c winbuild/getdelim.c
 	CC:=i686-w64-mingw32-gcc
 else
-	CFLAGS+=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -DUSE_DOUBLE -std=gnu99 -rdynamic -MD
+	CFLAGS+=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -DUSE_DOUBLE -std=gnu99 -MD
 	LDFLAGS+=-L/usr/local/lib -lpthread -lz -lm -g -llapacke  -lcblas -lm  -lusb-1.0
 	LDFLAGS_TOOLS+=-Llib -lsurvive -Wl,-rpath,lib -lX11 $(LDFLAGS)
 endif
@@ -25,13 +26,13 @@ CC?=gcc
 
 
 ifdef EXTRA_WARNINGS
-	CFLAGS+=-fsanitize=address -fsanitize=undefined -Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable -Wno-pointer-sign -Wno-parentheses
+	CFLAGS+=-fsanitize=address -fsanitize=undefined
 endif
 
 
 
 SBA:=redist/sba/sba_chkjac.c  redist/sba/sba_crsm.c  redist/sba/sba_lapack.c  redist/sba/sba_levmar.c  redist/sba/sba_levmar_wrap.c redist/minimal_opencv.c src/poser_epnp.c src/poser_sba.c src/epnp/epnp.c src/poser_general_optimizer.c
-LIBSURVIVE_CORE+=src/survive.c src/survive_process.c src/ootx_decoder.c src/survive_driverman.c src/survive_default_devices.c src/survive_playback.c src/survive_config.c src/survive_cal.c  src/poser.c src/survive_sensor_activations.c src/survive_disambiguator.c src/survive_imu.c
+LIBSURVIVE_CORE+=src/survive.c src/survive_process.c src/ootx_decoder.c src/survive_driverman.c src/survive_default_devices.c src/survive_playback.c src/survive_config.c src/survive_cal.c  src/poser.c src/survive_sensor_activations.c src/survive_disambiguator.c src/survive_imu.c src/survive_kalman.c
 MINIMAL_NEEDED+=src/survive_usb.c src/survive_charlesbiguator.c  src/survive_vive.c src/survive_reproject.c 
 AUX_NEEDED+=src/survive_turveybiguator.c  src/survive_statebased_disambiguator.c
 POSERS:=src/poser_dummy.c src/poser_imu.c src/poser_charlesrefine.c
@@ -48,12 +49,12 @@ ifeq ($(UNAME), Darwin) # Mac OSX
 	CFLAGS:=$(CFLAGS) -DRASTERIZER -DHIDAPI -I/usr/local/include -x objective-c
 	LDFLAGS:=$(LDFLAGS) -framework OpenGL -framework Cocoa -framework IOKit
 	DRAWFUNCTIONS=redist/CNFGFunctions.c redist/CNFGCocoaNSImageDriver.m
-	GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGCocoaNSImageDriver.o
+	GRAPHICS_LOFI:=redist/CNFGFunctions.c redist/CNFGCocoaNSImageDriver.c
 	REDISTS:=$(REDISTS) redist/hid-osx.c
 else                    # Linux / FreeBSD
 	LDFLAGS:=$(LDFLAGS)
 	DRAWFUNCTIONS=redist/CNFGFunctions.c redist/CNFGXDriver.c redist/CNFG3D.c
-	GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGXDriver.o
+	GRAPHICS_LOFI:=redist/CNFGFunctions.c redist/CNFGXDriver.c
 endif
 
 ifdef MINIMAL
@@ -76,7 +77,6 @@ LIBSURVIVE_D:=$(LIBSURVIVE_C:%.c=$(OBJDIR)/%.d)
 #Include all dependencies so if header files change, it updates.
 -include $(LIBSURVIVE_D)
 
-
 #### Tools
 
 testCocoa : testCocoa.c $(LIBRARY)
@@ -134,7 +134,7 @@ $(OBJDIR):
 	mkdir -p $(OBJDIR)/src/epnp
 
 $(LIBRARY): $(LIBSURVIVE_O) $(OBJDIR)
-	$(CC) -o $@ $(LIBSURVIVE_O) $(CFLAGS) $(LDFLAGS) -shared
+	$(CC) $(CFLAGS) -shared -o $@ $(LIBSURVIVE_O) $(LDFLAGS)
 
 $(STATIC_LIBRARY) : $(LIBSURVIVE_O) $(OBJDIR)
 	ar rcs --plugin=$$(gcc --print-file-name=liblto_plugin.so) lib/libsurvive.a $(LIBSURVIVE_O)
diff --git a/calibrate.c b/calibrate.c
index e21c310..de7fd3b 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -170,12 +170,6 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution)
 
 	CNFGColor((b << 16) | (g << 8) | r);
 
-	int x1, x2, y1, y2;
-
-	x1 = (int)(windowCenterX - minRectSize - ((pose.Pos[2] * 40.0)) + (pose.Pos[0] * sizeScale));
-	y1 = (int)(windowCenterY - minRectSize - ((pose.Pos[2] * 40.0)) + (pose.Pos[1] * sizeScale));
-	x2 = (int)(windowCenterX + minRectSize + ((pose.Pos[2] * 40.0)) + (pose.Pos[0] * sizeScale));
-	y2 = (int)(windowCenterY + minRectSize + ((pose.Pos[2] * 40.0)) + (pose.Pos[1] * sizeScale));
 	FLT xCenter = windowCenterX + (pose.Pos[0] * sizeScale);
 	FLT yCenter = windowCenterY + (pose.Pos[1] * sizeScale);
 
diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h
index 4cddf89..e71b34f 100644
--- a/include/libsurvive/poser.h
+++ b/include/libsurvive/poser.h
@@ -88,7 +88,6 @@ typedef struct
 	//If "lengths[...]" < 0, means not a valid piece of sweep information.
 	FLT  lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
 	FLT  angles [SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];  //2 Axes  (Angles in LH space)
-	FLT  synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES];
 
 	PoserDataIMU lastimu;
 } PoserDataFullScene;
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 66993df..9f692e9 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -273,7 +273,7 @@ SURVIVE_EXPORT void survive_close(SurviveContext *ctx);
 SURVIVE_EXPORT SurviveObject *survive_get_so_by_name(SurviveContext *ctx, const char *name);
 
 // Utilitiy functions.
-int survive_simple_inflate(SurviveContext *ctx, const char *input, int inlen, char *output, int outlen);
+int survive_simple_inflate(SurviveContext *ctx, const uint8_t *input, int inlen, uint8_t *output, int outlen);
 SURVIVE_EXPORT int survive_send_magic(SurviveContext *ctx, int magic_code, void *data, int datalen);
 
 // These functions search both the stored-general and temporary sections for a parameter and return it.
diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h
index 4d03038..892df38 100644
--- a/include/libsurvive/survive_imu.h
+++ b/include/libsurvive/survive_imu.h
@@ -31,8 +31,6 @@ typedef struct {
 
 } SurviveIMUTracker;
 
-void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose);
-
 void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data);
 void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timecode, SurviveIMUTracker *tracker,
 											   SurvivePose *pose, const FLT *variance);
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index 7fa5e0f..b4faaa7 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -47,6 +47,7 @@ typedef LinmathPose SurvivePose;
 #define BUTTON_EVENT_AXIS_CHANGED  3
 
 typedef uint32_t survive_timecode;
+survive_timecode survive_timecode_difference(survive_timecode most_recent, survive_timecode least_recent);
 
 typedef struct SurviveObject SurviveObject;
 typedef struct SurviveContext SurviveContext;
diff --git a/redist/CNFGXDriver.c b/redist/CNFGXDriver.c
index 91b7233..6037eff 100644
--- a/redist/CNFGXDriver.c
+++ b/redist/CNFGXDriver.c
@@ -215,10 +215,10 @@ void CNFGHandleInput()
 	XEvent report;
 
 	int bKeyDirection = 1;
-	int r;
+
 	while( XPending( CNFGDisplay ) )
 	{
-		r=XNextEvent( CNFGDisplay, &report );
+		XNextEvent(CNFGDisplay, &report);
 
 		bKeyDirection = 1;
 		switch  (report.type)
@@ -264,7 +264,7 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h )
 	static int depth;
 	static int lw, lh;
 	static unsigned char * lbuffer;
-	int r, ls;
+	int r;
 
 	if( !xi )
 	{
@@ -283,8 +283,6 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h )
 		lh = h;
 	}
 
-	ls = lw * lh;
-
 	XPutImage(CNFGDisplay, CNFGWindow, CNFGWindowGC, xi, 0, 0, 0, 0, w, h );
 }
 
diff --git a/redist/json_helpers.c b/redist/json_helpers.c
index ebacd2c..0741c84 100644
--- a/redist/json_helpers.c
+++ b/redist/json_helpers.c
@@ -120,8 +120,9 @@ char* load_file_to_mem(const char* path) {
 	int len = ftell( f );
 	fseek( f, 0, SEEK_SET );
 	char * JSON_STRING = malloc( len + 1);
-	memset(JSON_STRING,0,len+1); 
-	size_t i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value.
+	memset(JSON_STRING,0,len+1);
+	size_t i = fread(JSON_STRING, len, 1, f);
+	(void)i; // Ignore return value.
 	fclose( f );
 	return JSON_STRING;
 }
diff --git a/redist/linmath.h b/redist/linmath.h
index 78cac5c..aff46a3 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -14,7 +14,7 @@ extern "C" {
 #endif
 
 // Yes, I know it's kind of arbitrary.
-#define DEFAULT_EPSILON 0.001
+#define DEFAULT_EPSILON 0.0000000001
 
 // For printf
 #define PFTHREE(x) (x)[0], (x)[1], (x)[2]
diff --git a/redist/minimal_opencv.c b/redist/minimal_opencv.c
index 988dd0f..d569d96 100644
--- a/redist/minimal_opencv.c
+++ b/redist/minimal_opencv.c
@@ -116,11 +116,6 @@ static inline void *cvAlignPtr(const void *ptr, int align) {
 	return (void *)(((size_t)ptr + align - 1) & ~(size_t)(align - 1));
 }
 
-static inline int cvAlign(int size, int align) {
-	CV_DbgAssert((align & (align - 1)) == 0 && size < INT_MAX);
-	return (size + align - 1) & -align;
-}
-
 SURVIVE_LOCAL_ONLY void cvCreateData(CvMat *arr) {
 	if (CV_IS_MAT_HDR_Z(arr)) {
 		size_t step, total_size;
diff --git a/redist/sba/sba_crsm.c b/redist/sba/sba_crsm.c
index 9ba11f1..ea38042 100644
--- a/redist/sba/sba_crsm.c
+++ b/redist/sba/sba_crsm.c
@@ -22,9 +22,6 @@
 
 #include "sba.h"
 
-static void sba_crsm_print(struct sba_crsm *sm, FILE *fp);
-static void sba_crsm_build(struct sba_crsm *sm, int *m, int nr, int nc);
-
 /* allocate a sparse CRS matrix */
 void sba_crsm_alloc(struct sba_crsm *sm, int nr, int nc, int nnz) {
 	int msz;
@@ -50,46 +47,6 @@ void sba_crsm_free(struct sba_crsm *sm) {
 	sm->val = sm->colidx = sm->rowptr = NULL;
 }
 
-static void sba_crsm_print(struct sba_crsm *sm, FILE *fp) {
-	register int i;
-
-	fprintf(fp, "matrix is %dx%d, %d non-zeros\nval: ", sm->nr, sm->nc, sm->nnz);
-	for (i = 0; i < sm->nnz; ++i)
-		fprintf(fp, "%d ", sm->val[i]);
-	fprintf(fp, "\ncolidx: ");
-	for (i = 0; i < sm->nnz; ++i)
-		fprintf(fp, "%d ", sm->colidx[i]);
-	fprintf(fp, "\nrowptr: ");
-	for (i = 0; i <= sm->nr; ++i)
-		fprintf(fp, "%d ", sm->rowptr[i]);
-	fprintf(fp, "\n");
-}
-
-/* build a sparse CRS matrix from a dense one. intended to serve as an example for sm creation */
-static void sba_crsm_build(struct sba_crsm *sm, int *m, int nr, int nc) {
-	int nnz;
-	register int i, j, k;
-
-	/* count nonzeros */
-	for (i = nnz = 0; i < nr; ++i)
-		for (j = 0; j < nc; ++j)
-			if (m[i * nc + j] != 0)
-				++nnz;
-
-	sba_crsm_alloc(sm, nr, nc, nnz);
-
-	/* fill up the sm structure */
-	for (i = k = 0; i < nr; ++i) {
-		sm->rowptr[i] = k;
-		for (j = 0; j < nc; ++j)
-			if (m[i * nc + j] != 0) {
-				sm->val[k] = m[i * nc + j];
-				sm->colidx[k++] = j;
-			}
-	}
-	sm->rowptr[nr] = nnz;
-}
-
 /* returns the index of the (i, j) element. No bounds checking! */
 int sba_crsm_elmidx(struct sba_crsm *sm, int i, int j) {
 	register int low, high, mid, diff;
diff --git a/redist/sba/sba_levmar.c b/redist/sba/sba_levmar.c
index 2f85f2a..e5c499f 100644
--- a/redist/sba/sba_levmar.c
+++ b/redist/sba/sba_levmar.c
@@ -99,46 +99,6 @@ static double sba_mean_repr_error(int n, int mnp, double *x, double *hx, struct
 	return err / ((double)(nprojs));
 }
 
-/* print the solution in p using sba's text format. If cnp/pnp==0 only points/cameras are printed */
-static void sba_print_sol(int n, int m, double *p, int cnp, int pnp, double *x, int mnp, struct sba_crsm *idxij,
-						  int *rcidxs, int *rcsubs) {
-	register int i, j, ii;
-	int nnz;
-	double *ptr;
-
-	if (cnp) {
-		/* print camera parameters */
-		for (j = 0; j < m; ++j) {
-			ptr = p + cnp * j;
-			for (ii = 0; ii < cnp; ++ii)
-				printf("%g ", ptr[ii]);
-			printf("\n");
-		}
-	}
-
-	if (pnp) {
-		/* 3D & 2D point parameters */
-		printf("\n\n\n# X Y Z  nframes  frame0 x0 y0  frame1 x1 y1 ...\n");
-		for (i = 0; i < n; ++i) {
-			ptr = p + cnp * m + i * pnp;
-			for (ii = 0; ii < pnp; ++ii) // print 3D coordinates
-				printf("%g ", ptr[ii]);
-
-			nnz = sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero x_ij, j=0...m-1 */
-			printf("%d ", nnz);
-
-			for (j = 0; j < nnz; ++j) { /* point i projecting on camera rcsubs[j] */
-				ptr = x + idxij->val[rcidxs[j]] * mnp;
-
-				printf("%d ", rcsubs[j]);
-				for (ii = 0; ii < mnp; ++ii) // print 2D coordinates
-					printf("%g ", ptr[ii]);
-			}
-			printf("\n");
-		}
-	}
-}
-
 /* Compute e=x-y for two n-vectors x and y and return the squared L2 norm of e.
  * e can coincide with either x or y.
  * Uses loop unrolling and blocking to reduce bookkeeping overhead & pipeline
@@ -623,7 +583,7 @@ int sba_motstr_levmar_x(
 	 * measurements vector x
 	 */
 
-	double *pa, *pb, *ea, *eb, *dpa, *dpb; /* pointers into p, jac, eab and dp respectively */
+	double *ea, *eb, *dpa, *dpb; /* pointers into p, jac, eab and dp respectively */
 
 	/* submatrices sizes */
 	int Asz, Bsz, ABsz, Usz, Vsz, Wsz, Ysz, esz, easz, ebsz, YWtsz, Wtdasz, Sblsz, covsz;
@@ -793,8 +753,8 @@ int sba_motstr_levmar_x(
 	jac = W + Wsz + ((Wsz > ABsz) ? nvis * (Wsz - ABsz) : 0);
 
 	/* set up auxiliary pointers */
-	pa = p;
-	pb = p + m * cnp;
+	// pa = p;
+	// pb = p + m * cnp;
 	ea = eab;
 	eb = eab + m * cnp;
 	dpa = dp;
diff --git a/redist/sba/sba_levmar_wrap.c b/redist/sba/sba_levmar_wrap.c
index 159a040..50948ea 100644
--- a/redist/sba/sba_levmar_wrap.c
+++ b/redist/sba/sba_levmar_wrap.c
@@ -73,7 +73,7 @@ static void sba_motstr_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *r
 	register int i, j;
 	int cnp, pnp, mnp;
 	double *pa, *pb, *paj, *pbi, *pxij;
-	int n, m, nnz;
+	int m, nnz;
 	struct wrap_motstr_data_ *wdata;
 	void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *proj_adata);
 	void *proj_adata;
@@ -85,7 +85,7 @@ static void sba_motstr_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *r
 	proj = wdata->proj;
 	proj_adata = wdata->adata;
 
-	n = idxij->nr;
+	// n = idxij->nr;
 	m = idxij->nc;
 	pa = p;
 	pb = p + m * cnp;
@@ -117,7 +117,7 @@ static void sba_motstr_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, in
 	register int i, j;
 	int cnp, pnp, mnp;
 	double *pa, *pb, *paj, *pbi, *pAij, *pBij;
-	int n, m, nnz, Asz, Bsz, ABsz, idx;
+	int m, nnz, Asz, Bsz, ABsz, idx;
 	struct wrap_motstr_data_ *wdata;
 	void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *projac_adata);
 	void *projac_adata;
@@ -129,7 +129,7 @@ static void sba_motstr_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, in
 	projac = wdata->projac;
 	projac_adata = wdata->adata;
 
-	n = idxij->nr;
+	// n = idxij->nr;
 	m = idxij->nc;
 	pa = p;
 	pb = p + m * cnp;
diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c
index ff82fd4..057ac3d 100644
--- a/src/poser_general_optimizer.c
+++ b/src/poser_general_optimizer.c
@@ -37,6 +37,10 @@ bool general_optimizer_data_record_success(GeneralOptimizerData *d, FLT error) {
 		d->failures_to_reset_cntr = d->failures_to_reset;
 		return true;
 	}
+
+	if (d->failures_to_reset_cntr > 0)
+		d->failures_to_reset_cntr--;
+
 	return false;
 }
 
diff --git a/src/poser_sba.c b/src/poser_sba.c
index 8c9e328..07d8f25 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -17,6 +17,7 @@
 #include "string.h"
 #include "survive_cal.h"
 #include "survive_config.h"
+#include "survive_kalman.h"
 #include "survive_reproject.h"
 
 typedef struct {
@@ -26,12 +27,6 @@ typedef struct {
 	SurvivePose camera_params[2];
 } sba_context;
 
-typedef struct {
-	sba_context hdr;
-	int acode;
-	int lh;
-} sba_context_single_sweep;
-
 typedef struct SBAData {
 	GeneralOptimizerData opt;
 
@@ -44,7 +39,9 @@ typedef struct SBAData {
 	int use_jacobian_function;
 	int required_meas;
 
+	survive_kpose_t kpose;
 	SurviveIMUTracker tracker;
+
 	bool useIMU;
 
 	struct {
@@ -89,6 +86,8 @@ static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, Surviv
 	size_t rtn = 0;
 	SurviveObject *so = d->opt.so;
 
+	// fprintf(stderr, "#");
+
 	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)) {
@@ -96,24 +95,26 @@ static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, Surviv
 				// FLT a[2];
 				// survive_apply_bsd_calibration(so->ctx, lh, _a, a);
 				vmask[sensor * NUM_LIGHTHOUSES + lh] = 1;
-
 				if (cov) {
 					*(cov++) = d->sensor_variance +
-							   abs(pdl->timecode - scene->timecode[sensor][lh][0]) * d->sensor_variance_per_second /
-								   (double)so->timebase_hz;
+							   abs((int32_t)pdl->timecode - (int32_t)scene->timecode[sensor][lh][0]) *
+								   d->sensor_variance_per_second / (double)so->timebase_hz;
 					*(cov++) = 0;
 					*(cov++) = 0;
 					*(cov++) = d->sensor_variance +
-							   abs(pdl->timecode - scene->timecode[sensor][lh][1]) * d->sensor_variance_per_second /
-								   (double)so->timebase_hz;
+							   abs((int32_t)pdl->timecode - (int32_t)scene->timecode[sensor][lh][1]) *
+								   d->sensor_variance_per_second / (double)so->timebase_hz;
 				}
 				meas[rtn++] = a[0];
 				meas[rtn++] = a[1];
+				// fprintf(stderr, "%.04f %.04f ", a[0], a[1]);
 			} else {
 				vmask[sensor * NUM_LIGHTHOUSES + lh] = 0;
+				// fprintf(stderr, "%.06f %.06f ", sensor, lh, -2,-2);
 			}
 		}
 	}
+	// fprintf(stderr, "\n");
 	return rtn;
 }
 
@@ -126,43 +127,6 @@ void sba_set_cameras(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, S
 		ctx->obj_pose = LinmathPose_Identity;
 }
 
-typedef struct {
-	bool hasInfo;
-	SurvivePose poses;
-} sba_set_position_t;
-
-static void sba_set_position(SurviveObject *so, uint32_t timecode, SurvivePose *new_pose, void *_user) {
-	sba_set_position_t *user = _user;
-	assert(user->hasInfo == false);
-	user->hasInfo = 1;
-	user->poses = *new_pose;
-}
-void *GetDriver(const char *name);
-
-static void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, void *adata) {
-	SurvivePose obj = *(SurvivePose *)bi;
-	int sensor_idx = j >> 1;
-
-	sba_context_single_sweep *ctx = (sba_context_single_sweep *)(adata);
-	SurviveObject *so = ctx->hdr.so;
-	int lh = ctx->lh;
-	int acode = ctx->acode;
-
-	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]);
-
-	// std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl;
-	SurvivePose *camera = &so->ctx->bsd[lh].Pose;
-
-	FLT out[2];
-	survive_reproject_from_pose(so->ctx, lh, camera, xyz, out);
-	*xij = out[acode];
-}
-
 static void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
 	SurvivePose obj = *(SurvivePose *)bi;
 	int sensor_idx = j >> 1;
@@ -174,7 +138,7 @@ 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);
+	quatnormalize(obj.Rot, obj.Rot);
 
 	// std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl;
 	SurvivePose *camera = &so->ctx->bsd[lh].Pose;
@@ -193,7 +157,7 @@ static void str_metric_function_jac(int j, int i, double *bi, double *xij, void
 	assert(lh < 2);
 	assert(sensor_idx < so->sensor_ct);
 
-	// quatnormalize(obj.Rot, obj.Rot);
+	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],
@@ -208,8 +172,9 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 
 	char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES);
 	double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES);
-	double *cov =
-		d->sensor_variance_per_second > 0. ? alloca(sizeof(double) * 2 * 2 * so->sensor_ct * NUM_LIGHTHOUSES) : 0;
+	double *cov = (d->sensor_variance_per_second > 0. && d->sensor_variance)
+					  ? alloca(sizeof(double) * 2 * 2 * so->sensor_ct * NUM_LIGHTHOUSES)
+					  : 0;
 	size_t meas_size = construct_input_from_scene(d, pdl, scene, vmask, meas, cov);
 
 	static int failure_count = 500;
@@ -271,9 +236,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 		quatnormalize(soLocation.Rot, soLocation.Rot);
 		*out = soLocation;
 		rtn = info[1] / meas_size * 2;
-	} else
-
-	{
+	} else {
 		SurviveContext *ctx = so->ctx;
 		// Docs say info[0] should be divided by meas; I don't buy it really...
 		if (error_failure) {
@@ -418,6 +381,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 			if (error < 0) {
 
 			} else {
+				quatnormalize(estimate.Rot, estimate.Rot);
+
 				if (d->useIMU) {
 					FLT var_meters = 0.5;
 					FLT var_quat = error + .05;
@@ -428,6 +393,11 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 					estimate = d->tracker.pose;
 				}
 
+				LinmathVec3d pvar = {.1, .1, .1};
+				FLT rvar = .01;
+				survive_kpose_integrate_pose(&d->kpose, lightData->timecode, &estimate, pvar, rvar);
+				estimate = d->kpose.state.pose;
+
 				PoserData_poser_pose_func(&lightData->hdr, so, &estimate);
 			}
 		}
diff --git a/src/survive.c b/src/survive.c
index 58566c1..ce17295 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -581,7 +581,7 @@ int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int in
 
 #include <zlib.h>
 
-int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int inlen, char *output, int outlen) {
+int survive_simple_inflate(struct SurviveContext *ctx, const uint8_t *input, int inlen, uint8_t *output, int outlen) {
 	z_stream zs; // Zlib stream.  May only be used by configuration at beginning and by USB thread periodically.
 	memset(&zs, 0, sizeof(zs));
 	inflateInit(&zs); /// Consider checking error
@@ -614,4 +614,4 @@ const SurvivePose *survive_object_pose(SurviveObject *so) { return &so->OutPose;
 
 int8_t survive_object_sensor_ct(SurviveObject *so) { return so->sensor_ct; }
 const FLT *survive_object_sensor_locations(SurviveObject *so) { return so->sensor_locations; }
-const FLT *survive_object_sensor_normals(SurviveObject *so) { return so->sensor_normals; }
\ No newline at end of file
+const FLT *survive_object_sensor_normals(SurviveObject *so) { return so->sensor_normals; }
diff --git a/src/survive_cal.c b/src/survive_cal.c
index faf2ac7..ad62d22 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -417,7 +417,6 @@ static void handle_calibration( struct SurviveCalData *cd )
 	mkdir( "calinfo", 0755 );
 #endif
 	int sen, axis, lh;
-	FLT temp_syncs[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES];
 
 	//Just to get it out of the way early, we'll calculate the sync-pulse-lengths here.
 	FILE * sync_time_info = fopen( "calinfo/synctime.csv", "w" );
@@ -620,7 +619,6 @@ static void handle_calibration( struct SurviveCalData *cd )
 			fsd.lengths[i][j][1] = cd->avglens[dataindex+1];
 			fsd.angles[i][j][0] = cd->avgsweeps[dataindex + 0];
 			fsd.angles[i][j][1] = cd->avgsweeps[dataindex + 1];
-			fsd.synctimes[i][j] = temp_syncs[i][j];
 		}
 
 		int r = cd->ConfigPoserFn( cd->poseobjects[obj], (PoserData*)&fsd );
diff --git a/src/survive_imu.c b/src/survive_imu.c
index 3622b2d..f866caf 100644
--- a/src/survive_imu.c
+++ b/src/survive_imu.c
@@ -66,18 +66,6 @@ static inline uint32_t tick_difference(uint32_t most_recent, uint32_t least_rece
 	return diff;
 }
 
-void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose) {
-	tracker->pose = *pose;
-
-	for (int i = 0; i < 3; i++) {
-		tracker->current_velocity[i] = 0;
-	}
-	//(pose->Pos[i] - tracker->lastGT.Pos[i]) / tick_difference(timecode, tracker->lastGTTime) * 48000000.;
-
-	tracker->lastGTTime = timecode;
-	tracker->lastGT = *pose;
-}
-
 static const int imu_calibration_iterations = 100;
 
 static void RotateAccel(LinmathVec3d rAcc, const SurvivePose *pose, const LinmathVec3d accel) {
diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c
index dc5c0d4..e83b2fc 100644
--- a/src/survive_sensor_activations.c
+++ b/src/survive_sensor_activations.c
@@ -4,6 +4,9 @@
 bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, uint32_t tolerance,
 										  uint32_t timecode_now, uint32_t idx, int lh) {
 	const uint32_t *data_timecode = self->timecode[idx][lh];
+	if (self->lengths[idx][lh][0] == 0 || self->lengths[idx][lh][1] == 0)
+		return false;
+
 	return !(timecode_now - data_timecode[0] > tolerance || timecode_now - data_timecode[1] > tolerance);
 }
 
@@ -26,7 +29,7 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa
 
 	*angle = lightData->angle;
 	*data_timecode = lightData->timecode;
-	*length = lightData->length * 48000000;
+	*length = (uint32_t)(lightData->length * 48000000);
 }
 
 uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000) + 5000;
diff --git a/src/survive_turveybiguator.c b/src/survive_turveybiguator.c
index 8dbce46..5b10e63 100644
--- a/src/survive_turveybiguator.c
+++ b/src/survive_turveybiguator.c
@@ -155,8 +155,8 @@ static uint8_t remove_outliers(SurviveObject *so) {
 
 	//	uint8_t removed_outliers = 0;
 
-	uint32_t d1 = abs(*min - mean);
-	uint32_t d2 = abs(*max - mean);
+	uint32_t d1 = mean - *min;
+	uint32_t d2 = *max - mean;
 
 	if (d1 > d2) {
 		if (d1 > tau_test) {
@@ -199,6 +199,7 @@ static void handle_lightcap2_process_sweep_data(SurviveObject *so) {
 	{
 		unsigned int longest_pulse = 0;
 		unsigned int timestamp_of_longest_pulse = 0;
+		(void)timestamp_of_longest_pulse;
 		for (int i = 0; i < SENSORS_PER_OBJECT; i++) {
 			if (lcd->sweep.sweep_len[i] > longest_pulse) {
 				longest_pulse = lcd->sweep.sweep_len[i];
@@ -216,6 +217,7 @@ static void handle_lightcap2_process_sweep_data(SurviveObject *so) {
 
 			{
 				static int counts[SENSORS_PER_OBJECT][2] = {0};
+				(void)counts;
 
 				//				if (lcd->per_sweep.activeLighthouse == 0 && !allZero)
 				if (lcd->per_sweep.activeLighthouse > -1 && !allZero) {
diff --git a/src/survive_vive.c b/src/survive_vive.c
index d9cbc3e..88bc065 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -961,6 +961,8 @@ void calibrate_acc(SurviveObject* so, FLT* agm) {
 		agm[1] *= so->acc_scale[1];
 		agm[2] *= so->acc_scale[2];
 	}
+
+	fprintf(stderr, "%f\t%f\t%f\n", agm[0], agm[1], agm[2]);
 }
 
 void calibrate_gyro(SurviveObject* so, FLT* agm) {
diff --git a/test.c b/test.c
index 9fda5b1..7785b37 100644
--- a/test.c
+++ b/test.c
@@ -164,7 +164,6 @@ int main( int argc, char ** argv )
 	dump_iface( survive_get_so_by_name( ctx, "TR0" ), "TR0" );
 	dump_iface( survive_get_so_by_name( ctx, "WW0" ), "WW0" );
 	survive_startup(ctx);
-	survive_cal_install(ctx);
 
 	while(survive_poll(ctx) == 0)
 	{
diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js
index 26f77a6..56ef47f 100644
--- a/tools/viz/survive_viewer.js
+++ b/tools/viz/survive_viewer.js
@@ -143,7 +143,7 @@ function redrawCanvas(when) {
 						ang[0][1] < when[key] - visible_tolerance)
 						continue;
 
-					var half_fov = 1.0472 * 2;
+					var half_fov = 1.0472 * 2 / 5.;
 					var x = ang[0][0] / half_fov * canvas.width / 2 + canvas.width / 2;
 					var y = -ang[1][0] / half_fov * canvas.height / 2 + canvas.height / 2;