Publish the Kalman filter on github

.gitignore

@@ -0,0 +1,9 @@
+build
+.directory 
+*.swp
+*-swp
+*.swx
+*.sync-conflict-*
+*.o
+*.a
+*.so

Examples/Makefile

@@ -0,0 +1,29 @@
+include ../Makefile.inc
+
+LDLIBS += -lm
+LahDir = ../../linear-algebra-helpers
+CS := $(LDFLAGS) ../Lib/libkal.a $(LahDir)/Lib/liblah.a $(LDLIBS)
+
+KalIncDir = ../Include
+LahIncDir = $(LahDir)/Include
+I = -I$(KalIncDir) -I$(LahIncDir)
+
+INC_DIR := ../../Include
+BUILD_DIR := ./build
+
+all : test_Kalman
+
+test_Kalman: test_Kalman.o
+		$(CC) $(CF) $(I) -o $(BUILD_DIR)/$@ $(BUILD_DIR)/$< $(CS)
+
+%.o : %.c 
+		$(CC) $(CF) $(I) -o $(BUILD_DIR)/$@ -c $<
+
+.PHONY: clean purge
+
+clean:
+	- $(RM) .build/*.o
+
+purge: clean
+	- $(RM) -r ./build
+	- mkdir ./build

Examples/test_Kalman.c

@@ -0,0 +1,144 @@
+/*******************************************************************/
+/**  Example to test the Kalman-Filter functions                  **/
+/*******************************************************************/
+#include "kalman.h"
+#include "lah.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+/*the state prediction function: linear case for simplicity */
+
+const lah_value accel   = 0.4;
+const lah_value deltaT  = 0.1;
+const lah_value sigma_x = 1.5;
+const lah_value sigma_z = 10;
+
+struct optData 
+{
+    lah_index inits;
+    lah_index initm;
+    lah_mat* B; /* Control matrix*/
+};
+
+lah_Return timeEvolve(lah_mat* xp, const lah_mat *x,
+                      const lah_mat *u, lah_mat *A, 
+                      struct optData *Data)
+{
+	/* Set the state transition Matrix A
+     * if not done yet*/
+    lah_mat *B = Data->B;
+    lah_Return result;
+    if (Data->inits == 0 )
+    {
+        A->data[0] = 1;
+        A->data[A->incRow] = 0;
+        A->data[A->incCol] = deltaT;
+        A->data[A->incCol + A->incRow] = 1;
+
+        B->data[0] = deltaT * deltaT /2;
+        B->data[1] = deltaT;
+        Data->inits = 1;
+    }
+
+    /* compute predicted state */
+    result = lah_matMul(lahNorm, lahNorm, 0.0, 1.0 , xp, A, x);
+    result += lah_matMul(lahNorm, lahNorm, 1.0, 1.0, xp, B, u);
+
+    if (result != 0)
+        return lahReturnMathError;
+    else
+        return lahReturnOk;
+}
+
+/*  measurement prediction function */
+lah_Return measure(lah_mat *zp, const lah_mat *x,
+                   lah_mat *H, struct optData *Data)
+{
+    /* Set the state transition Matrix A
+     * if not done yet*/
+    if (Data->initm == 0 )
+    {
+        H->data[0] = 1;
+        H->data[H->incCol] = 0 ;
+
+        Data->initm = 1;
+    }
+
+    /* compute the measurement from state x */
+    zp->data[0] = x->data[0];
+
+    return lahReturnOk;
+}
+
+int main ()
+{
+    lah_index i;
+    lah_value vel = 0, pos = 0;
+    lah_value noise;
+    lah_mat *u = lah_matAlloc(1, 1, 1);
+    lah_mat *z = lah_matAlloc(1, 1, 1);
+    kal_Filter* kal;
+
+    const lah_index nState = 2;
+    const lah_index nMeas = 1;
+    /*Initialize the struct for prediction */
+    struct optData Data;
+    Data.initm = 0;
+    Data.inits = 0;
+    Data.B = lah_matAlloc(1, 2, 1);
+    kal = kal_create(nState, nMeas,
+                           (kal_StateFun) timeEvolve, 
+                           (kal_MeasFun) measure, (void*) &Data);
+
+    /* Initialization */ 
+    kal->P->data[0] = pow(sigma_x, 2) * pow(deltaT, 4) / 4;
+    kal->P->data[kal->P->incCol] = pow(sigma_x, 2) * pow(deltaT, 3) / 2;
+    kal->P->data[kal->P->incRow] = pow(sigma_x, 2) * pow(deltaT, 3) / 2; 
+    kal->P->data[kal->P->incRow + kal->P->incCol]
+        = pow(sigma_x, 2) * pow(deltaT, 2);
+    /* input and process noise variables */
+    u->data[0] = accel;
+    kal->R->data[0] = sigma_z * sigma_z;
+    LAH_ENTRY(kal->Q, 0, 0) = pow(sigma_x, 2) * pow(deltaT, 4) / 4;
+    LAH_ENTRY(kal->Q, 0, 1) = pow(sigma_x, 2) * pow(deltaT, 3) / 2;
+    LAH_ENTRY(kal->Q, 1, 0) = pow(sigma_x, 2) * pow(deltaT, 3) / 2;
+    LAH_ENTRY(kal->Q, 1, 1) = pow(sigma_x, 2) * pow(deltaT, 2);
+
+    /* initialize random number generator */
+    srand(0);
+
+    /* print header */
+    printf("i pos vel realpos realvel P11 P12 P21 P22 z zp\n");
+    /* loop over time and demonstrate Kalman filter */
+    for (i = 0; i < 5000; i++)
+    {
+        /* virtual measurement */
+        noise = lah_gaussNoise();
+        pos = u->data[0] / 2.0 * pow(i * deltaT, 2.0);
+        vel = u->data[0] * i * deltaT;
+	z->data[0] = pos + noise * sigma_z;
+
+        /* correct filter state */
+        if (lahReturnOk != kal_correct(kal, z))
+            printf("Error in kal_correct !!!\n");
+        /* print out */
+        printf("%d %g %g %g %g %g %g %g %g %g %g\n", i, 
+                kal->x->data[0], kal->x->data[1], pos, vel,
+                kal->P->data[0], kal->P->data[1],
+                kal->P->data[2], kal->P->data[3],
+                z->data[0]     , kal->zp->data[0]);
+
+		u->data[0] = accel;
+        /* predict the next filter state */
+        if (lahReturnOk != kal_predict(kal, u))
+            printf("Error in kal_predict !!!\n");
+    }
+
+    lah_matFree(u);
+    lah_matFree(z);
+    lah_matFree(Data.B);
+    kal_free(kal);
+
+    return 0;
+}

