Kalman filter logic along with tests of a simple one-dimensional velo…

…city model.

kalman.c

@@ -67,6 +67,11 @@ void free_filter(KalmanFilter f) {
 }
 
 void update(KalmanFilter f) {
+  predict(f);
+  estimate(f);
+}
+
+void predict(KalmanFilter f) {
   f.timestep++;
 
   /* Predict the state */
@@ -75,12 +80,14 @@ void update(KalmanFilter f) {
 
   /* Predict the state estimate covariance */
   multiply_matrix(f.state_transition, f.estimate_covariance,
-		  f.vertical_scratch);
-  multiply_by_transpose_matrix(f.vertical_scratch, f.state_transition,
+		  f.big_square_scratch);
+  multiply_by_transpose_matrix(f.big_square_scratch, f.state_transition,
 			       f.predicted_estimate_covariance);
   add_matrix(f.predicted_estimate_covariance, f.process_noise_covariance,
 	     f.predicted_estimate_covariance);
+}
 
+void estimate(KalmanFilter f) {
   /* Calculate innovation */
   multiply_matrix(f.observation_model, f.predicted_state,
 		  f.innovation);

kalman.h

@@ -64,15 +64,26 @@ KalmanFilter alloc_filter(int state_dimension,
 
 void free_filter(KalmanFilter f);
 
-/* Runs one timestep of prediction.
+/* Runs one timestep of prediction + estimation.
+
    Before each time step of running this, set f.observation to be the
    next time step's observation.
-   Before any time steps of running this, set:
+
+   Before the first step, define the model by setting:
    f.state_transition
    f.observation_model
    f.process_noise_covariance
    f.observation_noise_covariance
+
+   It is also advisable to initialize with reasonable guesses for
+   f.state_estimate
+   f.estimate_covariance
 */
 void update(KalmanFilter f);
 
+/* Just the prediction phase of update. */
+void predict(KalmanFilter f);
+/* Just the estimation phase of update. */
+void estimate(KalmanFilter f);
+
 #endif

kalman_test.c

@@ -4,6 +4,36 @@
 /* Test the example of a train moving along a 1-d track */
 void test_train() {
   KalmanFilter f = alloc_filter(2, 1);
+
+  /* The train state is a 2d vector containing position and velocity.
+     Velocity is measured in position units per timestep units. */
+  set_matrix(f.state_transition,
+	     1.0, 1.0,
+	     0.0, 1.0);
+
+  /* We only observe position */
+  set_matrix(f.observation_model, 1.0, 0.0);
+
+  /* The covariance matrices are blind guesses */
+  set_identity_matrix(f.process_noise_covariance);
+  set_identity_matrix(f.observation_noise_covariance);
+
+  /* Our knowledge of the start position is incorrect and unconfident */
+  double deviation = 1000.0;
+  set_matrix(f.state_estimate, 10 * deviation);
+  set_identity_matrix(f.estimate_covariance);
+  scale_matrix(f.estimate_covariance, deviation * deviation);
+
+  /* Test with time steps of the position gradually increasing */
+  for (int i = 0; i < 10; ++i) {
+    set_matrix(f.observation, (double) i);
+    update(f);
+  }
+
+  /* Our prediction should be close to (10, 1) */
+  printf("estimated position: %f\n", f.state_estimate.data[0][0]);
+  printf("estimated velocity: %f\n", f.state_estimate.data[1][0]);
+
   free_filter(f);
 }
 

makefile

@@ -7,3 +7,6 @@ OBJECTS = matrix.o kalman.o
 
 %.o : %.c
 	$(CC) $(CFLAGS) -c $<
+
+clean :
+	rm *.o *_test

matrix.c

@@ -172,6 +172,15 @@ bool equal_matrix(Matrix a, Matrix b, double tolerance) {
   return true;
 }
 
+void scale_matrix(Matrix m, double scalar) {
+  assert(scalar != 0.0);
+  for (int i = 0; i < m.rows; ++i) {
+    for (int j = 0; j < m.cols; ++j) {
+      m.data[i][j] *= scalar;
+    }
+  }
+}
+
 void swap_rows(Matrix m, int r1, int r2) {
   assert(r1 != r2);
   double* tmp = m.data[r1];

matrix.h

@@ -56,6 +56,9 @@ void transpose_matrix(Matrix input, Matrix output);
 /* Whether two matrices are approximately equal. */
 bool equal_matrix(Matrix a, Matrix b, double tolerance);
 
+/* Multiply a matrix by a scalar. */
+void scale_matrix(Matrix m, double scalar);
+
 /* Swap rows r1 and r2 of a matrix.
    This is one of the three "elementary row operations". */
 void swap_rows(Matrix m, int r1, int r2);