From 48f791f98d9eda5948de4ef6930d0530b7ca633e Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 4 Apr 2018 21:07:02 -0600 Subject: Added kalman to imu --- include/libsurvive/survive_imu.h | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'include/libsurvive') diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h index 124ad7e..8a86425 100644 --- a/include/libsurvive/survive_imu.h +++ b/include/libsurvive/survive_imu.h @@ -22,12 +22,17 @@ typedef struct { SurvivePose lastGT; uint32_t lastGTTime; + FLT P[7]; // estimate variance + float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki } SurviveIMUTracker; void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose); + void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data); +void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timecode, SurviveIMUTracker *tracker, + SurvivePose *pose, const FLT *variance); #ifdef __cplusplus }; -- cgit v1.2.3