Cleaned up debug code

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);