Added proper initialization to imu tracker

2 files changed, 10 insertions, 2 deletions

diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h
index 8a86425..11635aa 100644
--- a/include/libsurvive/survive_imu.h
+++ b/include/libsurvive/survive_imu.h
@@ -4,6 +4,7 @@
 #include "poser.h"
 #include "survive_types.h"
 #include <stdint.h>
+#include <stdbool.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -12,6 +13,8 @@ extern "C" {
 struct SurviveIMUTracker_p;
 
 typedef struct {
+	bool is_initialized;
+
 	FLT updir[3];
 	FLT accel_scale_bias;
 
diff --git a/src/survive_imu.c b/src/survive_imu.c
index 36d1aeb..8f4266a 100644
--- a/src/survive_imu.c
+++ b/src/survive_imu.c
@@ -174,7 +174,7 @@ static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, do
 }
 
 void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) {
-	if (tracker->last_data.timecode == 0) {
+	if (!tracker->is_initialized) {
 		tracker->pose.Rot[0] = 1.;
 		if (tracker->last_data.datamask == imu_calibration_iterations) {
 			tracker->last_data = *data;
@@ -182,7 +182,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);
-
+			tracker->is_initialized = true;
 			return;
 		}
 
@@ -233,6 +233,11 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker
 
 void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timecode, SurviveIMUTracker *tracker,
 											   SurvivePose *pose, const FLT *R) {
+	if (!tracker->is_initialized) {
+		tracker->pose = *pose;
+		return;
+	}
+		
 	// Kalman filter assuming:
 	// F -> Identity
 	// H -> Identity