rotationMatrixFromGravity

CMRotationMatrix rotationMatrixFromGravity(float x, float y, float z)
{	
	// The Z axis of our rotated frame is opposite gravity
	vec3f_t zAxis = vec3f_normalize(vec3f_init(-x, -y, -z));
	
	// The Y axis of our rotated frame is an arbitrary vector perpendicular to gravity
	// Note that this convention will have problems as zAxis.x approaches +/-1 since the magnitude of
	// [0, zAxis.z, -zAxis.y] will approach 0
	vec3f_t yAxis = vec3f_normalize(vec3f_init(0, zAxis.z, -zAxis.y));
	
	// The X axis is just the cross product of Y and Z
	vec3f_t xAxis = vec3f_crossProduct(yAxis, zAxis);
	
	// each array is a row
	CMRotationMatrix mat = {
		[ xAxis.x, xAxis.y, xAxis.z ],
		[ yAxis.x, yAxis.y, yAxis.z ],
		[ zAxis.x, zAxis.y, zAxis.z ] };

	return mat;
}

Applying a rotation. Useful if the sensor rotates in place without translating. Requires gyros and accelerometers:

octave> g1 = [ 1, 0, 0 ]
octave> r  = rotMatrix(g1)   # calculate the initial rotation matrix, converting device => N,E,D coordinates

octave> function r = d2r(deg)
  r = deg * pi / 180;
endfunction

octave> function R = degRotZMatrix(deg)
  R = [ cos(d2r(deg)), -sin(d2r(deg)), 0
        sin(d2r(deg)),  cos(d2r(deg)), 0
        0            ,  0            , 1 ];
endfunction

#In the next data-frame the device has rotated.  The device => N,E,D mapping needs updated
octave> g2 = [ 0.707, 0.707, 0 ]  # accelerometer readings after a 45deg clockwise rotation about Z as determined by gyros.
                                  # This should map to [ 0, 0, -1 ]

octave> newRotationMatrix = r * degRotZMatrix(-45)  # Clockwise is negative
octave> newRotationMatrix * g2'                     # This should eq N,E,D [ 0, 0 , -1 ]

ans =

   9.1881e-02
   6.9958e-05
  -9.9562e-01

Yay! it works!