Slightly smarter kalman only bootstrap
@@ -26,12 +26,15 @@ int PoserKalmanOnly(SurviveObject *so, void **user, PoserData *pd) {
if (so->tracker->model.t == 0) {
so->tracker->model.t = imu->hdr.timecode / (FLT)so->timebase_hz;
+ }
- SurvivePose pose;
+ while (so->tracker->stats.obs_count < 30) {
+ SurvivePose pose = {0};
LinmathVec3d up = {0, 0, 1};
- quatfrom2vectors(pose.Rot, imu->accel, up);
+ LinmathPoint3d accelNormal;
+ normalize3d(accelNormal, imu->accel);
+ quatfrom2vectors(pose.Rot, accelNormal, up);
- so->tracker->stats.obs_count = 30;
survive_kalman_tracker_integrate_observation(&imu->hdr, so->tracker, &pose, 0);
return 0;
}
