@wemap/providers 3.0.0

package/src/providers/attitude/EkfAttitude.js

@@ -0,0 +1,224 @@
+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))
+        };
+    }
+
+    /**
+     * Try to initialize filter.
+     * To initialize, we need at least current acceleration (acc)
+     */
+    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 {
+
+            const r = Vector3.dot(accNormalized, this.accRef) + 1;
+            const v = Vector3.cross(accNormalized, this.accRef);
+
+            let quaternion = [r, v[0], v[1], v[2]];
+            quaternion = Quaternion.normalize(quaternion);
+
+            this.quaternion = quaternion;
+        }
+
+        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

@@ -0,0 +1,114 @@
+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.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.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/absolute/AbsoluteAttitudeFromBrowserProvider.js

@@ -0,0 +1,224 @@
+import geomagnetism from 'geomagnetism';
+import isNumber from 'lodash.isnumber';
+
+import {
+    Attitude, Coordinates
+} from '@wemap/geo';
+import {
+    deg2rad, rad2deg, Quaternion, Rotations
+} from '@wemap/maths';
+import {
+    Browser, BrowserUtils
+} from '@wemap/utils';
+
+import Provider from '../../Provider';
+import EventType from '../../../events/EventType';
+import AskImuOnDesktopError from '../../../errors/AskImuOnDesktopError';
+import MissingMagnetometerError from '../../../errors/MissingMagnetometerError';
+import MissingSensorError from '../../../errors/MissingSensorError';
+import Availability from '../../../events/Availability';
+import { AbsolutePosition } from '../../../Providers';
+
+
+/**
+ * Absolute attitude provider gives the device attitude in East-North-Up (ENU) frame using
+ *  browser deviceorientation or deviceorientationabsolute
+ * The provider does not work until an AbsolutePosition is given. This is necessary to
+ *  calculate declination.
+ *
+ * -----------------------------------
+ * Overview of compatibilities:
+ * -----------------------------------
+ *
+ * Chrome Android (v72.0.3626): YES (via deviceorientationabsolute)
+ * Safari iOS (v12.0): YES (via deviceorientation and event.webkitCompassHeading)
+ * Opera Android (v50.2.2426): NO {@link https://developer.mozilla.org/en-US/docs/Web/API/Window/ondeviceorientation}
+ * Firefox Android (v65.0.1): NO {@link https://www.fxsitecompat.com/en-CA/docs/2018/various-device-sensor-apis-are-now-deprecated/}
+ *
+ * -----------------------------------
+ */
+class AbsoluteAttitudeFromBrowserProvider extends Provider {
+
+    // from http://tyrex.inria.fr/mobile/benchmarks-attitude/
+    static DEFAULT_ACCURACY = deg2rad(15);
+
+    /**
+    * @override
+    */
+    static get displayName() {
+        return 'Absolute Attitude from Browser';
+    }
+
+    /**
+     * @override
+     */
+    static get eventsType() {
+        return [EventType.AbsoluteAttitude];
+    }
+
+    /**
+     * @override
+     */
+    get _availability() {
+        return BrowserUtils.isMobile
+            ? Availability.yes()
+            : Availability.no(new AskImuOnDesktopError());
+    }
+
+    /**
+     * @override
+     */
+    start() {
+        switch (BrowserUtils.name) {
+        case Browser.CHROME:
+            window.addEventListener('deviceorientationabsolute',
+                this.onDeviceOrientationChromeEvent, true);
+            break;
+
+        case Browser.SAFARI:
+            window.addEventListener('deviceorientation',
+                this.onDeviceOrientationSafariEvent, true);
+            break;
+        }
+
+
+        const lastAbsolutePosition = AbsolutePosition.lastEvent;
+        if (lastAbsolutePosition) {
+            this.onAbsolutePositionEvent(lastAbsolutePosition);
+        } else {
+            this.absolutePositionProviderId = AbsolutePosition.addEventListener(
+                events => this.onAbsolutePositionEvent(events[0]),
+                this.notifyError,
+                this.name,
+                false
+            );
+        }
+
+    }
+
+    /**
+     * @override
+     */
+    stop() {
+        switch (BrowserUtils.name) {
+        case Browser.CHROME:
+            window.removeEventListener('deviceorientationabsolute',
+                this.onDeviceOrientationChromeEvent, true);
+            break;
+
+        case Browser.SAFARI:
+            window.removeEventListener('deviceorientation',
+                this.onDeviceOrientationSafariEvent, true);
+            break;
+        }
+
+        if (isNumber(this.absolutePositionProviderId)) {
+            AbsolutePosition.removeEventListener(this.absolutePositionProviderId);
+        }
+    }
+
+
+    onDeviceOrientationChromeEvent = e => {
+
+        this.magQuaternionTimestamp = e.timeStamp / 1e3;
+
+        if (!isNumber(e.alpha) || !isNumber(e.beta) || !isNumber(e.gamma)) {
+            this.notifyError(new MissingSensorError().from('deviceorientationabsolute'));
+            return;
+        }
+
+        this.magQuaternion = Rotations.eulerToQuaternionZXYDegrees(
+            [e.alpha, e.beta, e.gamma]);
+
+        this.compute();
+    };
+
+
+    onDeviceOrientationSafariEvent = e => {
+
+        this.magQuaternionTimestamp = e.timeStamp / 1e3;
+
+        if (!isNumber(e.beta) || !isNumber(e.gamma)) {
+            this.notifyError(new MissingSensorError().from('deviceorientation'));
+            return;
+        }
+
+        if (!isNumber(e.webkitCompassHeading)) {
+            super.notifyError(new MissingMagnetometerError().from('deviceorientation'));
+            return;
+        }
+
+        let webkitCompassHeading = -e.webkitCompassHeading;
+        // Be Careful: webkitCompassHeading is not continuous.
+        // Reference frame changes in function of beta with thresholds: beta > 30deg and beta < 62deg
+        // Below 30deg, East-North-Up (ENU) frame is used
+        // Above 62def, East-Up-South (EUS) frame is used
+        // Between 30deg and 62deg either ENU or EUS can be used
+        if (e.beta > 62) {
+            webkitCompassHeading = this.constructor.headingEusToEnu(
+                180 - webkitCompassHeading, e.beta, e.gamma);
+        }
+        this.magQuaternion = Rotations.eulerToQuaternionZXYDegrees(
+            [webkitCompassHeading, e.beta, e.gamma]);
+
+        this.compute();
+    };
+
+
+    compute() {
+
+        if (!this.declinationQuaternion || !this.magQuaternion) {
+            return;
+        }
+
+        const trueQuaternion = Quaternion.multiply(this.declinationQuaternion, this.magQuaternion);
+        const attitude = new Attitude(trueQuaternion, this.magQuaternionTimestamp,
+            this.constructor.DEFAULT_ACCURACY, this.name);
+        this.notify(this.createEvent(
+            EventType.AbsoluteAttitude,
+            attitude,
+            this.magQuaternionTimestamp,
+            [this.absolutePositionEvent]
+        ));
+    }
+
+    /**
+     * Initialized declination quaternion using current position.
+     * This method should be theoretically called every time the user moves.
+     * But in reality declination does not change as much.
+     * @param {Coordinates} positionEvent user position event
+     */
+    onAbsolutePositionEvent = positionEvent => {
+
+        if (!positionEvent) {
+            return;
+        }
+
+        this.absolutePositionEvent = positionEvent;
+
+        const position = positionEvent.data;
+        const wmmResult = geomagnetism.model().point([position.lat, position.lng]);
+        // Declination is given in NED frame and our code use ENU, that is why we have: "-decl"
+        this.declinationQuaternion = Quaternion.fromAxisAngle([0, 0, 1], - deg2rad(wmmResult.decl));
+
+        AbsolutePosition.removeEventListener(this.absolutePositionProviderId);
+        delete this.absolutePositionProviderId;
+        this.compute();
+    }
+
+    /**
+     * {@link https://math.stackexchange.com/questions/3181981/solve-a-system-of-rotation-matrices-z-x-z-z-x-y}
+     */
+    static headingEusToEnu(_alpha, _beta, _gamma) {
+        const alpha = deg2rad(_alpha);
+        const beta = deg2rad(_beta);
+        const gamma = deg2rad(_gamma);
+
+        return rad2deg(Math.atan2(
+            Math.cos(alpha) * Math.sin(gamma) + Math.cos(gamma) * Math.sin(alpha) * Math.sin(beta),
+            Math.sin(alpha) * Math.sin(gamma) - Math.cos(alpha) * Math.cos(gamma) * Math.sin(beta)
+        ));
+    }
+}
+
+export default AbsoluteAttitudeFromBrowserProvider;

