blob: 8a86425d0ff547f2d9cb9e9c0e555dcb67420ef8 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
|
#ifndef _SURVIVE_IMU_H
#define _SURVIVE_IMU_H
#include "poser.h"
#include "survive_types.h"
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
struct SurviveIMUTracker_p;
typedef struct {
FLT updir[3];
FLT accel_scale_bias;
LinmathVec3d current_velocity; // Velocity in world frame
PoserDataIMU last_data;
SurvivePose pose;
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
};
#endif
#endif
|
