Background

Recently I had a chance to implement Asynchronous reprojection (a.k.a Asynchronous timewarp) for VR runtime. It requires more than Affine transform, so I had to invest few days to figure out what I have to know and how I can use them in my code. I could inject quite nice math into my brain. Before volatilizing everything, let me record them.
webvr project page

OpenGL matrix

clear explanation of column major matrix for OpenGL

Column major matrix

e.g. translate(x, y, z) is

T = ((1,0,0,0), (0,1,0,0), (0,0,1,0), (x,y,z,1))

Model View Projection matrix

How to construct projection matrix
P·V·M·v = h, for h=[-1,1]
and then rasterized in the texture coord.

T·P·V·M·v = T·h = t
T = ((0.5,0,0,0), (0,0.5,0,0), (0,0,0,0), (0.5,0.5,0,1))

Reprojection

To reproject (i.e. rotation only) the given app texture, what kind of matrix is needed? Let say the matrix is Re

Re will be applied to vertex (i.e. 4 points square ((-1,-1),(1,-1),(-1,1),(1,1))) in homogeneous coord in vertex shader. In general, vertex to homogeneous v -> h transformed by P (i.e. projection), V (i.e. view), M (i.e. model) matrix. P·V·M·v = h
In VR case, M = I, so P·V·v = h
- For given field Of view fov, P is

Matrix4x4 ProjectionFromFieldOfView(
    const FieldOfView* fov) {
  // From WebXR spec. When a user changes depth value, no way to obtain the
  // value. Fortunately, the depth value affects the matrix insignificantly
  // unless the user sets the depths to some extreme value.
  constexpr float kDepthNear = 0.01f;
  constexpr float kDepthFar = 10000.0f;
  // Check http://www.songho.ca/opengl/gl_projectionmatrix.html
  // default upDegrees is 45
  float up_tan = tan(fov->upDegrees * PI / 180.0);
  float down_tan = tan(fov->downDegrees * PI / 180.0);
  float left_tan = tan(fov->leftDegrees * PI / 180.0);
  float right_tan = tan(fov->rightDegrees * PI / 180.0);
  float x_scale = 2.0f / (left_tan + right_tan);
  float y_scale = 2.0f / (up_tan + down_tan);

  /* clang-format off */
  return Matrix4x4(
      x_scale, 0.f, -((left_tan - right_tan) * x_scale * 0.5f), 0.f,
      0.f, y_scale, ((up_tan - down_tan) * y_scale * 0.5f), 0.f,
      0.f, 0.f,
      (kDepthNear + kDepthFar) / (kDepthNear - kDepthFar),
      (2 * kDepthFar * kDepthNear) / (kDepthNear - kDepthFar),
      0.0f, 0.0f, -1.f, 0.0f);
  /* clang-format on */
}

V can be decomposed by T (i.e. translate), R (i.e. rotate), Te (i.e. translate for eye), so V = Te·R·T
So we start from P·Te·R·T·v = h
- For given IPD (i.e. interpupillary distance) Te = ((1,0,0,0), (0,1,0,0), (0,0,1,0), (+-IPD/2,0,0,1))
As the depth info is lost, it doesn't make sense to apply inverse projection matrix P^. To approximate it, put the output texture at -1 z-plane in homogeneous coord. and transform it from homogeneous coord. to view world coord.
The transform has the value, which the projection matrix can transform it to entire viewport on screen again.
Let say the transform is Pv. Pv·P·Te·R·T·v = Te·R·T·v = Pv·h

Matrix4x4 Pv(const FieldOfView* fov) {
  // the assumed depth doesn't matter unless it's an extreme value.
  constexpr float kAssumedDepth = 1.f;

  // For x axis, [-1, 1] -> [d * l / n, d * r / n]
  float up_tan = tan(fov->upDegrees * PI / 180.0);
  float down_tan = tan(fov->downDegrees * PI / 180.0);
  float left_tan = tan(fov->leftDegrees * PI / 180.0);
  float right_tan = tan(fov->rightDegrees * PI / 180.0);
  float x_scale = (right_tan + left_tan) / 2.f * kAssumedDepth;
  float y_scale = (up_tan + down_tan) / 2.f * kAssumedDepth;
  float x_translate = (right_tan - left_tan) / 2.f * kAssumedDepth;
  float y_translate = (up_tan - down_tan) / 2.f * kAssumedDepth;
  float z_translate = -kAssumedDepth;

  // For z axis, 0 -> kAssumedDepth
  /* clang-format off */
  return Matrix4x4(
      x_scale, 0.f, 0.f, x_translate,
      0.f, y_scale, 0.f, y_translate,
      0.f, 0.f, 1.f, z_translate,
      0.f, 0.f, 0.f, 1.f);
  /* clang-format on */
}

Now it's straightforward. P·Te·dR·Te^·Pv·P·Te·R·T·v = P·Te·dR·R·T·v = P·Te·dR·Te^·Pv·h
So Re = P·Te·dR·Te^·Pv

