For MEMS Accelerometer as used in cell phone, after reading out three individual g values from x, y and z axis of sensor, what is the maths formula to calculate the 'resultant' vector (direction and magnitude)?
That is it: \$\vec{v} = (x, y, z)\$. Its magnitude is \$\lVert \vec{v} \rVert = \sqrt{x^2 + y^2 + z^2}\$. It is often useful to
normalize the direction to an unit length vector. In linear algebra form, I recommend column vectors, so that matrix-vector multiplication always has matrix on the left and vector on the right, and in general, operations have oldest or original rightmost, and advance leftwards.
In math terms:
$$\vec{v} = (x, y, z) = \left[ \begin{matrix} x \\ y \\ z \end{matrix} \right ]$$
Its length (or equivalently magnitude) is
$$n = \lVert\vec{v}\rVert = \sqrt{x^2 + y^2 + z^2}$$
and the unit direction vector is
$$\hat{v} = \left(\frac{x}{n}, \frac{y}{n}, \frac{z}{n}\right) = \left[ \begin{matrix} \frac{x}{\sqrt{x^2 + y^2 + z^2}} \\ \frac{y}{\sqrt{x^2 + y^2 + z^2}} \\ \frac{z}{\sqrt{x^2 + y^2 + z^2}} \end{matrix} \right ]$$
When a vector is normalized, it means the vector is scaled to unit length as above, by dividing each component by the magnitude of the original vector.
Do not use Euler or Tait-Bryan angles, or you will have gimbal lock. If you need complex rotations, use versors AKA unit quaternions, or equivalently bivectors.
How to calculate the difference between 'TWO VECTORS'? For example, if the sensor is placed flat on table, x and y axis is zero. Z is 9.81 ms-2 (g value). Call this \$\vec{v}_0\$. If I rotate the sesnor and the x,y,z resultant is \$\vec{v}_1\$. How to calculate the difference?
You have \$\vec{v}_0 = ( x_0, y_0, z_0 )\$ and \$\vec{v}_1 = ( x_1, y_1, z_1 )\$. First, calculate their lengths/magnitudes/Euclidean norms:
$$\begin{aligned}
n_0 &= \left\lVert \vec{v}_0 \right\rVert &= \sqrt{x_0^2 + y_0^2 + z_0^2} \\
n_1 &= \left\lVert \vec{v}_1 \right\rVert &= \sqrt{x_1^2 + y_1^2 + z_1^2} \\
\end{aligned}$$
Note that the magnitudes are acceleration in meters per second per second, m·s⁻². Next, calculate the corresponding unit vectors:
$$\begin{aligned}
\hat{v}_0 &= \left( \frac{x_0}{n_0}, \frac{y_0}{n_0}, \frac{z_0}{n_0} \right) \\
\hat{v}_1 &= \left( \frac{x_1}{n_1}, \frac{y_1}{n_1}, \frac{z_1}{n_1} \right) \\
\end{aligned}$$
The second direction corresponds to the first direction rotated by angle \$\varphi\$ around unit axis vector \$\hat{a}\$:
$$\begin{aligned}
\cos \varphi &= \hat{v}_0 \cdot \hat{v}_1 \\
\vec{a} &= \hat{v}_0 \times \hat{v}_1 \\
\sin \varphi &= \left\lVert \vec{a} \right\rVert \\
\hat{a} &= \frac{\vec{a}}{\left\lVert \vec{a} \right\rVert} \\
\end{aligned}$$
Note that \$-1 \le \hat{v}_0 \cdot \hat{v}_1 \le +1\$. If \$\hat{v}_0 \cdot \hat{v}_1 = 1\$, the two directions are the same, and \$\varphi = 0°\$. If \$\hat{v}_0 \cdot \hat{v}_1 = -1\$, the two directions are opposite. In both these cases \$\vec{a}\$ and thus \$\hat{a}\$ are undefined.
Any standard c language library to do the maths?
Don't need one, really.
#include <math.h>
#define EPS 0.00000001f // Largest positive value considered zero or neglible
typedef struct {
float x;
float y;
float z;
} vec3f;
vec3f Vec3f(float x, float y, float z) {
return (vec3f){ .x=x, .y=y, .z=z };
}
float vec3f_length(vec3f v) {
return sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
}
float vec3f_dot(vec3f a, vec3f b) {
return a.x*b.x + a.y*b.y + a.z*b.z;
}
vec3f vec3f_cross(vec3f a, vec3f b) {
return (vec3f){ .x = a.y*b.z-a.z*b.y, .y = a.z*b.x-a.x*b.z, .z = a.x*b.y-a.y*b.x };
}
vec3f vec3f_unit(vec3f a) {
const float n = vec3f_length(a);
if (a <= EPS) {
return (vec3f){ .x = 0.0f, .y = 0.0f, .z = 0.0f };
} else {
return (vec3f){ .x = a.x/n, .y = a.y/n, .z = a.z/n };
}
}
vec3f vec3f_mul(vec3f a, float c) {
return (vec3f){ .x = c*a.x, .y = c*a.y, .z = c*a.z };
}
vec3f vec3f_div(vec3f a, float d) {
return (vec3f){ .x = a.x/d, .y = a.y/d, .z = a.z/d };
}
with which the abovementioned problem is solved via
vec3f v0, v1;
float n0 = vec3f_length(v0);
float n1 = vec3f_length(v1);
if (n0 <= EPS) {
// 0 was in free fall, abort
} else
if (n1 <= EPS) {
// 1 was in free fall, abort
}
vec3f u0 = vec3f_div(v0, n0);
vec3f u1 = vec3f_div(v1, n0);
float c = vec3f_dot(u0, u1);
if (c >= 1.0f - EPS) {
// The two orientations are the same; abort.
} else
if (c <= EPS - 1.0f) {
// The two orientations are opposite; abort.
}
vec3f a = vec3f_cross(u0, u1);
float s = vec3f_length(a);
if (s >= -EPS && s <= EPS) {
// neglible rotation, so use zero vector.
a = (vec3f){ .x = 0.0f, .y = 0.0f, .z = 0.0f };
} else {
a = vec3f_div(s);
}
float varphi = atan2f(s, c); // Better precision than asinf(s) or acosf(c).
which gives
a as the rotation axis vector, and
varphi as the rotation angle around that vector in radians. Multiply
varphi by 360/(2π) ≃ 57.29578f to get the result in degrees.
If you need rotations, then orientation and rotation are best described using versor/bivector
typedef struct {
float x;
float y;
float z;
float r; // Often listed first; I like to put last for shenanigans
} rot3f;
rot3f Rot3f(float q, float i, float j, float k) {
float n = sqrtf(q*q + i*i + j*j + k*k);
if (n > EPS)
return (rot3f){ .x = i/n, .y = j/n, .z = k/n, .r = q/n };
else
return (rot3f){ .x = 0.0f, .y = 0.0f, .z = 0.0f, .r = 1.0f };
}
rot3f rot3f_around(vecf axis, float angle) {
float n = sqrtf(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);
if (n > EPS) {
float c = cosf(angle * 0.5f);
float s = sinf(angle * 0.5f) / n;
return (rot3f){ .r = c, .x = s*axis.x, .y = s*axis.y, .z = s*axis.z };
} else {
return (rot3f){ .r = 1.0f, .x = 0.0f, .y = 0.0f, .z = 0.0f };
}
}
and vectors are rotated using a 3-column, 4-vector matrix,
typedef struct {
vec3f x; // Unit x axis vector after rotation
vec3f y; // Unit y axis vector after rotation
vec3f z; // Unit z axis vector after rotation
vec3f t; // Translation after rotation
} trans3f;
void trans3f_apply(const trans3f *t, const vec3f *v, size_t n) {
while (n-->0) {
float x = v->x * t->x.x + v->y * t->y.x + v->z * t->z.x + t->t.x;
float y = v->x * t->x.y + v->y * t->y.y + v->z * t->z.y + t->t.y;
float z = v->x * t->x.z + v->y * t->y.z + v->z * t->z.z + t->t.z;
v->x = x;
v->y = y;
v->z = z;
v++;
}
}
I omitted the function how to rotate an orientation or rotation by another (
Hamilton product, orientation right, applied rotation left side), and how to turn an orientation into a matrix (
this), because I don't remember them out of hand.
Home
Help
Search
Login
Register