Include/kalman.h

@@ -0,0 +1,121 @@
+#ifndef _KALMAN_H_
+#define _KALMAN_H_
+#include <lah.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+/*****************************************************************************/
+/*  Implementation of the simple/extended kalman filter                      */
+/*****************************************************************************/
+
+/*--- Definitions for the Kalman-Filter -------------------------------------*/
+
+/* kal_StateFun
+ * -------------
+ * Function type to compute the predicted state of the Kalman-Filter
+ * This function can also change the state prediction Matrix of the
+ * filter. In which case the filter is an extended Kalman-Filter and
+ * the Matrix Jf is the Jacobi Matrix describing the
+ * linearization from current filter state.
+ */
+typedef lah_Return (*kal_StateFun)(lah_mat *xp, const lah_mat *x,
+                                   const lah_mat *u, 
+                                   lah_mat *Jf, void *optData);
+
+/* kal_MeasFun
+ * -------------
+ * Measurement function for the (extended) Kalman filter.
+ * Describes the function z = h(x), relating state to measurement.
+ * This function can also change the measurement prediction Matrix of
+ * the filter. In which case the filter is an extended Kalman-Filter 
+ * and the Matrix Jh is the Jacobi Matrix describing the
+ * linearization of the current measurement.
+ */
+typedef lah_Return (*kal_MeasFun)(lah_mat *zp, const lah_mat *x, 
+                                  lah_mat *Jh, void *optData);
+
+/* Data structure for the kalman filter */
+typedef struct
+{
+    lah_mat *x;            /* state vector */
+    lah_mat *xp;           /* predicted state vector */
+    lah_mat *zp;           /* pred. meas. vector */
+    lah_mat *zw;           /* workspace meas. vector */
+
+    lah_mat *P;            /* covariance (nState x nState) */
+
+    lah_mat *S;            /* innovation covariance */
+    int *ipiv;       /* Permutations for LU factorization */
+    lah_mat *W;            /* Workspace for operations */
+
+    /* Noise Matrices */
+    lah_mat *Q;
+    lah_mat *R;
+
+    /* Matrix Views wihtout own data*/
+    lah_mat *WView_P;
+    lah_mat *WView_U;
+    lah_mat *WView_U_trans;
+
+    /* Propagation */
+    lah_mat *Jf;           /* Jacobi of state transition */
+    lah_mat *Jh;           /* Jacobi of state transition */
+    kal_StateFun f;        /* state prediction function */
+	kal_MeasFun h;	       /* measurement prediction function */
+
+    void *optData;         /* pointer to optional user data for 
+                               prediction functions */
+
+} kal_Filter;
+
+/*--------------------------------------------------------------------------*/
+
+/*--- Kalman-Filter functions ----------------------------------------------*/    
+/* For details see "Dan Simon - Optimal State estimation"                   */
+/* The implementation design is inspired by                                 */ 
+/* https://github.com/dr-duplo/eekf/blob/master/                            */
+/* because it can use the linear and extended kalman filter at once         */
+/* This is achieved by defining an interface for the measurement/state      */
+/* prediction function.                                                     */
+/*--------------------------------------------------------------------------*/
+
+/* kal_predict:
+ * ------------
+ * This function predicts the next filter state using the current state, 
+ * the given action u and the given process covariance. 
+ * It will call the function f for state transition. 
+ * The dimensions of process noise Q should match the dimensions 
+ * of covariance P (nState x nState).
+ */
+lah_Return kal_predict(kal_Filter *kal, const lah_mat *u);
+
+/* kal_correct:
+ * ------------
+ * This function corrects the current filter state by using the given 
+ * measurement z and the given measurement process noise covariance R.
+ * The dimensions of measurement noise R should match the dimensions 
+ * of covariance P (nMeas x nMeas).
+ */
+lah_Return kal_correct(kal_Filter *kal, const lah_mat *z);
+
+/*--- Utility/Helper functions ---------------------------------------------*/
+
+/* Allocates all data for the Kalman struct*/
+kal_Filter *kal_create(lah_index nState, lah_index nMeas,
+                       kal_StateFun f, kal_MeasFun h, 
+                       void *optData);
+
+/* Free all data used by the Kalman-Filter struct*/
+kal_Filter *kal_free(kal_Filter *kal);
+
+/* Returns a View on the workspace kal->W compatible to matrix A */
+lah_mat *kal_getWorkspaceView(kal_Filter *kal, lah_mat *A);
+
+/*--------------------------------------------------------------------------*/
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _KALMAN_H_ */