Lens correction

Speeding up GPU barrel distortion correction in mobile VR
After reprojection done, apply lens correction.
Should use vertex shader mesh optimization rather than frag shader, it's because
- save GPU resources
- can merge reprojection and lens correction in one pass drawing. check below section
Good example of mesh optimization in OSVR

Homography transformation in VR reprojection

Math background of Homography transformation
Now we know reprojection matrix Re. However we cannot use it to transform vertex, because we use mesh based lens correction and all vertex must be still in homogeneous coord for correct lens correction.
So we should use Re in texture somehow. When Re is applied to texture ((0,0),(1,0),(0,1),(1,1)), it will be transformed something like ((0.5,0.3),(0.9,0.1),(0.5,0.7),(0.9,0.9)) trapezoid. The common mistake is to use it for texture coord. It scale up non-linearly the texture on screen because it samples smaller region on [(0,0), (1,1)] uv square.
How should the small trapezoid sample texture from [(0,0), (1,1)] square. It requires kind of inverse Re, but inverse Re cannot give the correct texel, because we already lost z-value, which is very significant for inverse Re.
Let say trapezoid region is source s and [(0,0), (1,1)] square is target t
Need to compute the transform matrix source s -> target t in texture coord. It's called Homography transformation, which is non-linear transform.
Here's the result formula we will use.

(tx, ty) = H(sx, sy)
         = (a·sx + b·sy + c, d·sx + e·sy + f) / (g·sx + h·sy + j)  // (1)

glsl shader needs to know a,b,c,d,e,f,g,h,j
Let say Mst = ((a,d,g),(b,e,h),(c,f,j)) in column major matrix
Mst = (t0 t1 t2)·Diagonal(τ30/σ30, τ31/σ31, τ32/σ32)·(s0 s1 s2)^ // (2)
τ3i and σ3i are coefficients in barycentric coordinates.

t3 = τ30·t0 + τ31·t1 + τ32·t2
s3 = σ30·s0 + σ31·s1 + σ32·s2

The coefficients can be obtained by Transpose(σ30 σ31 σ32) = (s0 s1 s2)^·s3
compute Mst and send it to the shader, which will compute (1) in vertex shader.
(sx, sy) in the shader is original texture uv, which can be [(0,0), (1,1)] square or texture uv fed by mesh.
the common vertex shader looks like

const char vert_str[] = SHADER(
    precision highp float;
    layout(location = 0) in vec4 a_pos;
    layout(location = 1) in vec2 a_tex_coord_r;
    layout(location = 2) in vec2 a_tex_coord_g;
    layout(location = 3) in vec2 a_tex_coord_b;
    uniform mat4 u_tex_matrix;
    out vec2 v_tex_r;
    out vec2 v_tex_g;
    out vec2 v_tex_b;

    vec2 TexTransform(vec2 tex);

    void main(void) {
      gl_Position = a_pos;
      v_tex_r = TexTransform(a_tex_coord_r);
      v_tex_g = TexTransform(a_tex_coord_g);
      v_tex_b = TexTransform(a_tex_coord_b);
    }
    uniform mat3 u_tex_homography_matrix;

    vec2 Reproject(vec2 tex) {
      vec3 homogeneous_tex = vec3(tex, 1);
      float denominator = dot(u_tex_homography_matrix[2], homogeneous_tex);
      return vec2(
          dot(u_tex_homography_matrix[0], homogeneous_tex) / denominator,
          dot(u_tex_homography_matrix[1], homogeneous_tex) / denominator);
    }

    vec2 TexTransform(vec2 tex) {
      vec2 reprojected_tex = Reproject(tex);
      return vec2(u_tex_matrix * vec4(reprojected_tex, 0, 1));
    });

const char frag_str[] = SHADER(
    precision mediump float;
    in vec2 v_tex_r;
    in vec2 v_tex_g;
    in vec2 v_tex_b;
    uniform sampler2D u_tex_sampler;
    layout(location = 0) out vec4 outColor;
    void main(void) {
      outColor.r = texture(u_tex_sampler, v_tex_r).r;
      outColor.g = texture(u_tex_sampler, v_tex_g).g;
      outColor.b = texture(u_tex_sampler, v_tex_b).b;
    });

Here's how eq(1) is derived
Here's how to get a,b,c,d,e,f,g,h,j from my project.

