diff --git a/Makefile b/Makefile
index 1452c2f..23cc81d 100644
--- a/Makefile
+++ b/Makefile
@@ -3,7 +3,7 @@
 NAME=quad
 
 SSRCS=crt0.s
-CSRCS=main.c i2c.c wmp.c timer.c interrupt.c uart.c event.c
+CSRCS=main.c i2c.c wmp.c timer.c interrupt.c uart.c event.c matrix.c dcm.c
 
 COPTIM?=-O1
 CFLAGS=-march=armv4t -msoft-float $(COPTIM) -Wall -Werror -Wextra
diff --git a/dcm.c b/dcm.c
new file mode 100644
index 0000000..28b1231
--- /dev/null
+++ b/dcm.c
@@ -0,0 +1,153 @@
+/* dcm.c */
+
+#ifdef WE_HAVE_SQRT
+#include <math.h>
+#endif
+#include "matrix.h"
+#include "dcm.h"
+#include "uart.h"
+
+/* Implementation of the DCM IMU concept as described by Premerlani
+ * and Bizard
+ */
+
+float dcm[3*3] = {1, 0, 0,
+		  0, 1, 0,
+		  0, 0, 1};
+
+float delta_t = 0.01;
+
+void dcm_update(float omega_x, float omega_y, float omega_z)
+{
+	float tx = delta_t * omega_x;
+	float ty = delta_t * omega_y;
+	float tz = delta_t * omega_z;
+
+	float update_matrix[3*3] = { 0, -tz,  ty,
+				    tz,   0, -tx,
+				   -ty,  tx,   0};
+	float temp_matrix[3*3];
+
+	matrix_multiply(temp_matrix, dcm, update_matrix, 3, 3, 3);
+	matrix_add(dcm, dcm, temp_matrix, 3, 3);
+
+	dcm_normalise();
+}
+
+void dcm_normalise(void)
+{
+	float error;
+	float tmp[6];
+	int i;
+
+	/* dot product of first two rows */
+	error = dcm[0]*dcm[3] + dcm[1]*dcm[4] + dcm[2]*dcm[5];
+
+	/* printf("error is %f\n", error); */
+
+	tmp[0] = dcm[3];
+	tmp[1] = dcm[4];
+	tmp[2] = dcm[5];
+	tmp[3] = dcm[0];
+	tmp[4] = dcm[1];
+	tmp[5] = dcm[2];
+
+	for (i = 0; i < 6; i++)
+		dcm[i] = dcm[i] - (tmp[i] * (0.5f * error));
+
+	/* third row = cross product of first two rows */
+	dcm[6] = dcm[1]*dcm[5] - dcm[2]*dcm[4];
+	dcm[7] = dcm[2]*dcm[3] - dcm[0]*dcm[5];
+	dcm[8] = dcm[0]*dcm[4] - dcm[1]*dcm[3];
+
+	if (!(dcm_renormalise(dcm+0) &&
+	      dcm_renormalise(dcm+3) &&
+	      dcm_renormalise(dcm+6))) {
+		/* Shit. I've been shot. */
+		dcm[0] = dcm[4] = dcm[8] = 1.0f;
+		dcm[1] = dcm[2] = dcm[3] = 0.0f;
+		dcm[5] = dcm[6] = dcm[7] = 0.0f;
+	}
+}
+
+bool dcm_renormalise(float *v)
+{
+	float f = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
+
+	/* printf("f is %f\n", f); */
+
+	if (f < 1.5625f && f > 0.64f) {
+		f = 0.5 * (3 - f);
+#ifdef WE_HAVE_SQRT
+	} else if (f < 100.0f && f > 0.01f) {
+		f = 1.0 / sqrt(f);
+		/* XXX log this event? */
+#endif
+	} else {
+		putstr("problem\r\n");
+		return FALSE;
+	}
+
+	v[0] = v[0] * f;
+	v[1] = v[1] * f;
+	v[2] = v[2] * f;
+
+	return TRUE;
+}
+
+void dcm_dump(void)
+{
+	putstr("dcm: ");
+	putint_s((int)(dcm[0] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[1] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[2] * 100000.0f));
+	putstr("\r\n     ");
+	putint_s((int)(dcm[3] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[4] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[5] * 100000.0f));
+	putstr("\r\n     ");
+	putint_s((int)(dcm[6] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[7] * 100000.0f));
+	putstr(", ");
+	putint_s((int)(dcm[8] * 100000.0f));
+	putstr("\r\n");
+}
+
+void puthexfloat(float f)
+{
+	union {
+		float f;
+		unsigned int i;
+	} u;
+
+	u.f = f;
+	puthex(u.i);
+}
+
+void dcm_send_packet(void)
+{
+	putstr("D:(");
+	puthexfloat(dcm[0]);
+	putstr(",");
+	puthexfloat(dcm[1]);
+	putstr(",");
+	puthexfloat(dcm[2]);
+	putstr(",");
+	puthexfloat(dcm[3]);
+	putstr(",");
+	puthexfloat(dcm[4]);
+	putstr(",");
+	puthexfloat(dcm[5]);
+	putstr(",");
+	puthexfloat(dcm[6]);
+	putstr(",");
+	puthexfloat(dcm[7]);
+	putstr(",");
+	puthexfloat(dcm[8]);
+	putstr(")\r\n");
+}
diff --git a/dcm.h b/dcm.h
new file mode 100644
index 0000000..164dd7a
--- /dev/null
+++ b/dcm.h
@@ -0,0 +1,9 @@
+/* dcm.h */
+
+#include "types.h"
+
+void dcm_update(float omega_x, float omega_y, float omega_z);
+void dcm_normalise(void);
+bool dcm_renormalise(float *v);
+void dcm_dump(void);
+void dcm_send_packet(void);
diff --git a/main.c b/main.c
index cbf306a..54986f5 100644
--- a/main.c
+++ b/main.c
@@ -279,6 +279,7 @@ void menu_handler(void)
 			putstr("Initialising timer... ");
 			timer_set_period(10*TIMER_MS);
 			reply("done");
