Quaternion renormalization

Question

I am trying to create 4 symbolic equations that correspond to the 4 elements of a normalized quaternion. The equation is for quaternion multiplication

\$\dot q = 0.5*q*[0, \omega]^T\$

\$\omega = [\omega_x, \omega_y, \omega_z]\$

\$q = [q_r, q_i, q_j, q_k]\$

and then I normalize the output. My solution is:

import sympy as sy

def quat_equ(q_l, q_w, renorm=1):
    # quaternion multiplication using equation: q_dot = 0.5 * q * [0, omega)^T
    # multiplication broken up into individual quaternion states
    q_r_d = 0.5 * (q_l[0] * q_w[0] - q_l[1] * q_w[1] + q_l[2] * q_w[2] +
                   q_l[3] * q_w[3])
    q_i_d = 0.5 * (q_l[0] * q_w[1] + q_l[1] * q_w[0] + q_l[2] * q_w[3] -
                   q_l[3] * q_w[2])
    q_j_d = 0.5 * (q_l[0] * q_w[2] + q_l[2] * q_w[0] + q_l[3] * q_w[1] -
                   q_l[1] * q_w[3])
    q_k_d = 0.5 * (q_l[0] * q_w[3] + q_l[3] * q_w[0] + q_l[1] * q_w[2] -
                   q_l[2] * q_w[1])
    # Re-normalizing the quaternion multiple times (heuristic I was taught/required to use)
    # The for loop is causing the slowdown of the code due to re-normalizing 
    for i in range(renorm):
        q_r_d *= 1 / sy.sqrt(q_r_d ** 2 + q_i_d ** 2 + q_j_d ** 2 + q_k_d ** 2)
        q_i_d *= 1 / sy.sqrt(q_r_d ** 2 + q_i_d ** 2 + q_j_d ** 2 + q_k_d ** 2)
        q_j_d *= 1 / sy.sqrt(q_r_d ** 2 + q_i_d ** 2 + q_j_d ** 2 + q_k_d ** 2)
        q_k_d *= 1 / sy.sqrt(q_r_d ** 2 + q_i_d ** 2 + q_j_d ** 2 + q_k_d ** 2)
    return [q_r_d, q_i_d, q_j_d, q_k_d]

# setting up symbolics
q_r, q_i, q_j, q_k, w_ib_l, w_ib_m, w_ib_n = sy.symbols("q_r q_i q_j q_k w_ib_l w_ib_m w_ib_n")
q1 = [q_r, q_i, q_j, q_k]
q2 = [0, w_ib_l, w_ib_m, w_ib_n]

# separation of symbolic equations for each state in q_dot (this is a requirement for the code)
quat0 = quat_equ(q1, q2, 3)[0]
quat1 = quat_equ(q1, q2, 3)[1]
quat2 = quat_equ(q1, q2, 3)[2]
quat3 = quat_equ(q1, q2, 3)[3]

# testing of the symbolic equations using substitution
qq = [1., 0., 0., 0., 1., 1., 1.]
var = [q_r, q_i, q_j, q_k, w_ib_l, w_ib_m, w_ib_n]

q0 = sy.Subs(quat0, var, qq).doit()
q1 = sy.Subs(quat1, var, qq).doit()
q2 = sy.Subs(quat2, var, qq).doit()
q3 = sy.Subs(quat3, var, qq).doit()

When I try to re-normalize the quaternion more than 2 times, this method becomes very slow. The symbolic equation also becomes massive because the re-normalization adds on basically 4 extra symbolic equations, which slows down later processes a lot. I couldn't think of a better method to create a normalized quaternion that 1) initializes faster and 2) is a smaller symbolic equation. I don't use symbolics in Python much, so most of Sympy is new to me.

I am doing this in Python 2.7 because I am required to. If that causes any issues, again please let me know.

toolic · Accepted Answer · 2025-10-28 11:22:16Z

Layout

While I understand the goal of keeping lines short, these 2 lines:

q_r_d = 0.5 * (q_l[0] * q_w[0] - q_l[1] * q_w[1] + q_l[2] * q_w[2] +
               q_l[3] * q_w[3])

can be combined into 1 line, which will not be very long (and even shorter than some of your other lines). This code is easier to understand:

q_r_d = 0.5 * (q_l[0] * q_w[0] - q_l[1] * q_w[1] + q_l[2] * q_w[2] + q_l[3] * q_w[3])
q_i_d = 0.5 * (q_l[0] * q_w[1] + q_l[1] * q_w[0] + q_l[2] * q_w[3] - q_l[3] * q_w[2])
q_j_d = 0.5 * (q_l[0] * q_w[2] + q_l[2] * q_w[0] + q_l[3] * q_w[1] - q_l[1] * q_w[3])
q_k_d = 0.5 * (q_l[0] * q_w[3] + q_l[3] * q_w[0] + q_l[1] * q_w[2] - q_l[2] * q_w[1])

Documentation

The PEP 8 style guide recommends adding docstrings for functions and at the top of the code. For the function, simply convert the comment:

def quat_equ(q_l, q_w, renorm=1):
    # quaternion multiplication using equation: q_dot = 0.5 * q * [0, omega)^T

into a docstring:

def quat_equ(q_l, q_w, renorm=1):
    """quaternion multiplication using equation: q_dot = 0.5 * q * [0, omega)^T"""

You could add a docstring like this to the top of the code:

"""
Create 4 symbolic equations that correspond to the 4 elements of a normalized quaternion. 
The equation is for quaternion multiplication.
Add more details here
"""

DRY

The right-hand side of this assignment is repeated 4 times:

q_r_d *= 1 / sy.sqrt(q_r_d ** 2 + q_i_d ** 2 + q_j_d ** 2 + q_k_d ** 2)

You can create a helper function to reduce the repetition. For example, add a function named something like renormalize:

def quat_equ(q_l, q_w, renorm=1):
    """quaternion multiplication using equation: q_dot = 0.5 * q * [0, omega)^T"""

    def renormalize():
        return 1 / sy.sqrt(q_r_d ** 2 + q_i_d ** 2 + q_j_d ** 2 + q_k_d ** 2)

    # multiplication broken up into individual quaternion states
    q_r_d = 0.5 * (q_l[0] * q_w[0] - q_l[1] * q_w[1] + q_l[2] * q_w[2] + q_l[3] * q_w[3])
    q_i_d = 0.5 * (q_l[0] * q_w[1] + q_l[1] * q_w[0] + q_l[2] * q_w[3] - q_l[3] * q_w[2])
    q_j_d = 0.5 * (q_l[0] * q_w[2] + q_l[2] * q_w[0] + q_l[3] * q_w[1] - q_l[1] * q_w[3])
    q_k_d = 0.5 * (q_l[0] * q_w[3] + q_l[3] * q_w[0] + q_l[1] * q_w[2] - q_l[2] * q_w[1])
    # Re-normalizing the quaternion multiple times (heuristic I was taught/required to use)
    # The for loop is causing the slowdown of the code due to re-normalizing 
    for i in range(renorm):
        q_r_d *= renormalize()
        q_i_d *= renormalize()
        q_j_d *= renormalize()
        q_k_d *= renormalize()
    return [q_r_d, q_i_d, q_j_d, q_k_d]

Naming

Many of the names are not too descriptive.

The function named quat_equ could be longer, like quaternion_mult.

Variable names like var and qq are too vague.

Stack Exchange Network

Quaternion renormalization

1 Answer 1

Layout

Documentation

DRY

Naming

You must log in to answer this question.

Hot Network Questions