@wemap/positioning 2.7.13 → 14.0.0-0

package/src/providers/attitude/EkfAttitude.js

@@ -1,238 +0,0 @@
-import {
-    Matrix, Matrix3, Matrix4, Quaternion, Vector, Vector3
-} from '@wemap/maths';
-
-
-const DEFAULT_RELATIVE_NOISES = {
-    acc: 0.5,
-    gyr: 0.3
-};
-
-const DEFAULT_ABSOLUTE_NOISES = {
-    acc: 0.5,
-    gyr: 0.3,
-    yc: 2
-};
-
-class EkfAttitude {
-
-    constructor(accRef = [0, 0, 1], ycRef = [-1, 0, 0]) {
-
-        this.accRef = accRef;
-        this.cRef = ycRef;
-
-        this.P = Matrix.diag(Array(4).fill(0.1 ** 2));
-
-        this.quaternion = null;
-
-
-        this.noises = {
-            relative: null,
-            absolute: null
-        };
-        this.setRelativeNoises(DEFAULT_RELATIVE_NOISES);
-        this.setAbsoluteNoises(DEFAULT_ABSOLUTE_NOISES);
-    }
-
-    setRelativeNoises(relativeNoises) {
-        this.noises.relative = {
-            accelerometer: Matrix.diag(Array(3).fill(relativeNoises.acc ** 2)),
-            gyroscope: Matrix.diag(Array(3).fill(relativeNoises.gyr ** 2))
-        };
-    }
-
-
-    setAbsoluteNoises(absoluteNoises) {
-        this.noises.absolute = {
-            accelerometer: Matrix.diag(Array(3).fill(absoluteNoises.acc ** 2)),
-            gyroscope: Matrix.diag(Array(3).fill(absoluteNoises.gyr ** 2)),
-            yc: Matrix.diag(Array(3).fill(absoluteNoises.yc ** 2))
-        };
-    }
-
-    /**
-     *
-     * Set yaw offset, this value will be used if the filter does not use magnetometer
-     * @param {Number} yaw yaw offset in radians and clockwise
-     */
-    setOrientationYaw(yaw) {
-        this.offsetYawQuaternion = Quaternion.fromAxisAngle(this.accRef, yaw);
-        this.quaternion = null;
-    }
-
-    /**
-     * Try to initialize filter.
-     * To initialize, we need two information: current acceleration (acc) and offset on yaw angle (given by the quaternion: this.offsetYawQuaternion)
-     */
-    tryInitialize(acc, mag) {
-
-        const accNormalized = Vector3.normalize(acc);
-
-        if (mag) {
-            const magNormalized = Vector3.normalize(mag);
-
-            const H = Vector3.normalize(Vector3.cross(magNormalized, accNormalized));
-            const M = Vector3.cross(accNormalized, H);
-
-            const R = [
-                [H[0], M[0], accNormalized[0]],
-                [H[1], M[1], accNormalized[1]],
-                [H[2], M[2], accNormalized[2]]
-            ];
-
-            this.quaternion = Quaternion.fromMatrix3(R);
-
-        } else {
-
-            if (!this.offsetYawQuaternion) {
-                return null;
-            }
-
-            const r = Vector3.dot(accNormalized, this.accRef) + 1;
-            const v = Vector3.cross(accNormalized, this.accRef);
-
-            let quaternionWithoutYaw = [r, v[0], v[1], v[2]];
-            quaternionWithoutYaw = Quaternion.normalize(quaternionWithoutYaw);
-
-            this.quaternion = Quaternion.multiply(this.offsetYawQuaternion, quaternionWithoutYaw);
-        }
-
-        return this.quaternion;
-    }
-
-    update(diffTime, acc, gyr, mag) {
-
-        if (!this.quaternion) {
-            return this.tryInitialize(acc, mag);
-        }
-
-        let q = this.quaternion;
-
-        /* ------------
-         *   ESTIMATION
-         * ------------*/
-
-        const qArray = q;
-        const gyrInt = Vector3.multiplyScalar(gyr, 0.5 * diffTime);
-        const F = this.computeC([1, gyrInt[0], gyrInt[1], gyrInt[2]]);
-        const qAPriori = Matrix.multiplyVector(F, q);
-        const E1 = Matrix.diag([qArray[0], qArray[0], qArray[0]]);
-        const eSkew = Matrix3.skew([qArray[1], qArray[2], qArray[3]]);
-
-        const qPart = [-1 * qArray[1], -1 * qArray[2], -1 * qArray[3]];
-        const E = Matrix.concatRow([qPart], Matrix3.add(eSkew, E1));
-
-        const Qk = Matrix.multiplyScalar(
-            Matrix.multiply(
-                Matrix.multiply(E, this.noises[mag ? 'absolute' : 'relative'].gyroscope),
-                Matrix.transpose(E)
-            ),
-            (diffTime / 2) ** 2
-        );
-
-        const pAPriori = Matrix4.add(
-            Matrix.multiply(
-                Matrix.multiply(F, this.P),
-                Matrix.transpose(F)
-            ),
-            Qk
-        );
-
-        /* ------------
-         *   CORRECTION
-         * ------------*/
-
-        const accNormalized = Vector3.normalize(acc);
-        let dz, K, H;
-
-        if (mag) {
-
-            const magNormalized = Vector3.normalize(mag);
-            const yc = Vector3.cross(accNormalized, magNormalized);
-            const ycNormalized = Vector3.normalize(yc);
-
-            const dzYc = Vector3.subtract(ycNormalized, Quaternion.rotate(qAPriori, this.cRef));
-            const dzAcc = Vector3.subtract(accNormalized, Quaternion.rotate(qAPriori, this.accRef));
-            dz = Vector.concat(dzYc, dzAcc);
-
-            const HYc = this.jacobianES(qAPriori, this.cRef);
-            const HAcc = this.jacobianES(qAPriori, this.accRef);
-            H = Matrix.concatRow(HYc, HAcc);
-
-            const RYc = Matrix.concatLine(this.noises.absolute.yc, Matrix3.zeros);
-            const RAcc = Matrix.concatLine(Matrix3.zeros, this.noises.absolute.accelerometer);
-            const R = Matrix.concatRow(RYc, RAcc);
-
-            K = Matrix.multiply(
-                Matrix.multiply(pAPriori, Matrix.transpose(H)),
-                Matrix.inverse(
-                    Matrix.add(
-                        Matrix.multiply(
-                            Matrix.multiply(H, pAPriori),
-                            Matrix.transpose(H)
-                        ),
-                        R
-                    )
-                )
-            );
-        } else {
-            dz = Vector3.subtract(accNormalized, Quaternion.rotate(qAPriori, this.accRef));
-            H = this.jacobianES(qAPriori, this.accRef);
-            const R = this.noises.relative.accelerometer;
-
-            K = Matrix.multiply(
-                Matrix.multiply(pAPriori, Matrix.transpose(H)),
-                Matrix3.inverse(
-                    Matrix3.add(
-                        Matrix.multiply(
-                            Matrix.multiply(H, pAPriori),
-                            Matrix.transpose(H)
-                        ),
-                        R
-                    )
-                )
-            );
-        }
-
-        q = Quaternion.add(
-            qAPriori,
-            Matrix.multiplyVector(K, dz)
-        );
-        const P = Matrix.multiply(
-            Matrix4.subtract(
-                Matrix4.identity,
-                Matrix.multiply(K, H)
-            ),
-            pAPriori
-        );
-
-        q = Quaternion.normalize(q);
-        this.quaternion = q;
-        this.P = P;
-
-        return q;
-    }
-
-    computeC(b) {
-        return [
-            [b[0], -b[1], -b[2], -b[3]],
-            [b[1], b[0], b[3], -b[2]],
-            [b[2], -b[3], b[0], b[1]],
-            [b[3], b[2], -b[1], b[0]]
-        ];
-    }
-
-    jacobianES(q, v) {
-
-        const [qw, qx, qy, qz] = q;
-        const [vx, vy, vz] = v;
-
-        return [
-            [2 * qz * vy - 2 * qy * vz, 2 * qy * vy + 2 * qz * vz, 2 * qx * vy - 2 * qw * vz - 4 * qy * vx, 2 * qw * vy + 2 * qx * vz - 4 * qz * vx],
-            [2 * qx * vz - 2 * qz * vx, 2 * qw * vz - 4 * qx * vy + 2 * qy * vx, 2 * qx * vx + 2 * qz * vz, 2 * qy * vz - 2 * qw * vx - 4 * qz * vy],
-            [2 * qy * vx - 2 * qx * vy, 2 * qz * vx - 4 * qx * vz - 2 * qw * vy, 2 * qw * vx - 4 * qy * vz + 2 * qz * vy, 2 * qx * vx + 2 * qy * vy]
-        ];
-    }
-}
-
-export default EkfAttitude;