void CalculateHomographyMatrix(
    float (&out_homography_matrix)[9],
    const gfx::Transform& reprojection_transform,
    bool y_flip) {
  /* clang-format off */
  // For x axis, [-1, 1] -> [0, 1]
  const gfx::Transform homogeneous_to_tex_matrix =
      gfx::Transform(.5f, 0.f, 0.f, .5f,
                     0.f, .5f, 0.f, .5f,
                     0.f, 0.f, 1.f, 0.f,
                     0.f, 0.f, 0.f, 1.f);
  // For x axis, [0, 1] -> [-1, 1]
  const gfx::Transform tex_to_homogeneous_matrix =
      gfx::Transform(2.f, 0.f, 0.f, -1.f,
                     0.f, 2.f, 0.f, -1.f,
                     0.f, 0.f, 1.f, 0.f,
                     0.f, 0.f, 0.f, 1.f);
  const gfx::Transform y_flip_matrix =
      gfx::Transform(1.f, 0.f, 0.f, 0.f,
                     0.f, -1.f, 0.f, 0.f,
                     0.f, 0.f, 1.f, 0.f,
                     0.f, 0.f, 0.f, 1.f);
  /* clang-format on */

  // reprojection_transform R works in homogeneous coord. Let's convert it to
  // tex coord. H is tex to homogeneous matrix
  // h2 = R·h1
  // H·t2 = R·H·t1
  // t2 = H^·R·H·t1
  gfx::Transform tex_reprojection;
  if (y_flip) {
    // The reprojected output will be y-flipped while sensor doesn't have any
    // idea. So kind of flip dR from sensor to offset to future flip in the
    // shader.
    tex_reprojection = homogeneous_to_tex_matrix * y_flip_matrix *
                       reprojection_transform * y_flip_matrix *
                       tex_to_homogeneous_matrix;
  } else {
    tex_reprojection = homogeneous_to_tex_matrix * reprojection_transform *
                       tex_to_homogeneous_matrix;
  }

  SkVector4 t00(0, 0, 0, 1);
  SkVector4 t10(1, 0, 0, 1);
  SkVector4 t01(0, 1, 0, 1);
  SkVector4 t11(1, 1, 0, 1);

  SkVector4 s00 = tex_reprojection.matrix() * t00;
  SkVector4 s10 = tex_reprojection.matrix() * t10;
  SkVector4 s01 = tex_reprojection.matrix() * t01;
  SkVector4 s11 = tex_reprojection.matrix() * t11;

  // We computes following code using 2D affine matrix, but chromium doesn't
  // support column major 3x3 matrix, so we abuse SkMatirx44 and SkVector4.
  SkVector4 t0(0, 0, 1, 0);
  SkVector4 t1(1, 0, 1, 0);
  SkVector4 t2(0, 1, 1, 0);
  SkVector4 t3(1, 1, 1, 0);

  // 3D -> 2D projection loses z-value.
  SkVector4 s0(s00.fData[0] / s00.fData[3], s00.fData[1] / s00.fData[3], 1, 0);
  SkVector4 s1(s10.fData[0] / s10.fData[3], s10.fData[1] / s10.fData[3], 1, 0);
  SkVector4 s2(s01.fData[0] / s01.fData[3], s01.fData[1] / s01.fData[3], 1, 0);
  SkVector4 s3(s11.fData[0] / s11.fData[3], s11.fData[1] / s11.fData[3], 1, 0);

  // Need to compute the transform matrix source s -> target t in texture coord.
  // ...
  // Let's compute Mst and send it to the shader.

  // τ3i won't be changed. τ3 = (-1 1 1)
  SkVector4 tau3(-1, 1, 1, 0);
  SkMatrix44 t012 = ColumnsToMatrix(t0, t1, t2);

  // Get (s0 s1 s2)^ and (σ30 σ31 σ32)
  gfx::Matrix3F s012 = gfx::Matrix3F::Identity();
  s012.set_column(0, ToVector3dF(s0));
  s012.set_column(1, ToVector3dF(s1));
  s012.set_column(2, ToVector3dF(s2));
  SkMatrix44 inv_s012 = ToSkMatrix44(s012.Inverse());
  SkVector4 sigma3 = inv_s012 * s3;

  // Get Diagonal(τ30/σ30, τ31/σ31, τ32/σ32)
  SkMatrix44 diagonal_tau_sigma(SkMatrix44::kUninitialized_Constructor);
  diagonal_tau_sigma.setScale(tau3.fData[0] / sigma3.fData[0],
                              tau3.fData[1] / sigma3.fData[1],
                              tau3.fData[2] / sigma3.fData[2]);

  // Time to get Mst
  SkMatrix44 m_st = t012 * diagonal_tau_sigma * inv_s012;
  float m16[16];
  // Want a,b,c order, not a,d,g order
  m_st.asRowMajorf(m16);
  To3x3(out_homography_matrix, m16);
}

Quaternion and rotation

Quaternions in detail

Basic

q = a + bi + cj + dk
i^2 = j^2 = k^2 = ijk = −1
(v_a, a)·(v_b, b) = (a·v_b + b·v_a + v_a x v_b, ab - v_a·v_b)
understanding quaternions

Rotation

3D Rotations and Quaternion Exponentials

Youtube: 3D Rotations and Quaternion Exponentials: Special Case
How rotation in vector space can be expressed by quaternion in special case.

Rodrigues Rotation Formula

Youtube: 3D Rotations in General: Rodrigues Rotation Formula and Quaternion Exponentials
Check Rodrigues Rotation Formula, which is general case of vector rotation
Now let's see how general vector rotation is expressed by quaternion. It's kinda magic, isn't it?