Lib/Makefile

@@ -0,0 +1,65 @@
+#####################################################################
+## MAKEFILE to compile the Kalman Libraby 						   ##
+#####################################################################
+
+# To just compile the library, do:
+#       make
+# resp.  
+#       make VERBOSE=1
+#       make CC=gcc OPENMP=1
+#       make CC=/opt/local/bin/clang-mp-3.7 OPT=1 OPENMP=1
+#
+# To run a demo using the library 
+#       cd ../Demo ; make
+#
+
+LIBRARY = libkal
+
+include ../Makefile.inc
+
+RANLIB = ranlib
+ARCHIVE = $(AR) $(ARFLAGS)
+CP = cp -f
+KalIncDir = ../Include
+LahIncDir = ../../linear-algebra-helpers/Include
+I = -I$(KalIncDir) -I$(LahIncDir)
+
+AR_TARGET = $(LIBRARY).a
+SO_TARGET = $(LIBRARY).so
+
+all: install
+
+KAL_OBJ = \
+ kal_util.o \
+ kal_predict.o \
+ kal_correct.o \
+ ukal_util.o \
+ ukal_predict.o \
+ ukal_correct.o \
+ ukal_sigmaPoints.o
+
+$(KAL_OBJ): $(KalIncDir)/kalman.h $(LahIncDir)/lah.h Makefile
+
+%.o: ../Source/%.c  $(KalIncDir)/kalman.h $(LahIncDir)/lah.h 
+	$(CC) $(CF) $(I) -c $< 
+
+static: $(AR_TARGET)
+
+$(SO_TARGET): $(KAL_OBJ)
+	$(CC) $(CF) $(KAL_OBJ) -o $@ $(LDFLAGS) -shared
+
+$(AR_TARGET): $(KAL_OBJ)
+	$(ARCHIVE)  $@ $^
+	- $(RANLIB) $@
+
+# install archive in this directory
+install: $(AR_TARGET) $(SO_TARGET)
+
+.PHONY: clean purge
+
+clean:
+	- $(RM) *.o
+
+purge: clean
+	- $(RM) *.a *.obj *.dll *.dylib *.so *.so.*
+

Makefile

@@ -0,0 +1,21 @@
+#------------------------------------------------------------------------------
+# Main MakeFile for kalman project 
+#------------------------------------------------------------------------------
+
+C:
+	( cd Lib ; $(MAKE) )
+
+all: C 
+
+library:
+	( cd Lib ; $(MAKE) )
+
+.PHONY: clean purge
+
+clean:
+	( cd Lib ; $(MAKE) clean )
+
+purge:
+	( cd Lib ; $(MAKE) purge )
+
+