package/src/providers/attitude/EkfAttitude.spec.js

@@ -1,116 +0,0 @@
-import chai from 'chai';
-
-import { Quaternion } from '@wemap/maths';
-
-import EkfAttitude from './EkfAttitude';
-
-const expect = chai.expect;
-
-const dt = [
-    0.02, 0.02, 0.02, 0.02, 0.02
-];
-const accData = [
-    [-0.034561157, 3.81073, 8.860977],
-    [-0.030700684, 3.814499, 8.818954],
-    [-0.018234253, 3.8023376, 8.85762],
-    [-0.020080566, 3.8205414, 8.8676605],
-    [-0.054519653, 3.8456726, 8.810287]
-];
-
-const gyrData = [
-    [0.0047454834, 0.0028076172, 0.0022888184],
-    [0.005218506, 0.0020446777, 0.0012207031],
-    [0.0044555664, 0.0023040771, 4.4250488E-4],
-    [0.0044555664, 0.0027618408, 0.0011444092],
-    [0.0040893555, 0.0020446777, 0.0025787354]
-];
-
-const magData = [
-    [-7.260132, -29.21753, -30.532837],
-    [-8.378601, -29.589844, -29.684448],
-    [-8.784485, -29.97284, -29.86145],
-    [-8.784485, -30.06134, -30.036926],
-    [-8.917236, -29.666138, -28.616333]
-];
-
-const expectationsAbsolute = [
-    [0.22501367907095468, 0.048112975227173178, -0.19586497565981961, -0.95325279813673824],
-    [0.22521836492577899, 0.048187611771482071, -0.19604062637627487, -0.953164579168738],
-    [0.22546875479675244, 0.048146964451479861, -0.19603487950732199, -0.95310861733648156],
-    [0.22571872218138991, 0.04816067876157637, -0.19609909097002176, -0.95303554708035731],
-    [0.22598409746972753, 0.048415228863568645, -0.1963289784536566, -0.95291242279877153]
-];
-
-const expectationsRelative = [
-    [0.979449872594084, 0.201679452107589, 0.001829118097573, 0],
-    [0.979412310855378, 0.201861876976980, 0.001818648953412, 0.000022005516141],
-    [0.979416707676713, 0.201841237873455, 0.001739180982152, 0.000051642340567],
-    [0.979405268570585, 0.201897124314688, 0.001692389320913, 0.000083114532679],
-    [0.979351148979101, 0.202158360999907, 0.001821014027950, 0.000089325563504]
-];
-
-describe('initAbsolute', () => {
-    it('Should return the good value', () => {
-        const ekf = new EkfAttitude();
-        ekf.setAbsoluteNoises({
-            acc: 0.5,
-            gyr: 0.3,
-            yc: 2
-        });
-        const result = ekf.update(dt[0], accData[0], gyrData[0], magData[0]);
-        const distance = Quaternion.distance(result, expectationsAbsolute[0]);
-        expect(distance).to.below(0.000001);
-    });
-});
-
-describe('updateAbsolute', () => {
-    it('Should return the good value', () => {
-        const ekf = new EkfAttitude();
-        ekf.setAbsoluteNoises({
-            acc: 0.5,
-            gyr: 0.3,
-            yc: 2
-        });
-        let result;
-        let distance;
-
-        for (let i = 0; i < accData.length; i++) {
-            result = ekf.update(dt[i], accData[i], gyrData[i], magData[i]);
-            distance = Quaternion.distance(result, expectationsAbsolute[i]);
-            expect(distance).to.below(0.000001);
-        }
-    });
-});
-
-describe('initRelative', () => {
-    it('Should return the good value', () => {
-        const ekf = new EkfAttitude();
-        ekf.setOrientationYaw(0);
-        ekf.setRelativeNoises({
-            acc: 0.5,
-            gyr: 0.3
-        });
-        const result = ekf.update(dt[0], accData[0], gyrData[0]);
-        const distance = Quaternion.distance(result, expectationsRelative[0]);
-        expect(distance).to.below(0.000001);
-    });
-});
-
-describe('updateRelative', () => {
-    it('Should return the good value', () => {
-        const ekf = new EkfAttitude();
-        ekf.setOrientationYaw(0);
-        ekf.setRelativeNoises({
-            acc: 0.5,
-            gyr: 0.3
-        });
-        let result;
-        let distance;
-
-        for (let i = 0; i < accData.length; i++) {
-            result = ekf.update(dt[i], accData[i], gyrData[i]);
-            distance = Quaternion.distance(result, expectationsRelative[i]);
-            expect(distance).to.below(0.000001);
-        }
-    });
-});

