Z

sw/in_progress/inertial/C/6dof.h

@@ -0,0 +1,10 @@
+#ifndef _6DOF_H
+#define _6DOF_H
+
+#define AXIS_X  0
+#define AXIS_Y  1
+#define AXIS_Z  2
+#define AXIS_NB 3
+
+
+#endif /* _6DOF_H */

sw/in_progress/inertial/C/Makefile

@@ -5,16 +5,6 @@ LDFLAGS = `pkg-config glib-2.0 --libs` -lm
 %.o: %.c
 	gcc -c -Wall $(CFLAGS) -o $@ $<
 
-OBJS =  main.o    \
-	linalg.o  \
-	random.o  \
-	ukf.o     \
-	imu.o
-
-test_ukf: $(OBJS)
-	gcc  -o $@ $^ $(LDFLAGS)
-
-
 CFLAGS += `pkg-config gtk+-2.0 --cflags`
 LDFLAGS += `pkg-config gtk+-2.0 --libs` -lgtkdatabox 
 
@@ -43,63 +33,72 @@ OBJS_TILT_EKF =	tilt_data.o 	\
 		ekf.o		\
 		matrix.o        \
 
-CFLAGS += -DEKF_UPDATE_DISCRETE
-#CFLAGS += -DEKF_UPDATE_CONTINUOUS
+#CFLAGS += -DEKF_UPDATE_DISCRETE
+CFLAGS += -DEKF_UPDATE_CONTINUOUS
 tilt_ekf: $(OBJS_TILT_EKF)
 	gcc  -o $@ $^ $(LDFLAGS) 
 
 
-OBJS_TILT_EKF_OPTIM =	tilt_data.o 	 \
+OBJS_TILT_FAST_EKF =	tilt_data.o 	 \
 			tilt_display.o	 \
 			tilt_utils.o     \
 		        random.o       	 \
-			tilt_ekf_optim.o \
+			tilt_fast_ekf.o
 
-tilt_ekf_optim: $(OBJS_TILT_EKF_OPTIM)
-	gcc  -o $@ $^ $(LDFLAGS)
-
-#
-#
-# AHRS
-#
-#
-
-OBJS_AHRS_UKF = ahrs_ukf.o 	\
-		ahrs_utils.o    \
-		ahrs_data.o     \
-		ahrs_display.o	\
-		ukf.o           \
-		linalg.o
-
-ahrs_ukf: $(OBJS_AHRS_UKF)
-	gcc  -o $@ $^ $(LDFLAGS)
-
-OBJS_AHRS_EKF = ahrs_ekf.o 	\
-		ahrs_utils.o    \
-		ahrs_data.o     \
-		ahrs_display.o	\
-		ekf.o           \
-		matrix.o
-
-#CFLAGS += -DAHRS
-ahrs_ekf: $(OBJS_AHRS_EKF)
-	gcc  -o $@ $^ $(LDFLAGS)
-
-
-OBJS_AHRS_FAST_EKF = ahrs_fast_ekf_run.o	\
-		     ahrs_fast_ekf.o    	\
-		     ahrs_data.o		\
-		     ahrs_display.o		\
-		     ahrs_utils.o		\
-
-
-ahrs_fast_ekf: $(OBJS_AHRS_FAST_EKF)
+tilt_fast_ekf: $(OBJS_TILT_FAST_EKF)
 	gcc  -o $@ $^ $(LDFLAGS)
 
 
 #
 #
-# Tests
+# AHRS EULER
+#
+#
+
+
+
+
+#
+#
+# AHRS QUAT
+#
+#
+
+OBJS_AHRS_QUAT_UKF = ahrs_quat_ukf.o \
+		     ahrs_utils.o    \
+		     ahrs_data.o     \
+		     ahrs_display.o  \
+		     ukf.o           \
+		     linalg.o
+
+ahrs_quat_ukf: $(OBJS_AHRS_QUAT_UKF)
+	gcc  -o $@ $^ $(LDFLAGS)
+
+OBJS_AHRS_QUAT_EKF = ahrs_quat_ekf.o \
+		     ahrs_utils.o    \
+		     ahrs_data.o     \
+		     ahrs_display.o  \
+		     ekf.o           \
+		     matrix.o
+
+ahrs_quat_ekf: $(OBJS_AHRS_QUAT_EKF)
+	gcc  -o $@ $^ $(LDFLAGS)
+
+
+OBJS_AHRS_QUAT_FAST_EKF = ahrs_quat_fast_ekf_main.o	\
+		          ahrs_quat_fast_ekf.o    	\
+		     	   ahrs_data.o		        \
+		           ahrs_display.o		\
+		           ahrs_utils.o		        \
+
+
+ahrs_quat_fast_ekf: $(OBJS_AHRS_QUAT_FAST_EKF)
+	gcc  -o $@ $^ $(LDFLAGS)
+
+
+#
+#
+# Test
 #
 #
 
@@ -109,5 +108,14 @@ OBJS_TEST_MATRIX = 	test_matrix.o 	\
 test_matrix: $(OBJS_TEST_MATRIX)
 	gcc  -o $@ $^ $(LDFLAGS) 
 
+OBJS_TEST_UKF =  main.o    \
+		linalg.o  \
+		random.o  \
+		ukf.o     \
+
+test_ukf: $(OBJS_TEST_UKF)
+	gcc  -o $@ $^ $(LDFLAGS)
+
+
 clean:
-	rm -f *~ *.o test_ukf tilt_ukf tilt_ekf tilt_ekf_optim ahrs_ukf ahrs_ekf ahrs_fast_ekf test_matrix
+	rm -f *~ *.o tilt_ukf tilt_ekf tilt_fast_ekf ahrs_quat_ukf ahrs_quat_ekf ahrs_quat_fast_ekf test_matrix test_ukf

sw/in_progress/inertial/C/ahrs_data.c

@@ -0,0 +1,172 @@
+#include "ahrs_data.h"
+
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <stdio.h>
+#include <glib.h>
+
+#include "random.h"
+#include "ahrs_utils.h"
+
+struct ahrs_data* ahrs_data_new(int len, double dt) {
+
+  struct ahrs_data* ad = malloc(sizeof(struct ahrs_data));
+  ad->dt = dt;
+  ad->nb_samples = len;
+
+  ad->t = malloc(ad->nb_samples*sizeof(double));
+
+  ad->phi   = malloc(ad->nb_samples*sizeof(double));
+  ad->theta = malloc(ad->nb_samples*sizeof(double));
+  ad->psi   = malloc(ad->nb_samples*sizeof(double));
+
+  ad->bias_p = malloc(ad->nb_samples*sizeof(double));
+  ad->bias_q = malloc(ad->nb_samples*sizeof(double));
+  ad->bias_r = malloc(ad->nb_samples*sizeof(double));
+
+
+  ad->gyro_p = malloc(ad->nb_samples*sizeof(double));
+  ad->gyro_q = malloc(ad->nb_samples*sizeof(double));
+  ad->gyro_r = malloc(ad->nb_samples*sizeof(double));
+  
+  ad->accel_x = malloc(ad->nb_samples*sizeof(double));
+  ad->accel_y = malloc(ad->nb_samples*sizeof(double));
+  ad->accel_z = malloc(ad->nb_samples*sizeof(double));
+
+  ad->mag_x = malloc(ad->nb_samples*sizeof(double));
+  ad->mag_y = malloc(ad->nb_samples*sizeof(double));
+  ad->mag_z = malloc(ad->nb_samples*sizeof(double));
+
+  ad->m_phi = malloc(ad->nb_samples*sizeof(double));;
+  ad->m_theta = malloc(ad->nb_samples*sizeof(double));;
+  ad->m_psi = malloc(ad->nb_samples*sizeof(double));;
+
+  ad->est_phi   = malloc(ad->nb_samples*sizeof(double));
+  ad->est_theta = malloc(ad->nb_samples*sizeof(double));
+  ad->est_psi   = malloc(ad->nb_samples*sizeof(double));
+  
+  ad->est_bias_p = malloc(ad->nb_samples*sizeof(double));
+  ad->est_bias_q = malloc(ad->nb_samples*sizeof(double));
+  ad->est_bias_r = malloc(ad->nb_samples*sizeof(double));
+
+  ad->P11 = malloc(ad->nb_samples*sizeof(double));
+  ad->P22 = malloc(ad->nb_samples*sizeof(double));
+  ad->P33 = malloc(ad->nb_samples*sizeof(double));
+  ad->P44 = malloc(ad->nb_samples*sizeof(double));
+  ad->P55 = malloc(ad->nb_samples*sizeof(double));
+  ad->P66 = malloc(ad->nb_samples*sizeof(double));
+  ad->P77 = malloc(ad->nb_samples*sizeof(double));
+
+  return ad;
+}
+
+
+struct ahrs_data* ahrs_data_read_log(const char* filename) {
+  GError* _err = NULL;
+  GIOChannel* in_c = g_io_channel_new_file(filename, "r", &_err); 
+  if (_err) {
+    g_free(_err);
+    return NULL;
+  }
+
+  GArray* ga_gx = g_array_new(FALSE, FALSE, sizeof(double));
+  GArray* ga_gy = g_array_new(FALSE, FALSE, sizeof(double));
+  GArray* ga_gz = g_array_new(FALSE, FALSE, sizeof(double));
+  GArray* ga_ax = g_array_new(FALSE, FALSE, sizeof(double));
+  GArray* ga_ay = g_array_new(FALSE, FALSE, sizeof(double));
+  GArray* ga_az = g_array_new(FALSE, FALSE, sizeof(double));
+  GArray* ga_mx = g_array_new(FALSE, FALSE, sizeof(double));
+  GArray* ga_my = g_array_new(FALSE, FALSE, sizeof(double));
+  GArray* ga_mz = g_array_new(FALSE, FALSE, sizeof(double));
+
+  GString *str = g_string_sized_new(256);
+  GIOStatus st;
+  do {
+    double x, y, z;
+    st = g_io_channel_read_line_string(in_c, str, NULL, &_err);
+    if (g_str_has_prefix(str->str, "IMU_GYRO")) {
+      if (sscanf(str->str, "IMU_GYRO %lf %lf %lf", &x, &y, &z) == 3) {
+	g_array_append_val(ga_gx, x);
+	g_array_append_val(ga_gy, y);
+	g_array_append_val(ga_gz, z);
+      }
+    }
+    else if (g_str_has_prefix(str->str, "IMU_ACCEL")) {
+      if (sscanf(str->str, "IMU_ACCEL %lf %lf %lf", &x, &y, &z) == 3) {
+	g_array_append_val(ga_ax, x);
+	g_array_append_val(ga_ay, y);
+	g_array_append_val(ga_az, z);
+      }
+    }
+    else if (g_str_has_prefix(str->str, "IMU_MAG")) {
+      if (sscanf(str->str, "IMU_MAG %lf %lf %lf", &x, &y, &z) == 3) {
+	g_array_append_val(ga_mx, x);
+	g_array_append_val(ga_my, y);
+	g_array_append_val(ga_mz, z);
+      }
+    }
+  }
+  while (st != G_IO_STATUS_EOF);
+  g_string_free(str, TRUE);
+  g_free(in_c);
+
+  struct ahrs_data* ad = ahrs_data_new(ga_gx->len, 0.015625);
+  int i;
+  for (i=0; i<ga_gx->len; i++) {
+    ad->t[i] = (double)i * ad->dt;
+
+    double* ggx = &g_array_index(ga_gx, double, i);
+    ad->gyro_p[i] = *ggx; 
+    double* ggy = &g_array_index(ga_gy, double, i);
+    ad->gyro_q[i] = *ggy; 
+    double* ggz = &g_array_index(ga_gz, double, i);
+    ad->gyro_r[i] = *ggz;
+
+    double* gax = &g_array_index(ga_ax, double, i);
+    ad->accel_x[i] = *gax; 
+    double* gay = &g_array_index(ga_ay, double, i);
+    ad->accel_y[i] = *gay; 
+    double* gaz = &g_array_index(ga_az, double, i);
+    ad->accel_z[i] = *gaz;
+
+    double* gmx = &g_array_index(ga_mx, double, i);
+    ad->mag_x[i] = *gmx; 
+    double* gmy = &g_array_index(ga_my, double, i);
+    ad->mag_y[i] = *gmy; 
+    double* gmz = &g_array_index(ga_mz, double, i);
+    ad->mag_z[i] = *gmz;
+  }
+  
+  return ad;
+}
+
+void ahrs_data_save_state(struct ahrs_data* ad, int idx, double* X, double* P) {
+  double eulers[3];
+  eulers_of_quat(eulers, X);
+  ad->est_phi[idx] = eulers[0];
+  ad->est_theta[idx] = eulers[1];
+  ad->est_psi[idx] = eulers[2];
+
+  ad->est_bias_p[idx] = X[4];
+  ad->est_bias_q[idx] = X[5];
+  ad->est_bias_r[idx] = X[6];
+
+  ad->P11[idx] = P[0];
+  ad->P22[idx] = P[1*7 + 1];
+  ad->P33[idx] = P[2*7 + 2];
+  ad->P44[idx] = P[3*7 + 3];
+  ad->P55[idx] = P[4*7 + 4];
+  ad->P66[idx] = P[5*7 + 5];
+  ad->P77[idx] = P[6*7 + 6];
+
+}
+
+void ahrs_data_save_measure(struct ahrs_data* ad, int idx) {
+
+  ad->m_phi[idx] = phi_of_accel(ad->accel_x[idx], ad->accel_y[idx], ad->accel_z[idx]);
+  ad->m_theta[idx] = theta_of_accel(ad->accel_x[idx], ad->accel_y[idx], ad->accel_z[idx]);
+  ad->m_psi[idx] = psi_of_mag(ad->est_phi[idx], ad->est_theta[idx], 
+			      ad->mag_x[idx], ad->mag_y[idx], ad->mag_z[idx]);
+
+}

