From 135904406d50bc2199698c3177307e470151e7f9 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 20 Apr 2018 23:06:50 -0600 Subject: Cleaned up debug code --- src/survive_kalman.h | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 src/survive_kalman.h (limited to 'src/survive_kalman.h') 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 +#include + +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); -- cgit v1.2.3