Convert to Affine transform

wiki Quaternion-derived_rotation_matrix
wiki Conversion between quaternions and Euler angles
Prove
Let say q = (con(0/2), sin(0/2) * n_v) = (w, x, y, z)

Matrix4x4(const Quaternion& q)
    : matrix_(SkMatrix44::kUninitialized_Constructor) {
  double x = q.x();
  double y = q.y();
  double z = q.z();
  double w = q.w();

  // Implicitly calls matrix.setIdentity()
  matrix_.set3x3(1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y + z * w),       2.0 * (x * z - y * w),
                 2.0 * (x * y - z * w),       1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z + x * w),
                 2.0 * (x * z + y * w),       2.0 * (y * z - x * w),       1.0 - 2.0 * (x * x + y * y));
}

`dq = q2·q1*`

dq from q1 to q2 is q2·q1*, because

v' = q·v·q*
v1 = q1·v0·q1*
v2 = q2·v0·q2* = q2·q1*·q1·v0·q1*·q1·q2*

dq = q2·q1*
q2 = dq·q1

rotation of the coordinate frame

When your IMU (i.e. Inertial measurement unit) sensor gives orientation as a quaternion, you should consider it.
In the case, the quaternion represents the rotation of the coordinate frame.
frame rotation commutes quaternion operation because it's kind of opposite rotation.
L(f) = q*·f·q
Check chapter 8 in Quaternions in detail
dq = q1*·q2, which has also opposite commutation.
Let me explain briefly why commutation is opposite.
- Given point orientation q
- Frame orientation is inverse p = q*
- L(v) = q·v·q* = p*·v·p

Why orientation `q` in VR is quaternion for the coordinate frame?

Let the orientation of the sensor is q1 at t1 and q2 at t2
Let v1 (i.e. (1,0,0)) is the vector in sensor coordinates. The corresponding w in world coordinates is w = q1·v1·q1*.
Why we use L(f) = q*·f·q (i.e. frame transform) even though it looks like q working on a vector in sensor coordinates. It's because the unit vector of q1 is described in w coordinates.
Let say map w to v1 to v2, it can be described v1 = q1*·w·q1 and then v2 = q2*·v1·q2. Note: the unit vector of q2 is described in q1 coordinates, which is actually angular velocity ω. q1 is the map to world to v1 and q2 is the map to v1 to v2.
q2 = exp(ω dt / 2) = dq Note: ω is described in q1 coordinates. **It's the key point why dq is applied behind. **
so v2 = dq*·q1*·w·q1·dq
so q(t) = q0·dq
Check 2 Quaternion representation for more rigorous analysis.

dq from gyro

In same sense, dq from gyro sensor must be operated like q2 = q1·dq, because dq is physically meaningful in q1 sensor coordinates.
gyro sensor provides angular velocity ω. dq = Quaterion(ω*dt/2)
q2 = q1·dq

camera matrix

eventually, q is used for camera orientation matrix
camera orientation matrix is view matrix among P·V·M
V is inverse matrix of sensor orientation q, because we want to watch the scene from camera (i.e. sensor) perspective.
We already use q to map world to sensor, which is kind of inverse matrix.
it's why blink xr code gives inverse matrix of q as pose.getViewMatrix(view).

`dq = q1*·q2` for VR

dq from q1 to q2 is q1*·q2, because

v' = q*·v·q
v1 = q1*·v0·q1
v2 = q2*·v0·q2 = q2*·q1·q1*·v0·q1·q1*·q2 = q2*·q1·v1·q1*·q2

the unit vector of dq is expressed in v1 coordinates.
dq = q1*·q2
q2 = q1·dq

Expected `q(t)` with given orientation quaternion `q0` and angular velocity `w`

Let say IMU sensor gives orientation quaternion q0 and angular velocity w
Wanna know q(t) after t
IMU sensor give value from frame rotation perspective, so dq = q0*·q1 -> q1 = q0·dq
dθ(t) = w*t
dq(t) = exp(dθ(t)/2) = exp(w*t/2)
q(t) = q0·dq(t) = q0·exp(w*t/2)

Quaternion differentiation `dq(t)/dt`

Quaternion differentiation
Above q(t) is derived for frame rotation. In general, with constant angular velocity w, q(t) = dq(t)·q0 = exp(w*t/2)·q0.
dq(t)/dt = d/dt(q0·exp(w*t/2)) = q0·d(exp(w*t/2))/dt = q0·exp(w*t/2)·w/2 = 1/2 q(t)·w

dq/dt

Rotation by mouse or touch using quaternion

difference between previous mouse position and current position is converted to dq
dx is kind of angle for y axis, and dy is for x axis.
dq is as follows

angle = sqrt(dx*dx + dy*dy)
n_v = (dy, dx, 0) / angle
amplifier = 0.5 // for some reasons, 0.5 is good in my app. Take your value.
angle = angle * amplifier
dq = (cos(angle/2), sin(angle/2)*n_v)