sw/in_progress/inertial/C/ahrs_data.h

@@ -0,0 +1,60 @@
+#ifndef AHRS_DATA_H
+#define AHRS_DATA_H
+
+struct ahrs_data {
+  double dt;
+  int nb_samples;
+  double* t;
+
+  /* true state */
+  double* phi;
+  double* theta;
+  double* psi;
+  
+  double* bias_p;
+  double* bias_q;
+  double* bias_r;
+
+  /* sensors    */
+  double* gyro_p;
+  double* gyro_q;
+  double* gyro_r;
+
+  double* accel_x;
+  double* accel_y;
+  double* accel_z;
+
+  double* mag_x;
+  double* mag_y;
+  double* mag_z;
+
+  /* measures */
+  double* m_phi;
+  double* m_theta;
+  double* m_psi;
+
+  /* estimated state */
+  double* est_phi;
+  double* est_theta;
+  double* est_psi;
+
+  double* est_bias_p;
+  double* est_bias_q;
+  double* est_bias_r;
+
+  double* P11;
+  double* P22;
+  double* P33;
+  double* P44;
+  double* P55;
+  double* P66;
+  double* P77;
+};
+
+extern struct ahrs_data* ahrs_data_new(int len, double dt);
+extern struct ahrs_data* ahrs_data_read_log(const char* filename);
+extern void ahrs_data_save_state(struct ahrs_data* ad, int idx, double* X, double* P);
+extern void ahrs_data_save_measure(struct ahrs_data* ad, int idx);
+extern void ahrs_data_save_state(struct ahrs_data* ad, int idx, double* X, double* P);
+
+#endif /* AHRS_DATA_H */

sw/in_progress/inertial/C/ahrs_display.c

@@ -0,0 +1,160 @@
+#include "ahrs_display.h"
+
+#include <gtkdatabox.h>
+#include <gtkdatabox_points.h>
+#include <gtkdatabox_lines.h>
+#include <gtkdatabox_bars.h>
+#include <gtkdatabox_grid.h>
+#include <gtkdatabox_cross_simple.h>
+#include <gtkdatabox_marker.h>
+
+GtkWidget* ahrs_display(struct ahrs_data* ad) {
+  GtkWidget* window = gtk_window_new (GTK_WINDOW_TOPLEVEL);
+  gtk_widget_set_size_request (window, 1280, 800);
+
+  GtkWidget* vbox1 = gtk_vbox_new (FALSE, 0);
+  gtk_container_add (GTK_CONTAINER (window), vbox1);
+
+  GdkColor red = {0, 65535, 0, 0};
+  GdkColor light_red = {0, 65535, 31875, 31875};
+  GdkColor green = {0, 0, 65535, 0};
+  GdkColor light_green = {0, 31875, 65535, 31875};
+  GdkColor blue = {0, 0, 0, 65535};
+  GdkColor light_blue = {0, 14025, 39525, 65535};
+  //  GdkColor pink = {0, 65535, 0, 65535};
+  //  GdkColor black = {0, 0, 0, 0};
+
+  GtkWidget *frame = gtk_frame_new ("rate");
+  gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
+  gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
+
+  GtkWidget* databox = gtk_databox_new();
+  gfloat* ft = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_mrate_p = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_mrate_q = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_mrate_r = g_new0 (gfloat, ad->nb_samples);
+  int idx;
+  for (idx=0; idx<ad->nb_samples; idx++) {
+    ft[idx] = ad->t[idx];
+    f_mrate_p[idx] = ad->gyro_p[idx];
+    f_mrate_q[idx] = ad->gyro_q[idx];
+    f_mrate_r[idx] = ad->gyro_r[idx];
+  }
+  GtkDataboxGraph *g_mrate_p = gtk_databox_lines_new (ad->nb_samples, ft, f_mrate_p, &red, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_mrate_p);
+  GtkDataboxGraph *g_mrate_q = gtk_databox_lines_new (ad->nb_samples, ft, f_mrate_q, &green, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_mrate_q);
+  GtkDataboxGraph *g_mrate_r = gtk_databox_lines_new (ad->nb_samples, ft, f_mrate_r, &blue, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_mrate_r);
+
+  gtk_container_add (GTK_CONTAINER (frame), databox );
+  gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
+
+
+
+  frame = gtk_frame_new ("angle");
+  gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
+  gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
+  
+  databox = gtk_databox_new();
+  //  databox = gtk_databox_create_box_with_scrollbars_and_rulers();
+  gfloat* f_ephi = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_etheta = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_epsi = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_mphi = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_mtheta = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_mpsi = g_new0 (gfloat, ad->nb_samples);
+  for (idx=0; idx<ad->nb_samples; idx++) {
+    f_ephi[idx] = ad->est_phi[idx];
+    f_etheta[idx] = ad->est_theta[idx];
+    f_epsi[idx] = ad->est_psi[idx];
+    f_mphi[idx] = ad->m_phi[idx];
+    f_mtheta[idx] = ad->m_theta[idx];
+    f_mpsi[idx] = ad->m_psi[idx];
+  }
+  GtkDataboxGraph *g_ephi = gtk_databox_lines_new (ad->nb_samples, ft, f_ephi, &red, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_ephi);
+  GtkDataboxGraph *g_etheta = gtk_databox_lines_new (ad->nb_samples, ft, f_etheta, &green, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_etheta);
+  GtkDataboxGraph *g_epsi = gtk_databox_lines_new (ad->nb_samples, ft, f_epsi, &blue, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_epsi);
+  GtkDataboxGraph *g_mphi = gtk_databox_lines_new (ad->nb_samples, ft, f_mphi, &light_red, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_mphi);
+  GtkDataboxGraph *g_mtheta = gtk_databox_lines_new (ad->nb_samples, ft, f_mtheta, &light_green, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_mtheta);
+  GtkDataboxGraph *g_mpsi = gtk_databox_lines_new (ad->nb_samples, ft, f_mpsi, &light_blue, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_mpsi);
+
+  gtk_container_add (GTK_CONTAINER (frame), databox );
+  gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
+
+
+
+  frame = gtk_frame_new ("biase");
+  gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
+  gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
+  
+  databox = gtk_databox_new();
+  gfloat* f_ebias_p = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_ebias_q = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_ebias_r = g_new0 (gfloat, ad->nb_samples);
+  for (idx=0; idx<ad->nb_samples; idx++) {
+    f_ebias_p[idx] = ad->est_bias_p[idx];
+    f_ebias_q[idx] = ad->est_bias_q[idx];
+    f_ebias_r[idx] = ad->est_bias_r[idx];
+  }
+  GtkDataboxGraph *ge_bias_p = gtk_databox_lines_new (ad->nb_samples, ft, f_ebias_p, &red, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), ge_bias_p);
+  GtkDataboxGraph *ge_bias_q = gtk_databox_lines_new (ad->nb_samples, ft, f_ebias_q, &green, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), ge_bias_q);
+  GtkDataboxGraph *ge_bias_r = gtk_databox_lines_new (ad->nb_samples, ft, f_ebias_r, &blue, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), ge_bias_r);
+
+  gtk_container_add (GTK_CONTAINER (frame), databox );
+  gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
+
+
+
+  frame = gtk_frame_new ("covariance");
+  gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
+  gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
+  
+  databox = gtk_databox_new();
+  gfloat* f_p11 = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_p22 = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_p33 = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_p44 = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_p55 = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_p66 = g_new0 (gfloat, ad->nb_samples);
+  gfloat* f_p77 = g_new0 (gfloat, ad->nb_samples);
+  for (idx=0; idx<ad->nb_samples; idx++) {
+    f_p11[idx] = ad->P11[idx];
+    f_p22[idx] = ad->P22[idx];
+    f_p33[idx] = ad->P33[idx];
+    f_p44[idx] = ad->P44[idx];
+    f_p55[idx] = ad->P55[idx];
+    f_p66[idx] = ad->P66[idx];
+    f_p77[idx] = ad->P77[idx];
+  }
+  GtkDataboxGraph *g_p11 = gtk_databox_lines_new (ad->nb_samples, ft, f_p11, &red, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_p11);
+  GtkDataboxGraph *g_p22 = gtk_databox_lines_new (ad->nb_samples, ft, f_p22, &red, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_p22);
+  GtkDataboxGraph *g_p33 = gtk_databox_lines_new (ad->nb_samples, ft, f_p33, &red, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_p33);
+  GtkDataboxGraph *g_p44 = gtk_databox_lines_new (ad->nb_samples, ft, f_p44, &red, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_p44);
+  GtkDataboxGraph *g_p55 = gtk_databox_lines_new (ad->nb_samples, ft, f_p55, &blue, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_p55);
+  GtkDataboxGraph *g_p66 = gtk_databox_lines_new (ad->nb_samples, ft, f_p66, &blue, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_p66);
+  GtkDataboxGraph *g_p77 = gtk_databox_lines_new (ad->nb_samples, ft, f_p77, &blue, 1);
+  gtk_databox_graph_add (GTK_DATABOX(databox), g_p77);
+
+  gtk_container_add (GTK_CONTAINER (frame), databox );
+  gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
+
+
+  gtk_widget_show_all(window);
+  return window;
+}

