diff options
author | Justin Berger <j.david.berger@gmail.com> | 2018-04-07 07:35:41 -0600 |
---|---|---|
committer | Justin Berger <j.david.berger@gmail.com> | 2018-04-07 07:35:41 -0600 |
commit | 9dc7a7ed589db16cc040c7eeeae0343977f3f885 (patch) | |
tree | 23fe619d170101ce84c402d0d6796b6eda892c53 | |
parent | 55f498db296ff353a0cf870c51bffd92cc360484 (diff) | |
download | libsurvive-9dc7a7ed589db16cc040c7eeeae0343977f3f885.tar.gz libsurvive-9dc7a7ed589db16cc040c7eeeae0343977f3f885.tar.bz2 |
Added proper initialization to imu tracker
-rw-r--r-- | include/libsurvive/survive_imu.h | 3 | ||||
-rw-r--r-- | src/survive_imu.c | 9 |
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 |