package/src/providers/attitude/RelativeAttitudeProvider.js

@@ -1,121 +0,0 @@
-import Provider from '../Provider';
-import EventType from '../../events/EventType';
-import EkfAttitude from './EkfAttitude';
-import ImuProvider from '../others/ImuProvider';
-import { Attitude } from '@wemap/geo';
-import { deg2rad } from '@wemap/maths';
-
-
-/**
- * Relative attitude provider gives the device attitude in East-North-Up (ENU) frame using
- *  browser deviceorientation
- * The provider does not work until an offset is given.
- */
-class RelativeAttitudeProvider extends Provider {
-
-    lastTimestamp = 0;
-
-    /**
-     * @override
-     */
-    constructor(onEvent, onError, options) {
-        super(onEvent, onError, options);
-
-        this.ekfAttitude = new EkfAttitude();
-        this.relativeOffsetQuaternion = [1, 0, 0, 0];
-
-        this.imuProvider = new ImuProvider(this.onImuEvent,
-            onError, { require: [EventType.Acceleration, EventType.AngularRate] });
-
-    }
-
-    /**
-     * @override
-     */
-    static get name() {
-        return 'RelativeAttitude';
-    }
-
-    /**
-    * @override
-    */
-    static get displayName() {
-        return 'Relative Attitude from Ekf';
-    }
-
-    /**
-     * @override
-     */
-    static get eventsType() {
-        return [EventType.RelativeAttitude];
-    }
-
-    /**
-     * @override
-     */
-    static getRequiredProviders() {
-        return [ImuProvider];
-    }
-
-    /**
-     * @override
-     */
-    startInternal() {
-        this.imuProvider.start();
-    }
-
-    /**
-     * @override
-     */
-    stopInternal() {
-        this.imuProvider.stop();
-    }
-
-    /**
-     * @private
-     */
-    onImuEvent = imuEvent => {
-
-        let timestamp, acceleration, angularRate;
-        imuEvent.forEach(event => {
-            if (event.dataType === EventType.Acceleration) {
-                acceleration = event.data;
-                timestamp = event.timestamp;
-            } else if (event.dataType === EventType.AngularRate) {
-                angularRate = event.data;
-            }
-        });
-
-        // Handle timestamps and dt
-        if (this.lastTimestamp === 0) {
-            this.lastTimestamp = timestamp;
-            return;
-        }
-        const diffTime = timestamp - this.lastTimestamp;
-        this.lastTimestamp = timestamp;
-
-        const quaternion = this.ekfAttitude.update(diffTime, acceleration, angularRate);
-
-        if (quaternion) {
-            this.notify(this.createEvent(EventType.RelativeAttitude, new Attitude(quaternion), timestamp));
-        }
-    };
-
-    /**
-     *
-     * Set yaw offset, this value will be used as the filter does not use magnetometer
-     * @param {Number} heading heading offset in radians and clockwise
-     */
-    setOffset(_heading) {
-
-        // Minus before "heading" is here because ENU attitude is counter-clockwise whereas WGS84 heading is clockwise.
-        let heading = -_heading;
-
-        // Offset from window orientation
-        heading += deg2rad(window.orientation || 0);
-
-        this.ekfAttitude.setOrientationYaw(heading);
-    }
-}
-
-export default RelativeAttitudeProvider;