sw/in_progress/inertial/C/ahrs_display.h

@@ -0,0 +1,10 @@
+#ifndef AHRS_DISPLAY_H
+#define AHRS_DISPLAY_H
+
+#include <gtk/gtk.h>
+
+#include "ahrs_data.h"
+
+extern GtkWidget* ahrs_display(struct ahrs_data* td);
+
+#endif /* AHRS_DISPLAY_H */

sw/in_progress/inertial/C/ahrs_quat_ekf.c

@@ -0,0 +1,194 @@
+#include "ahrs_data.h"
+#include "ahrs_display.h"
+#include "ahrs_utils.h"
+#include "ekf.h"
+
+#include <string.h>
+#include <math.h>
+
+static struct ahrs_data* ad;
+#define UPDATE_PHI   0
+#define UPDATE_THETA 1
+#define UPDATE_PSI   2
+#define UPDATE_NB    3
+static int ahrs_state;
+
+void linear_filter(double *u, double* X, double* dt, double *Xdot, double* F) {
+  *dt = ad->dt;
+
+  /*
+   * quat_dot = Wxq(pqr) * quat
+   * bias_dot = 0
+   *
+   * Wxq is the quaternion omega matrix:
+   *
+   *              [ 0, -p, -q, -r ]
+   *      1/2 *   [ p,  0,  r, -q ]
+   *              [ q, -r,  0,  p ]
+   *              [ r,  q, -p,  0 ]
+   */
+  double p = u[0] - X[4];
+  double q = u[1] - X[5];
+  double r = u[2] - X[6];
+  double q0 = X[0];
+  double q1 = X[1];
+  double q2 = X[2];
+  double q3 = X[3];
+
+  double q0_dot =      -p*q1 -q*q2 -r*q3;
+  double q1_dot = p*q0       +r*q2 -q*q3;
+  double q2_dot = q*q0 -r*q1       +p*q3;
+  double q3_dot = r*q0 +q*q1 -p*q2      ;
+
+  Xdot[0] = q0_dot;
+  Xdot[1] = q1_dot;
+  Xdot[2] = q2_dot;
+  Xdot[3] = q3_dot;
+  Xdot[4] = 0.;
+  Xdot[5] = 0.;
+  Xdot[6] = 0.;
+
+  /*
+   * F is the Jacobian of Xdot wrt the state
+   *
+   *
+   *
+   */
+  double my_f[] = { 0., -p, -q, -r,  q1,  q2,  q3,
+		    p , 0.,  r, -q, -q0,  q3, -q2,
+		    q,  -r, 0.,  p, -q3, -q0,  q1,
+		    r,   q, -p, 0.,  q2, -q1, -q0,
+		    0., 0., 0., 0.,  0.,  0.,  0., 
+		    0., 0., 0., 0.,  0.,  0.,  0., 
+		    0., 0., 0., 0.,  0.,  0.,  0. };
+  memcpy(F, my_f, sizeof(my_f));
+
+}
+
+void linear_measure(double *y, double* err, double*X, double *H) {
+  double eulers[3];
+  eulers_of_quat(eulers, X);
+  *err = y[0] - eulers[ahrs_state];
+  /* H is the jacobian of the measure wrt X   */
+  double q0 = X[0];
+  double q1 = X[1];
+  double q2 = X[2];
+  double q3 = X[3];
+  double dcm00 = 1.0-2*(q2*q2 + q3*q3);
+  double dcm01 =     2*(q1*q2 + q0*q3);
+  double dcm02 =     2*(q1*q3 - q0*q2);
+  double dcm12 =     2*(q2*q3 + q0*q1);
+  double dcm22 = 1.0-2*(q1*q1 + q2*q2);
+  switch (ahrs_state) {
+  case UPDATE_PHI: {
+    WARP(*err, M_PI);
+    const double phi_err = 2. / (dcm22*dcm22 + dcm12*dcm12);
+    H[0] = (q1 * dcm22) * phi_err;
+    H[1] = (q0 * dcm22 + 2 * q1 * dcm12) * phi_err;
+    H[2] = (q3 * dcm22 + 2 * q2 * dcm12) * phi_err;
+    H[3] = (q2 * dcm22) * phi_err;
+    break;
+  }
+  case UPDATE_THETA: {
+    WARP(*err, M_PI_2);
+    const double theta_err = -2. / sqrt( 1 - dcm02*dcm02 );
+    H[0] = -q2 * theta_err;
+    H[1] =  q3 * theta_err;
+    H[2] = -q0 * theta_err;
+    H[3] =  q1 * theta_err;
+    break;
+  }
+  case UPDATE_PSI: {
+    WARP(*err, M_PI);
+    const double psi_err = 2. / (dcm00*dcm00 + dcm01*dcm01);
+    H[0] = (q3 * dcm00) * psi_err;
+    H[1] = (q2 * dcm00) * psi_err;
+    H[2] = (q1 * dcm00 + 2 * q2 * dcm01) * psi_err;
+    H[3] = (q0 * dcm00 + 2 * q3 * dcm01) * psi_err;
+    break;
+  }
+  }
+}
+
+#define P0Q 1.0
+#define P0B 1.0
+
+#define Q0G 0.001
+#define Q0B 0.008
+
+void run_ekf (void) {
+  /* initial state covariance matrix */
+  double P[7*7] = {P0Q,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0, 
+		   0.0,   P0Q,   0.0,   0.0,   0.0,   0.0,   0.0,
+		   0.0,   0.0,   P0Q,   0.0,   0.0,   0.0,   0.0,
+		   0.0,   0.0,   0.0,   P0Q,   0.0,   0.0,   0.0,
+		   0.0,   0.0,   0.0,   0.0,   P0B,   0.0,   0.0,
+		   0.0,   0.0,   0.0,   0.0,   0.0,   P0B,   0.0,
+		   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   P0B };
+  /* model noise covariance matrix */
+  double Q[7*7] = {Q0G,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0, 
+		   0.0,   Q0G,   0.0,   0.0,   0.0,   0.0,   0.0,
+		   0.0,   0.0,   Q0G,   0.0,   0.0,   0.0,   0.0,
+		   0.0,   0.0,   0.0,   Q0G,   0.0,   0.0,   0.0,
+		   0.0,   0.0,   0.0,   0.0,   Q0B,   0.0,   0.0,
+		   0.0,   0.0,   0.0,   0.0,   0.0,   Q0B,   0.0,
+		   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   Q0B };
+  /* measurement noise covariance matrix */
+  double R[1]={1.7};
+  /* state [q0, q1, q2, q3, bp, bq, br] */
+  double X[7];
+  /* measure */
+  double Y[1];
+  /* command */
+  double U[3] = {0.0, 0.0, 0.0};
+  
+  struct ekf_filter* ekf;
+  ekf = ekf_filter_new(7, 1, Q, R, linear_filter, linear_measure);
+  ahrs_init(ad, 150, X);
+  ekf_filter_reset(ekf, X, P);
+
+  /* filter run */
+  int iter;
+  for (iter=0; iter < ad->nb_samples; iter++) {
+    U[0] = ad->gyro_p[iter];// - X[4];
+    U[1] = ad->gyro_q[iter];// - X[5];
+    U[2] = ad->gyro_r[iter];// - X[6];
+    ekf_filter_predict(ekf, U);
+    norm_quat(X);
+    ahrs_state = iter%3;
+    switch (ahrs_state) {
+    case UPDATE_PHI:
+      Y[0] = phi_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
+      break;
+    case UPDATE_THETA:
+      Y[0] = theta_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
+      break;
+    case UPDATE_PSI: {
+      double eulers[3];
+      eulers_of_quat(eulers, X);
+      Y[0] = psi_of_mag(eulers[0], eulers[1], ad->mag_x[iter], ad->mag_y[iter], ad->mag_z[iter]);
+      break;
+    }
+    }
+    ekf_filter_update(ekf, Y);
+    norm_quat(X);
+    //    printf("P66 %f\n", P[6*7 + 6]);
+    ekf_filter_get_state(ekf, X, P);
+    ahrs_data_save_state(ad, iter, X, P);
+    ahrs_data_save_measure(ad, iter);
+  }
+}
+
+int
+main(int argc, char *argv[]) {
+  gtk_init(&argc, &argv);
+
+  //ad = ahrs_data_read_log("../data/log_ahrs_bug");
+  ad = ahrs_data_read_log("../data/log_ahrs_roll");
+  //ad = ahrs_data_read_log("../data/log_ahrs_yaw_pitched");
+  run_ekf();
+  ahrs_display(ad);
+  gtk_main();
+
+  return 0;
+}

