From fe025b0ff6bfb440da7cec8f388fa951910a86f0 Mon Sep 17 00:00:00 2001
From: Justin Berger <j.david.berger@gmail.com>
Date: Tue, 3 Apr 2018 23:40:29 -0600
Subject: Removed high pass filter

---
 include/libsurvive/survive_imu.h |  2 --
 src/poser_sba.c                  |  3 ++-
 src/survive_imu.c                | 25 ++++---------------------
 3 files changed, 6 insertions(+), 24 deletions(-)

diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h
index e7a3d90..124ad7e 100644
--- a/include/libsurvive/survive_imu.h
+++ b/include/libsurvive/survive_imu.h
@@ -16,10 +16,8 @@ typedef struct {
 	FLT accel_scale_bias;
 
 	LinmathVec3d current_velocity;	// Velocity in world frame
-	LinmathVec3d current_velocity_lp; // Velocity in world frame
 	PoserDataIMU last_data;
 	SurvivePose pose;
-	LinmathPoint3d pos_lp;
 
 	SurvivePose lastGT;
 	uint32_t lastGTTime;
diff --git a/src/poser_sba.c b/src/poser_sba.c
index a23eb0f..f6b1131 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -396,7 +396,7 @@ 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 = true;
+		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 =
@@ -411,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) {
diff --git a/src/survive_imu.c b/src/survive_imu.c
index cdf254b..e49da3e 100644
--- a/src/survive_imu.c
+++ b/src/survive_imu.c
@@ -121,8 +121,7 @@ void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode,
 	tracker->pose = *pose;
 
 	for (int i = 0; i < 3; i++) {
-		tracker->current_velocity_lp[i] = tracker->current_velocity[i] = 0;
-		tracker->pos_lp[i] = 0;
+		tracker->current_velocity[i] = 0;
 	}
 	//(pose->Pos[i] - tracker->lastGT.Pos[i]) / tick_difference(timecode, tracker->lastGTTime) * 48000000.;
 
@@ -156,7 +155,7 @@ static void iterate_position(SurviveIMUTracker *tracker, double time_diff, const
 	scale3d(rAcc, rAcc, acc_mul);
 
 	for (int i = 0; i < 3; i++) {
-		out[i] += time_diff * (vel[i] - tracker->current_velocity_lp[i]); // + rAcc[i];
+		out[i] += time_diff * vel[i] + rAcc[i];
 	}
 }
 
@@ -170,7 +169,6 @@ static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, do
 
 	LinmathVec3d rAcc = {0};
 	RotateAccel(rAcc, pose, acc);
-	// fprintf(stderr, "%f\n", time_diff);
 	scale3d(rAcc, rAcc, time_diff);
 	add3d(result, result, rAcc);
 }
@@ -211,26 +209,11 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker
 		FLT next[3];
 		iterate_position(tracker, time_diff, data, next);
 
-		LinmathVec3d v_next, lp_add;
+		LinmathVec3d v_next;
 		iterate_velocity(v_next, tracker, time_diff, data);
 
 		scale3d(tracker->current_velocity, v_next, 1);
-
-		FLT v_alpha = 1;
-		FLT p_alpha = 1;
-		scale3d(tracker->current_velocity_lp, tracker->current_velocity_lp, v_alpha);
-
-		scale3d(lp_add, v_next, 1 - v_alpha);
-		add3d(tracker->current_velocity_lp, tracker->current_velocity_lp, lp_add);
-
-		LinmathPoint3d p_add;
-		scale3d(p_add, next, 1 - p_alpha);
-		scale3d(tracker->pos_lp, tracker->pos_lp, p_alpha);
-		add3d(tracker->pos_lp, tracker->pos_lp, p_add);
-
-		for (int i = 0; i < 3; i++)
-			tracker->pose.Pos[i] = next[i] - tracker->pos_lp[i];
-		// scale3d(tracker->pose.Pos, next, 1);
+		scale3d(tracker->pose.Pos, next, 1);
 	}
 
 	tracker->last_data = *data;
-- 
cgit v1.2.3