package/src/providers/others/ImuProvider.js

@@ -1,179 +0,0 @@
-import { deg2rad } from '@wemap/maths';
-import {
-    Browser, BrowserUtils
-} from '@wemap/utils';
-
-import isnumber from 'lodash.isnumber';
-import Provider from '../Provider';
-import EventType from '../../events/EventType';
-import MissingAccelerometerError from '../../errors/MissingAccelerometerError';
-import MissingGyroscopeError from '../../errors/MissingGyroscopeError';
-import AskImuOnDesktopError from '../../errors/AskImuOnDesktopError';
-import Availability from '../../events/Availability';
-
-/**
- * Imu (Inertial Measurement Unit) provider retrieve acceleration data
- *  and/or angular rate data from inertial sensors.
- * opions.require has to be defined in order to work
- *
- * -----------------------------------
- * Overview of compatibilities:
- * -----------------------------------
- *
- * Chrome Android (v72.0.3626): YES (via devicemotion)
- * Safari iOS (v12.0): YES (via devicemotion)
- * Opera Android (v50.2.2426): NO {@link https://developer.mozilla.org/en-US/docs/Web/API/DeviceMotionEvent}
- * Firefox Android (v65.0.1): YES (via devicemotion)
- *
- * -----------------------------------
- */
-class ImuProvider extends Provider {
-
-    requiredSensors = [];
-
-    /**
-     * Constructor of ImuProvider
-     * options.require has to be filled by a list of EventType
-     * @example options = {require: [EventType.Acceleration, EventType.AngularRate]};
-     * @example options = {require: [EventType.Acceleration]};
-     *
-     * @param {Function} onEvent @see Provider#constructor
-     * @param {Function} onError @see Provider#constructor
-     * @param {Object} options @see Provider#constructor
-     */
-    constructor(onEvent, onError, options) {
-        super(onEvent, onError, options);
-
-        if (!options.hasOwnProperty('require')) {
-            throw new Error('options.require is missing in ImuProvider constructor');
-        }
-
-        if (options.require.length === 0) {
-            throw new Error('option.require is empty');
-        }
-
-        options.require.forEach(elem => {
-            if (!ImuProvider.eventsType.includes(elem)) {
-                throw new Error(elem + ' is not recognised in options.require');
-            }
-        });
-
-        this.requiredSensors = options.require;
-    }
-
-    /**
-     * @override
-     */
-    static get name() {
-        return 'Imu';
-    }
-
-    /**
-    * @override
-    */
-    static get displayName() {
-        return 'Inertial Measurement Unit';
-    }
-
-    /**
-     * @override
-     */
-    static get eventsType() {
-        return [EventType.AngularRate, EventType.Acceleration];
-    }
-
-    /**
-     * @override
-     */
-    static checkAvailability(options) {
-        return Availability.merge(
-            super.checkAvailability(options),
-            BrowserUtils.isMobile
-                ? Availability.yes()
-                : Availability.no(new AskImuOnDesktopError())
-        );
-    }
-
-    /**
-     * @override
-     */
-    startInternal() {
-        window.addEventListener('devicemotion', this.parseDeviceMotionEvent, true);
-    }
-
-    /**
-     * @override
-     */
-    stopInternal() {
-        window.removeEventListener('devicemotion', this.parseDeviceMotionEvent, true);
-    }
-
-    /**
-     * devicemotion callback
-     * @param {DeviceMotionEvent} e device motion event
-     * @returns {ProviderEvent[]} an array of provider events
-     * @private
-     */
-    parseDeviceMotionEvent = e => {
-
-        const events = [];
-
-        const timestamp = e.timeStamp / 1e3;
-
-        if (this.requiredSensors.includes(EventType.Acceleration)) {
-
-            let acc;
-            if (e.accelerationIncludingGravity) {
-                const {
-                    x, y, z
-                } = e.accelerationIncludingGravity;
-
-                if (isnumber(x) && isnumber(y) && isnumber(z)) {
-                    acc = [x, y, z];
-
-                    if (BrowserUtils.name === Browser.SAFARI) {
-                        acc[0] *= -1;
-                        acc[1] *= -1;
-                        acc[2] *= -1;
-                    }
-                }
-            }
-
-            if (!acc) {
-                this.notifyError(new MissingAccelerometerError().from('devicemotion'));
-                return;
-            }
-
-            events.push(this.createEvent(EventType.Acceleration, acc, timestamp));
-        }
-
-
-        if (this.requiredSensors.includes(EventType.AngularRate)) {
-
-            let gyr;
-            if (e.rotationRate) {
-                const {
-                    alpha, beta, gamma
-                } = e.rotationRate;
-
-                if (isnumber(alpha) && isnumber(beta) && isnumber(gamma)) {
-                    gyr = [deg2rad(alpha), deg2rad(beta), deg2rad(gamma)];
-                }
-            }
-
-            if (!gyr) {
-                this.notifyError(new MissingGyroscopeError().from('devicemotion'));
-                return;
-            }
-
-            events.push(this.createEvent(EventType.AngularRate, gyr, timestamp));
-        }
-
-        if (events.length !== 0) {
-            this.notify(...events);
-        }
-    }
-
-}
-
-export default ImuProvider;