Merge pull request #122 from cnlohr/imu

Imu

23 files changed, 569 insertions, 281 deletions

diff --git a/Makefile b/Makefile
index dcef7ba..c5763cb 100644
--- a/Makefile
+++ b/Makefile
@@ -39,7 +39,8 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/minimal_ope
 ifeq ($(UNAME), Darwin)
 REDISTS:=$(REDISTS) redist/hid-osx.c
 endif
-LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o src/poser_charlesrefine.o
+
+LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o src/poser_charlesrefine.o src/survive_imu.o src/poser_imu.o
 
 #If you want to use HIDAPI on Linux.
 #CFLAGS:=$(CFLAGS) -DHIDAPI
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 0558dc8..65343b7 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -115,7 +115,7 @@ struct SurviveObject {
 	haptic_func haptic;
 
 	SurviveSensorActivations activations;
-	void* user_ptr;
+	void *user_ptr;
 	// Debug
 	int tsl;
 };
diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h
new file mode 100644
index 0000000..124ad7e
--- /dev/null
+++ b/include/libsurvive/survive_imu.h
@@ -0,0 +1,36 @@
+#ifndef _SURVIVE_IMU_H
+#define _SURVIVE_IMU_H
+
+#include "poser.h"
+#include "survive_types.h"
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct SurviveIMUTracker_p;
+
+typedef struct {
+	FLT updir[3];
+	FLT accel_scale_bias;
+
+	LinmathVec3d current_velocity;	// Velocity in world frame
+	PoserDataIMU last_data;
+	SurvivePose pose;
+
+	SurvivePose lastGT;
+	uint32_t lastGTTime;
+
+	float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki
+
+} 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);
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index edce3e9..367c391 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -50,7 +50,8 @@ typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro,
 typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh);
 typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val);
 typedef void (*pose_func)(SurviveObject *so, uint32_t timecode, SurvivePose *pose);
-typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, SurvivePose *object_pose);
+typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose,
+									 SurvivePose *object_pose);
 
 // For lightcap, etc.  Don't change this structure at all.  Regular vive is dependent on it being exactly as-is.
 // When you write drivers, you can use this to send survive lightcap data.
diff --git a/redist/linmath.h b/redist/linmath.h
index 094c230..e268e96 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -7,7 +7,6 @@
 extern "C" {
 #endif
 
-
 #ifdef _WIN32
 #define LINMATH_EXPORT __declspec(dllexport)
 #else
@@ -116,7 +115,7 @@ LINMATH_EXPORT void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin);
 LINMATH_EXPORT void quatsub(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b);
 LINMATH_EXPORT void quatadd(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b);
 LINMATH_EXPORT void quatrotateabout(LinmathQuat qout, const LinmathQuat a,
-					 const LinmathQuat b); // same as quat multiply, not piecewise multiply.
+									const LinmathQuat b); // same as quat multiply, not piecewise multiply.
 LINMATH_EXPORT void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s);
 FLT quatinnerproduct(const LinmathQuat qa, const LinmathQuat qb);
 LINMATH_EXPORT void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb);
diff --git a/redist/symbol_enumerator.c b/redist/symbol_enumerator.c
index 58bbfaa..31bb68e 100644
--- a/redist/symbol_enumerator.c
+++ b/redist/symbol_enumerator.c
@@ -60,18 +60,14 @@ BOOL WINAPI SymCleanup(
 	HANDLE hProcess
 );
 