so new q2 can be obtained by q2 = dq·q1
sample code for javascript

<script src="js/gl-matrix.js"></script>
            // -- Mouse Behaviour
            var modelMatrix = mat4.create();
            var modelQuat = quat.create();
            var mouseDown = false;
            var lastMouseX = 0;
            var lastMouseY = 0;

            canvas.onmousedown = function (event) {
                mouseDown = true;
                lastMouseX = event.clientX;
                lastMouseY = event.clientY;
            };

            canvas.onmouseup = function (event) {
                mouseDown = false;
            };

            canvas.onmousemove = function (event) {
                if (mouseDown) {
                    var newX = event.clientX;
                    var newY = event.clientY;

                    var amplifier = 0.5;
                    var deltaY = (newX - lastMouseX) * amplifier;
                    var deltaX = (newY - lastMouseY) * amplifier;

                    var dq = quat.create();
                    quat.fromEuler(dq, deltaX, deltaY, 0);
                    // q2 = dq·q1
                    quat.multiply(modelQuat, dq, modelQuat);
                    mat4.fromQuat(modelMatrix, modelQuat);

                    lastMouseX = newX;
                    lastMouseY = newY;
                }
            };

BTW, I found Youtube 360 video may not use quaternion not to rotate z-axis. The code should be like

this.rotationVec_ = vec3.create();
...

  onMouseMove(event) {
    if (this.mouseDown) {
      const newX = event.clientX;
      const newY = event.clientY;

      const amplifier = 0.1 * Math.PI / 180;
      let deltaX = -(newY - this.lastMouseY) * amplifier;
      let deltaY = -(newX - this.lastMouseX) * amplifier;

      let newXRot = this.rotationVec_[0] + deltaX;
      newXRot = Math.max(Math.min(newXRot, Math.PI / 2), -Math.PI / 2);
      const newYRot = this.rotationVec_[1] + deltaY;
      vec3.set(this.rotationVec_, newXRot, newYRot, 0);

      const xRotMat = mat4.create();
      mat4.fromXRotation(xRotMat, newXRot);
      const yRotMat = mat4.create();
      mat4.fromYRotation(yRotMat, newYRot);
      const RotMat = mat4.create();
      mat4.multiply(this.mvMatrix_, xRotMat, yRotMat);

      this.lastMouseX = newX;
      this.lastMouseY = newY;
    }
  }

Extract yaw quaternion

note: y-axis is head axis in the code

gfx::Quaternion VRTransforms::ExtractYaw(const gfx::Quaternion& q) {
  double norm = sqrt(q.w()*q.w() + q.y()*q.y());
  return gfx::Quaternion(0, q.y() / norm, 0, q.w() / norm);
}

above code works, but hard to analize mathematically. When I try to do below which is more understandable in geometry, I got same result. However, formula looks so different. How do they same? lol

gfx::Quaternion ExtractYaw(const gfx::Quaternion& q) {
  gfx::Quaternion eye(0, 0, 1.f, 0);
  gfx::Quaternion eye_world = q * eye * q.inverse();

  // project |eye_world| on the zx plane.
  float cos_yaw = eye_world.z();
  float sin_yaw = eye_world.x();
  float norm = sqrt(cos_yaw * cos_yaw + sin_yaw * sin_yaw);
  cos_yaw /= norm;
  sin_yaw /= norm;

  // https://en.wikipedia.org/wiki/List_of_trigonometric_identities#Half-angle_formulae
  float sin_half = sqrt((1.f - cos_yaw) / 2.f);
  float cos_half = sqrt((1.f + cos_yaw) / 2.f);
  // Check the below picture why
  if (sin_yaw < 0)
    cos_half *= -1.f

  return gfx::Quaternion(0, sin_half, 0, cos_half);
}

It's why the above code change the sign of cos_half.

SLERP

dq = q2·q1* = exp(dθ/2 * n_v)
dq = (a, n_v), t=percent
axis = n_v
dθ = acos(a) * 2

Where t is a percentage: dq(t)

dθ(t) = dθ * t = acos(a) * 2 * t
dq(t) = exp(dθ(t)/2 * n_v) = (sin(dθ(t)/2), cos(dθ(t)/2) * n_v)
Slerp(q1,q2;t) = dq(t)·q1

TODO: IDK, it's somehow expressed in geometric slerp

cos Ω = doc(q1, q2)
Slerp(q1,q2;t)=sin((1-t)Ω)/sinΩ * q1 + sin(tΩ)/sinΩ * q2

Note: sin(Ω-θ) = sinΩ*cosθ - cosΩ*sinθ

Sensor fusion

IMU data fusing - everthing together

Kalman filter

How a Kalman filter works, in pictures - very easy to understand with pictures
Kalman_filter wiki - easy to see all formular
chromium impl for pointer input - it's kind of most simple case but easy to understand how to impl it.
IMU kalman filter simple
kalman filter youtube - extended

Complementary filter

Advanced