sw/in_progress/inertial/C/ahrs_quat_fast_ekf.c

@@ -0,0 +1,404 @@
+#include "ahrs_quat_fast_ekf.h"
+
+#include <math.h>
+#include <inttypes.h>
+#include <string.h>
+
+
+/* Time step */
+#define afe_dt 0.015625
+
+/* We have seven variables in our state -- the quaternion attitude
+ * estimate and three gyro bias values 
+ */
+FLOAT_T afe_q0, afe_q1, afe_q2, afe_q3;
+FLOAT_T afe_bias_p, afe_bias_q, afe_bias_r;
+
+/* We maintain unbiased rates. */
+FLOAT_T afe_p, afe_q, afe_r;
+
+/* We maintain eulers */
+FLOAT_T afe_phi, afe_theta, afe_psi;
+
+/* time derivative of state
+ * we know that bias_p_dot = bias_q_dot = bias_r_dot = 0
+ */
+FLOAT_T afe_q0_dot, afe_q1_dot, afe_q2_dot, afe_q3_dot;
+
+/*
+ * The Direction Cosine Matrix is used to help rotate measurements
+ * to and from the body frame.  We only need five elements from it,
+ * so those are computed explicitly rather than the entire matrix
+ *
+ * External routines might want these (to until sensor readings),
+ * so we export them.
+ */
+FLOAT_T afe_dcm00;
+FLOAT_T afe_dcm01;
+FLOAT_T afe_dcm02;
+FLOAT_T afe_dcm12;
+FLOAT_T afe_dcm22;
+
+/*
+ * Covariance matrix and covariance matrix derivative
+ */
+FLOAT_T afe_P[7][7];
+FLOAT_T afe_Pdot[7][7];
+
+/*
+ * F represents the Jacobian of the derivative of the system with respect
+ * its states.  We do not allocate the bottom three rows since we know that
+ * the derivatives of bias_dot are all zero.
+ */
+FLOAT_T afe_F[4][7];
+
+/*
+ * Kalman filter variables.
+ */
+FLOAT_T afe_PHt[7];
+FLOAT_T afe_K[7];
+FLOAT_T afe_E;
+
+/*
+ * H represents the Jacobian of the measurements of the attitude
+ * with respect to the states of the filter.  We do not allocate the bottom
+ * three rows since we know that the attitude measurements have no
+ * relationship to gyro bias.
+ */
+FLOAT_T afe_H[4];
+/* temporary variable used during state covariance update */
+FLOAT_T afe_FP[4][7];
+
+/*
+ * Q is our estimate noise variance.  It is supposed to be an NxN
+ * matrix, but with elements only on the diagonals.  Additionally,
+ * since the quaternion has no expected noise (we can't directly measure
+ * it), those are zero.  For the gyro, we expect around 5 deg/sec noise,
+ * which is 0.08 rad/sec.  The variance is then 0.08^2 ~= 0.0075.
+ */
+/* I measured about 0.009 rad/s noise */
+#define afe_Q_gyro 8e-03
+
+/*
+ * R is our measurement noise estimate.  Like Q, it is supposed to be
+ * an NxN matrix with elements on the diagonals.  However, since we can
+ * not directly measure the gyro bias, we have no estimate for it.
+ * We only have an expected noise in the pitch and roll accelerometers
+ * and in the compass.
+ */
+#define AFE_R_PHI   1.3 * 1.3
+#define AFE_R_THETA 1.3 * 1.3
+#define AFE_R_PSI   2.5 * 2.5
+
+#include "ahrs_quat_fast_ekf_utils.h"
+
+/*
+ * Call ahrs_state_update every dt seconds with the raw body frame angular
+ * rates.  It updates the attitude state estimate via this function:
+ *
+ *      quat_dot = Wxq(pqr) * quat
+ *      bias_dot = 0
+ *
+ * Since F also contains Wxq, we fill it in here and then reuse the computed
+ * values.  This avoids the extra floating point math.
+ *
+ * Wxq is the quaternion omega matrix:
+ *
+ *              [ 0, -p, -q, -r ]
+ *      1/2 *   [ p,  0,  r, -q ]
+ *              [ q, -r,  0,  p ]
+ *              [ r,  q, -p,  0 ]
+ *
+ *
+ *
+ *
+ *                 [ 0  -p  -q  -r   q1  q2  q3]
+ *   F =   1/2 *   [ p   0   r  -q  -q0  q3 -q2]
+ *                 [ q  -r   0   p  -q3  q0  q1]
+ *                 [ r   q  -p   0   q2 -q1 -q0]
+ *                 [ 0   0   0   0    0   0   0]
+ *                 [ 0   0   0   0    0   0   0]
+ *                 [ 0   0   0   0    0   0   0]
+ *
+ */
+void afe_predict( const FLOAT_T* gyro ) {
+  /* Unbias our gyro values */
+  afe_p = gyro[0] -  afe_bias_p;
+  afe_q = gyro[1] -  afe_bias_q;
+  afe_r = gyro[2] -  afe_bias_r;
+
+  /* compute F 
+     F is only needed later on to update the state covariance P.
+     However, its [0:3][0:3] region is actually the Wxq(pqr) which is needed to
+     compute the time derivative of the quaternion, so we compute F now */
+
+  /* Fill in Wxq(pqr) into F */
+  afe_F[0][0] = afe_F[1][1] = afe_F[2][2] = afe_F[3][3] = 0;
+  afe_F[1][0] = afe_F[2][3] = afe_p * 0.5;
+  afe_F[2][0] = afe_F[3][1] = afe_q * 0.5;
+  afe_F[3][0] = afe_F[1][2] = afe_r * 0.5;
+
+  afe_F[0][1] = afe_F[3][2] = -afe_F[1][0];
+  afe_F[0][2] = afe_F[1][3] = -afe_F[2][0];
+  afe_F[0][3] = afe_F[2][1] = -afe_F[3][0];
+  /* Fill in [4:6][0:3] region */
+  afe_F[0][4] = afe_F[2][6] = afe_q1 * 0.5;
+  afe_F[0][5] = afe_F[3][4] = afe_q2 * 0.5;
+  afe_F[0][6] = afe_F[1][5] = afe_q3 * 0.5;
+
+  afe_F[1][4] = afe_F[2][5] = afe_F[3][6] = afe_q0 * -0.5;
+  afe_F[3][5] = -afe_F[0][4];
+  afe_F[1][6] = -afe_F[0][5];
+  afe_F[2][4] = -afe_F[0][6];
+
+  /* update state */
+  /* quat_dot = Wxq(pqr) * quat */
+  afe_q0_dot =                        afe_F[0][1] * afe_q1 + afe_F[0][2] * afe_q2 + afe_F[0][3] * afe_q3;
+  afe_q1_dot = afe_F[1][0] * afe_q0                        + afe_F[1][2] * afe_q2 + afe_F[1][3] * afe_q3;
+  afe_q2_dot = afe_F[2][0] * afe_q0 + afe_F[2][1] * afe_q1                        + afe_F[2][3] * afe_q3;
+  afe_q3_dot = afe_F[3][0] * afe_q0 + afe_F[3][1] * afe_q1 + afe_F[3][2] * afe_q2;
+  
+  /* quat = quat + quat_dot * dt */
+  afe_q0 += afe_q0_dot * afe_dt;
+  afe_q1 += afe_q1_dot * afe_dt;
+  afe_q2 += afe_q2_dot * afe_dt;
+  afe_q3 += afe_q3_dot * afe_dt;
+
+  /* normalize quaternion */
+  AFE_NORM_QUAT();
+  /* */
+  AFE_DCM_OF_QUAT();
+  AFE_EULER_OF_DCM();
+
+#ifdef EKF_UPDATE_DISCRETE
+  /*
+   * update covariance
+   * Pdot = F*P*F' + Q
+   * P += Pdot * dt
+   */
+  uint8_t i, j, k;
+  for (i=0; i<4; i++) {
+    for (j=0; j<7; j++) {
+      afe_FP[i][j] = 0.;
+      for (k=0; k<7; k++) {
+	afe_FP[i][j] += afe_F[i][k] * afe_P[k][j];
+      }
+    }
+  }
+  for (i=0; i<4; i++) {
+    for (j=0; j<4; j++) {
+      afe_Pdot[i][j] = 0.;
+      for (k=0; k<7; k++) {
+	afe_Pdot[i][j] += afe_FP[i][k] * afe_F[j][k];
+      }
+    }
+  }
+  /* add Q !!! added below*/
+  //  Pdot[4][4] = afe_Q_gyro;
+  //  Pdot[5][5] = afe_Q_gyro;
+  //  Pdot[6][6] = afe_Q_gyro;
+
+  /* P = P + Pdot * dt */
+  for (i=0; i<4; i++)
+    for (j=0; j<4; j++)
+      afe_P[i][j] += afe_Pdot[i][j] * afe_dt;
+  afe_P[4][4] += afe_Q_gyro * afe_dt;
+  afe_P[5][5] += afe_Q_gyro * afe_dt;
+  afe_P[6][6] += afe_Q_gyro * afe_dt;
+#endif
+
+#ifdef EKF_UPDATE_CONTINUOUS
+  /* Pdot = F*P + F'*P + Q */
+  memset( afe_Pdot, 0, sizeof(afe_Pdot) );
+  /*
+   * Compute the A*P+P*At for the bottom rows of P and A_tranpose
+   */
+  uint8_t i, j, k;
+  for( i=0 ; i<4 ; i++ )
+    for( j=0 ; j<7 ; j++ )
+      for( k=4 ; k<7 ; k++ )
+        {
+          const FLOAT_T F_i_k = afe_F[i][k];
+          afe_Pdot[i][j] += F_i_k * afe_P[k][j];
+          afe_Pdot[j][i] += afe_P[j][k] * F_i_k;
+        }
+  /*
+   * Compute F*P + P*Ft for the region [0..3][0..3]
+   */
+  for( i=0 ; i<4 ; i++ )
+    for( j=0 ; j<4 ; j++ )
+      for( k=0 ; k<4 ; k++ )
+        {
+          /* The diagonals of A are zero */
+          if( i == k && j == k )
+            continue;
+          if( j == k )
+            afe_Pdot[i][j] += afe_F[i][k] * afe_P[k][j];
+          else
+            if( i == k )
+              afe_Pdot[i][j] += afe_P[i][k] * afe_F[j][k];
+            else
+              afe_Pdot[i][j] += ( afe_F[i][k] * afe_P[k][j] +
+				  afe_P[i][k] * afe_F[j][k] );
+        }
+  /* Add in the non-zero parts of Q.  The quaternion portions
+   * are all zero, and all the gyros share the same value.
+   */
+  for( i=4 ; i<7 ; i++ )
+    afe_Pdot[i][i] += afe_Q_gyro;
+
+ /* Compute P = P + Pdot * dt */
+  for( i=0 ; i<7 ; i++ )
+    for( j=0 ; j<7 ; j++ )
+      afe_P[i][j] += afe_Pdot[i][j] * afe_dt;
+#endif
+}
+
+
+/*
+ * 
+ */
+static void run_kalman( const FLOAT_T R_axis, const FLOAT_T error ) {
+  int i, j;
+
+  /* PHt = P * H' */
+  afe_PHt[0] = afe_P[0][0] * afe_H[0] + afe_P[0][1] * afe_H[1] + afe_P[0][2] * afe_H[2] + afe_P[0][3] * afe_H[3];
+  afe_PHt[1] = afe_P[1][0] * afe_H[0] + afe_P[1][1] * afe_H[1] + afe_P[1][2] * afe_H[2] + afe_P[1][3] * afe_H[3];
+  afe_PHt[2] = afe_P[2][0] * afe_H[0] + afe_P[2][1] * afe_H[1] + afe_P[2][2] * afe_H[2] + afe_P[2][3] * afe_H[3];
+  afe_PHt[3] = afe_P[3][0] * afe_H[0] + afe_P[3][1] * afe_H[1] + afe_P[3][2] * afe_H[2] + afe_P[3][3] * afe_H[3];
+  afe_PHt[4] = afe_P[4][0] * afe_H[0] + afe_P[4][1] * afe_H[1] + afe_P[4][2] * afe_H[2] + afe_P[4][3] * afe_H[3];
+  afe_PHt[5] = afe_P[5][0] * afe_H[0] + afe_P[5][1] * afe_H[1] + afe_P[5][2] * afe_H[2] + afe_P[5][3] * afe_H[3];
+  afe_PHt[6] = afe_P[6][0] * afe_H[0] + afe_P[6][1] * afe_H[1] + afe_P[6][2] * afe_H[2] + afe_P[6][3] * afe_H[3];
+
+  /*  E = H * PHt + R */
+  afe_E = R_axis;
+  afe_E += afe_H[0] * afe_PHt[0];
+  afe_E += afe_H[1] * afe_PHt[1];
+  afe_E += afe_H[2] * afe_PHt[2];
+  afe_E += afe_H[3] * afe_PHt[3];
+
+  /* Compute the inverse of E */
+  afe_E = 1.0 / afe_E;
+
+  /* Compute K = P * H' * inv(E) */
+  afe_K[0] = afe_PHt[0] * afe_E;
+  afe_K[1] = afe_PHt[1] * afe_E;
+  afe_K[2] = afe_PHt[2] * afe_E;
+  afe_K[3] = afe_PHt[3] * afe_E;
+  afe_K[4] = afe_PHt[4] * afe_E;
+  afe_K[5] = afe_PHt[5] * afe_E;
+  afe_K[6] = afe_PHt[6] * afe_E;
+
+  /* Update our covariance matrix: P = P - K * H * P */
+
+  /* Compute HP = H * P, reusing the PHt array. */
+  afe_PHt[0] = afe_H[0] * afe_P[0][0] + afe_H[1] * afe_P[1][0] + afe_H[2] * afe_P[2][0] + afe_H[3] * afe_P[3][0];
+  afe_PHt[1] = afe_H[0] * afe_P[0][1] + afe_H[1] * afe_P[1][1] + afe_H[2] * afe_P[2][1] + afe_H[3] * afe_P[3][1];
+  afe_PHt[2] = afe_H[0] * afe_P[0][2] + afe_H[1] * afe_P[1][2] + afe_H[2] * afe_P[2][2] + afe_H[3] * afe_P[3][2];
+  afe_PHt[3] = afe_H[0] * afe_P[0][3] + afe_H[1] * afe_P[1][3] + afe_H[2] * afe_P[2][3] + afe_H[3] * afe_P[3][3];
+  afe_PHt[4] = afe_H[0] * afe_P[0][4] + afe_H[1] * afe_P[1][4] + afe_H[2] * afe_P[2][4] + afe_H[3] * afe_P[3][4];
+  afe_PHt[5] = afe_H[0] * afe_P[0][5] + afe_H[1] * afe_P[1][5] + afe_H[2] * afe_P[2][5] + afe_H[3] * afe_P[3][5];
+  afe_PHt[6] = afe_H[0] * afe_P[0][6] + afe_H[1] * afe_P[1][6] + afe_H[2] * afe_P[2][6] + afe_H[3] * afe_P[3][6];
+
+  /* Compute P -= K * HP (aliased to PHt) */
+  for( i=0 ; i<4 ; i++ )
+    for( j=0 ; j<7 ; j++ )
+      afe_P[i][j] -= afe_K[i] * afe_PHt[j];
+
+  /* Update our state: X += K * error */
+  afe_q0     += afe_K[0] * error;
+  afe_q1     += afe_K[1] * error;
+  afe_q2     += afe_K[2] * error;
+  afe_q3     += afe_K[3] * error;
+  afe_bias_p += afe_K[4] * error;
+  afe_bias_q += afe_K[5] * error;
+  afe_bias_r += afe_K[6] * error;
+
+  /* normalize quaternion */
+  AFE_NORM_QUAT();
+}
+
+
+/*
+ * Do the Kalman filter on the acceleration and compass readings.
+ * This is normally a very simple:
+ *
+ *      E = H * P * H' + R
+ *      K = P * H' * inv(E)
+ *      P = P - K * H * P
+ *      X = X + K * error
+ *
+ * We notice that P * H' is used twice, so we can cache the
+ * results of it.
+ *
+ * H represents the Jacobian of measurements to states, which we know
+ * to only have the top four rows filled in since the attitude
+ * measurement does not relate to the gyro bias.  This allows us to
+ * ignore parts of PHt
+ *
+ * We also only process one axis at a time to avoid having to perform
+ * the 3x3 matrix inversion.
+ */
+
+void afe_update_phi( const FLOAT_T* accel ) {
+  AFE_COMPUTE_H_PHI();
+  FLOAT_T accel_phi = afe_phi_of_accel(accel);
+  FLOAT_T err_phi = accel_phi - afe_phi;
+  AFE_WARP( err_phi, M_PI);
+  run_kalman( AFE_R_PHI, err_phi );
+  AFE_DCM_OF_QUAT();
+  AFE_EULER_OF_DCM();
+}
+
+void afe_update_theta( const FLOAT_T* accel ) {
+  AFE_COMPUTE_H_THETA();
+  FLOAT_T accel_theta = afe_theta_of_accel(accel);
+  FLOAT_T err_theta = accel_theta - afe_theta;
+  AFE_WARP( err_theta, M_PI_2);
+  run_kalman( AFE_R_THETA, err_theta );
+  AFE_DCM_OF_QUAT();
+  AFE_EULER_OF_DCM();
+}
+
+void afe_update_psi( const int16_t* mag ) {
+  AFE_COMPUTE_H_PSI();
+  FLOAT_T mag_psi = afe_psi_of_mag(mag);
+  FLOAT_T err_psi = mag_psi - afe_psi;
+  AFE_WARP( err_psi, M_PI);
+  run_kalman( AFE_R_PSI, err_psi );
+  AFE_DCM_OF_QUAT();
+  AFE_EULER_OF_DCM();
+}
+
+/*
+ * Initialize the AHRS state data and covariance matrix.
+ */
+void afe_init( const int16_t *mag, const FLOAT_T* accel, const FLOAT_T* gyro ) {
+
+  /* F and P will be updated only on the non zero locations */
+  memset( (void*) afe_F, 0, sizeof( afe_F ) );
+  memset( (void*) afe_P, 0, sizeof( afe_P ) );
+  int i;
+  for( i=0 ; i<4 ; i++ )
+    afe_P[i][i] = 1.;
+  for( i=4 ; i<7 ; i++ )
+    afe_P[i][i] = .5;
+
+  /* assume vehicle is still, so initial bias are gyro readings */
+  afe_bias_p = gyro[0];
+  afe_bias_q = gyro[1];
+  afe_bias_r = gyro[2];
+
+  afe_phi = afe_phi_of_accel(accel);
+  afe_theta = afe_theta_of_accel(accel);
+  afe_psi = 0.;
+
+  AFE_QUAT_OF_EULER();
+  AFE_DCM_OF_QUAT();
+  afe_psi = afe_psi_of_mag( mag );
+
+  AFE_QUAT_OF_EULER();
+  AFE_DCM_OF_QUAT();
+
+
+}