-BOOL mycb(
-  PSYMBOL_INFO pSymInfo,
-  ULONG        SymbolSize,
-  PVOID        UserContext
-  ){
-	  SymEnumeratorCallback cb = (SymEnumeratorCallback)UserContext;
-	  return !cb( "", &pSymInfo->Name[0], (void*)pSymInfo->Address, (long) pSymInfo->Size );
+BOOL mycb(PSYMBOL_INFO pSymInfo, ULONG SymbolSize, PVOID UserContext) {
+	SymEnumeratorCallback cb = (SymEnumeratorCallback)UserContext;
+	return !cb("", &pSymInfo->Name[0], (void *)pSymInfo->Address, (long)pSymInfo->Size);
   }
   
 int EnumerateSymbols( SymEnumeratorCallback cb )
 {
-	HANDLE proc =  GetCurrentProcess();
+	HANDLE proc = GetCurrentProcess();
 	if( !SymInitialize( proc, 0, 1 ) ) return -1;
 	if( !SymEnumSymbols( proc, 0, "*!*", &mycb, (void*)cb ) )
 	{
diff --git a/simple_pose_test.c b/simple_pose_test.c
index d28e78d..06a9901 100644
--- a/simple_pose_test.c
+++ b/simple_pose_test.c
@@ -43,9 +43,9 @@ FLT hpos[3];
 FLT hpos2[3];
 
 void testprog_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose) {
-	survive_default_raw_pose_process(so, timecode, pose );
+	survive_default_raw_pose_process(so, timecode, pose);
 
-	if( strcmp( so->codename, "HMD" ) != 0 )
+	if (strcmp(so->codename, "HMD") != 0)
 		return;
 
 	// print the pose;
@@ -168,7 +168,7 @@ int main( int argc, char ** argv )
 	survive_install_pose_fn(ctx, testprog_raw_pose_process);
 	//survive_install_angle_fn(ctx, testprog_angle_process );
 
-#if 0 //Don't reset poses
+#if 0 // Don't reset poses
 	ctx->bsd[0].PositionSet = ctx->bsd[1].PositionSet = 1;
 	int i;
 	for( i = 0; i < 2; i++ )
diff --git a/src/poser.c b/src/poser.c
index 20334f7..499e0ff 100644
--- a/src/poser.c
+++ b/src/poser.c
@@ -14,7 +14,7 @@ static uint32_t PoserData_timecode(PoserData *poser_data) {
 		return lightData->timecode;
 	}
 	case POSERDATA_FULL_SCENE: {
-		PoserDataFullScene* pdfs = (PoserDataFullScene *)(poser_data);
+		PoserDataFullScene *pdfs = (PoserDataFullScene *)(poser_data);
 		return -1;
 	}
 	case POSERDATA_IMU: {
diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c
index f9c60e2..388ba77 100644
--- a/src/poser_charlesrefine.c
+++ b/src/poser_charlesrefine.c
@@ -1,4 +1,4 @@
-//Driver works, but you _must_ start it near the origin looking in +Z.
+// Driver works, but you _must_ start it near the origin looking in +Z.
 
 #include <poser.h>
 #include <survive.h>
@@ -7,18 +7,16 @@
 #include "epnp/epnp.h"
 #include "linmath.h"
 #include <math.h>
+#include <stdint.h>
 #include <stdio.h>
 #include <string.h>
-#include <stdint.h>
 
 #define MAX_PT_PER_SWEEP 32
 
-
-typedef struct
-{
+typedef struct {
 	int sweepaxis;
 	int sweeplh;
-	FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; //Value is actually normalized, not just normal to sweep plane.
+	FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; // Value is actually normalized, not just normal to sweep plane.
 	FLT quantity_errors[MAX_PT_PER_SWEEP];
 	FLT angles_at_pts[MAX_PT_PER_SWEEP];
 	SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP];
@@ -26,11 +24,10 @@ typedef struct
 	int ptsweep;
 } CharlesPoserData;
 
-
-
 int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
-	CharlesPoserData * dd = so->PoserData;
-	if( !dd ) so->PoserData = dd = calloc( sizeof(CharlesPoserData), 1 );
+	CharlesPoserData *dd = so->PoserData;
+	if (!dd)
+		so->PoserData = dd = calloc(sizeof(CharlesPoserData), 1);
 
 	SurviveSensorActivations *scene = &so->activations;
 	switch (pd->pt) {
@@ -38,18 +35,17 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 		// Really should use this...
 		PoserDataIMU *imuData = (PoserDataIMU *)pd;
 
-
-		//TODO: Actually do Madgwick's algorithm
-		LinmathQuat	applymotion;
-		const SurvivePose * object_pose = &so->OutPose;
+		// TODO: Actually do Madgwick's algorithm
+		LinmathQuat applymotion;
+		const SurvivePose *object_pose = &so->OutPose;
 		imuData->gyro[0] *= -0.0005;
 		imuData->gyro[1] *= -0.0005;
 		imuData->gyro[2] *= 0.0005;
-		quatfromeuler( applymotion, imuData->gyro );
-		//printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] );
+		quatfromeuler(applymotion, imuData->gyro);
+		// printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] );
 		SurvivePose object_pose_out;
-		quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion );
-		copy3d( object_pose_out.Pos, object_pose->Pos );
+		quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion);
+		copy3d(object_pose_out.Pos, object_pose->Pos);
 		PoserData_poser_pose_func(pd, so, &object_pose_out);
 
 		return 0;
@@ -59,70 +55,67 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 		PoserDataLight *ld = (PoserDataLight *)pd;
 		int lhid = ld->lh;
 		int senid = ld->sensor_id;
-		BaseStationData * bsd = &so->ctx->bsd[ld->lh];
-		if( !bsd->PositionSet ) break;
-		SurvivePose * lhp = &bsd->Pose;
+		BaseStationData *bsd = &so->ctx->bsd[ld->lh];
+		if (!bsd->PositionSet)
+			break;
+		SurvivePose *lhp = &bsd->Pose;
 		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.
-		//if( lhid == 1 ) break;
+		// FOR NOW, drop LH1.
+		// if( lhid == 1 ) break;
 
+		//		const FLT * sensor_normal = &so->sensor_normals[senid*3];
+		//		FLT sensor_normal_worldspace[3];
+		//		ApplyPoseToPoint(sensor_normal_worldspace,   object_pose, sensor_inpos);
 
-//		const FLT * sensor_normal = &so->sensor_normals[senid*3];
-//		FLT sensor_normal_worldspace[3];
-//		ApplyPoseToPoint(sensor_normal_worldspace,   object_pose, sensor_inpos);
-
-		const FLT * sensor_inpos = &so->sensor_locations[senid*3];
+		const FLT *sensor_inpos = &so->sensor_locations[senid * 3];
 		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.
+		// 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);
 
-
-		//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] );
+		// 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] );
 		// = sensor position, relative to lighthouse center.
-		FLT sensorpos_rel_lh[3];	
-		sub3d( sensorpos_rel_lh, sensor_position_worldspace, lhp->Pos );
+		FLT sensorpos_rel_lh[3];
+		sub3d(sensorpos_rel_lh, sensor_position_worldspace, lhp->Pos);
 
-		//Next, define a normal in global space of the plane created by the sweep hit.
-		//Careful that this must be normalized.
+		// Next, define a normal in global space of the plane created by the sweep hit.
+		// Careful that this must be normalized.
 		FLT sweep_normal[3];
 
-		//If 1, the "y" axis. //XXX Check me.
-		if( axis )  		//XXX Just FYI this should include account for skew
+		// If 1, the "y" axis. //XXX Check me.
+		if (axis) // XXX Just FYI this should include account for skew
 		{
 			sweep_normal[0] = 0;
-			sweep_normal[1] = cos(angle );
-			sweep_normal[2] = sin( angle );
-			//printf( "+" );
-		}
-		else
-		{
-			sweep_normal[0] = cos( angle );
+			sweep_normal[1] = cos(angle);
+			sweep_normal[2] = sin(angle);
+			// printf( "+" );
+		} else {
+			sweep_normal[0] = cos(angle);
 			sweep_normal[1] = 0;
-			sweep_normal[2] = -sin( angle );
-			//printf( "-" );
+			sweep_normal[2] = -sin(angle);
+			// printf( "-" );
 		}
 
-		//Need to apply the lighthouse's transformation to the sweep's normal.
-		quatrotatevector( sweep_normal, lhp->Rot, sweep_normal);
+		// Need to apply the lighthouse's transformation to the sweep's normal.
+		quatrotatevector(sweep_normal, lhp->Rot, sweep_normal);
 
-		//Compute point-line distance between sensorpos_rel_lh and the plane defined by sweep_normal.
-		//Do this by projecting sensorpos_rel_lh (w) onto sweep_normal (v).
-		//You can do this by |v dot w| / |v| ... But we know |v| is 1. So...
-		FLT dist = dot3d( sensorpos_rel_lh, sweep_normal );
+		// Compute point-line distance between sensorpos_rel_lh and the plane defined by sweep_normal.
+		// Do this by projecting sensorpos_rel_lh (w) onto sweep_normal (v).
+		// You can do this by |v dot w| / |v| ... But we know |v| is 1. So...
+		FLT dist = dot3d(sensorpos_rel_lh, sweep_normal);
 
-		if( (i = dd->ptsweep) < MAX_PT_PER_SWEEP )
-		{
-			memcpy( dd->normal_at_errors[i], sweep_normal, sizeof(FLT)*3 );
+		if ((i = dd->ptsweep) < MAX_PT_PER_SWEEP) {
+			memcpy(dd->normal_at_errors[i], sweep_normal, sizeof(FLT) * 3);
 			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], object_pose, sizeof(SurvivePose));
 			dd->ptsweep++;
 		}
 
@@ -133,197 +126,201 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 		PoserDataLight *l = (PoserDataLight *)pd;
 		int lhid = l->lh;
 
-
-		//you can get sweepaxis and sweeplh.
-		if( dd->ptsweep )
-		{
+		// you can get sweepaxis and sweeplh.
+		if (dd->ptsweep) {
 			int i;
 			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;
+			BaseStationData *bsd = &so->ctx->bsd[lhid];
+			SurvivePose *lh_pose = &bsd->Pose;
 
 			int validpoints = 0;
 			int ptvalid[MAX_PT_PER_SWEEP];
 			FLT avgerr = 0.0;
-			FLT vec_correct[3] = { 0., 0. , 0. };
+			FLT vec_correct[3] = {0., 0., 0.};
 			FLT avgang = 0.0;
 
-//Tunable parameters:
-#define MIN_HIT_QUALITY 0.5			//Determines which hits to cull.
-#define HIT_QUALITY_BASELINE 0.0001	//Determines which hits to cull.  Actually SQRT(baseline) if 0.0001, it is really 1cm
+// Tunable parameters:
+#define MIN_HIT_QUALITY 0.5 // Determines which hits to cull.
+#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_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 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_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
 
-			//Step 1: Determine standard of deviation, and average in order to
+			// Step 1: Determine standard of deviation, and average in order to
 			// drop points that are likely in error.
 			{
-				//Calculate average
+				// Calculate average
 				FLT avgerr_orig = 0.0;
 				FLT stddevsq = 0.0;
-				for( i = 0; i < pts; i++ )
+				for (i = 0; i < pts; i++)
 					avgerr_orig += dd->quantity_errors[i];
-				avgerr_orig/=pts;
+				avgerr_orig /= pts;
 
-				//Calculate standard of deviation.
-				for( i = 0; i < pts; i++ )
-				{
-					FLT diff = dd->quantity_errors[i]-avgerr_orig;
-					stddevsq += diff*diff;
+				// Calculate standard of deviation.
+				for (i = 0; i < pts; i++) {
+					FLT diff = dd->quantity_errors[i] - avgerr_orig;
+					stddevsq += diff * diff;
 				}
-				stddevsq/=pts;
+				stddevsq /= pts;
 
-				for( i = 0; i < pts; i++ )
-				{
+				for (i = 0; i < pts; i++) {
 					FLT err = dd->quantity_errors[i];
-					FLT diff = err-avgerr_orig;
+					FLT diff = err - avgerr_orig;
 					diff *= diff;
-					int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE)?1:0;
+					int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE) ? 1 : 0;
 					ptvalid[i] = isptvalid;
-					if( isptvalid )
-					{
+					if (isptvalid) {
 						avgang += dd->angles_at_pts[i];
 						avgerr += err;
-						validpoints ++;
+						validpoints++;
 					}
 				}
 				avgang /= validpoints;
 				avgerr /= validpoints;
 			}
 
-			//Step 2: Determine average lateral error.
-			//We can actually always perform this operation.  Even with only one point.
+			// Step 2: Determine average lateral error.
+			// We can actually always perform this operation.  Even with only one point.
 			{
-				FLT avg_err[3] = { 0, 0, 0 }; //Positional error.
-				for( i = 0; i < pts; i++ )
-				{
-					if( !ptvalid[i] ) continue;
-					FLT * nrm = dd->normal_at_errors[i];
+				FLT avg_err[3] = {0, 0, 0}; // Positional error.
+				for (i = 0; i < pts; i++) {
+					if (!ptvalid[i])
+						continue;
+					FLT *nrm = dd->normal_at_errors[i];
 					FLT err = dd->quantity_errors[i];
 					avg_err[0] = avg_err[0] + nrm[0] * err;
 					avg_err[1] = avg_err[1] + nrm[1] * err;
 					avg_err[2] = avg_err[2] + nrm[2] * err;
 				}
 
-				//NOTE: The "avg_err" is not geometrically centered.  This is actually
-				//probably okay, since if you have sevearl data points to one side, you
-				//can probably trust that more.
-				scale3d(avg_err, avg_err, 1./validpoints);
+				// NOTE: The "avg_err" is not geometrically centered.  This is actually
+				// probably okay, since if you have sevearl data points to one side, you
+				// can probably trust that more.
+				scale3d(avg_err, avg_err, 1. / validpoints);
 
-				//We have "Average error" now.  A vector in worldspace.
-				//This can correct for lateral error, but not distance from camera.
+				// We have "Average error" now.  A vector in worldspace.
+				// This can correct for lateral error, but not distance from camera.
 
-				//XXX TODO: Should we check to see if we only have one or
-				//two points to make sure the error on this isn't unusually high?
-				//If calculated error is unexpectedly high, then we should probably
-				//Not apply the transform.
-				scale3d( avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT );
-				add3d( vec_correct, vec_correct, avg_err );
+				// XXX TODO: Should we check to see if we only have one or
+				// two points to make sure the error on this isn't unusually high?
+				// If calculated error is unexpectedly high, then we should probably
+				// Not apply the transform.
+				scale3d(avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT);
+				add3d(vec_correct, vec_correct, avg_err);
 			}
 
-			//Step 3: Control telecoption from lighthouse.
+			// Step 3: Control telecoption from lighthouse.
 			// we need to find out what the weighting is to determine "zoom"
-			if( validpoints > 1 )	//Can't correct "zoom" with only one point.
+			if (validpoints > 1) // Can't correct "zoom" with only one point.
 			{
 				FLT zoom = 0.0;
 				FLT rmsang = 0.0;
-				for( i = 0; i < pts; i++ )
-				{
-					if( !ptvalid[i] ) continue;
+				for (i = 0; i < pts; i++) {
+					if (!ptvalid[i])
+						continue;
 					FLT delang = dd->angles_at_pts[i] - avgang;
 					FLT delerr = dd->quantity_errors[i] - avgerr;
-					if( axis ) delang *= -1;	//Flip sign on alternate axis because it's measured backwards.
+					if (axis)
+						delang *= -1; // Flip sign on alternate axis because it's measured backwards.
 					zoom += delerr * delang;
 					rmsang += delang * delang;
 				}
 
-				//Control into or outof lighthouse.
-				//XXX Check to see if we need to sqrt( the rmsang), need to check convergance behavior close to lighthouse.
-				//This is a questionable step.
- 				zoom /= sqrt(rmsang);
+				// Control into or outof lighthouse.
+				// XXX Check to see if we need to sqrt( the rmsang), need to check convergance behavior close to
+				// lighthouse.
+				// This is a questionable step.
+				zoom /= sqrt(rmsang);
 
 				zoom *= CORRECT_TELESCOPTION_COEFFICIENT;
 
 				FLT veccamalong[3];
-				sub3d( veccamalong, lh_pose->Pos, object_pose->Pos );
-				normalize3d( veccamalong, veccamalong );
-				scale3d( veccamalong, veccamalong, zoom );
-				add3d( vec_correct, veccamalong, vec_correct );
+				sub3d(veccamalong, lh_pose->Pos, object_pose->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);
 
-			quatcopy( object_pose_out.Rot, object_pose->Rot );
+			quatcopy(object_pose_out.Rot, object_pose->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
-			//we would actually be able to perform this on a per-hit basis.
-			if( 1 ) {
+			// 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
+			// we would actually be able to perform this on a per-hit basis.
+			if (1) {
 				LinmathQuat correction;
-				quatcopy( correction, LinmathQuat_Identity );
-				for( i = 0; i < pts; i++ )
-				{
-					if( !ptvalid[i] ) continue;
-					FLT dist = dd->quantity_errors[i]-avgerr;
+				quatcopy(correction, LinmathQuat_Identity);
+				for (i = 0; i < pts; i++) {
+					if (!ptvalid[i])
+						continue;
+					FLT dist = dd->quantity_errors[i] - avgerr;
 					FLT angle = dd->angles_at_pts[i];
 					int sensor_id = dd->sensor_ids[i];
-					FLT * normal =  dd->normal_at_errors[i];
-					const SurvivePose * object_pose_at_hit = &dd->object_pose_at_hit[i];
-					const FLT * sensor_inpos = &so->sensor_locations[sensor_id*3];
+					FLT *normal = dd->normal_at_errors[i];
+					const SurvivePose *object_pose_at_hit = &dd->object_pose_at_hit[i];
+					const FLT *sensor_inpos = &so->sensor_locations[sensor_id * 3];
 
 					LinmathQuat world_to_object_space;
 					quatgetreciprocal(world_to_object_space, object_pose_at_hit->Rot);
-					FLT correction_in_object_space[3];  //The amount across the surface of the object the rotation should happen.
+					FLT correction_in_object_space[3]; // The amount across the surface of the object the rotation
+													   // should happen.
 
-					quatrotatevector(correction_in_object_space, world_to_object_space, normal );
+					quatrotatevector(correction_in_object_space, world_to_object_space, normal);
 					dist *= CORRECT_ROTATION_COEFFICIENT;
-					if( dist > ROTATIONAL_CORRECTION_MAXFORCE ) dist = ROTATIONAL_CORRECTION_MAXFORCE;
-					if( dist <-ROTATIONAL_CORRECTION_MAXFORCE ) dist =-ROTATIONAL_CORRECTION_MAXFORCE;
+					if (dist > ROTATIONAL_CORRECTION_MAXFORCE)
+						dist = ROTATIONAL_CORRECTION_MAXFORCE;
+					if (dist < -ROTATIONAL_CORRECTION_MAXFORCE)
+						dist = -ROTATIONAL_CORRECTION_MAXFORCE;
 
-					//Now, we have a "tug" vector in object-local space.  Need to apply the torque.
+					// Now, we have a "tug" vector in object-local space.  Need to apply the torque.
 					FLT vector_from_center_of_object[3];
-					normalize3d( vector_from_center_of_object, sensor_inpos );
-					//scale3d(vector_from_center_of_object, sensor_inpos, 10.0 );
-		//			vector_from_center_of_object[2]*=-1;
-	//				vector_from_center_of_object[1]*=-1;
-//					vector_from_center_of_object[0]*=-1;
-					//vector_from_center_of_object
-					scale3d(vector_from_center_of_object,vector_from_center_of_object, 1);
-
+					normalize3d(vector_from_center_of_object, sensor_inpos);
+					// scale3d(vector_from_center_of_object, sensor_inpos, 10.0 );
+					//			vector_from_center_of_object[2]*=-1;
+					//				vector_from_center_of_object[1]*=-1;
+					//					vector_from_center_of_object[0]*=-1;
+					// vector_from_center_of_object
+					scale3d(vector_from_center_of_object, vector_from_center_of_object, 1);
 
 					FLT new_vector_in_object_space[3];
-					//printf( "%f %f %f %f\n", object_pose_at_hit->Rot[0], object_pose_at_hit->Rot[1], object_pose_at_hit->Rot[2], object_pose_at_hit->Rot[3] );
-					//printf( "%f %f %f  // %f %f %f // %f\n", vector_from_center_of_object[0], vector_from_center_of_object[1], vector_from_center_of_object[2], correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], dist );
-					scale3d( correction_in_object_space, correction_in_object_space, -dist );
-					add3d( new_vector_in_object_space, vector_from_center_of_object, correction_in_object_space );
+					// printf( "%f %f %f %f\n", object_pose_at_hit->Rot[0], object_pose_at_hit->Rot[1],
+					// object_pose_at_hit->Rot[2], object_pose_at_hit->Rot[3] );
+					// printf( "%f %f %f  // %f %f %f // %f\n", vector_from_center_of_object[0],
+					// vector_from_center_of_object[1], vector_from_center_of_object[2], correction_in_object_space[0],
+					// correction_in_object_space[1], correction_in_object_space[2], dist );
+					scale3d(correction_in_object_space, correction_in_object_space, -dist);
+					add3d(new_vector_in_object_space, vector_from_center_of_object, correction_in_object_space);
 
-					normalize3d( new_vector_in_object_space, new_vector_in_object_space );
+					normalize3d(new_vector_in_object_space, new_vector_in_object_space);
 
 					LinmathQuat corrective_quaternion;
-					quatfrom2vectors(corrective_quaternion, vector_from_center_of_object, new_vector_in_object_space );
-					quatrotateabout( correction, correction, corrective_quaternion );
-					//printf( "%f -> %f %f %f => %f %f %f [%f %f %f %f]\n", dist, vector_from_center_of_object[0], vector_from_center_of_object[1], vector_from_center_of_object[2],
-						//correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2],
-						//corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]);
+					quatfrom2vectors(corrective_quaternion, vector_from_center_of_object, new_vector_in_object_space);
+					quatrotateabout(correction, correction, corrective_quaternion);
+					// printf( "%f -> %f %f %f => %f %f %f [%f %f %f %f]\n", dist, vector_from_center_of_object[0],
+					// vector_from_center_of_object[1], vector_from_center_of_object[2],
+					// correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2],
+					// corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]);
 				}
-				//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 );
+				// 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 ) );
+			// 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;
 			}
 
@@ -332,11 +329,11 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
 		}
 
 		dd->sweepaxis = l->acode & 1;
-		//printf( "SYNC %d %p\n", l->acode, dd );
+		// printf( "SYNC %d %p\n", l->acode, dd );
 		break;
 	}
 	case POSERDATA_FULL_SCENE: {
-		//return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd));
+		// return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd));
 	}
 	}
 	return -1;
diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c
index f725334..5cac973 100644
--- a/src/poser_charlesslow.c
+++ b/src/poser_charlesslow.c
@@ -256,7 +256,7 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr
 			int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
 			if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
 
-			FLT out[2] = { 0 };
+			FLT out[2] = {0};
 			survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out);
 
 			//Find out where our ray shoots forth from.
@@ -343,7 +343,7 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr
 		if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
 
 		//Find out where our ray shoots forth from.
-		FLT out[2] = { 0 };
+		FLT out[2] = {0};
 		survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out);
 
 		// Find out where our ray shoots forth from.
diff --git a/src/poser_imu.c b/src/poser_imu.c
new file mode 100644
index 0000000..7615170
--- /dev/null
+++ b/src/poser_imu.c
@@ -0,0 +1,33 @@
+#include <survive.h>
+#include <survive_imu.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+
+int PoserIMU(SurviveObject *so, PoserData *pd) {
+	PoserType pt = pd->pt;
+	SurviveContext *ctx = so->ctx;
+	SurviveIMUTracker *dd = so->PoserData;
+
+	if (!dd) {
+		so->PoserData = dd = malloc(sizeof(SurviveIMUTracker));
+		*dd = (SurviveIMUTracker){};
+	}
+
+	switch (pt) {
+	case POSERDATA_IMU: {
+		PoserDataIMU *imu = (PoserDataIMU *)pd;
+
+		survive_imu_tracker_integrate(so, dd, imu);
+
+		PoserData_poser_pose_func(pd, so, &dd->pose);
+
+		// if(magnitude3d(dd->pose.Pos) > 1)
+		// SV_ERROR("IMU drift");
+		return 0;
+	}
+	}
+	return -1;
+}
+
+REGISTER_LINKTIME(PoserIMU);
diff --git a/src/poser_sba.c b/src/poser_sba.c
index e74bb20..f0d5645 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -3,11 +3,12 @@
 #define USE_DOUBLE
 #endif
 
-#include <sba/sba.h>
 #include <malloc.h>
+#include <sba/sba.h>
 
 #include "poser.h"
 #include <survive.h>
+#include <survive_imu.h>
 
 #include "assert.h"
 #include "linmath.h"
@@ -47,6 +48,9 @@ typedef struct SBAData {
 
 	int required_meas;
 
+	SurviveIMUTracker tracker;
+	bool useIMU;
+
 	SurviveObject *so;
 } SBAData;
 
@@ -268,12 +272,12 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 								info);			// info
 
 	if (currentPositionValid) {
-		FLT distp[3];
-		sub3d(distp, so->OutPose.Pos, soLocation.Pos);
-		FLT distance = magnitude3d(distp);
-		;
-		if (distance > 1.)
-			status = -1;
+		// FLT distp[3];
+		// sub3d(distp, so->OutPose.Pos, soLocation.Pos);
+		// FLT distance = magnitude3d(distp);
+
+		// if (distance > 1.)
+		//	status = -1;
 	}
 	if (status > 0 && (info[1] / meas_size * 2) < d->max_error) {
 		d->failures_to_reset_cntr = d->failures_to_reset;
@@ -292,7 +296,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 		}
 	}
 
-	return info[1] / meas_size * 2;
+	return status; // info[1] / meas_size * 2;
 }
 
 // Optimizes for LH position assuming object is posed at 0
@@ -392,12 +396,12 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 		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);
-
+		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, 0.001);
+		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;
 
@@ -407,6 +411,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 		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-use-imu: %d", d->useIMU);
 	}
 	SBAData *d = so->PoserData;
 	switch (pd->pt) {
@@ -421,6 +426,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 		FLT error = -1;
 		if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) {
 			error = run_sba_find_3d_structure(d, lightData, scene, 100, .5);
+
 			d->last_lh = lightData->lh;
 			d->last_acode = lightData->acode;
 		}