Mahony - Nonlinear Complementary Filters
Mahony - quaternion form
Madgwick - which I used
- my Madgwick code for chromium

class OmegaVRDevice::AHRS {
 public:
  AHRS() = default;
  ~AHRS() = default;

  void reset();
  gfx::Quaternion getOrientation() const { return q_; }
  void update(const gfx::Vector3dF& angular_velocity,
              const gfx::Vector3dF& acceleration,
              float dt);

 private:
  void updateMadgwick(const gfx::Vector3dF& angular_velocity,
                      const gfx::Vector3dF& acceleration,
                      float dt);

  void updateMahony(const gfx::Vector3dF& angular_velocity,
                    const gfx::Vector3dF& acceleration,
                    float dt);

  enum Algorithm { Madgwick, Mahony } static constexpr kAlgorithm = Madgwick;
  // the divergence rate of a quaternion derivative corresponding to the
  // gyroscope measurement error
  float beta_ = 0.01f;

  // ζ: the gain of gyroscope measurements error (i.e. gyro drift)
  float zeta_ = 0.1f;

  // http://www.olliw.eu/2013/imu-data-fusing/#chapter34
  float Kp_ = 0.5f;
  float Ki_ = Kp_ * Kp_ * 0.08f;
  gfx::Vector3dF e_I_;

  gfx::Quaternion q_;
};

void AHRS::reset() {
  q_ = gfx::Quaternion();
}

void AHRS::update(const gfx::Vector3dF& angular_velocity,
                                 const gfx::Vector3dF& acceleration,
                                 float dt) {
  switch (kAlgorithm) {
    case Algorithm::Madgwick:
      updateMadgwick(angular_velocity, acceleration, dt);
      return;
    case Algorithm::Mahony:
      updateMahony(angular_velocity, acceleration, dt);
      return;
  }
}

void AHRS::updateMadgwick(const gfx::Vector3dF& angular_velocity,
                                         const gfx::Vector3dF& acceleration,
                                         float dt) {
  gfx::Vector3dF unit_a;
  if (!acceleration.GetNormalized(&unit_a))
    return;

  // The gravity vector is (0, -1, 0)
  const gfx::Transform jacobian_tranpose = gfx::Transform(
      -2 * q_.z(), 0, 2 * q_.x(), 0,
      -2 * q_.y(), 4 * q_.x(), 2 * q_.w(), 0,
      -2 * q_.x(), 0, -2 * q_.z(), 0,
      -2 * q_.w(), 4 * q_.z(), -2 * q_.y(), 0);
  SkVector4 f(-2 * (q_.w() * q_.z() + q_.x() * q_.y()) - unit_a.x(),
              -2 * (1.f / 2 - q_.x() * q_.x() - q_.z() * q_.z()) - unit_a.y(),
              -2 * (q_.y() * q_.z() - q_.w() * q_.x()) - unit_a.z(),
              0);

  // the direction of the error, dq_e/dt
  SkVector4 s = jacobian_tranpose.matrix() * f;
  // NOTE: 0 index is w
  gfx::Quaternion dq_e(s.fData[1], s.fData[2], s.fData[3], s.fData[0]);
  dq_e = dq_e.Normalized();
  gfx::Quaternion w(angular_velocity.x(), angular_velocity.y(),
                    angular_velocity.z(), 0);

  // handle gyro drift
  if (zeta_) {
    gfx::Quaternion w_e = 2.f * q_.inverse() * dq_e;
    w = w - zeta_ * w_e * dt;
  }

  // the rate of change of orientation measured by the gyroscopes, dq_w/dt
  gfx::Quaternion dq_w = 0.5f * q_ * w;

  // the estimated rate of change of orientation, dq_est/dt
  gfx::Quaternion dq_est = dq_w - beta_ * dq_e;

  q_ = q_ + dq_est * dt;
  q_ = q_.Normalized();
}

void AHRS::updateMahony(const gfx::Vector3dF& angular_velocity,
                                       const gfx::Vector3dF& acceleration,
                                       float dt) {
  gfx::Vector3dF unit_a;
  if (!acceleration.GetNormalized(&unit_a))
    return;

  // q_*·e_q·q_
  // The gravity vector is (0, -1, 0)
  gfx::Vector3dF gravity_direction(
      -2 * (q_.w() * q_.z() + q_.x() * q_.y()),
      -2 * (1.f / 2 - q_.x() * q_.x() - q_.z() * q_.z()),
      -2 * (q_.y() * q_.z() - q_.w() * q_.x()));

  gfx::Vector3dF error_vector(unit_a);
  error_vector.Cross(gravity_direction);

  gfx::Vector3dF error_I = error_vector;
  error_I.Scale(Ki_ * dt);
  e_I_ += error_I;

  gfx::Vector3dF error_P = error_vector;
  error_P.Scale(Kp_);

  gfx::Vector3dF corrected_w = angular_velocity + error_P + e_I_;
  gfx::Quaternion w(corrected_w.x(), corrected_w.y(), corrected_w.z(), 0);

  // the rate of change of orientation measured by the gyroscopes, dq/dt
  gfx::Quaternion dq = 0.5f * q_ * w;
  q_ = q_ + dq * dt;
  q_ = q_.Normalized();
}

