diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
new file mode 100644
index 00000000000..b94ed1c6969
--- /dev/null
+++ b/libraries/AP_Math/AP_Math.h
@@ -0,0 +1,7 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+// Assorted useful math operations for ArduPilot(Mega)
+
+#include "vector2.h"
+#include "vector3.h"
+#include "matrix3.h"
diff --git a/libraries/AP_Math/keywords.txt b/libraries/AP_Math/keywords.txt
new file mode 100644
index 00000000000..a5b7938f472
--- /dev/null
+++ b/libraries/AP_Math/keywords.txt
@@ -0,0 +1,29 @@
+Vector2          KEYWORD1
+Vector2i         KEYWORD1
+Vector2ui        KEYWORD1
+Vector2l         KEYWORD1
+Vector2ul        KEYWORD1
+Vector2f         KEYWORD1
+Vector3          KEYWORD1
+Vector3i         KEYWORD1
+Vector3ui        KEYWORD1
+Vector3l         KEYWORD1
+Vector3ul        KEYWORD1
+Vector3f         KEYWORD1
+Matrix3          KEYWORD1
+Matrix3i         KEYWORD1
+Matrix3ui        KEYWORD1
+Matrix3l         KEYWORD1
+Matrix3ul        KEYWORD1
+Matrix3f         KEYWORD1
+length_squared   KEYWORD2
+length           KEYWORD2
+normalize        KEYWORD2
+normalized       KEYWORD2
+reflect          KEYWORD2
+project          KEYWORD2
+projected        KEYWORD2
+angle            KEYWORD2
+angle_normalized KEYWORD2
+rotate           KEYWORD2
+rotated          KEYWORD2
diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h
new file mode 100644
index 00000000000..23bf28ba5d0
--- /dev/null
+++ b/libraries/AP_Math/matrix3.h
@@ -0,0 +1,153 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+// Copyright 2010 Michael Smith, all rights reserved.
+
+//	This library is free software; you can redistribute it and / or
+//	modify it under the terms of the GNU Lesser General Public
+//	License as published by the Free Software Foundation; either
+//	version 2.1 of the License, or (at your option) any later version.
+
+// Inspired by:
+/****************************************
+ * 3D Vector Classes
+ * By Bill Perone (billperone@yahoo.com)
+ */
+
+#ifndef MATRIX3_H
+#define MATRIX3_H
+
+#include "vector3.h"
+
+// 3x3 matrix with elements of type T
+template <typename T>
+class Matrix3 {
+public:
+
+	// Vectors comprising the rows of the matrix
+	Vector3<T>	a, b, c;
+
+	// trivial ctor
+	Matrix3<T>() {}
+
+	// setting ctor
+	Matrix3<T>(const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0): a(a0), b(b0), c(c0) {}
+
+	// function call operator
+	void operator () (const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0)
+	{	a = a0; b = b0; c = c0;  }
+
+	// test for equality
+	bool operator == (const Matrix3<T> &m)
+	{	return (a==m.a && b==m.b && c==m.c);	}
+
+	// test for inequality
+	bool operator != (const Matrix3<T> &m)
+	{	return (a!=m.a || b!=m.b || c!=m.c);	}
+
+	// set to value
+	const Matrix3<T> &operator = (const Matrix3<T> &m)
+	{
+		a= m.a; b= m.b;	c= m.c;
+		return *this;
+	}
+
+	// negation
+	const Matrix3<T> operator - (void) const
+	{	return Matrix3<T>(-a,-b,-c);	}
+
+	// addition
+	const Matrix3<T> operator + (const Matrix3<T> &m) const
+	{   return Matrix3<T>(a+m.a, b+m.b, c+m.c);	 }
+	const Matrix3<T> &operator += (const Matrix3<T> &m)
+	{	return *this = *this + m;	}
+
+	// subtraction
+	const Matrix3<T> operator - (const Matrix3<T> &m) const
+	{   return Matrix3<T>(a-m.a, b-m.b, c-m.c);	 }
+	const Matrix3<T> &operator -= (const Matrix3<T> &m)
+	{	return *this = *this - m;	}
+	   
+	// uniform scaling
+	const Matrix3<T> operator * (const T num) const
+	{	return Matrix3<T>(a*num, b*num, c*num);	}
+	const Matrix3<T> operator *= (const T num)
+	{	return *this = *this * num;	}
+	const Matrix3<T> operator / (const T num) const
+	{	return Matrix3<T>(a/num, b/num, c/num);	}
+	const Matrix3<T> operator /= (const T num)
+	{	return *this = *this / num;	}
+	
+	// multiplication by a vector
+	const Vector3<T> operator *(const Vector3<T> &v) const
+	{
+		return Vector3<T>(a.x * v.x + a.y * v.x + a.z * v.x,
+						  b.x * v.y + b.y * v.y + b.z * v.y,
+						  c.x * v.z + c.y * v.z + c.z * v.z);
+	}
+
+	// multiplication by another Matrix3
+	const Matrix3<T> operator *(const Matrix3<T> &m) const
+	{
+		Matrix3<T> temp = (Vector3<T>(a.x * m.a.x + a.y * m.b.x + a.z * m.c.x,
+									  a.x * m.a.y + a.y * m.b.y + a.z * m.c.y,
+									  a.x * m.a.z + a.y * m.b.z + a.z * m.c.z),
+						   Vector3<T>(b.x * m.a.x + b.y * m.b.x + b.z * m.c.x,
+									  b.x * m.a.y + b.y * m.b.y + b.z * m.c.y,
+									  b.x * m.a.z + b.y * m.b.z + b.z * m.c.z),
+						   Vector3<T>(c.x * m.a.x + c.y * m.b.x + c.z * m.c.x,
+									  c.x * m.a.y + c.y * m.b.y + c.z * m.c.y,
+									  c.x * m.a.z + c.y * m.b.z + c.z * m.c.z));
+		return temp;
+	}
+	const Matrix3<T> operator *=(const Matrix3<T> &m)
+	{	return *this = this * m;	}
+
+	// rotation (90 degrees left)
+	const Matrix3<T> 	rotated(void)
+	{
+		return Matrix3<T>(Vector3<T>(a.z, b.z, c.z),
+						  Vector3<T>(a.y, b.y, c.y),
+						  Vector3<T>(a.x, b.x, c.x));
+	}
+	const Matrix3<T>	rotate(void)
+	{	return *this = rotated();	}
+
+};
+
+// macro to make creating the ctors for derived matrices easier
+#define MATRIX3_CTORS(name, type)									\
+	/* trivial ctor */												\
+	name() {}														\
+	/* make a,b,c combination into a matrix */						\
+	name(Vector3<type> &a0, Vector3<type> &b0, Vector3<type> &c0) : Matrix3<type>(a0, b0, c0) {}
+
+class Matrix3i : public Matrix3<int>
+{
+public:
+	MATRIX3_CTORS(Matrix3i, int)
+};
+
+class Matrix3ui : public Matrix3<unsigned int>
+{
+public:
+	MATRIX3_CTORS(Matrix3ui, unsigned int)
+};
+
+class Matrix3l : public Matrix3<long>
+{
+public:
+	MATRIX3_CTORS(Matrix3l, long)
+};
+class Matrix3ul : public Matrix3<unsigned long>
+{
+public:
+	MATRIX3_CTORS(Matrix3ul, unsigned long)
+};
+
+class Matrix3f : public Matrix3<float>
+{
+public:
+	MATRIX3_CTORS(Matrix3f, float)
+};
+
+#endif // MATRIX3_H
diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h
new file mode 100644
index 00000000000..e00df5d05fa
--- /dev/null
+++ b/libraries/AP_Math/vector2.h
@@ -0,0 +1,208 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+// Copyright 2010 Michael Smith, all rights reserved.
+
+//	This library is free software; you can redistribute it and / or
+//	modify it under the terms of the GNU Lesser General Public
+//	License as published by the Free Software Foundation; either
+//	version 2.1 of the License, or (at your option) any later version.
+
+// Derived closely from:
+/****************************************
+ * 2D Vector Classes
+ * By Bill Perone (billperone@yahoo.com)
+ * Original: 9-16-2002
+ * Revised: 19-11-2003
+ *          18-12-2003
+ *          06-06-2004
+ *
+ * © 2003, This code is provided "as is" and you can use it freely as long as
+ * credit is given to Bill Perone in the application it is used in
+ ****************************************/
+
+#ifndef VECTOR2_H
+#define VECTOR2_H
+
+#include <math.h>
+
+template <typename T>
+struct Vector2
+{
+	T x, y;
+
+
+	// trivial ctor
+	Vector2<T>() {}
+
+	// setting ctor
+	Vector2<T>(const T x0, const T y0): x(x0), y(y0) {}
+
+	// array indexing
+	T &operator [](unsigned int i)
+	{   return *(&x+i);   }
+
+	// array indexing
+	const T &operator [](unsigned int i) const
+	{	return *(&x+i);   }
+
+	// function call operator
+	void operator ()(const T x0, const T y0)
+	{	x= x0; y= y0;	}
+
+	// test for equality
+	bool operator==(const Vector2<T> &v)
+	{	return (x==v.x && y==v.y);	}
+
+	// test for inequality
+	bool operator!=(const Vector2<T> &v)
+	{	return (x!=v.x || y!=v.y);	}
+
+	// set to value
+	const Vector2<T> &operator =(const Vector2<T> &v)
+	{
+		x= v.x; y= v.y;
+		return *this;
+	}
+
+	// negation
+	const Vector2<T> operator -(void) const
+	{	return Vector2<T>(-x, -y);	}
+
+	// addition
+	const Vector2<T> operator +(const Vector2<T> &v) const
+	{	return Vector2<T>(x+v.x, y+v.y);	}
+
+	// subtraction
+	const Vector2<T> operator -(const Vector2<T> &v) const
+	{   return Vector2<T>(x-v.x, y-v.y);	}
+
+	// uniform scaling
+	const Vector2<T> operator *(const T num) const
+	{
+		Vector2<T> temp(*this);
+		return temp*=num;
+	}
+
+	// uniform scaling
+	const Vector2<T> operator /(const T num) const
+	{
+		Vector2<T> temp(*this);
+		return temp/=num;
+	}
+
+	// addition
+	const Vector2<T> &operator +=(const Vector2<T> &v)
+	{
+		x+=v.x;	y+=v.y;
+		return *this;
+	}
+
+	// subtraction
+	const Vector2<T> &operator -=(const Vector2<T> &v)
+	{
+		x-=v.x;	y-=v.y;
+		return *this;
+	}
+
+	// uniform scaling
+	const Vector2<T> &operator *=(const T num)
+	{
+		x*=num;	y*=num;
+		return *this;
+	}
+
+	// uniform scaling
+	const Vector2<T> &operator /=(const T num)
+	{
+		x/=num;	y/=num;
+		return *this;
+	}
+
+	// dot product
+	T operator *(const Vector2<T> &v) const
+	{	return x*v.x + y*v.y;	}
+
+	// gets the length of this vector squared
+	T length_squared() const
+	{	return (T)(*this * *this);   }
+
+	// gets the length of this vector
+	T length() const
+	{	return (T)sqrt(*this * *this);   }
+
+	// normalizes this vector
+	void normalize()
+	{	*this/=length();	}
+
+	// returns the normalized vector
+	Vector2<T> normalized() const
+	{   return  *this/length();  }
+
+	// reflects this vector about n
+	void reflect(const Vector2<T> &n)
+	{
+		Vector2<T> orig(*this);
+		project(n);
+		*this= *this*2 - orig;
+	}
+
+	// projects this vector onto v
+	void project(const Vector2<T> &v)
+	{	*this= v * (*this * v)/(v*v);	}
+
+	// returns this vector projected onto v
+	Vector2<T> projected(const Vector2<T> &v)
+	{   return v * (*this * v)/(v*v);	}
+
+	// computes the angle between 2 arbitrary vectors
+	static inline T angle(const Vector2<T> &v1, const Vector2<T> &v2)
+	{   return (T)acosf((v1*v2) / (v1.length()*v2.length()));  }
+
+	// computes the angle between 2 normalized arbitrary vectors
+	static inline T angle_normalized(const Vector2<T> &v1, const Vector2<T> &v2)
+	{   return (T)acosf(v1*v2);  }
+
+};
+
+
+// macro to make creating the ctors for derived vectors easier
+#define VECTOR2_CTORS(name, type)   \
+	/* trivial ctor */				\
+	name() {}						\
+	/* down casting ctor */			\
+	name(const Vector2<type> &v): Vector2<type>(v.x, v.y) {}	\
+	/* make x,y combination into a vector */					\
+	name(type x0, type y0): Vector2<type>(x0, y0) {}
+
+
+
+struct Vector2i: public Vector2<int>
+{
+	VECTOR2_CTORS(Vector2i, int)
+};
+
+
+struct Vector2ui: public Vector2<unsigned int>
+{
+	VECTOR2_CTORS(Vector2ui, unsigned int)
+};
+
+
+struct Vector2l: public Vector2<long>
+{
+	VECTOR2_CTORS(Vector2l, long)
+};
+
+
+struct Vector2ul: public Vector2<unsigned long>
+{
+	VECTOR2_CTORS(Vector2ul, unsigned long)
+};
+
+
+struct Vector2f: public Vector2<float>
+{
+	VECTOR2_CTORS(Vector2f, float)
+};
+
+#endif // VECTOR2_H
diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h
new file mode 100644
index 00000000000..b2873232d78
--- /dev/null
+++ b/libraries/AP_Math/vector3.h
@@ -0,0 +1,230 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+// Copyright 2010 Michael Smith, all rights reserved.
+
+//	This library is free software; you can redistribute it and / or
+//	modify it under the terms of the GNU Lesser General Public
+//	License as published by the Free Software Foundation; either
+//	version 2.1 of the License, or (at your option) any later version.
+
+// Derived closely from:
+/****************************************
+ * 3D Vector Classes
+ * By Bill Perone (billperone@yahoo.com)
+ * Original: 9-16-2002
+ * Revised: 19-11-2003
+ *          11-12-2003
+ *          18-12-2003
+ *          06-06-2004
+ *
+ * © 2003, This code is provided "as is" and you can use it freely as long as
+ * credit is given to Bill Perone in the application it is used in
+ *
+ * Notes:
+ * if a*b = 0 then a & b are orthogonal
+ * a%b = -b%a
+ * a*(b%c) = (a%b)*c
+ * a%b = a(cast to matrix)*b
+ * (a%b).length() = area of parallelogram formed by a & b
+ * (a%b).length() = a.length()*b.length() * sin(angle between a & b)
+ * (a%b).length() = 0 if angle between a & b = 0 or a.length() = 0 or b.length() = 0
+ * a * (b%c) = volume of parallelpiped formed by a, b, c
+ * vector triple product: a%(b%c) = b*(a*c) - c*(a*b)
+ * scalar triple product: a*(b%c) = c*(a%b) = b*(c%a)
+ * vector quadruple product: (a%b)*(c%d) = (a*c)*(b*d) - (a*d)*(b*c)
+ * if a is unit vector along b then a%b = -b%a = -b(cast to matrix)*a = 0
+ * vectors a1...an are linearly dependant if there exists a vector of scalars (b) where a1*b1 + ... + an*bn = 0
+ *           or if the matrix (A) * b = 0
+ *
+ ****************************************/
+
+#ifndef VECTOR3_H
+#define VECTOR3_H
+
+#include <math.h>
+
+template <typename T>
+class Vector3
+{
+public:
+	T x, y, z;
+
+	// trivial ctor
+	Vector3<T>() {}
+
+	// setting ctor
+	Vector3<T>(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) {}
+
+	// function call operator
+	void operator ()(const T x0, const T y0, const T z0)
+	{	x= x0; y= y0; z= z0;  }
+
+	// test for equality
+	bool operator==(const Vector3<T> &v)
+	{	return (x==v.x && y==v.y && z==v.z);	}
+
+	// test for inequality
+	bool operator!=(const Vector3<T> &v)
+	{	return (x!=v.x || y!=v.y || z!=v.z);	}
+
+	// set to value
+	const Vector3<T> &operator =(const Vector3<T> &v)
+	{
+		x= v.x; y= v.y;	z= v.z;
+		return *this;
+	}
+
+	// negation
+	const Vector3<T> operator -(void) const
+	{	return Vector3<T>(-x,-y,-z);	}
+
+	// addition
+	const Vector3<T> operator +(const Vector3<T> &v) const
+	{   return Vector3<T>(x+v.x, y+v.y, z+v.z);	 }
+
+	// subtraction
+	const Vector3<T> operator -(const Vector3<T> &v) const
+	{   return Vector3<T>(x-v.x, y-v.y, z-v.z);	 }
+
+	// uniform scaling
+	const Vector3<T> operator *(const T num) const
+	{
+		Vector3<T> temp(*this);
+		return temp*=num;
+	}
+
+	// uniform scaling
+	const Vector3<T> operator /(const T num) const
+	{
+		Vector3<T> temp(*this);
+		return temp/=num;
+	}
+
+	// addition
+	const Vector3<T> &operator +=(const Vector3<T> &v)
+	{
+		x+=v.x;	y+=v.y;	z+=v.z;
+		return *this;
+	}
+
+	// subtraction
+	const Vector3<T> &operator -=(const Vector3<T> &v)
+	{
+		x-=v.x;	y-=v.y;	z-=v.z;
+		return *this;
+	}
+
+	// uniform scaling
+	const Vector3<T> &operator *=(const T num)
+	{
+		x*=num; y*=num; z*=num;
+		return *this;
+	}
+
+	// uniform scaling
+	const Vector3<T> &operator /=(const T num)
+	{
+		x/=num; y/=num; z/=num;
+		return *this;
+	}
+
+	// dot product
+	T operator *(const Vector3<T> &v) const
+	{	return x*v.x + y*v.y + z*v.z;	}
+
+	// cross product
+	const Vector3<T> operator %(const Vector3<T> &v) const
+	{
+		Vector3<T> temp(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);
+		return temp;
+	}
+
+	// gets the length of this vector squared
+	T length_squared() const
+	{	return (T)(*this * *this);   }
+
+	// gets the length of this vector
+	float length() const
+	{	return (T)sqrt(*this * *this);   }
+
+	// normalizes this vector
+	void normalize()
+	{	*this/=length();	}
+
+	// returns the normalized version of this vector
+	Vector3<T> normalized() const
+	{   return  *this/length();  }
+
+	// reflects this vector about n
+	void reflect(const Vector3<T> &n)
+	{
+		Vector3<T> orig(*this);
+		project(n);
+		*this= *this*2 - orig;
+	}
+
+	// projects this vector onto v
+	void project(const Vector3<T> &v)
+	{	*this= v * (*this * v)/(v*v);	}
+
+	// returns this vector projected onto v
+	Vector3<T> projected(const Vector3<T> &v)
+	{   return v * (*this * v)/(v*v);	}
+
+	// computes the angle between 2 arbitrary vectors
+	static inline T angle(const Vector3<T> &v1, const Vector3<T> &v2)
+	{   return (T)acosf((v1*v2) / (v1.length()*v2.length()));  }
+
+	// computes the angle between 2 arbitrary normalized vectors
+	static inline T angle_normalized(const Vector3<T> &v1, const Vector3<T> &v2)
+	{   return (T)acosf(v1*v2);  }
+
+};
+
+// macro to make creating the ctors for derived vectors easier
+#define VECTOR3_CTORS(name, type)   \
+	/* trivial ctor */				\
+	name() {}						\
+	/* down casting ctor */			\
+	name(const Vector3<type> &v): Vector3<type>(v.x, v.y, v.z) {}	\
+	/* make x,y,z combination into a vector */						\
+	name(type x0, type y0, type z0): Vector3<type>(x0, y0, z0) {}
+
+
+
+
+class Vector3i: public Vector3<int>
+{
+public:
+	VECTOR3_CTORS(Vector3i, int)
+};
+
+
+class Vector3ui: public Vector3<unsigned int>
+{
+public:
+	VECTOR3_CTORS(Vector3ui, unsigned int)
+};
+
+
+class Vector3l: public Vector3<long>
+{
+public:
+	VECTOR3_CTORS(Vector3l, long)
+};
+
+
+class Vector3ul: public Vector3<unsigned long>
+{
+public:
+	VECTOR3_CTORS(Vector3ul, unsigned long)
+};
+
+
+class Vector3f: public Vector3<float>
+{
+public:
+	VECTOR3_CTORS(Vector3f, float)
+};
+
+#endif // VECTOR3_H