@@ -429,6 +435,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 			if (d->failures_to_reset_cntr > 0)
 				d->failures_to_reset_cntr--;
 		} else {
+			if (d->useIMU) {
+				survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose);
+			}
 			if (d->successes_to_reset_cntr > 0)
 				d->successes_to_reset_cntr--;
 		}
@@ -442,6 +451,15 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 		// std::cerr << "Average reproj error: " << error << std::endl;
 		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);
+	  }
+	} // INTENTIONAL FALLTHROUGH
 	default: {
 		const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP");
 		PoserCB driver = (PoserCB)GetDriver(subposer);
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 243051e..b2eed7f 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1631,8 +1631,7 @@ static void QuickPose(SurviveObject *so, PoserData *pd, SurvivePose *additionalT
 			SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 0, additionalTx, lh, 0);
 
 			//printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
-			if (so->ctx->poseproc)
-			{
+			if (so->ctx->poseproc) {
 				so->ctx->poseproc(so, lh, &pose);
 			}
 
diff --git a/src/survive.c b/src/survive.c
index f6f98fc..b024bbb 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -86,9 +86,8 @@ void survive_verify_FLT_size(uint32_t user_size) {
 	if (sizeof(FLT) != user_size) {
 		fprintf(stderr, "FLT type incompatible; the shared library libsurvive has FLT size %lu vs user program %u\n",
 				(unsigned long)sizeof(FLT), user_size);
-		fprintf(stderr,
-				"Add '#define FLT %s' before including survive.h or recompile the shared library with the "
-				"appropriate flag. \n",
+		fprintf(stderr, "Add '#define FLT %s' before including survive.h or recompile the shared library with the "
+						"appropriate flag. \n",
 				sizeof(FLT) == sizeof(double) ? "double" : "float");
 		exit(-1);
 	}
@@ -559,11 +558,8 @@ int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int in
 	return len;
 }
 
-
-
 #endif
 
-
 const char *survive_object_codename(SurviveObject *so) { return so->codename; }
 
 const char *survive_object_drivername(SurviveObject *so) { return so->drivername; }
diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c
index 6b65752..2e47b9e 100644
--- a/src/survive_default_devices.c
+++ b/src/survive_default_devices.c
@@ -144,11 +144,10 @@ 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;
-				so->acc_bias[0] *= .125; // XXX Wat?  Observed by CNL.  Biasing
-										 // by more than this seems to hose
-										 // things.
-				so->acc_bias[1] *= .125;
-				so->acc_bias[2] *= .125;
+				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) {
diff --git a/src/survive_disambiguator.c b/src/survive_disambiguator.c
index 6c19475..73e61a8 100644
--- a/src/survive_disambiguator.c
+++ b/src/survive_disambiguator.c
@@ -1,19 +1,21 @@
 #include "survive.h"
 
+#include "survive_playback.h"
 #include <os_generic.h>
 #include <stdio.h>
-#include "survive_playback.h"
 
 //#define LOG_LIGHTDATA
 
-void handle_lightcap(SurviveObject *so, LightcapElement *le)
-{
-  survive_recording_lightcap(so, le);
+void handle_lightcap(SurviveObject *so, LightcapElement *le) {
+	survive_recording_lightcap(so, le);
 #ifdef LOG_LIGHTDATA
-	static FILE * flog;
+	static FILE *flog;
 	static double start = 0;
-	if( !flog ) { flog = fopen( "lightcap.txt", "wb" ); start = OGGetAbsoluteTime(); }
-	fprintf( flog, "%.6f %2d %4d %9d\n", OGGetAbsoluteTime()-start, le->sensor_id, le->length, le->timestamp );
+	if (!flog) {
+		flog = fopen("lightcap.txt", "wb");
+		start = OGGetAbsoluteTime();
+	}
+	fprintf(flog, "%.6f %2d %4d %9d\n", OGGetAbsoluteTime() - start, le->sensor_id, le->length, le->timestamp);
 #endif
 	so->ctx->lightcapfunction(so, le);
 }
diff --git a/src/survive_imu.c b/src/survive_imu.c
new file mode 100644
index 0000000..e49da3e
--- /dev/null
+++ b/src/survive_imu.c
@@ -0,0 +1,220 @@
+#include "survive_imu.h"
+#include "linmath.h"
+#include "survive_internal.h"
+#include <survive_imu.h>
+
+//---------------------------------------------------------------------------------------------------
+// Definitions
+
+#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
+
+//---------------------------------------------------------------------------------------------------
+// Function declarations
+
+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;
+		}
+
+		// Apply proportional feedback
+		gx += twoKp * halfex;
+		gy += twoKp * halfey;
+		gz += twoKp * halfez;
+	}
+
+	// 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;
+}
+
+static inline uint32_t tick_difference(uint32_t most_recent, uint32_t least_recent) {
+	uint32_t diff = 0;
+	if (most_recent > least_recent) {
+		diff = most_recent - least_recent;
+	} else {
+		diff = least_recent - most_recent;
+	}
+
+	if (diff > 0xFFFFFFFF / 2)
+		return 0x7FFFFFFF / 2 - diff;
+	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->integralFBx = tracker->integralFBy = tracker->integralFBz = 0.0;
+	tracker->lastGTTime = timecode;
+	tracker->lastGT = *pose;
+}
+
+static const int imu_calibration_iterations = 100;
+
+static void RotateAccel(LinmathVec3d rAcc, const SurvivePose *pose, const LinmathVec3d accel) {
+	quatrotatevector(rAcc, pose->Rot, accel);
+	LinmathVec3d G = {0, 0, -1};
+	add3d(rAcc, rAcc, G);
+	scale3d(rAcc, rAcc, 9.8066);
+	FLT m = magnitude3d(rAcc);
+}
+static void iterate_position(SurviveIMUTracker *tracker, double time_diff, const PoserDataIMU *pIMU, FLT *out) {
+	const SurvivePose *pose = &tracker->pose;
+	const FLT *vel = tracker->current_velocity;
+
+	for (int i = 0; i < 3; i++)
+		out[i] = pose->Pos[i];
+
+	FLT acc_mul = time_diff * time_diff / 2;
+
+	LinmathVec3d acc;
+	scale3d(acc, pIMU->accel, tracker->accel_scale_bias);
+	LinmathVec3d rAcc = {0};
+	RotateAccel(rAcc, pose, acc);
+	scale3d(rAcc, rAcc, acc_mul);
+
+	for (int i = 0; i < 3; i++) {
+		out[i] += time_diff * vel[i] + rAcc[i];
+	}
+}
+
+static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, double time_diff, PoserDataIMU *pIMU) {
+	const SurvivePose *pose = &tracker->pose;
+	const FLT *vel = tracker->current_velocity;
+	scale3d(result, vel, 1.);
+
+	LinmathVec3d acc;
+	scale3d(acc, pIMU->accel, tracker->accel_scale_bias);
+
+	LinmathVec3d rAcc = {0};
+	RotateAccel(rAcc, pose, acc);
+	scale3d(rAcc, rAcc, time_diff);
+	add3d(result, result, rAcc);
+}
+
+void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) {
+	if (tracker->last_data.timecode == 0) {
+
+		if (tracker->last_data.datamask == imu_calibration_iterations) {
+			tracker->last_data = *data;
+			tracker->pose.Rot[0] = 1.;
+
+			const FLT up[3] = {0, 0, 1};
+			quatfrom2vectors(tracker->pose.Rot, tracker->updir, up);
+			tracker->accel_scale_bias = 1. / magnitude3d(tracker->updir);
+			return;
+		}
+
+		tracker->last_data.datamask++;
+
+		tracker->updir[0] += data->accel[0] / imu_calibration_iterations;
+		tracker->updir[1] += data->accel[1] / imu_calibration_iterations;
+		tracker->updir[2] += data->accel[2] / imu_calibration_iterations;
+		return;
+	}
+
+	for (int i = 0; i < 3; i++) {
+		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);
+
+	FLT time_diff = tick_difference(data->timecode, tracker->last_data.timecode) / (FLT)so->timebase_hz;
+
+	if (tick_difference(data->timecode, tracker->lastGTTime) < 3200000 * 3) {
+		FLT next[3];
+		iterate_position(tracker, time_diff, data, next);
+
+		LinmathVec3d v_next;
+		iterate_velocity(v_next, tracker, time_diff, data);
+
+		scale3d(tracker->current_velocity, v_next, 1);
+		scale3d(tracker->pose.Pos, next, 1);
+	}
+
+	tracker->last_data = *data;
+}
diff --git a/src/survive_playback.c b/src/survive_playback.c
index 6789a66..d5d1c08 100644
--- a/src/survive_playback.c
+++ b/src/survive_playback.c
@@ -17,8 +17,8 @@
 typedef long ssize_t;
 #define SSIZE_MAX LONG_MAX
 