sw/in_progress/inertial/C/ahrs_quat_fast_ekf.h

@@ -0,0 +1,26 @@
+#ifndef AHRS_QUAT_FAST_EKF_H
+#define AHRS_QUAT_FAST_EKF_H
+
+#include <inttypes.h>
+#include "6dof.h"
+
+#define FLOAT_T double
+//#define FLOAT_T float
+
+/* ekf state : quaternion and gyro biases */
+extern FLOAT_T afe_q0, afe_q1, afe_q2, afe_q3;
+extern FLOAT_T afe_bias_p, afe_bias_q, afe_bias_r;
+/* we maintain unbiased rates */
+extern FLOAT_T afe_p, afe_q, afe_r;
+/* we maintain eulers angles */
+extern FLOAT_T afe_phi, afe_theta, afe_psi;
+
+extern FLOAT_T afe_P[7][7]; /* covariance */
+
+extern void afe_init( const int16_t *mag, const FLOAT_T* accel, const FLOAT_T* gyro );
+extern void afe_predict( const FLOAT_T* gyro );
+extern void afe_update_phi( const FLOAT_T* accel);
+extern void afe_update_theta( const FLOAT_T* accel);
+extern void afe_update_psi( const int16_t* mag);
+
+#endif /* AHRS_QUAT_FAST_EKF_H_H */

