# 2 sensor orientation to rotate normalised 3d vector to reference vector

I'm trying to understand the implementation of accelerometer and magnetometer as an orientation device.

If I make some assumptions that gravity is uniform and no external acceleration occurs I have 1 known vector. G=1 (Gx=0,Gy=0,Gz=1)

My magnetometer gives another vector Magnetic north. assuming corrected for Hard and Soft Iron offsets and normalised. M=1 (Mx=0,My=1,Mz=0)

It is the cross product of these two vectors that gives me my 3rd vector (magnetic east?) which is perpendicular to both previous vectors. This I understand but I don't understand how to create a value for this from my 2 readings. in a formula sense what combination of Gx/Gy/Gz || Mx/My/Mz = Ex/Ey/Ez

all sensor readings are normalized so the vector would represent -1 \ 1 as min and max values and essentially represents points around a sphere.

My G should allow me to form a rotation matrix around X and Y axis. My M should allow me to forma a rotation matrix around X and Z axis My E should allow me to form a rotation matrix around Y and Z axis. allowing for cross referencing.

does sound like reasonable mathematical approach to this problem. I have read many articles online which all seem to jump straight to C+ coding which I'm not savvy with so I am trying to understand the mathematics so I can work out the implementation.

Thank you. apologies if it is a done question before but I can't find a clear solution or answer that makes sense. my Maths understand used to be much better that it is now, but even so matrixes etc were not something I studied at A level physics or O level Maths. I'm a surgeon now, so neither plays an ongoing role in my work.

Let's look at it from a different perspective. (No pun intended.) But first, some basic vector algebra: Let $$\bbox{\vec{a} = \left[\begin{matrix} a_x \\ a_y \\ a_z \end{matrix}\right]} , \quad \bbox{\vec{b} = \left[\begin{matrix} b_x \\ b_y \\ b_z \end{matrix}\right]} , \quad \bbox{\vec{c} = \left[\begin{matrix} c_x \\ c_y \\ c_z \end{matrix}\right]} , \quad \bbox{\mathbf{R} = \left[\begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{matrix}\right]}$$ Vector dot product is $$\bbox{\vec{a} \cdot \vec{b}} = \bbox{ a_x b_x + a_y b_y + a_z b_z }$$ Length or norm is $$\left\lVert\vec{a}\right\rVert = \sqrt{\vec{a} \cdot \vec{a}} = \sqrt{ a_x^2 + a_y^2 + a_z^2}$$ Vector cross product is $$\bbox{\vec{c} = \vec{a} \times \vec{b}} \quad \iff \quad \bbox{\begin{cases} c_x = a_y \, b_z - a_z \, b_y \\ c_y = a_z \, b_x - a_x \, b_z \\ c_z = a_x \, b_y - a_y \, b_x \\ \end{cases}}$$ Matrix-vector product is $$\bbox{\vec{c} = \mathbf{R}\vec{a}} \quad \iff \quad \bbox{\begin{cases} c_x = r_{11} \, a_x \; + \; r_{12} \, a_y \; + \; r_{13} \, a_z \\ c_y = r_{21} \, a_x \; + \; r_{22} \, a_y \; + \; r_{23} \, a_z \\ c_z = r_{31} \, a_x \; + \; r_{32} \, a_y \; + \; r_{33} \, a_z \\ \end{cases}}$$

Let's say your device tells you that the acceleration vector $$\vec{G}$$ and magnetic north vector $$\vec{N}$$ are $$\bbox{\vec{G} = \left[ \begin{matrix} G_x \\ G_y \\ G_z \end{matrix} \right ]} , \quad \bbox{\vec{N} = \left[ \begin{matrix} N_x \\ N_y \\ N_z \end{matrix} \right ]}$$ What we want to do first, is to use these to generate a set of orthogonal basis vectors $$\vec{e}_1$$, $$\vec{e}_2$$, and $$\vec{e}_3$$, that we can then normalize (scale) to unit length orthonormal basis vectors $$\hat{e}_1$$, $$\hat{e}_2$$, and $$\hat{e}_3$$.