+			wmp_start_zero();
 			break;
 		default:
 			reply("Unrecognised command.");
diff --git a/matrix.c b/matrix.c
new file mode 100644
index 0000000..29a4614
--- /dev/null
+++ b/matrix.c
@@ -0,0 +1,147 @@
+/* matrix.c */
+
+#ifdef MATRIX_DEBUG
+#include <stdio.h>
+#endif
+
+/* dest[r][c] = m1[r][n] * m2[n][c] */
+void matrix_multiply(float *dest, float *m1, float *m2, int r, int c, int n)
+{
+	int i, j, k;
+
+	for (i = 0; i < r; i++) {
+		for (j = 0; j < c; j++) {
+			dest[i*c+j] = 0;
+			for (k = 0; k < n; k++) {
+				dest[i*c+j] += m1[i*n+k] * m2[k*c+j];
+			}
+		}
+	}
+}
+
+/* dest[r][c] = m1[r][n] * m2[c][n] */
+void matrix_multiply_t(float *dest, float *m1, float *m2, int r, int c, int n)
+{
+	int i, j, k;
+
+	for (i = 0; i < r; i++) {
+		for (j = 0; j < c; j++) {
+			dest[i*c+j] = 0;
+			for (k = 0; k < n; k++) {
+				dest[i*c+j] += m1[i*n+k] * m2[j*n+k];
+			}
+		}
+	}
+}
+
+/* dest[r][c] = m1[r][c] + m2[r][c] */
+void matrix_add(float *dest, float *m1, float *m2, int r, int c)
+{
+	int i, j;
+
+	for (i = 0; i < r; i++) {
+		for (j = 0; j < c; j++) {
+			dest[i*c+j] = m1[i*c+j] + m2[i*c+j];
+		}
+	}
+}
+
+#ifdef MATRIX_DEBUG
+void dump_matrix(float *m, int r, int c)
+{
+	int i, j;
+
+	for (i = 0; i < r; i++) {
+		printf("(");
+		for (j = 0; j < c; j++) {
+			printf("%f", *(m++));
+			if (j < c-1)
+				printf(" ");
+		}
+		printf(")\n");
+	}
+	printf("\n");
+}
+#endif
+
+/* Invert src into dst.
+ * NOTE: destroys the src matrix
+ */
+void matrix_invert(float *dst, float *src, int size)
+{
+	int i, j, k;
+
+	/* Initialise answer with identity matrix */
+	for (i = 0; i < size*size; i++)
+		dst[i] = 0.0;
+	for (i = 0; i < size; i++)
+		dst[i*(size+1)] = 1.0;
+
+	/* Manipulate the matrix into row-echelon form */
+	for (i = 0; i < size; i++) {
+		float max;
+		int maxi;
+
+		/* find a pivot */
+		max = src[i*size+i];
+		maxi = i;
+		for (j = i+1; j < size; j++) {
+			if (src[j*size+i] > max) {
+				max = src[j*size+i];
+				maxi = j;
+			}
+		}
+		if (maxi != i) {
+			/* swap rows */
+			for (j = 0; j < size; j++) {
+				float tmp;
+
+				tmp = src[i*size+j];
+				src[i*size+j] = src[maxi*size+j];
+				src[maxi*size+j] = tmp;
+				tmp = dst[i*size+j];
+				dst[i*size+j] = dst[maxi*size+j];
+				dst[maxi*size+j] = tmp;
+			}
+		}
+		for (j = size-1; j > i; j--) {
+			float factor;
+
+			factor = src[j*size+i] / max;
+			for (k = 0; k < size; k++) {
+				src[j*size+k] = src[j*size+k] -
+						    src[i*size+k] * factor;
+				dst[j*size+k] = dst[j*size+k] -
+						    dst[i*size+k] * factor;
+			}
+		}
+	}
+
+	/* Back-substitute to get src into diagonal form */
+	for (i = size-1; i > 0; i--) {
+		for (j = 0; j < i; j++) {
+			float factor;
+
+			factor = src[j*size+i] / src[i*size+i];
+			for (k = 0; k < size; k++) {
+				src[j*size+k] = src[j*size+k] -
+						    src[i*size+k] * factor;
+				dst[j*size+k] = dst[j*size+k] -
+						    dst[i*size+k] * factor;
+			}
+		}
+	}
+
+	/* Divide each row by its diagonal element */
+	for (i = 0; i < size; i++) {
+		float factor;
+
+		factor = src[i*size+i];
+		for (j = 0; j < size; j++) {
+			src[i*size+j] = src[i*size+j] / factor;
+			dst[i*size+j] = dst[i*size+j] / factor;
+		}
+	}
+
+	/* src should now be the identiy matrix while dst holds the answer */
+}
diff --git a/matrix.h b/matrix.h
new file mode 100644
index 0000000..f7566e5
--- /dev/null
+++ b/matrix.h
@@ -0,0 +1,19 @@
+/* matrix.h */
+
+/* dest[r][c] = m1[r][n] * m2[n][c] */
+void matrix_multiply(float *dest, float *m1, float *m2, int r, int c, int n);
+
+/* dest[r][c] = m1[r][n] * m2[c][n] */
+void matrix_multiply_t(float *dest, float *m1, float *m2, int r, int c, int n);
+
+/* dest[r][c] = m1[r][c] + m2[r][c] */
+void matrix_add(float *dest, float *m1, float *m2, int r, int c);
+
+#ifdef MATRIX_DEBUG
+void dump_matrix(float *m, int r, int c);
+#endif
+
+/* Invert src into dst.
+ * NOTE: destroys the src matrix
+ */
+void matrix_invert(float *dst, float *src, int size);
diff --git a/uart.c b/uart.c
index 1009fd4..99b5235 100644
--- a/uart.c
+++ b/uart.c
@@ -155,6 +155,31 @@ void putint(unsigned int n) {
 	putstr(s+i);
 }
 