package/src/providers/attitude/absolute/AbsoluteAttitudeFromRelAttProvider.js

@@ -0,0 +1,134 @@
+import { Quaternion } from '@wemap/maths';
+import { Attitude } from '@wemap/geo';
+
+import Provider from '../../Provider';
+import EventType from '../../../events/EventType';
+import {
+    RelativeAttitude, AbsoluteAttitude
+} from '../../../Providers';
+
+/**
+ * Absolute attitude provider gives the device attitude in East-North-Up (ENU) frame
+ */
+class AbsoluteAttitudeFromRelAttProvider extends Provider {
+
+    accuracy = 0;
+
+    /**
+    * @override
+    */
+    static get displayName() {
+        return 'Absolute Attitude';
+    }
+
+    /**
+     * @override
+     */
+    static get eventsType() {
+        return [EventType.AbsoluteAttitude];
+    }
+
+    /**
+     * @override
+     */
+    get _availability() {
+        return RelativeAttitude.availability;
+    }
+
+    /**
+     * @override
+     */
+    start() {
+
+        this.relativeAttitudeProviderId = RelativeAttitude.addEventListener(
+            events => this.onRelativeAttitudeEvent(events[0]),
+            this.notifyError,
+            this.name);
+
+
+        this.onAbsoluteAttitudeEvent(AbsoluteAttitude.lastEvent);
+        this.absoluteAttitudeProviderId = AbsoluteAttitude.addEventListener(
+            events => this.onAbsoluteAttitudeEvent(events[0]),
+            this.notifyError,
+            this.name,
+            false
+        );
+    }
+
+    /**
+     * @override
+     */
+    stop() {
+        RelativeAttitude.removeEventListener(this.relativeAttitudeProviderId);
+        AbsoluteAttitude.removeEventListener(this.absoluteAttitudeProviderId);
+    }
+
+
+    onRelativeAttitudeEvent(relativeAttitudeEvent) {
+
+        /**
+         * Calculate relative accuracy
+         */
+        if (this.relativeAttitudeEvent) {
+            const {
+                accuracy, time
+            } = relativeAttitudeEvent.data;
+            const diffTime = time - this.relativeAttitudeEvent.data.time;
+            this.accuracy += diffTime * accuracy;
+        }
+
+        this.relativeAttitudeEvent = relativeAttitudeEvent;
+        this.compute();
+    }
+
+
+    onAbsoluteAttitudeEvent = absoluteAttitudeEvent => {
+
+        if (!absoluteAttitudeEvent) {
+            return;
+        }
+
+        /**
+         * Use absolute attitude events only when they are not from this provider
+         */
+        if (absoluteAttitudeEvent.providersStack.includes(this.name)) {
+            return;
+        }
+        this.absoluteAttitudeEvent = absoluteAttitudeEvent;
+        this.accuracy = 0;
+
+        // preprocess zOffset for "compute" function
+        const currentHeading = this.relativeAttitudeEvent ? this.relativeAttitudeEvent.data.heading : 0;
+        this.zOffset = Quaternion.fromAxisAngle([0, 0, 1], -absoluteAttitudeEvent.data.heading + currentHeading);
+
+        this.compute();
+    };
+
+
+    compute() {
+        if (!this.absoluteAttitudeEvent || !this.relativeAttitudeEvent) {
+            return;
+        }
+
+        const {
+            quaternion, time
+        } = this.relativeAttitudeEvent.data;
+
+        const absoluteAttitudeAccuracy = this.absoluteAttitudeEvent.data.accuracy;
+
+        const absoluteQuat = Quaternion.multiply(this.zOffset, quaternion);
+        const newAccuracy = absoluteAttitudeAccuracy + this.accuracy;
+        const attitude = new Attitude(absoluteQuat, time, newAccuracy, this.name);
+
+        this.notify(this.createEvent(
+            EventType.AbsoluteAttitude,
+            attitude,
+            time,
+            [this.relativeAttitudeEvent, this.absoluteAttitudeEvent]
+        ));
+
+    }
+
+}
+
+export default AbsoluteAttitudeFromRelAttProvider;