above updateMahony uses 1-order taylor approximation as the paper says. However, it's not necessary.

// With given theta vector v_theta, rotation quaternion q is
// q = exp(v_theta/2)
//   = (con(abs(v_theta)/2), v_theta/abs(v_theta)sin(abs(v_theta)/2)
gfx::Quaternion RotationQuaternion(const gfx::Vector3dF& v_theta) {
  float abs_theta = v_theta.Length();
  // lim(abs_theta->0)(scale) = 0.5
  double scale = 0.5;
  constexpr double kEpsilon = 1e-5;
  if (abs_theta > kEpsilon) {
    scale = sin(abs_theta * .5f) / abs_theta;
  }

  gfx::Quaternion destination;
  destination.set_x(scale * v_theta.x());
  destination.set_y(scale * v_theta.y());
  destination.set_z(scale * v_theta.z());
  destination.set_w(cos(abs_theta * .5f));
  return destination;
}

void AHRS::updateMahony(const gfx::Vector3dF& angular_velocity,
                                       const gfx::Vector3dF& acceleration,
                                       float dt) {
  ...
  error_P.Scale(Kp_);

  gfx::Vector3dF d_theta = angular_velocity + error_P + e_I_;
  d_theta.Scale(dt);
  gfx::Quaternion dq = RotationQuaternion(d_theta);
  q_ = q_ * dq;
  q_ = q_.Normalized();
}

Observation
- it doesn't fix gyro drift no matter |zeta_| or not. Note: my WinMR device has gyro bias.
- after many iterations, quaternion becomes very skewed rotation.
- Mahony is a bit better than Madgwick in my case (i.e. only gravity correction by acceleration). Madgwick's quaternion is getting rotating by z-axis (i.e. eye vector in VR) no matter different gain.
Questions:
- (13) - why not dq = q1*·q2?
- (36) - can the complementary filter be applied to a quaternion? quaternion operation is not linear dq = q1*·q2
- (39) - what the... is it legit approximation?
- (44) - hmm, is it physically error of quaternion derivative?
- (49) - Is it really needed? the complementary filter in (36) already has low pass accelerometer correction. In addition, dq_e is purely derived from an accelerometer sensor, but (49) considers it as gyro drift. nonsense!

Complementary Filter on Quaternion

Background

Let me introduce Complementary Filter on Quaternion when gyro provides angular velocity and accelerometer provides acceleration.
I've read many complementary filters on the internet but all of them applies complementary filter to roll, pitch, yaw respectively. It's not correct because sensor roll can affect pitch and yaw in world coordinates.
Complementary filters consist of low pass filter for acceleromether and high pass filter for gyro.
θ' = (1-α)(θ + ωdt) + αθg
However, we cannot find single solution θg by g acceleration because gravity cannot determin yaw angle.
Mahony and Madrick has different approach. They calculare angular velocity correct term by gravity with gyro ω and finds ground truth angular velocity ω. The difference of the methods is how to approximate it. Mahony uses PID and Madrick uses gradient descent. However, I fail to understand how gravity is related to angular velocity in physics. It doesn't physically make sense.
See more detail of Mahony and Madrick
Fortunately, Mahony and Madrick provides awesome math foundation. Let me introduce new complementary filter which makes sense in physics.

Quaternion wrap up

Let say we have quaternion q(n, θ) which represents the orientation of the sensor by unit vector n and angle θ in Rodrigues Rotation Formula. n is represented in world coordinates.
so w = q·v·q*. When v is x-axis (1,0,0), we can know w in world coordinates.
however, we use v = q*·w·q equation more, because the unit vector n is represented in w coordinates and it makes it possible to derive this eq in the chain. (i.e. w to v1 to v2)
In general,

Complementary filter for quaternion

complementary_q

now only one thing we have to have is q_a. how can we have it?

all together with `q_a` solution

Let g_E is the gravity vector in world coordinates. we want to have g_S, which is the gravity vector in sensor coordinates. It can get via g_S = q*·g_E·q.
g_S should be similar to a_R which is measured value by the accelerometer. We want to compensate the diff to orientation quaternion q.

complementary_q_imu

The quaternion exp(dφ/2) makes sense in the sense that the quaternion of ω is exp(ω dt/2).
In physics, it can be explained as follows. We transform the gravity vector from world to sensor. Although the yaw angle of gravity vector is degenerated in world coordinated, q knows how to transform it to sensor coordinates, which is g_S.
our q transforms g_E to g_S (i.e. expected), but we wish q can transform g_E to a_R (i.e. real), which is measured by accelerometer. So q should be rotated more by dφ

calculate `dφ`

qaq

now we know dφ and ω, so we can update previous q to brand new q.

Why `φ = v_R x v_S` rather than `φ = v_S x v_R`