-ssize_t getdelim(char ** lineptr, size_t * n, int delimiter, FILE *stream);
-ssize_t getline(char **lineptr, size_t * n, FILE *stream);
+ssize_t getdelim(char **lineptr, size_t *n, int delimiter, FILE *stream);
+ssize_t getline(char **lineptr, size_t *n, FILE *stream);
 #endif
 
 typedef struct SurviveRecordingData {
diff --git a/src/survive_process.c b/src/survive_process.c
index e013327..abde02d 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -49,20 +49,20 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
 
 	//Need to now do angle correction.
 	static int use_bsd_cal = -1;
-	if(use_bsd_cal == -1) {
-	  use_bsd_cal = survive_configi(ctx, "use-bsd-cal", SC_GET, 1);
-	  if (use_bsd_cal == 0) {
-		  SV_INFO("Not using BSD calibration values");
-	  }
+	if (use_bsd_cal == -1) {
+		use_bsd_cal = survive_configi(ctx, "use-bsd-cal", SC_GET, 1);
+		if (use_bsd_cal == 0) {
+			SV_INFO("Not using BSD calibration values");
+		}
 	}
-	if(use_bsd_cal) {
-	  BaseStationData * bsd = &ctx->bsd[base_station];
+	if (use_bsd_cal) {
+		BaseStationData *bsd = &ctx->bsd[base_station];
 
-	  // XXX TODO: This seriously needs to be worked on.  See: https://github.com/cnlohr/libsurvive/issues/18
-	  // angle += (use_bsd_cal == 2 ? -1 : 1) * bsd->fcal.phase[axis];
-	  //	angle += bsd->fcaltilt[axis] * predicted_angle(axis1);
+		// XXX TODO: This seriously needs to be worked on.  See: https://github.com/cnlohr/libsurvive/issues/18
+		// angle += (use_bsd_cal == 2 ? -1 : 1) * bsd->fcal.phase[axis];
+		//	angle += bsd->fcaltilt[axis] * predicted_angle(axis1);
 
-	  //TODO!!!
+		// TODO!!!
 	}
 
 	FLT length_sec = length / (FLT)so->timebase_hz;
diff --git a/src/survive_reproject.c b/src/survive_reproject.c
index 0eaceb2..d6ba70b 100644
--- a/src/survive_reproject.c
+++ b/src/survive_reproject.c
@@ -43,7 +43,6 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv
 		if (f & SVCal_Gib)
 			out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis];
 	}
-
 }
 
 void survive_apply_bsd_calibration_by_config(SurviveContext *ctx, int lh, struct survive_calibration_config *config,
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 3c60b2a..1ffb737 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -1485,26 +1485,33 @@ void survive_data_cb( SurviveUSBInterface * si )
 				obj->oldcode = code;
 
 				//XXX XXX BIG TODO!!! Actually recal gyro data.
-				FLT agm[9] = { acceldata[0].v, acceldata[1].v, acceldata[2].v,
-								acceldata[3].v, acceldata[4].v, acceldata[5].v,
-								0,0,0 };
-
-				agm[0]*=(float)(1./8192.0);
-				agm[1]*=(float)(1./8192.0);
-				agm[2]*=(float)(1./8192.0);
-				calibrate_acc(obj, agm);
+				FLT agm[9] = {acceldata[0].v,
+							  acceldata[1].v,
+							  acceldata[2].v,
+							  acceldata[3].v,
+							  acceldata[4].v,
+							  acceldata[5].v,
+							  0,
+							  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);
 
-
-				agm[3]*=(float)((1./32.768)*(3.14159/180.));
-				agm[4]*=(float)((1./32.768)*(3.14159/180.));
-				agm[5]*=(float)((1./32.768)*(3.14159/180.));
-				calibrate_gyro(obj, agm+3);
-
-				//65.5 deg/s for gyroscope, from MPU6500 datasheet
-				//1000 deg/s for gyroscope, from MPU6500 datasheet
+				// 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 );
 			}
diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js
index d7f80ad..c8a7b23 100644
--- a/tools/viz/survive_viewer.js
+++ b/tools/viz/survive_viewer.js
@@ -164,8 +164,8 @@ function create_tracked_object(info) {
 	group.sensors = [];
 	if (info.config && info.config.lighthouse_config) {
 		for (var idx in info.config.lighthouse_config.modelPoints) {
-            var p = info.config.lighthouse_config.modelPoints[idx];
-            var pn = info.config.lighthouse_config.modelNormals[idx];
+			var p = info.config.lighthouse_config.modelPoints[idx];
+			var pn = info.config.lighthouse_config.modelNormals[idx];
 			var color = idx / info.config.lighthouse_config.modelPoints * 0xFFFFFF;
 			if (idx === 0)
 				color = 0x00ff00;
@@ -173,10 +173,11 @@ function create_tracked_object(info) {
 			var newSensor = new THREE.Mesh(sensorGeometry, sensorMaterial);
 			newSensor.position.set(p[0], p[1], p[2]);
 
-            var normalGeom = new THREE.Geometry();
-            normalGeom.vertices.push(newSensor.position,
-				new THREE.Vector3(p[0] + pn[0] * .02, p[1] + pn[1] * .02, p[2] + pn[2] * .02));
-            var normal= new THREE.Line(normalGeom, new THREE.LineBasicMaterial({color : idx == 4 ? 0xFF0000 : 0x00FF00}));
+			var normalGeom = new THREE.Geometry();
+			normalGeom.vertices.push(newSensor.position,
+									 new THREE.Vector3(p[0] + pn[0] * .02, p[1] + pn[1] * .02, p[2] + pn[2] * .02));
+			var normal =
+				new THREE.Line(normalGeom, new THREE.LineBasicMaterial({color : idx == 4 ? 0xFF0000 : 0x00FF00}));
 			group.add(normal);
 			group.sensors[idx] = sensorMaterial;
 			group.add(newSensor);
@@ -299,7 +300,7 @@ var survive_log_handlers = {
 				downAxes[obj.tracker] = new THREE.Geometry();
 				downAxes[obj.tracker].vertices.push(new THREE.Vector3(0, 0, 0), new THREE.Vector3(0, 0, 0));
 
-                var line = new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff}));
+				var line = new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff}));
 				scene.add(line);
 				}
 
