Pose estimation from accelerometer and gyroscope

pose_est.cpp

// libshovel.cpp : Defines the entry point for the application.
//
//

#include "Eigen/Eigen"

namespace shovel {
    
    	struct Pose
	{
		double x;
		double y;
		double z;

		Pose(): x(0), y(0), z(0)
        {
        }

        Pose(double x, double y, double z) : x(x), y(y), z(z)
		{
		}
	};

	class PoseEstimator
	{
	public:
		PoseEstimator();

		~PoseEstimator();

		void initializeRotationFromAccelerometer(double x, double y, double z);
		void updateGyroscope(double x, double y, double z, std::uint64_t timestamp_ms);
		void updateAccelerometer(double x, double y, double z, std::uint64_t timestamp_ms);
		Pose getCurrentPose()
		{
			return Pose(_position.x(), _position.y(), _position.z());
		}

	private:
		// ReSharper disable once CppInconsistentNaming
		const Eigen::Vector3d GRAVITY_ACCELERATION = Eigen::Vector3d(0., 0., 1.);

		bool _rotationInitialized = false;
		Eigen::Quaterniond _originalRotation;
		Eigen::Quaterniond _rotation;
		Eigen::Vector3d _position;

		std::uint64_t _lastGyroscopeUpdatedMs = 0;
		std::uint64_t _lastAccelerometerUpdatedMs = 0;

		static Eigen::Quaterniond getRotationFromGyroscope(double x, double y, double z, std::uint64_t dt);
		static Eigen::Vector3d getAccelerationRotatedBack(double x, double y, double z, const Eigen::Quaterniond& rotation);
	};

    PoseEstimator::PoseEstimator()
    {
    }

    PoseEstimator::~PoseEstimator()
    {
    }

    void PoseEstimator::initializeRotationFromAccelerometer(double x, double y, double z)
    {
        Eigen::Vector3d acceleration(x, y, z);
        acceleration.normalize();

        _rotation = Eigen::Quaterniond::FromTwoVectors(GRAVITY_ACCELERATION, acceleration);
        _originalRotation = _rotation;

        _rotationInitialized = true;
    }

    void PoseEstimator::updateGyroscope(double x, double y, double z, std::uint64_t timestamp_ms)
    {
        if (_rotationInitialized && _lastGyroscopeUpdatedMs)
        {
            const Eigen::Quaterniond deltaQ = getRotationFromGyroscope(x, y, z, timestamp_ms - _lastGyroscopeUpdatedMs);
            _rotation = (_rotation * deltaQ).normalized();
        }
        _lastGyroscopeUpdatedMs = timestamp_ms;
    }

    void PoseEstimator::updateAccelerometer(double x, double y, double z, std::uint64_t timestamp_ms)
    {
        // if not initialized initial pose, initialize it
        if (!_rotationInitialized)
        {
            initializeRotationFromAccelerometer(x, y, z);
        }
        else
        {
            auto acceleration = getAccelerationRotatedBack(x, y, z, _rotation);
            acceleration -= GRAVITY_ACCELERATION;

            const auto dt = timestamp_ms - _lastAccelerometerUpdatedMs;

            const Eigen::Vector3d distance = acceleration * dt * dt * 9.81; // a * t^2 * g

            _position += distance;
        }
        _lastAccelerometerUpdatedMs = timestamp_ms;
    }

    Eigen::Quaterniond PoseEstimator::getRotationFromGyroscope(double x, double y, double z, std::uint64_t dt)
    {
        // first order linearization of the rotation
        const Eigen::Quaterniond deltaQ(1, 0.5 * dt * x, 0.5 * dt * y, 0.5 * dt * z);
        return deltaQ;
    }

    Eigen::Vector3d PoseEstimator::getAccelerationRotatedBack(double x, double y, double z, const Eigen::Quaterniond& rotation)
    {
        const auto inverseRotation = rotation.normalized().inverse(); // Inverse rotation
        return inverseRotation * Eigen::Vector3d(x, y, z);  // Rotated
    }

}

pose_est.ts

import * as THREE from "three";

var rotation: THREE.Quaternion;
var position: THREE.Vector3;

/**
 * 
 * @param x  Accelerometer X
 * @param y  Accelerometer Y
 * @param z  Accelerometer Z
 */
function initPose(x: number, y: number, z: number) {
    rotation = calculateInitPose(x, y, z);
}

/**
 * 
 * @param x Gyroscope X in rad
 * @param y Gyroscope Y in rad
 * @param z Gyroscope Z in rad
 * @param dt Time delta in s
 */
function getRotationFromGyroscope(x: number, y: number, z: number, dt: number, original: THREE.Quaternion): THREE.Quaternion {
    // first order linearization of the rotation
    const deltaQ = new THREE.Quaternion(0.5 * dt * x, 0.5 * dt * y, 0.5 * dt * z, 1);
    const finalQ = original.multiply(deltaQ).normalize();
    return finalQ;
}

/**
 * 
 * @param x Acceleration X in g
 * @param y Acceleration Y in g
 * @param z Acceleration Z in g
 */
function calculateInitPose(x: number, y: number, z: number): THREE.Quaternion {
    var gravityV = new THREE.Vector3(0, 0, -1);
    var accelV = new THREE.Vector3(x, y, z);
    accelV.normalize();

    var rotation = new THREE.Quaternion();
    rotation.setFromUnitVectors(gravityV, accelV);
    return rotation;
}

function getMovementFromAccelerationRotation(x: number, y: number, z: number, dt: number, rotation: THREE.Quaternion): THREE.Vector3 {
    const acceleration = new THREE.Vector3(x, y, z);

    // Get rotation quat back to reference rotation
    const quaternionCurrentToReferenceRotation = rotation.normalize().inverse();

    // Rotate current acceleration back to reference rotation
    var accelerationInReferenceRotation = acceleration.applyQuaternion(quaternionCurrentToReferenceRotation);

    // Minus gravity
    const accelerationGravity = new THREE.Vector3(0, 0, -1);
    accelerationInReferenceRotation.sub(accelerationGravity);

    // Try average between current accel and last accel
    var velocity = accelerationInReferenceRotation.multiplyScalar(dt * 9.81);
    var movement = velocity.multiplyScalar(dt);

    return movement;
}