sw/in_progress/inertial/C/ahrs_quat_fast_ekf_main.c

@@ -0,0 +1,80 @@
+
+#include <inttypes.h>
+#include <math.h>
+
+#include "ahrs_quat_fast_ekf.h"
+#include "ahrs_data.h"
+#include "ahrs_display.h"
+
+
+#define UPDATE_PHI   0
+#define UPDATE_THETA 1
+#define UPDATE_PSI   2
+#define UPDATE_NB    3
+
+int main(int argc, char** argv) {
+  gtk_init(&argc, &argv);
+
+  struct ahrs_data* ad = ahrs_data_read_log("../data/log_ahrs_bug");
+  //struct ahrs_data* ad = ahrs_data_read_log("../data/log_ahrs_roll");
+  //struct ahrs_data* ad = ahrs_data_read_log("../data/log_ahrs_yaw_pitched");
+
+  int idx;
+  int len = 150;
+  double gp=0., gq=0., gr=0., ax = 0., ay=0., az=0., mx=0., my=0., mz=0.;
+  for (idx=0; idx<len; idx++) {
+    gp+=ad->gyro_p[idx]; gq+=ad->gyro_q[idx]; gr+=ad->gyro_r[idx];
+    ax+=ad->accel_x[idx]; ay+=ad->accel_y[idx]; az+=ad->accel_z[idx];
+    mx+=ad->mag_x[idx]; my+=ad->mag_y[idx]; mz+=ad->mag_z[idx];
+  }
+  gp/=len; gq/=len; gr/=len;
+  ax/=len; ay/=len; az/=len;
+  mx/=len; my/=len; mz/=len;
+  int16_t mag_init[3] = {mx, my, mz};
+  FLOAT_T accel_init[3] = {ax, ay, az};
+  FLOAT_T gyro_init[3] =  {gp, gq, gr};
+  afe_init(mag_init, accel_init, gyro_init);
+
+  int i;
+  for (i=0; i<ad->nb_samples; i++) {
+    FLOAT_T gyro[3] = {ad->gyro_p[i], ad->gyro_q[i], ad->gyro_r[i]};
+    afe_predict(gyro);
+    int update_stage = i%UPDATE_NB;
+    switch(update_stage) {
+    case UPDATE_PHI: {
+      FLOAT_T accel[3] = {ad->accel_x[i], ad->accel_y[i], ad->accel_z[i]};
+      afe_update_phi(accel);
+      break;
+    }
+    case UPDATE_THETA: {
+      FLOAT_T accel[3] = {ad->accel_x[i], ad->accel_y[i], ad->accel_z[i]};
+      afe_update_theta(accel);
+      break;
+    }
+    case UPDATE_PSI: {
+      int16_t mag[3] = {ad->mag_x[i], ad->mag_y[i], ad->mag_z[i]};
+      afe_update_psi(mag);
+      break;
+    }
+    }
+    /* save state for displaying */
+    double X[] = { afe_q0, afe_q1, afe_q2, afe_q3, afe_bias_p, afe_bias_q, afe_bias_r };
+    double P[] = { afe_P[0][0], afe_P[0][1], afe_P[0][2], afe_P[0][3], afe_P[0][4], afe_P[0][5], afe_P[0][6],
+		   afe_P[1][0], afe_P[1][1], afe_P[1][2], afe_P[1][3], afe_P[1][4], afe_P[1][5], afe_P[1][6],
+		   afe_P[2][0], afe_P[3][1], afe_P[2][2], afe_P[2][3], afe_P[2][4], afe_P[2][5], afe_P[2][6],
+		   afe_P[3][0], afe_P[4][1], afe_P[3][2], afe_P[3][3], afe_P[3][4], afe_P[3][5], afe_P[3][6],
+		   afe_P[4][0], afe_P[5][1], afe_P[4][2], afe_P[4][3], afe_P[4][4], afe_P[4][5], afe_P[4][6],
+		   afe_P[5][0], afe_P[6][1], afe_P[5][2], afe_P[5][3], afe_P[5][4], afe_P[5][5], afe_P[5][6],
+		   afe_P[6][0], afe_P[7][1], afe_P[6][2], afe_P[6][3], afe_P[6][4], afe_P[6][5], afe_P[6][6]};
+    //    printf("P66 %f\n", afe_P[6][6]);
+    ahrs_data_save_state(ad, i, X, P);
+    /* save measures for displaying */
+    ahrs_data_save_measure(ad, i);
+
+  }
+
+  ahrs_display(ad);
+
+  gtk_main();
+  return 0;
+}

sw/in_progress/inertial/C/ahrs_quat_fast_ekf_utils.h

