From 3d1c9b988dff1d95f36cdd1df3e84d7a2365501c Mon Sep 17 00:00:00 2001
From: romain-chiap <romain.chiap@gmail.com>
Date: Thu, 5 Aug 2021 01:04:57 +0200
Subject: [PATCH] quaternion exponential (#164)

---
 matrix/Quaternion.hpp | 72 +++++++++++++++++++++++++++++++++++++++++++
 test/attitude.cpp     | 46 +++++++++++++++++++++++++++
 2 files changed, 118 insertions(+)

diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
index 309cd53952..ddcbdf9ec0 100644
--- a/matrix/Quaternion.hpp
+++ b/matrix/Quaternion.hpp
@@ -325,6 +325,78 @@ public:
         return v * q  * Type(0.5);
     }
 
+    /**
+      * Computes the quaternion exponential of the 3D vector u
+      * as proposed in
+      * [1] Sveier A, Sjøberg AM, Egeland O. "Applied Runge–Kutta–Munthe-Kaas Integration
+      *     for the Quaternion Kinematics".Journal of Guidance, Control, and Dynamics. 2019
+      *
+      * return a quaternion computed as
+      * expq(u)=[cos||u||, sinc||u||*u]
+      * sinc(x)=sin(x)/x in the sin cardinal function
+      *
+      * This can be used to update a quaternion from the body rates
+      * rather than using
+      * qk+1=qk+qk.derivative1(wb)*dt
+      * we can use
+      * qk+1=qk*expq(dt*wb/2)
+      * which is a more robust update.
+      * A re-normalization step might necessary with both methods.
+      *
+      * @param u 3D vector u
+      */
+    static Quaternion expq(const Vector3<Type> &u)
+    {
+        const Type tol = Type(0.2);           // ensures an error < 10^-10
+        const Type c2 = Type(1.0 / 2.0);      // 1 / 2!
+        const Type c3 = Type(1.0 / 6.0);      // 1 / 3!
+        const Type c4 = Type(1.0 / 24.0);     // 1 / 4!
+        const Type c5 = Type(1.0 / 120.0);    // 1 / 5!
+        const Type c6 = Type(1.0 / 720.0);    // 1 / 6!
+        const Type c7 = Type(1.0 / 5040.0);   // 1 / 7!
+
+        Type u_norm = u.norm();
+        Type sinc_u, cos_u;
+
+        if (u_norm < tol) {
+            Type u2 = u_norm * u_norm;
+            Type u4 = u2 * u2;
+            Type u6 = u4 * u2;
+
+            // compute the first 4 terms of the Taylor serie
+            sinc_u = Type(1.0) - u2 * c3 + u4 * c5 - u6 * c7;
+            cos_u = Type(1.0) - u2 * c2 + u4 * c4 - u6 * c6;
+        } else {
+            sinc_u = Type(sin(u_norm) / u_norm);
+            cos_u = Type(cos(u_norm));
+        }
+        Vector<Type, 3> v = sinc_u * u;
+        return Quaternion<Type> (cos_u, v(0), v(1), v(2));
+    }
+
+    /** inverse right Jacobian of the quaternion logarithm u
+      * equation (20) in reference
+      * [1] Sveier A, Sjøberg AM, Egeland O. "Applied Runge–Kutta–Munthe-Kaas Integration
+      *     for the Quaternion Kinematics".Journal of Guidance, Control, and Dynamics. 2019
+      *
+      * This can be used to update a quaternion kinematic cleanly
+      * with higher order integration methods (like RK4) on the quaternion logarithm u.
+      *
+      * @param u 3D vector u
+      */
+    static Dcm<Type> inv_r_jacobian (const Vector3<Type> &u)
+    {
+        const Type tol = Type(1.0e-4);
+        Type u_norm = u.norm();
+        Dcm<Type> u_hat = u.hat();
+
+        if (u_norm < tol) { 	// result smaller than O(||.||^3)
+            return Type(0.5) * (Dcm<Type>() + u_hat + (Type(1.0 / 3.0) + u_norm * u_norm / Type(45.0)) * u_hat * u_hat);
+        } else {
+            return Type(0.5) * (Dcm<Type>() + u_hat + (Type(1.0) - u_norm * Type(cos(u_norm) / sin(u_norm))) / (u_norm * u_norm) * u_hat * u_hat);
+        }
+    }
+
     /**
      * Invert quaternion in place
      */
diff --git a/test/attitude.cpp b/test/attitude.cpp
index cbdcf3d6df..d8954965d3 100644
--- a/test/attitude.cpp
+++ b/test/attitude.cpp
@@ -89,6 +89,52 @@ int main()
     q = Quatf();
     TEST(isEqual(q, Quatf(1, 0, 0, 0)));
 
+    // quaternion exponential with v=0
+    v = Vector3f();
+    q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
+    Dcmf M = Dcmf()*0.5f;
+    TEST(isEqual(q, Quatf::expq(v)));
+    TEST(isEqual(M, Quatf::inv_r_jacobian(v)));
+
+    // quaternion exponential with small v
+    v = Vector3f(0.001f,0.002f,-0.003f);
+    q = Quatf(0.999993000008167f, 0.000999997666668f,
+              0.001999995333337f, -0.002999993000005f);
+    {
+        float M_data[] =  {
+            0.499997833331311f, 0.001500333333644f,  0.000999499999533f,
+            -0.001499666666356f, 0.499998333331778f, -0.000501000000933f,
+            -0.001000500000467f, 0.000498999999067f,  0.499999166665889f
+        };
+        M = Dcmf(M_data);
+    }
+    TEST(isEqual(q, Quatf::expq(v)));
+    TEST(isEqual(M, Quatf::inv_r_jacobian(v)));
+
+    // quaternion exponential with v
+    v = Vector3f(1.0f, -2.0f, 3.0f);
+    q = Quatf(-0.825299062075259f, -0.150921327219964f,
+              0.301842654439929f, -0.452763981659893f);
+    {
+        float M_data[] =  {
+            2.574616981530584f, -1.180828156687602f, -1.478757764968596f,
+            1.819171843312398f,  2.095859216561988f,  0.457515529937193f,
+            0.521242235031404f,  1.457515529937193f,  1.297929608280994f
+        };
+        M = Dcmf(M_data);
+    }
+    TEST(isEqual(q, Quatf::expq(v)));
+    TEST(isEqual(M, Quatf::inv_r_jacobian(v)));
+
+    // quaternion kinematic update
+    q = Quatf();
+    float h=0.001f;    // sampling time [s]
+    Vector3f w_B=Vector3f(0.1f,0.2f,0.3f);     // body rate in body frame
+    Quatf qa=q+0.5f*h*q.derivative1(w_B);
+    qa.normalize();
+    Quatf qb=q*Quatf::expq(0.5f*h*w_B);
+    TEST(isEqual(qa, qb));
+
     // euler to quaternion
     q = Quatf(euler_check);
     TEST(isEqual(q, q_check));