Added Dave's Kalman Filter. Right now just performs the "Predict" phase.

1 files changed, 29 insertions, 0 deletions

diff --git a/dave/main.c b/dave/main.c
new file mode 100644
index 0000000..ff187aa
--- /dev/null
+++ b/dave/main.c
@@ -0,0 +1,29 @@
+#include <stdio.h>
+#include "kalman_filter.h"
+
+int main()
+{
+    KAL_VEC(xhat_k_km1);    /* OUTPUT: (S)     Predicted state at time 'k' */
+    KAL_MAT(P_k_km1);       /* OUTPUT: (S x S) Predicted covariance at time 'k' */
+    KAL_MAT(P_km1_km1);     /* INPUT:  (S x S) Updated covariance from time 'k-1' */
+    KAL_VEC(xhat_km1_km1);  /* INPUT:  (S)     Updated state from time 'k-1' */
+    KAL_MAT(F_k);           /* INPUT:  (S x S) State transition model */
+    KAL_MAT(B_k);           /* INPUT:  (S x U) Control input model */
+    KAL_VEC(u_k);           /* INPUT:  (U)     Control vector */
+    KAL_MAT(Q_k);           /* INPUT:  (S x S) Covariance of process noise */
+
+    KalmanPredict(
+        xhat_k_km1,    /* OUTPUT: (S)     Predicted state at time 'k' */
+        P_k_km1,       /* OUTPUT: (S x S) Predicted covariance at time 'k' */
+        P_km1_km1,     /* INPUT:  (S x S) Updated covariance from time 'k-1' */
+        xhat_km1_km1,  /* INPUT:  (S)     Updated state from time 'k-1' */
+        F_k,           /* INPUT:  (S x S) State transition model */
+        B_k,           /* INPUT:  (S x U) Control input model */
+        u_k,           /* INPUT:  (U)     Control vector */
+        Q_k,           /* INPUT:  (S x S) Covariance of process noise */
+        36,            /* INPUT:          Number of dimensions in state vector */
+        36);            /* INPUT:          Size of control input vector */
+
+    return 0;
+}
+