The easiest method is to use the Gram-Schmidt process: we pick the one we trust most, and take the part of the second vector that is perpendicular to the first. The third is their vector cross product.

Let's say we trust the acceleration most, so we choose $$\hat{e}_1 = \left[\begin{matrix} x_1 \\ y_1 \\ z_1 \end{matrix}\right] = \frac{\vec{G}}{\left\lVert\vec{G}\right\rVert} = \left[\begin{matrix} \frac{G_x}{\sqrt{G_x^2 + G_y^2 + G_z^2}} \\ \frac{G_y}{\sqrt{G_x^2 + G_y^2 + G_z^2}} \\ \frac{G_z}{\sqrt{G_x^2 + G_y^2 + G_z^2}} \\ \end{matrix}\right]$$ The second is magnetic north, but we only want the part that is perpendicular to $$\hat{e}_1$$: $$\vec{n} = \bbox{\left[\begin{matrix} n_x \\ n_y \\ n_z \end{matrix} \right]} = \vec{N} - \hat{e}_1 \left ( \hat{e}_1 \cdot \vec{N} \right )$$ i.e. $$\begin{cases} n_x = N_x - x_1 ( x_1 N_x + y_1 N_y + z_1 N_z ) \\ n_y = N_y - y_1 ( x_1 N_x + y_1 N_y + z_1 N_z ) \\ n_z = N_z - z_1 ( x_1 N_x + y_1 N_y + z_1 N_z ) \\ \end{cases}$$ with the second orthonormal basis vector being $$\hat{e}_2 = \bbox{\left[\begin{matrix} x_2 \\ y_2 \\ z_2 \end{matrix}\right]} = \frac{\vec{e}_2}{\left\lVert\vec{e}_2\right\rVert} = \bbox{\left [ \begin{matrix} \frac{n_x}{\sqrt{n_x^2 + n_y^2 + n_z^2}} \\ \frac{n_y}{\sqrt{n_x^2 + n_y^2 + n_z^2}} \\ \frac{n_z}{\sqrt{n_x^2 + n_y^2 + n_z^2}} \end{matrix}\right]}$$ The third orthonormal basis vector is the vector cross product of the first two, $$\hat{e}_3 = \bbox{\left[\begin{matrix} x_3 \\ y_3 \\ z_3 \end{matrix}\right]} = \hat{e}_1 \times \hat{e}_2$$ i.e. $$\begin{cases} x_3 = x_2 \, y_3 - x_3 \, y_2 \\ y_3 = y_2 \, x_3 - x_2 \, y_3 \\ z_3 = x_1 \, y_2 - x_1 \, y_2 \\ \end{cases}$$ These three give us the orthonormal basis vectors we need.

If we want $$\vec{G}$$ to correspond to the positive $$z$$ axis, and $$\vec{N}$$ to the positive $$y$$ axis, then the matrix that rotates that coordinate system to the current orientation measured is $$\mathbf{M}$$, $$\mathbf{M} = \bbox{\bigr[\begin{matrix} \vec{e}_3 & \vec{e}_2 & \vec{e}_1 \end{matrix}\bigr]} = \bbox{\left[\begin{matrix} x_3 & x_2 & x_1 \\ y_3 & y_2 & y_1 \\ z_3 & z_2 & z_1 \end{matrix}\right]}$$ Because the three column vectors of $$\mathbf{M}$$ are orthogonal, $$\mathbf{M}$$ is orthogonal as well, and its inverse is its transpose.