diff --git a/winbuild/getdelim.c b/winbuild/getdelim.c
index ca808ab..69020ee 100644
--- a/winbuild/getdelim.c
+++ b/winbuild/getdelim.c
@@ -30,40 +30,29 @@
    THE SOFTWARE.
 */
 
-
 #include <errno.h>
 #include <limits.h>
-#include <stdlib.h>
 #include <stdio.h>
-
+#include <stdlib.h>
 
 #if __STDC_VERSION__ >= 199901L
 /* restrict is a keyword */
 #else
-# define restrict
+#define restrict
 #endif
 
-
 #ifndef _POSIX_SOURCE
 typedef long ssize_t;
 #define SSIZE_MAX LONG_MAX
 #endif
 
+ssize_t getdelim(char **restrict lineptr, size_t *restrict n, int delimiter, FILE *restrict stream);
+ssize_t getline(char **restrict lineptr, size_t *restrict n, FILE *restrict stream);
 
-ssize_t getdelim(char **restrict lineptr, size_t *restrict n, int delimiter,
-                 FILE *restrict stream);
-ssize_t getline(char **restrict lineptr, size_t *restrict n,
-                FILE *restrict stream);
-
+#define _GETDELIM_GROWBY 128 /* amount to grow line buffer by */
+#define _GETDELIM_MINLEN 4   /* minimum line buffer size */
 
