Added kalman to imu

2 files changed, 69 insertions, 7 deletions

diff --git a/src/poser_sba.c b/src/poser_sba.c
index f0d5645..d106d6c 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -186,7 +186,8 @@ static void str_metric_function(int j, int i, double *bi, double *xij, void *ada
 }
 
 static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene,
-										int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) {
+										int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/,
+										SurvivePose *out) {
 	double *covx = 0;
 	SurviveObject *so = d->so;
 
@@ -279,10 +280,13 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 		// if (distance > 1.)
 		//	status = -1;
 	}
+
+	double rtn = -1;
 	if (status > 0 && (info[1] / meas_size * 2) < d->max_error) {
 		d->failures_to_reset_cntr = d->failures_to_reset;
 		quatnormalize(soLocation.Rot, soLocation.Rot);
-		PoserData_poser_pose_func(&pdl->hdr, so, &soLocation);
+		*out = soLocation;
+		rtn = info[1] / meas_size * 2;
 	}
 
 	{
@@ -290,13 +294,13 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 		// 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));
+			// 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;
 		}
 	}
 
-	return status; // info[1] / meas_size * 2;
+	return rtn;
 }
 
 // Optimizes for LH position assuming object is posed at 0
@@ -421,11 +425,12 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 			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_sba_find_3d_structure(d, lightData, scene, 100, .5);
+			error = run_sba_find_3d_structure(d, lightData, scene, 100, .5, &estimate);
 
 			d->last_lh = lightData->lh;
 			d->last_acode = lightData->acode;
@@ -436,8 +441,16 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 				d->failures_to_reset_cntr--;
 		} else {
 			if (d->useIMU) {
-				survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose);
+				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);
 			if (d->successes_to_reset_cntr > 0)
 				d->successes_to_reset_cntr--;
 		}
diff --git a/src/survive_imu.c b/src/survive_imu.c
index e49da3e..205e2c2 100644
--- a/src/survive_imu.c
+++ b/src/survive_imu.c
@@ -183,6 +183,7 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker
 			const FLT up[3] = {0, 0, 1};
 			quatfrom2vectors(tracker->pose.Rot, tracker->updir, up);
 			tracker->accel_scale_bias = 1. / magnitude3d(tracker->updir);
+
 			return;
 		}
 
@@ -216,5 +217,53 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker
 		scale3d(tracker->pose.Pos, next, 1);
 	}
 
+	FLT var_meters = .000001;
+	FLT var_quat = .05;
+	const FLT Q[7] = {var_meters, var_meters, var_meters, var_quat, var_quat, var_quat, var_quat};
+
+	// Note that this implementation is somewhat truncated. Instead of modeling velocity and velocities
+	// covariance with position explicitly, we just square the variance for the position indexes. This
+	// gives more or less the same calculation without having to do matrix multiplication.
+	for (int i = 0; i < 3; i++)
+		tracker->P[i] = tracker->P[i] * tracker->P[i] + Q[i];
+	for (int i = 3; i < 7; i++)
+		tracker->P[i] += Q[i];
+
 	tracker->last_data = *data;
 }
+
+void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timecode, SurviveIMUTracker *tracker,
+											   SurvivePose *pose, const FLT *R) {
+	// Kalman filter assuming:
+	// F -> Identity
+	// H -> Identity
+	// Q / R / P -> Diagonal matrices; just treat them as such. This assumption might need some checking but it
+	// makes the # of calculations needed much smaller so we may be willing to tolerate some approximation here
+
+	FLT *xhat = &tracker->pose.Pos[0];
+	FLT *zk = &pose->Pos[0];
+
+	FLT yk[7];
+	for (int i = 0; i < 7; i++)
+		yk[i] = zk[i] - xhat[i];
+
+	FLT sk[7];
+	for (int i = 0; i < 7; i++)
+		sk[i] = R[i] + tracker->P[i];
+
+	FLT K[7];
+	for (int i = 0; i < 7; i++)
+		K[i] = tracker->P[i] / sk[i];
+
+	for (int i = 0; i < 7; i++)
+		xhat[i] += K[i] * yk[i];
+	for (int i = 0; i < 7; i++)
+		tracker->P[i] *= (1. - K[i]);
+
+	FLT time_diff = tick_difference(timecode, tracker->lastGTTime) / (FLT)so->timebase_hz;
+	for (int i = 0; i < 3; i++)
+		tracker->current_velocity[i] = 0.5 * (tracker->pose.Pos[i] - tracker->lastGT.Pos[i]) / time_diff;
+
+	tracker->lastGTTime = timecode;
+	tracker->lastGT = tracker->pose;
+}
\ No newline at end of file