This means that to rotate the current orientation so that $$\vec{G}$$ is on the positive $$z$$ axis, and $$\vec{N}$$ on the positive $$y$$ axis, we need to apply matrix $$\mathbf{M}^{-1} = \mathbf{M}^T = \bbox{\left[\begin{matrix} x_3 & y_3 & z_3 \\ x_2 & y_2 & z_2 \\ x_1 & y_1 & z_1 \end{matrix}\right]}$$

You do not want to use Euler or Tait-Bryan angles. They are nasty. They are actually a large family of related definitions, depending on the order of the axes being rotated. Instead, just use the above matrices directly.

Let's say you have a vector $$\vec{a}$$ that points to say east, i.e. $$\vec{a} = (-1, 0, 0)$$. In the coordinate system defined by the sensor readings, east is $$\vec{c} = \mathbf{M}\vec{a}$$. (You could then say draw an arrow on a 2D screen using just $$(c_x , c_y)$$.)

Inversely, if we want to know what a vector $$\vec{c}$$ in current coordinate system defined by the sensor readings is in the world coordinate system, we do $$\vec{a} = \mathbf{M}^T \vec{c}$$.

There is usually no need for any kind of axis-angle representation, but if you do need it, you can convert the matrix (either one) to other representations using the formulae outlined in the Wikipedia Rotation matrix article.

• ok, that is actually surprisingly clear - I think. Nov 7, 2018 at 3:16
• ok, that is actually surprisingly clear - I think. hm, can't seem to enter for new line so will be continuous text apologies. following these for M I get (-1,0,0 / 0,1,0 / 0,0,1) sorry I can't make that come as a picture. M equates to what my reference frame would be with the device perfectly orientated to gravity through Z and Magnetic north through Y and my cross product e3 through X axis. if I now rotate my device the vector values would change say I turn it 90 clockwise around Z. M1= (0,1,0 / -1,0,0 / 0,0,1). Nov 7, 2018 at 3:29
• so then using the formula I can output this as a quat or euler angles or whatever just from vector values. does it matter that Magnetic north is not 100% perpendicular to gravity or is that what the little n calculation factors out? in Australia with have declination of about 10 degrees Nov 7, 2018 at 3:29
• @IanBloomfield: (Formatting guide is here). Yes, that's it for $\mathbf{M}$. The $\vec{n}$ calculation does exactly that, factors out the non-perpendicular part. Nov 7, 2018 at 17:58
• Great so I'm actually really happy I've created a 3x3 matrix of floats and as I rotate my device it gives me outputs -1 / 1 which moves through the correct axis so in my head and on paper I could calculate the exact orientation using this which is what I want. Nov 7, 2018 at 19:55

Didn’t understand your context completely but just to aid the computations putting across the formulas

cross product of two vectors in Cartesian form is given by Rotation matrices are as follows: An example is shown below To multiply z component by 90 deg • Thank you so to understand this the letters I, j, k are the XYZ components of my new vector? so in y normalized example would be -1,0,0 this would reflect a vector to the west. Nov 7, 2018 at 1:43
• I think in essence what it seems like I am trying to understand is the direction cosine matrix. I thought the rotation is calculated by the difference in the vector - ie. as my body moves in relation to the fixed reference frame is the same as assuming the body remains still but the fixed reference frame rotates. so to obtain the rotation can that be calculated from the difference in the vectors or do I use my gyroscope to know how much I've rotated then compare that to what my vector should be? or is there a reverse calculation somewhere Nov 7, 2018 at 1:46
• So I constructed a 3x3 matrix with float values using the outputs normalized from my 3 sensors. can I check a few outputs 90 roll (0,0,1 / 0,1,0 / 1,0,0) then return to reference -90 pitch (nosedive) ( -1,0,0 / 0,0,-1 \ 0,1,0) I'll do some combination readings of various angles. so then if I construct another 3x3 matrix using the gyroscope - do I have to create a 3x3 matrix for each angle or is there a way to create it all in 1 3x3 matrix combining the 3 axis rotations using deltaT*angular velocity? Nov 7, 2018 at 6:16