-
-#define _GETDELIM_GROWBY 128    /* amount to grow line buffer by */
-#define _GETDELIM_MINLEN 4      /* minimum line buffer size */
-
-
-ssize_t getdelim(char **restrict lineptr, size_t *restrict n, int delimiter,
-                 FILE *restrict stream)
-{
+ssize_t getdelim(char **restrict lineptr, size_t *restrict n, int delimiter, FILE *restrict stream) {
 	char *buf, *pos;
 	int c;
 	ssize_t bytes;
@@ -110,7 +99,7 @@ ssize_t getdelim(char **restrict lineptr, size_t *restrict n, int delimiter,
 			*lineptr = buf;
 		}
 
-		*pos++ = (char) c;
+		*pos++ = (char)c;
 		if (c == delimiter) {
 			break;
 		}
@@ -125,19 +114,14 @@ ssize_t getdelim(char **restrict lineptr, size_t *restrict n, int delimiter,
 	return bytes;
 }
 
-
-ssize_t getline(char **restrict lineptr, size_t *restrict n,
-                FILE *restrict stream)
-{
+ssize_t getline(char **restrict lineptr, size_t *restrict n, FILE *restrict stream) {
 	return getdelim(lineptr, n, '\n', stream);
 }
 
-
 #ifdef _TEST_GETDELIM
 
 /* TODO: this isn't a very extensive test. */
-int main(void)
-{
+int main(void) {
 	char *line = NULL;
 	size_t n = 0;
 	while (getline(&line, &n, stdin) > 0) {