@wemap/positioning 1.2.2 → 2.1.0

package/src/providers/attitude/AbsoluteAttitudeProvider.js

@@ -0,0 +1,207 @@
+import geomagnetism from 'geomagnetism';
+
+import {
+    Attitude, WGS84
+} 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 Logger from '@wemap/logger';
+
+
+/**
+ * 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 AbsoluteAttitudeProvider extends Provider {
+
+    /**
+    * @override
+    */
+    static get displayName() {
+        return 'Absolute Attitude from Browser';
+    }
+
+    /**
+     * @override
+     */
+    static get eventsType() {
+        return [EventType.AbsoluteAttitude];
+    }
+
+    /**
+     * @override
+     */
+    static checkAvailabilityErrors() {
+
+        if (BrowserUtils.isMobile) {
+            return [];
+        }
+
+        return [
+            AbsoluteAttitudeProvider.createError(
+                EventType.AbsoluteAttitude,
+                new AskImuOnDesktopError()
+            )
+        ];
+    }
+
+    /**
+     * @override
+     */
+    startInternal() {
+        switch (BrowserUtils.name) {
+        case Browser.CHROME:
+            window.addEventListener('deviceorientationabsolute',
+                this.onDeviceOrientationChromeEvent, true);
+            break;
+
+        case Browser.SAFARI:
+            window.addEventListener('deviceorientation',
+                this.onDeviceOrientationSafariEvent, true);
+            break;
+        }
+    }
+
+    /**
+     * @override
+     */
+    stopInternal() {
+        switch (BrowserUtils.name) {
+        case Browser.CHROME:
+            window.removeEventListener('deviceorientationabsolute',
+                this.onDeviceOrientationChromeEvent, true);
+            break;
+
+        case Browser.SAFARI:
+            window.removeEventListener('deviceorientation',
+                this.onDeviceOrientationSafariEvent, true);
+            break;
+        }
+    }
+
+
+    onDeviceOrientationChromeEvent = e => {
+
+        const timestamp = e.timeStamp / 1e3;
+
+        if (!e.alpha || !e.beta || !e.gamma) {
+            super.notifyError(this.createError(
+                EventType.AbsoluteAttitude,
+                new MissingSensorError().from('deviceorientationabsolute'),
+                timestamp
+            ));
+            return;
+        }
+        this.onDeviceOrientationCommonEvent(timestamp,
+            Rotations.eulerToQuaternionZXYDegrees([e.alpha, e.beta, e.gamma]));
+    };
+
+
+    onDeviceOrientationSafariEvent = e => {
+
+        const timestamp = e.timeStamp / 1e3;
+
+        if (!e.beta || !e.gamma) {
+            super.notifyError(this.createError(
+                EventType.AbsoluteAttitude,
+                new MissingSensorError().from('deviceorientation'),
+                timestamp
+            ));
+            return;
+        }
+
+        if (!e.webkitCompassHeading) {
+            super.notifyError(this.createError(
+                EventType.AbsoluteAttitude,
+                new MissingMagnetometerError().from('deviceorientation'),
+                timestamp
+            ));
+            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 = AbsoluteAttitudeProvider.headingEusToEnu(
+                180 - webkitCompassHeading, e.beta, e.gamma);
+        }
+        const quaternion = Rotations.eulerToQuaternionZXYDegrees(
+            [webkitCompassHeading, e.beta, e.gamma]);
+
+        this.onDeviceOrientationCommonEvent(timestamp, quaternion);
+    };
+
+
+    onDeviceOrientationCommonEvent = (timestamp, quaternion) => {
+
+        if (!this.declinationQuaternion) {
+            Logger.warn('Location of AbsoluteAttitude provider is not set yet. '
+                + 'Please call setLocation() before.');
+            return;
+        }
+        const trueQuaternion = Quaternion.multiply(this.declinationQuaternion, quaternion);
+
+        super.notify(
+            this.createEvent(
+                EventType.AbsoluteAttitude,
+                new Attitude(trueQuaternion),
+                timestamp
+            )
+        );
+    }
+
+    /**
+     * Initialized declination quaternion using current location.
+     * This method should be theoretically called every time the user moves.
+     * But in reality declination does not change as much.
+     * @param {WGS84} location user location
+     */
+    setLocation(location) {
+        const wmmResult = geomagnetism.model().point([location.lat, location.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));
+    }
+
+    /**
+     * {@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 AbsoluteAttitudeProvider;

package/src/providers/attitude/EkfAttitude.js

@@ -0,0 +1,238 @@
+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(
+                Matrix.identity4(),
+                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,116 @@
+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

@@ -0,0 +1,129 @@
+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';
+import ProviderError from '../../events/ProviderError';
+
+
+/**
+ * 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,
+            this.onImuError, { require: [EventType.Acceleration, EventType.AngularRate] });
+
+    }
+
+    /**
+    * @override
+    */
+    static get displayName() {
+        return 'Relative Attitude from Ekf';
+    }
+
+    /**
+     * @override
+     */
+    static get eventsType() {
+        return [EventType.RelativeAttitude];
+    }
+
+    /**
+     * @override
+     */
+    static get requiredProviders() {
+        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);
+    }
+
+    onImuError = imuErrors => {
+        this.notifyError(...ProviderError.modifyArrayDataType(imuErrors, EventType.RelativeAttitude));
+    }
+
+    /**
+     * @override
+     */
+    static checkAvailabilityErrors() {
+        return ProviderError.modifyArrayDataType(
+            super.checkAvailabilityErrors(),
+            EventType.RelativeAttitude
+        );
+    }
+}
+
+export default RelativeAttitudeProvider;