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

4 files changed, 305 insertions, 0 deletions

diff --git a/dave/Makefile b/dave/Makefile
new file mode 100644
index 0000000..a9e5c65
--- /dev/null
+++ b/dave/Makefile
@@ -0,0 +1,4 @@
+all:
+	gcc -o kalman_filter kalman_filter.c main.c -llapack -lblas -lm
+clean:
+	rm -f kalman_filter
diff --git a/dave/kalman_filter.c b/dave/kalman_filter.c
new file mode 100644
index 0000000..eb8ba53
--- /dev/null
+++ b/dave/kalman_filter.c
@@ -0,0 +1,159 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "kalman_filter.h"
+
+/*
+ * Performs alpha * A * B + beta * C
+ */
+extern void sgemm_(
+        char *TRANSA,  // INPUT: Do we transpose A? 'n' no transpose, 't' transpose
+		char *TRANSB,  // INPUT: Do we transpose B? 'n' no transpose, 't' transpose
+		int *M,        // INPUT: Size parameter 'M'
+		int *N,        // INPUT: Size parameter 'N'
+		int *K,        // INPUT: Size parameter 'K'
+		float *ALPHA,  // INPUT: scaling coefficient for A * B
+		void *A,       // INPUT: (float) Column array 'A' (M by K)
+		int *LDA,      // INPUT: Column stride for 'A'
+		void *B,       // INPUT: (float) Column array 'B' (K by N)
+		int *LDB,      // INPUT: Column stride for 'B'
+		float *BETA,   // INPUT: Scaling factor for 'C'
+		void *C,       // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE
+		int *LDC); 	   // INPUT: Column stride for 'C'
+
+/*
+ * General A X = B solution
+ */
+extern void sgesv_(
+    int *N,            // INPUT: The order of matrix A
+    int *NRHS,         // INPUT: the number of columns of matrix B
+    void *A,           // INPUT/OUTPUT: (float) Column array 
+                       //  entry: (N by N) matrix A
+                       //  exit:  (L and U) from factorization A = P*L*U
+    int *LDA,          // INPUT: Column stride of A
+    int *IPIV,         // OUTPUT: Ineger array (dimension N) the pivot indices of the permutation matrix
+    int *B,            // INPUT/OUTPUT: (float) Column array
+                       //  entry: (N by NRHS) matrix of right hand side matrix 'B'
+                       //  exit:  (if INFO=0) N-NRHS solution matrix 'X'
+    int *LDB,          // INPUT: Column stride of B
+    int *INFO);        // OUTPUT: Did it work?
+                       //   0: success
+                       // < 0: if INFO==-i, the -ith argument had illegal val
+                       // > 0: if INFO== i, U(i,i) is exactly zero, thus the factorization is singular (error).
+
+
+void KalmanPredict(
+    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 */
+    int S,                  /* INPUT:          Number of dimensions in state vector */
+    int U)                  /* INPUT:          Size of control input vector */
+{
+    /* Temporary storage */
+    KAL_MAT(F_k__times__P_km1_km1);
+    #define B_k__times__u_k  xhat_k_km1    /* This vector overwrite the same memory  */
+
+    /* Fortran pass by pointer */
+    float zerof=0.0f;
+    float onef=1.0f;
+    int one=1;
+    int stride=KAL_STRIDE;
+
+    int x,y;
+
+    /* Calculate B_k__times__u_k */
+    if (U == 0) {
+        /* If no control input model, then zero out */
+        memset(B_k__times__u_k, 0, S*sizeof(float));
+    } else {
+
+        /* Otherwise: B_k * u_k */
+        sgemm_(
+            "n",  // INPUT: Do we transpose A? 'n' no transpose, 't' transpose
+    		"n",  // INPUT: Do we transpose B? 'n' no transpose, 't' transpose
+    		&S,               // INPUT: Size parameter 'M'
+    		&one,             // INPUT: Size parameter 'N'
+    		&U,               // INPUT: Size parameter 'K'
+    		&onef,            // INPUT: scaling coefficient for A * B
+    	    B_k,              // INPUT: (float) Column array 'A' (M by K)
+    		&stride,          // INPUT: Column stride for 'A'
+    		u_k,              // INPUT: (float) Column array 'B' (K by N)
+    		&stride,          // INPUT: Column stride for 'B'
+    		&zerof,           // INPUT: Scaling factor for 'C'
+    		B_k__times__u_k,  // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE
+    		&stride); 	      // INPUT: Column stride for 'C'        
+    }
+
+    /* Calculate xhat_k_km1 */
+    sgemm_(
+        "n",  // INPUT: Do we transpose A? 'n' no transpose, 't' transpose
+		"n",  // INPUT: Do we transpose B? 'n' no transpose, 't' transpose
+		&S,               // INPUT: Size parameter 'M'
+		&one,             // INPUT: Size parameter 'N'
+		&S,               // INPUT: Size parameter 'K'
+		&onef,            // INPUT: scaling coefficient for A * B
+	    F_k,              // INPUT: (float) Column array 'A' (M by K)
+		&stride,          // INPUT: Column stride for 'A'
+		xhat_km1_km1,     // INPUT: (float) Column array 'B' (K by N)
+		&stride,          // INPUT: Column stride for 'B'
+		&onef,            // INPUT: Scaling factor for 'C'
+		xhat_k_km1,       // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE
+		&stride); 	      // INPUT: Column stride for 'C'        
+
+    /* Calculate F_k * P_km1_km1*/
+    sgemm_(
+        "n",  // INPUT: Do we transpose A? 'n' no transpose, 't' transpose
+		"n",  // INPUT: Do we transpose B? 'n' no transpose, 't' transpose
+		&S,               // INPUT: Size parameter 'M'
+		&S,               // INPUT: Size parameter 'N'
+		&S,               // INPUT: Size parameter 'K'
+		&onef,            // INPUT: scaling coefficient for A * B
+	    F_k,              // INPUT: (float) Column array 'A' (M by K)
+		&stride,          // INPUT: Column stride for 'A'
+		P_km1_km1,        // INPUT: (float) Column array 'B' (K by N)
+		&stride,          // INPUT: Column stride for 'B'
+		&zerof,           // INPUT: Scaling factor for 'C'
+		F_k__times__P_km1_km1,  // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE
+		&stride); 	      // INPUT: Column stride for 'C'
+
+    /* Calculate P_k_km1 */
+    for (x=0; x<S; x++) {
+        for (y=0; y<S; y++) {
+            P_k_km1[x][y] = Q_k[x][y];
+        }
+    }
+    sgemm_(
+        "n",  // INPUT: Do we transpose A? 'n' no transpose, 't' transpose
+		"t",  // INPUT: Do we transpose B? 'n' no transpose, 't' transpose
+		&S,               // INPUT: Size parameter 'M'
+		&S,               // INPUT: Size parameter 'N'
+		&S,               // INPUT: Size parameter 'K'
+		&onef,            // INPUT: scaling coefficient for A * B
+	    F_k__times__P_km1_km1,  // INPUT: (float) Column array 'A' (M by K)
+		&stride,          // INPUT: Column stride for 'A'
+		F_k,              // INPUT: (float) Column array 'B' (K by N)
+		&stride,          // INPUT: Column stride for 'B'
+		&zerof,           // INPUT: Scaling factor for 'C'
+		P_k_km1,          // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE
+		&stride); 	      // INPUT: Column stride for 'C'
+}
+
+
+void KalmanUpdate(
+    KAL_VEC(xhat_k_k),      /* OUTPUT: Updated state at time 'k' */
+    KAL_MAT(P_k_k),         /* OUTPUT: Updated covariance at time 'k' */
+    KAL_VEC(xhat_k_km1),    /* INPUT:  Predicted state at time 'k' */
+    KAL_MAT(P_k_km1),       /* INPUT:  Predicted covariance at time 'k' */
+    KAL_MAT(H_k),           /* INPUT:  Observational model */
+    KAL_MAT(R_k),           /* INPUT:  Covariance of observational noise */
+    int B,                  /* INPUT:  Number of observations in observation vector */
+    int S)                  /* INPUT:  Number of measurements in the state vector */
+{
+}
+
+
diff --git a/dave/kalman_filter.h b/dave/kalman_filter.h
new file mode 100644
index 0000000..d9f090a
--- /dev/null
+++ b/dave/kalman_filter.h
@@ -0,0 +1,113 @@
+#ifndef __KALMAN_FILTER_H__
+#define __KALMAN_FILTER_H__
+
+/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ * Legend:
+ *   xhat_k_km1    --   Predicted state at time 'k'   using information available at time 'k-1'
+ *      P_k_km1    --   Predicted covariance at time 'k' using information available at time 'k-1'
+ *      F_k        --   State transition model
+ *      u_k        --   Control vector (i.e. external process)
+ *      B_k        --   Control input model
+ *      w_k        --   Gaussian White noise
+ *      H_k        --   Observation model (to transform true state into measurements)
+ *      Q_k        --   Covariance matrix of process noise
+ *      R_k        --   Covariance of observational noise
+ *      v_k        --   Gaussian white observational noise
+ *
+ * PREDICTION PHASE
+ *   Predicted state:        xhat_k_km1 = Fk * xhat_km1_km1    +  Bk * uk
+ *   Predicted covar:           P_k_km1 = Fk * P_km1_km1 * Fk' +  Qk
+ *
+ * UPDATE PHASE
+ *   Measurement residual:       yhat_k = zk - Hk * xhat_k_km1
+ *   Residual covariance:           S_k = H_k * P_k_km1 * H_k' + R_k
+ *   Optimal Kalman gain:           K_k = P_k_km1 * H_k' * inv(S_k)
+ *   Updated state esti:       xhat_k_k = xhat_k_km1 + K_k * yhat_k
+ *   Updated covariance:          P_k_k = (I - K_k * H_k) * P_k_km1
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
+#define KAL_STRIDE 36
+#define KAL_MAT(_var)  float _var[KAL_STRIDE][KAL_STRIDE]
+#define KAL_VEC(_var)  float _var[KAL_STRIDE]
+
+void KalmanPredict(
+    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 */
+    int S,                  /* INPUT:          Number of dimensions in state vector */
+    int U);                 /* INPUT:          Size of control input vector */
+
+void KalmanUpdate(
+    KAL_VEC(xhat_k_k),      /* OUTPUT: Updated state at time 'k' */
+    KAL_MAT(P_k_k),         /* OUTPUT: Updated covariance at time 'k' */
+    KAL_VEC(xhat_k_km1),    /* INPUT:  Predicted state at time 'k' */
+    KAL_MAT(P_k_km1),       /* INPUT:  Predicted covariance at time 'k' */
+    KAL_MAT(H_k),           /* INPUT:  Observational model */
+    KAL_MAT(R_k),           /* INPUT:  Covariance of observational noise */
+    int B,                  /* INPUT:  Number of observations in observation vector */
+    int S);                 /* INPUT:  Number of measurements in the state vector */
+
+/* * * * * * * * * * * *
+ * State vector format:
+ *  xhat[12] = { x, y, z, ix, iy, iz, jx, jy, jz, kx, ky, kz, vx, vy, vz, vix, viy, viz, vjx, vjy, vjz, vkx, vky, vkz, ax, ay, az, aix, aiy, aiz, ajx, ajy, ajz, akx, aky, akz },
+ *               0  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
+ *  where,
+ *    i  --  right direction vector
+ *    j  --  forward direction vector
+ *    k  --  up direction vector
+ * * * * * * * * * * * */
+
+/* positional state */
+#define ST_X   0
+#define ST_Y   1
+#define ST_Z   2
+#define ST_IX  3
+#define ST_IY  4
+#define ST_IZ  5
+#define ST_JX  6
+#define ST_JY  7
+#define ST_JZ  8
+#define ST_KX  9
+#define ST_KY 10
+#define ST_KZ 11
+
+/* velocity state */
+#define ST_VX   0
+#define ST_VY   1
+#define ST_VZ   2
+#define ST_VIX  3
+#define ST_VIY  4
+#define ST_VIZ  5
+#define ST_VJX  6
+#define ST_VJY  7
+#define ST_VJZ  8
+#define ST_VKX  9
+#define ST_VKY 10
+#define ST_VKZ 11
+
+/* acceleration state */
+#define ST_AX   0
+#define ST_AY   1
+#define ST_AZ   2
+#define ST_AIX  3
+#define ST_AIY  4
+#define ST_AIZ  5
+#define ST_AJX  6
+#define ST_AJY  7
+#define ST_AJZ  8
+#define ST_AKX  9
+#define ST_AKY 10
+#define ST_AKZ 11
+
+
+/* * * * * * * * * * * *
+ * Measurement:
+ * * * * * * * * * * * */
+
+#endif
+
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;
+}
+