Quaternion estimation using the Gauss-Newton method.

gauss_newton_quat.py

#!/usr/bin/env python2


import math
import numpy as np
import sympy


def rotation_quat(v, a):
  q = np.zeros(4)
  q[0:3] = v * math.sin(a / 2)
  q[3] = math.cos(a / 2)
  return q

def rotate_vec(q, x):

  a = q[3]
  w = q[0:3]

  return x + 2 * a * np.cross(w, x) + 2 * np.cross(w, np.cross(w, x))

def estimate_q(y0, y1):

  # Initial guess.
  a = 1
  b = 1
  c = 1
  d = 1

  def form_m(R):
    return np.hstack((np.vstack((R, np.zeros((3, 3)))), np.vstack((np.zeros((3, 3)), R))))

  for _ in range(0, 5):
    # Compute the rotation matrix.
    R = np.matrix([
        [d * d + a * a - b * b - c * c, 2 * (a * b - c * d), 2 * (a * c + b * d)],
        [2 * (a * b + c * d), d * d + b * b - a * a - c * c, 2 * (b * c - a * d)],
        [2 * (a * c - b * d), 2 * (b * c + a * d), d * d + c * c - b * b - a * a]
    ])
    M = form_m(R)

    # Compute the derivative of the rotation.
    dRda = np.array([
        [ 2 * a,  2 * b,  2 * c],
        [ 2 * b, -2 * a, -2 * d],
        [ 2 * c,  2 * d, -2 * a],
    ])
    dRdb = np.array([
        [-2 * b,  2 * a,  2 * d],
        [ 2 * a,  2 * b,  2 * c],
        [-2 * d,  2 * c, -2 * b]
    ])
    dRdc = np.array([
        [-2 * c, -2 * d,  2 * a],
        [ 2 * d, -2 * c,  2 * b],
        [ 2 * a,  2 * b,  2 * c]
    ])
    dRdd = np.array([
        [ 2 * d, -2 * c,  2 * b],
        [ 2 * c,  2 * d, -2 * a],
        [-2 * b,  2 * a,  2 * d]
    ])

    # Compute the derivative of M by assembling Rs.
    dMda = form_m(dRda)
    dMdb = form_m(dRdb)
    dMdc = form_m(dRdc)
    dMdd = form_m(dRdd)

    # Compute the columns of the Jacobian matrix.
    Ja = np.dot(dMda, y0)
    Jb = np.dot(dMdb, y0)
    Jc = np.dot(dMdc, y0)
    Jd = np.dot(dMdd, y0)

    # Compute the error.
    err = y1 - np.dot(M, y0)
    print np.dot(err.T, err)
    # Assemble the Jacobian.
    J = np.hstack((Ja, Jb, Jc, Jd))
    JtJ = np.dot(np.linalg.inv(np.dot(J.T, J)), J.T)

    # Compute the update term.
    n = np.dot(JtJ, err)

    # Update the quaternion.
    a += n[0,0]
    b += n[1,0]
    c += n[2,0]
    d += n[3,0]

  return np.array([a, b, c, d])

n0 = np.array([0, -1,  0])
m0 = np.array([1,  0,  0])

q = rotation_quat(np.array([0, 0, 1]), math.pi / 4)

n1 = rotate_vec(q, n0)
m1 = rotate_vec(q, m0)

print estimate_q(
    np.array([n0, m0]).flatten().reshape((6, 1)),
    np.array([n1, m1]).flatten().reshape((6, 1))
), q