QuaternionRate.h

 
#pragma once
 
#include <Eigen/Dense>
 
namespace Ship
{
 
constexpr double kQuaternionNormGain = 2.0;
 
inline Eigen::Vector4d QuaternionRate(const Eigen::Vector4d& q, const Eigen::Vector3d& omega, double gain)
{
    const double norm = q.norm();
    const double qw   = q(0) / norm;
    const Eigen::Vector3d qVec = q.tail<3>() / norm;
 
    Eigen::Matrix3d dyadic;
    dyadic << 0, -qVec(2), qVec(1), qVec(2), 0, -qVec(0), -qVec(1), qVec(0), 0;
 
    Eigen::Vector4d rate;
    // The same expressions as the Ship used before, so that a unit quaternion gives the same bits.
    rate(0)        = (-0.5 * qVec.transpose() * omega)(0, 0);
    rate.tail<3>() = 0.5 * (qw * Eigen::Matrix3d::Identity() - dyadic) * omega;
    rate += gain * (1.0 - q.squaredNorm()) * q;
    return rate;
}
 
} // namespace Ship