@@ -0,0 +1,172 @@
+#ifndef AHRS_QUAT_FAST_EKF_UTILS_H
+#define AHRS_QUAT__FAST_EKF_UTILS_H
+
+#include "ahrs_quat_fast_ekf.h"
+
+/*
+ * Compute the five elements of the DCM that we use for our
+ * rotations and Jacobians.  This is used by several other functions
+ * to speedup their computations.
+ */
+#define AFE_DCM_OF_QUAT() {				\
+    afe_dcm00 = 1.0-2*(afe_q2*afe_q2 + afe_q3*afe_q3);	\
+    afe_dcm01 =     2*(afe_q1*afe_q2 + afe_q0*afe_q3);	\
+    afe_dcm02 =     2*(afe_q1*afe_q3 - afe_q0*afe_q2);	\
+    afe_dcm12 =     2*(afe_q2*afe_q3 + afe_q0*afe_q1);	\
+    afe_dcm22 = 1.0-2*(afe_q1*afe_q1 + afe_q2*afe_q2);	\
+  }
+/*
+ * Compute Euler angles from our DCM.
+ */
+#define AFE_PHI_OF_DCM()   { afe_phi = atan2( afe_dcm12, afe_dcm22 );}
+#define AFE_THETA_OF_DCM() { afe_theta = -asin( afe_dcm02 );}
+#define AFE_PSI_OF_DCM()   { afe_psi = atan2( afe_dcm01, afe_dcm00 );}
+#define AFE_EULER_OF_DCM() { AFE_PHI_OF_DCM(); AFE_THETA_OF_DCM(); AFE_PSI_OF_DCM()}
+
+/*
+ * initialise the quaternion from the set of eulers
+ */
+#define AFE_QUAT_OF_EULER() {			                             \
+    const FLOAT_T phi2     = afe_phi / 2.0;	                             \
+    const FLOAT_T theta2   = afe_theta / 2.0;	                             \
+    const FLOAT_T psi2     = afe_psi / 2.0;	                             \
+						                             \
+    const FLOAT_T sinphi2   = sin( phi2 );	                             \
+    const FLOAT_T cosphi2   = cos( phi2 );	                             \
+						                             \
+    const FLOAT_T sintheta2 = sin( theta2 );	                             \
+    const FLOAT_T costheta2 = cos( theta2 );	                             \
+						                             \
+    const FLOAT_T sinpsi2   = sin( psi2 );	                             \
+    const FLOAT_T cospsi2   = cos( psi2 );	                             \
+    									     \
+    afe_q0 =  cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2; \
+    afe_q1 = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2; \
+    afe_q2 =  cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2; \
+    afe_q3 =  cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2; \
+  }
+
+/*
+ * normalize quaternion
+ */
+#define AFE_NORM_QUAT() {			\
+    FLOAT_T  mag = afe_q0*afe_q0 + afe_q1*afe_q1	\
+      + afe_q2*afe_q2 + afe_q3*afe_q3;		\
+    mag = sqrt( mag );				\
+    afe_q0 /= mag;				\
+    afe_q1 /= mag;				\
+    afe_q2 /= mag;				\
+    afe_q3 /= mag;				\
+  }
+
+
+
+
+/*
+ * Compute the Jacobian of the measurements to the system states.
+ */
+#define AFE_COMPUTE_H_PHI() {          						\
+    const FLOAT_T phi_err =  2 / (afe_dcm22*afe_dcm22 + afe_dcm12*afe_dcm12);     \
+    afe_H[0] = (afe_q1 * afe_dcm22) * phi_err;				        \
+    afe_H[1] = (afe_q0 * afe_dcm22 + 2 * afe_q1 * afe_dcm12) * phi_err;	        \
+    afe_H[2] = (afe_q3 * afe_dcm22 + 2 * afe_q2 * afe_dcm12) * phi_err;	        \
+    afe_H[3] = (afe_q2 * afe_dcm22) * phi_err;				        \
+}
+
+#define AFE_COMPUTE_H_THETA() {						        \
+    const FLOAT_T theta_err  = -2 / sqrt( 1 - afe_dcm02*afe_dcm02 );	        \
+    afe_H[0] = -afe_q2 * theta_err;						\
+    afe_H[1] =  afe_q3 * theta_err;						\
+    afe_H[2] = -afe_q0 * theta_err;						\
+    afe_H[3] =  afe_q1 * theta_err;						\
+  }
+
+#define AFE_COMPUTE_H_PSI() {					                \
+  const FLOAT_T psi_err =  2 / (afe_dcm00*afe_dcm00 + afe_dcm01*afe_dcm01);       \
+  afe_H[0] = (afe_q3 * afe_dcm00) * psi_err;					\
+  afe_H[1] = (afe_q2 * afe_dcm00) * psi_err;					\
+  afe_H[2] = (afe_q1 * afe_dcm00 + 2 * afe_q2 * afe_dcm01) * psi_err;	        \
+  afe_H[3] = (afe_q0 * afe_dcm00 + 2 * afe_q3 * afe_dcm01) * psi_err;	        \
+  }
+
+
+/* convert sensor reading into euler angle measurement */
+static inline FLOAT_T afe_phi_of_accel( const FLOAT_T* accel ) {
+  return atan2(accel[AXIS_Y], accel[AXIS_Z]);
+}
+
+static inline FLOAT_T afe_theta_of_accel( const FLOAT_T* accel) {
+  FLOAT_T g2 =					
+    accel[AXIS_X]*accel[AXIS_X] +	
+    accel[AXIS_Y]*accel[AXIS_Y] +	
+    accel[AXIS_Z]*accel[AXIS_Z];
+  return -asin( accel[AXIS_X] / sqrt( g2 ) );     
+}
+/*
+ * The rotation matrix to rotate from NED frame to body frame without
+ * rotating in the yaw axis is:
+ *
+ * [ 1      0         0    ] [ cos(Theta)  0  -sin(Theta) ]
+ * [ 0  cos(Phi)  sin(Phi) ] [      0      1       0      ]
+ * [ 0 -sin(Phi)  cos(Phi) ] [ sin(Theta)  0   cos(Theta) ]
+ *
+ * This expands to:
+ *
+ * [  cos(Theta)              0      -sin(Theta)         ]
+ * [  sin(Phi)*sin(Theta)  cos(Phi)   sin(Phi)*cos(Theta)]
+ * [  cos(Phi)*sin(Theta)  -sin(Phi)  cos(Phi)*cos(Theta)]
+ *
+ * However, to untilt the compass reading, we need to use the 
+ * transpose of this matrix.
+ *
+ * [  cos(Theta)  sin(Phi)*sin(Theta)  cos(Phi)*sin(Theta) ]
+ * [      0       cos(Phi)            -sin(Phi)            ]
+ * [ -sin(Theta)  sin(Phi)*cos(Theta)  cos(Phi)*cos(Theta) ]
+ *
+ * Additionally,
+ * since we already have the DCM computed for our current attitude,
+ * we can short cut all of the trig. substituting
+ * in from the definition of euler2quat and quat2euler, we have:
+ *
+ * [ cos(Theta)   -dcm12*dcm02   -dcm22*dcm02 ]
+ * [    0          dcm22         -dcm12       ]
+ * [ dcm02         dcm12          dcm22       ]
+ *
+ */
+static inline FLOAT_T afe_psi_of_mag( const int16_t* mag ) {
+  const FLOAT_T ctheta  = cos( afe_theta );
+#if 0
+  const FLOAT_T    mn = ctheta * mag[0] 
+    - (afe_dcm12 * mag[1] + afe_dcm22 * mag[2]) * afe_dcm02 / ctheta;
+  
+  const FLOAT_T    me =
+    (afe_dcm22 * mag[1] - afe_dcm12 * mag[2]) / ctheta;
+#else
+  const FLOAT_T stheta  = sin( afe_theta );
+  const FLOAT_T cphi  = cos( afe_phi );
+  const FLOAT_T sphi  = sin( afe_phi );
+
+  const FLOAT_T mn = 
+    ctheta*      mag[0]+
+    sphi*stheta* mag[1]+
+    cphi*stheta* mag[2];
+  const FLOAT_T me = 
+    0*     mag[0]+
+    cphi*  mag[1]+
+    -sphi* mag[2];
+#endif
+  
+  const FLOAT_T psi = -atan2( me, mn );
+  return psi;
+}
+
+#define AFE_WARP(x, b) {			\
+    while( x < -b )				\
+      x += 2 * b;				\
+    while( x > b )				\
+      x -= 2 * b;				\
+  }
+
+
+
+#endif /* AHRS_QUAT_FAST_EKF_UTILS_H */

sw/in_progress/inertial/C/ahrs_quat_ukf.c

@@ -0,0 +1,123 @@
+#include "ahrs_data.h"
+#include "ahrs_display.h"
+#include "ahrs_utils.h"
+#include "ukf.h"
+
+#define UPDATE_PHI   0
+#define UPDATE_THETA 1
+#define UPDATE_PSI   2
+#define UPDATE_NB    3
+
+static struct ahrs_data* ad;
+static int ahrs_state;
+
+void linear_filter(double *x1, double *x0, double *u) {
+  /*
+   * quat_dot = Wxq(pqr) * quat
+   * bias_dot = 0
+   *
+   * Wxq is the quaternion omega matrix:
+   *
+   *              [ 0, -p, -q, -r ]
+   *      1/2 *   [ p,  0,  r, -q ]
+   *              [ q, -r,  0,  p ]
+   *              [ r,  q, -p,  0 ]
+   */
+  double p = u[0] - x0[4];
+  double q = u[1] - x0[5];
+  double r = u[2] - x0[6];
+  double q0_dot =         -p*x0[1] -q*x0[2] -r*x0[3];
+  double q1_dot = p*x0[0]          +r*x0[2] -q*x0[3];
+  double q2_dot = q*x0[0] -r*x0[1]          +p*x0[3];
+  double q3_dot = r*x0[0] +q*x0[1] -p*x0[2];
+
+  x1[0] = x0[0] + q0_dot * ad->dt;
+  x1[1] = x0[1] + q1_dot * ad->dt;
+  x1[2] = x0[2] + q2_dot * ad->dt;
+  x1[3] = x0[3] + q3_dot * ad->dt;
+  x1[4] = x0[4];
+  x1[5] = x0[5];
+  x1[6] = x0[6];
+
+  norm_quat(x1);
+}  
+
+void linear_measure(double *y, double *x) {
+  double eulers[3];
+  eulers_of_quat(eulers, x);
+  y[0] = eulers[ahrs_state];
+}
+
+
+
+void run_ukf(void) {
+  /* initial state covariance matrix */
+  double P[7*7] = {1.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0, 
+		   0.0,   1.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+		   0.0,   0.0,   1.0,   0.0,   0.0,   0.0,   0.0,
+		   0.0,   0.0,   0.0,   1.0,   0.0,   0.0,   0.0,
+		   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+		   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+		   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0 };
+  /* model noise covariance matrix */
+  double Q[7*7] = {0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0, 
+		   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+		   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+		   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
+		   0.0,   0.0,   0.0,   0.0,   0.008, 0.0,   0.0,
+		   0.0,   0.0,   0.0,   0.0,   0.0,   0.008, 0.0,
+		   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.008 };
+  /* measurement noise covariance matrix */
+  double R[1]={0.3};
+  /* state [q0, q1, q2, q3, bp, bq, br] */
+  double X[7];
+  /* measure */
+  double Y[1];
+  /* command */
+  double U[3] = {0.0, 0.0, 0.0};
+
+  ukf_filter filter;
+  filter = ukf_filter_new(7, 1, Q, R, linear_filter, linear_measure);
+  ahrs_init(ad, 150, X);
+  ukf_filter_reset(filter, X, P);
+  ukf_filter_compute_weights(filter, 1.1, 0.0, 2.0);
+
+  int iter;
+  for (iter=0; iter < ad->nb_samples; iter++) {
+    U[0] = ad->gyro_p[iter];// - X[4];
+    U[1] = ad->gyro_q[iter];// - X[5];
+    U[2] = ad->gyro_r[iter];// - X[6];
+    ahrs_state = iter%3;
+    switch (ahrs_state) {
+    case UPDATE_PHI:
+      Y[0] = phi_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
+      break;
+    case UPDATE_THETA:
+      Y[0] = theta_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
+      break;
+    case UPDATE_PSI: {
+      double eulers[3];
+      eulers_of_quat(eulers, X);
+      Y[0] = psi_of_mag(eulers[0], eulers[1], ad->mag_x[iter], ad->mag_y[iter], ad->mag_z[iter]);
+      break;
+    }
+    }
+    ukf_filter_update(filter, Y, U);
+    ukf_filter_get_state(filter, X, P);
+    ahrs_data_save_state(ad, iter, X, P);
+  }
+
+  ukf_filter_delete(filter);
+}
+
+int main(int argc, char** argv) {
+  gtk_init(&argc, &argv);
+
+  //ad = ahrs_data_read_log("../data/log_ahrs_bug");
+  ad = ahrs_data_read_log("../data/log_ahrs_roll");
+  // ad = ahrs_data_read_log("../data/log_ahrs_yaw_pitched");
+  run_ukf();
+  ahrs_display(ad);
+  gtk_main();
+  return 0;
+}

