diff options
Diffstat (limited to 'src/survive_kalman.h')
-rw-r--r-- | src/survive_kalman.h | 33 |
1 files changed, 33 insertions, 0 deletions
diff --git a/src/survive_kalman.h b/src/survive_kalman.h new file mode 100644 index 0000000..405b369 --- /dev/null +++ b/src/survive_kalman.h @@ -0,0 +1,33 @@ + +#include <linmath.h> +#include <survive_types.h> + +typedef struct { + SurvivePose pose; + LinmathVec3d velocity; +} survive_kalman_pose_state_t ; + +typedef struct { + LinmathVec3d pos_variance; + FLT rot_variance; + LinmathVec3d vel_variance; +} survive_kalman_pose_var_t ; + +typedef struct { + survive_kalman_pose_state_t state; + survive_kalman_pose_var_t variance; + + survive_timecode timecode; + survive_kalman_pose_var_t process_variance_per_second; +} survive_kpose_t; + + +void survive_kpose_predict(const survive_kpose_t* kpose, survive_timecode when, SurvivePose* out); + +void survive_kpose_integrate_pose(survive_kpose_t* kpose, survive_timecode timecode, + const SurvivePose* pose, + const LinmathVec3d pos_variance, FLT rot_variance); + +void survive_kpose_integrate_pose_velocity(survive_kpose_t* kpose, survive_timecode timecode, + const SurvivePose* pose, const LinmathVec3d vel, + const LinmathVec3d pos_variance, FLT rot_variance, const LinmathVec3d vel_var); |