As you can see the correction in the above pic, I thought φ = v_S x v_R in the first time, but it was wrong.
Let's make similar problem in earth to sensor mapping, which we already know.
In this problem, we know v_E and v_S but we don't know q, in which v_S = q*·v_E·q.
If q ~ v_S x v_E, we can say φ = v_R x v_S. It's explained in the below pic.

Code and observation

when |alpha_| is 0.002, it shows pretty good results.

void AHRS::updateComplemetary(
    const gfx::Vector3dF& angular_velocity,
    const gfx::Vector3dF& acceleration,
    float dt) {
  gfx::Vector3dF unit_a;
  if (!acceleration.GetNormalized(&unit_a))
    return;

  gfx::Vector3dF d_theta = angular_velocity;
  d_theta.Scale(dt);
  gfx::Quaternion q_theta = RotationQuaternion(d_theta);

  // gravity_vector in sensor, q_*·g_w·q_
  // The gravity vector |g_w| is (0, -1, 0)
  gfx::Vector3dF gravity_vector(
      -2 * (q_.w() * q_.z() + q_.x() * q_.y()),
      -2 * (1.f / 2 - q_.x() * q_.x() - q_.z() * q_.z()),
      -2 * (q_.y() * q_.z() - q_.w() * q_.x()));

  gfx::Vector3dF phi_vector(unit_a);
  phi_vector.Cross(gravity_vector);
  float d_phi = phi_vector.Length();
  constexpr double kEpsilon = 1e-5;
  if (d_phi > kEpsilon) {
    float norm = asin(d_phi) / d_phi;
    phi_vector.Scale(norm);
  }
  phi_vector.Scale(alpha_);
  gfx::Quaternion q_phi = RotationQuaternion(phi_vector);

  q_ = (q_ * q_theta) * q_phi;
  q_ = q_.Normalized();
}

The experience is quite similar to Mahony and Magwick.
I still have gyro drift.
I prefer this way more than Mahony and Madgwick, because it more makes sense in physics and easier to adjust coefficients, which is only |alpha_|.

Limitation of complementary filter

dq is not unique. However, as it's an iterative process, q_R is eventually quite close to the ground truth.
It cannot find the ground truth of the angular momenta. It just trusts ω from gyro. If it has quite white noise, the final orientation will have quite white noise.
It cannot fix gyro drift in the same sense.

Extended Kalman filter on quaternion and quaternion derivative `(q, dq/dt)`

Background

The above complementary filter as well as Madgwick and Mahony don't do anything to white noise of ω from gyro.
The rational next step is using extended kalman filter on (q, dq/dt). I cannot find this approach on the internet. It may be because of mathematically challenging. I may try this approach first time.
Fortunately, Madgwick and Mahony provide enough mathematics already. Let's move on.

Kalman filter wrap up

I copy those from wikipedia.

Extended Kalman filter on `(q, dq/dt)`

Now we know F_k_8x8 and H_k_6x8 so easily calculates S_k_6x6 and K_k_8x6.

Code

TBD. 8x8 matrix discourages me to implement it lol

VR mathematics: opengl matrix, transform, quaternion, euler angles, homography transformation, reprojection, IMU sensor fusion - ds-hwang/wiki GitHub Wiki

Background

OpenGL matrix

Column major matrix

Model View Projection matrix

Reprojection

Lens correction

Homography transformation in VR reprojection

Quaternion and rotation

Basic

Rotation

3D Rotations and Quaternion Exponentials

Rodrigues Rotation Formula

Convert to Affine transform

dq = q2·q1*

rotation of the coordinate frame

Why orientation q in VR is quaternion for the coordinate frame?

dq = q1*·q2 for VR

Expected q(t) with given orientation quaternion q0 and angular velocity w

Quaternion differentiation dq(t)/dt

Rotation by mouse or touch using quaternion

Extract yaw quaternion

SLERP

Sensor fusion

Kalman filter

Complementary filter

Advanced

Complementary Filter on Quaternion

Background

Quaternion wrap up

Complementary filter for quaternion

all together with q_a solution

calculate dφ

Why φ = v_R x v_S rather than φ = v_S x v_R

Code and observation

Limitation of complementary filter

Extended Kalman filter on quaternion and quaternion derivative (q, dq/dt)

Background

Kalman filter wrap up

Extended Kalman filter on (q, dq/dt)

Code

⚠️ **GitHub.com Fallback** ⚠️

`dq = q2·q1*`

Why orientation `q` in VR is quaternion for the coordinate frame?

`dq = q1*·q2` for VR

Expected `q(t)` with given orientation quaternion `q0` and angular velocity `w`

Quaternion differentiation `dq(t)/dt`

all together with `q_a` solution

calculate `dφ`

Why `φ = v_R x v_S` rather than `φ = v_S x v_R`

Extended Kalman filter on quaternion and quaternion derivative `(q, dq/dt)`

Extended Kalman filter on `(q, dq/dt)`

⚠️ GitHub.com Fallback ⚠️