+void putint_s(int n) {
+	char s[12];
+	int i;
+	int neg;
+
+	/* OK, technically, this might not work properly for the most
+	 * negative possible number. Oh well.
+	 */
+	neg = (n < 0);
+	if (neg)
+		n = -n;
+
+	i = 11;
+	s[i] = '\0';
+
+	do {
+		s[--i] = n % 10 + '0';
+	} while ((n /= 10) > 0);
+
+	if (neg)
+		s[--i] = '-';
+
+	putstr(s+i);
+}
+
 void puthex(unsigned int n) {
 	char s[9];
 	int i;
diff --git a/uart.h b/uart.h
index 6617a96..cff52fc 100644
--- a/uart.h
+++ b/uart.h
@@ -7,6 +7,7 @@ void init_uart(void);
 void putch(char c);
 void putstr(char *s);
 void putint(unsigned int n);
+void putint_s(int n);
 void puthex(unsigned int n);
 bool getch(char *c);
 
diff --git a/wmp.c b/wmp.c
index 8f174ca..6790956 100644
--- a/wmp.c
+++ b/wmp.c
@@ -2,6 +2,9 @@
 #include "wmp.h"
 #include "i2c.h"
 #include "uart.h"
+#include "dcm.h"
+
+#define WMP_ZERO_COUNT 100
 
 unsigned char wmp_init_command[2] = {0xfe, 0x04};
 
@@ -81,21 +84,31 @@ unsigned int wmp_yaw;
 unsigned int wmp_pitch;
 unsigned int wmp_roll;
 
+unsigned int wmp_yaw_zero;
+unsigned int wmp_pitch_zero;
+unsigned int wmp_roll_zero;
+
 bool wmp_yaw_fast;
 bool wmp_pitch_fast;
 bool wmp_roll_fast;
 
+bool wmp_update;
+bool wmp_zero;
+
+#define TWO_PI 6.28318531f
+#define DEG_TO_RAD (TWO_PI/360.0f)
+
 /* There's considerable debate about these values, and they may vary
  * between different models of the Wii Motion Plus. It would be nice
  * to be able to use the calibration data stored on the device itself
  * but we don't know the format yet.
  */
-#define SLOW_YAW_STEP	(1000/20)
-#define SLOW_PITCH_STEP (1000/20)
-#define SLOW_ROLL_STEP  (1000/20)
-#define FAST_YAW_STEP	(1000/4)
-#define FAST_PITCH_STEP (1000/4)
-#define FAST_ROLL_STEP  (1000/4)
+#define SLOW_YAW_STEP	(20 / DEG_TO_RAD)
+#define SLOW_PITCH_STEP (20 / DEG_TO_RAD)
+#define SLOW_ROLL_STEP  (20 / DEG_TO_RAD)
+#define FAST_YAW_STEP	(4 / DEG_TO_RAD)
+#define FAST_PITCH_STEP (4 / DEG_TO_RAD)
+#define FAST_ROLL_STEP  (4 / DEG_TO_RAD)
 
 bool wmp_sample(void)
 {
@@ -128,6 +141,8 @@ bool wmp_start_sample(void)
 
 void wmp_event_handler(void)
 {
+	float yaw, pitch, roll;
+
 	if (wmp_result != I2C_SUCCESS)
 		return;
 
@@ -142,15 +157,58 @@ void wmp_event_handler(void)
 	wmp_pitch_fast = !(wmp_sample_data[3] & 0x1);
 	wmp_roll_fast = !(wmp_sample_data[4] & 0x2);
 
-	wmp_generation++;
-	if ((wmp_generation % 100) == 0) {
-		putstr("(");
-		puthex(wmp_roll);
-		putstr(", ");
-		puthex(wmp_pitch);
-		putstr(", ");
-		puthex(wmp_yaw);
-		putstr(")\r\n");
-
+	if (wmp_update) {
+		int tmp_yaw = wmp_yaw;
+		int tmp_pitch = wmp_pitch;
+		int tmp_roll = wmp_roll;
+
+		tmp_yaw -= wmp_yaw_zero;
+		tmp_pitch -= wmp_pitch_zero;
+		tmp_roll -= wmp_roll_zero;
+
+		if (wmp_yaw_fast)
+			yaw = ((float)tmp_yaw) / FAST_YAW_STEP;
+		else
+			yaw = ((float)tmp_yaw) / SLOW_YAW_STEP;
+
+		if (wmp_pitch_fast)
+			pitch = ((float)tmp_pitch) / FAST_PITCH_STEP;
+		else
+			pitch = ((float)tmp_pitch) / SLOW_PITCH_STEP;
+
+		if (wmp_roll_fast)
+			roll = ((float)tmp_roll) / FAST_ROLL_STEP;
+		else
+			roll = ((float)tmp_roll) / SLOW_ROLL_STEP;
+
+		dcm_update(roll, pitch, yaw);
+
+		wmp_generation++;
+
+		if ((wmp_generation % 2) == 0)
+			dcm_send_packet();
+
+	} else if (wmp_zero) {
+		wmp_yaw_zero += wmp_yaw;
+		wmp_pitch_zero += wmp_pitch;
+		wmp_roll_zero += wmp_roll;
+		wmp_generation++;
+		if (wmp_generation >= WMP_ZERO_COUNT) {
+			wmp_zero = FALSE;
+			wmp_update = TRUE;
+			wmp_generation = 0;
+			wmp_yaw_zero /= WMP_ZERO_COUNT;
+			wmp_pitch_zero /= WMP_ZERO_COUNT;
+			wmp_roll_zero /= WMP_ZERO_COUNT;
+			putstr("Zero finished\r\n");
+		}
 	}
 }
+
+void wmp_start_zero(void)
+{
+	wmp_zero = TRUE;
+	wmp_update = FALSE;
+	wmp_generation = 0;
+	putstr("Starting zero\r\n");
+}
diff --git a/wmp.h b/wmp.h
index 236ad07..e9a3bc8 100644
--- a/wmp.h
+++ b/wmp.h
@@ -18,5 +18,6 @@ bool wmp_sample(void);
 bool wmp_read_calibration_data(void);
 bool wmp_start_sample(void);
 void wmp_event_handler(void);
+void wmp_start_zero(void);
 
 #endif /* __WMP_H */