sw/in_progress/inertial/C/ahrs_utils.c

@@ -0,0 +1,107 @@
+#include "ahrs_utils.h"
+
+#include <math.h>
+#include <stdio.h>
+
+extern double phi_of_accel(double an, double ae, double ad) {
+  return atan2(ae, ad);
+}
+
+extern double theta_of_accel(double an, double ae, double ad) {
+  const double g2 = an * an + ae * ae + ad * ad;
+  return -asin(an / sqrt(g2));
+}
+
+extern double psi_of_mag(double phi, double theta, double mx, double my, double mz) {
+  const float cphi   = cos( phi );
+  const float sphi   = sin( phi );
+  const float ctheta = cos( theta );
+  const float stheta = sin( theta );
+  const float mn = 
+           ctheta * mx +
+    sphi * stheta * my +
+    cphi * stheta * mz;
+  const float me = 
+     0    * mx+
+     cphi * my+
+    -sphi * mz;
+  return -atan2( me, mn );
+}
+
+void quat_of_eulers (double* quat, double phi, double theta, double psi ) {
+  const float phi2     = phi / 2.0;
+  const float theta2   = theta / 2.0;
+  const float psi2     = psi / 2.0;
+
+  const float sinphi2   = sin( phi2 );
+  const float cosphi2   = cos( phi2 );
+
+  const float sintheta2 = sin( theta2 );
+  const float costheta2 = cos( theta2 );
+
+  const float sinpsi2   = sin( psi2 );
+  const float cospsi2   = cos( psi2 );
+
+  quat[0] =  cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2;
+  quat[1] = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2;
+  quat[2] =  cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2;
+  quat[3] =  cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2;
+}
+
+void eulers_of_quat(double* euler, double* quat) {
+  double dcm00 = 1.0-2*(quat[2]*quat[2] + quat[3]*quat[3]);
+  double dcm01 =     2*(quat[1]*quat[2] + quat[0]*quat[3]);
+  double dcm02 =     2*(quat[1]*quat[3] - quat[0]*quat[2]);
+  double dcm12 =     2*(quat[2]*quat[3] + quat[0]*quat[1]);
+  double dcm22 = 1.0-2*(quat[1]*quat[1] + quat[2]*quat[2]);
+
+  euler[0] = atan2( dcm12, dcm22 );
+  euler[1] = -asin( dcm02 );
+  euler[2] = atan2( dcm01, dcm00 );
+}
+
+void norm_quat(double* quat) {
+  double  mag = quat[0]*quat[0] + quat[1]*quat[1] + 
+                quat[2]*quat[2] + quat[3]*quat[3];
+  mag = sqrt( mag );
+  quat[0] /= mag;
+  quat[1] /= mag;
+  quat[2] /= mag;
+  quat[3] /= mag;
+}
+
+void ahrs_init(struct ahrs_data* ad, int len, double* X) {
+  double init_phi = 0.;
+  double init_theta = 0.;
+  double init_mx = 0.;
+  double init_my = 0.;
+  double init_mz = 0.;
+  X[4] = 0.;
+  X[5] = 0.;
+  X[6] = 0.;
+  int iter;
+  for (iter = 0; iter < len; iter++) {
+    /* average attitude */
+    init_phi += phi_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]); 
+    init_theta += theta_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]); 
+    init_mx += ad->mag_x[iter];
+    init_my += ad->mag_y[iter];
+    init_mz += ad->mag_z[iter];
+    /* sum gyros */
+    X[4] += ad->gyro_p[iter];
+    X[5] += ad->gyro_q[iter];
+    X[6] += ad->gyro_r[iter];
+  }
+  init_phi /= (double)len;
+  init_theta /= (double)len;
+  init_mx /= (double)len;
+  init_my /= (double)len;
+  init_mz /= (double)len;
+  double init_psi = psi_of_mag(init_phi, init_theta, init_mx, init_my, init_mz);
+  quat_of_eulers(X, init_phi, init_theta, init_psi);
+  X[4] /= (double)len;
+  X[5] /= (double)len;
+  X[6] /= (double)len;
+  printf("ahrs init : eulers [%f %f %f] biases [%f %f %f]\n", init_phi, init_theta, init_psi, X[4], X[5], X[6]);
+
+}

sw/in_progress/inertial/C/ahrs_utils.h

@@ -0,0 +1,23 @@
+#ifndef AHRS_UTILS_H
+#define AHRS_UTILS_H
+
+#include "ahrs_data.h"
+
+extern double phi_of_accel(double an, double ae, double ad);
+extern double theta_of_accel(double an, double ae, double ad);
+extern double psi_of_mag(double phi, double theta, double mx, double my, double mz);
+extern void quat_of_eulers(double* quat, double phi, double theta, double psi );
+extern void eulers_of_quat(double* euler, double* quat);
+extern void norm_quat(double* quat);
+
+extern void ahrs_init(struct ahrs_data* ad, int len, double* X);
+
+#define WARP(x, y) {	 \
+    while ( x < -y )     \
+      x += 2 * y;	 \
+    while ( x > y )	 \
+      x -= 2 * y;	 \
+}
+
+
+#endif /* AHRS_UTILS_H */

sw/in_progress/inertial/C/tilt_fast_ekf.c

@@ -0,0 +1,99 @@
+#include <math.h>
+
+#include "tilt_data.h"
+#include "tilt_display.h"
+#include "tilt_utils.h"
+
+static struct tilt_data* td;
+
+
+#define CONTINUOUS
+
+void run_tilt(void) {
+  /* state */
+  double X[2];
+  /* state covariance matrix */
+  double P[4] = { 1., 0.,
+		  0., 1. };
+  /* model noise covariance matrix */
+  double Q[4]={0.001, 0.0, 
+	       0.0, 0.003 };
+  /* jacobian of the measure wrt X */
+  const double H[2] = { 1., 0. };
+  /* measurement covariance noise    */
+  const double R = 0.5;
+
+  tilt_init(td, 150, X);
+  
+  int iter;
+  for (iter=0; iter<td->nb_samples; iter++) {
+
+    double rate = td->gyro[iter] - X[1];
+    /* X += Xdot * dt */
+    X[0] += rate * td->dt;
+
+#ifdef EKF_UPDATE_CONTINUOUS
+    /* Pdot = F*P + P*F' + Q 
+     * F = { 1, 0,
+     *       0, 0 };
+     */
+    double Pdot[4] = { Q[0] - P[0*2 + 1] - P[1*2 + 0], -P[1*2 + 1],
+		      -P[1*2 + 1]                    ,  Q[1*2 + 1] };
+#endif
+#ifdef EKF_UPDATE_DISCRETE
+    /* Pdot = F*P*F' + Q */
+    double Pdot[4] = { P[1*2 + 1] +Q[0*2 + 0], 0.,
+		       0.                    ,  Q[1*2 + 1] };
+#endif
+    /* P += Pdot * dt */
+    P[0*2 + 0] += Pdot[0*2 + 0] * td->dt; 
+    P[0*2 + 1] += Pdot[0*2 + 1] * td->dt; 
+    P[1*2 + 0] += Pdot[1*2 + 0] * td->dt; 
+    P[1*2 + 1] += Pdot[1*2 + 1] * td->dt; 
+    
+    /* E = H * P * H' + R */
+    const double PHt_0 = P[0*2 + 0] * H[0]; // + P[0*2 + 1] * H[1]
+    const double PHt_1 = P[1*2 + 0] * H[0]; // + P[1*2 + 1] * H[1]
+    const double E = H[0] * PHt_0 + // H[1] * PHt1
+                     R;
+    
+    /* K = P * H' * inv(E) */
+    const double K_0 = PHt_0 / E;
+    const double K_1 = PHt_1 / E;
+
+    /* P = P - K * H * P */
+    const double t_0 = PHt_0; /* H[0] * P[0][0] + H[1] * P[1][0] */
+    const double t_1 = H[0] * P[0*2+ 1]; /* + H[1] * P[1][1]  = 0 */
+    P[0*2 + 0] -= K_0 * t_0;
+    P[0*2 + 1] -= K_0 * t_1;
+    P[1*2 + 0] -= K_1 * t_0;
+    P[1*2 + 1] -= K_1 * t_1;
+
+    /* X += K * err */
+    double err =  td->m_angle[iter] - X[0];
+    X[0] += K_0 * err;
+    X[1] += K_1 * err;
+
+    tilt_data_save_state(td, iter, X, P);
+    
+  }
+}
+
+
+int main(int argc, char *argv[]) {
+  gtk_init(&argc, &argv);
+
+  //td = tilt_data_gen();
+  //td = tilt_data_read_log("../data/log_ahrs_still");
+  //td = tilt_data_read_log("../data/log_ahrs_roll");
+  td = tilt_data_read_log("../data/log_ahrs_bug");
+  //td = tilt_data_read_log("../data/log_ahrs_yaw_pitched");
+
+  run_tilt();
+  
+  tilt_display(td);
+
+  gtk_main();
+
+  return 0;
+}

sw/in_progress/inertial/C/tilt_ukf.c

@@ -69,10 +69,10 @@ main(int argc, char *argv[]) {
   gtk_init(&argc, &argv);
 
   //td = tilt_data_gen();
-  //td = tilt_data_read_log("data/log_ahrs_still");
-  td = tilt_data_read_log("data/log_ahrs_roll");
-  //td = tilt_data_read_log("data/log_ahrs_bug");
-  //td = tilt_data_read_log("data/log_ahrs_yaw_pitched");
+  //td = tilt_data_read_log("../data/log_ahrs_still");
+  td = tilt_data_read_log("../data/log_ahrs_roll");
+  //td = tilt_data_read_log("../data/log_ahrs_bug");
+  //td = tilt_data_read_log("../data/log_ahrs_yaw_pitched");
   run_ukf(td);
   tilt_display(td);