@wemap/providers 12.10.8 → 12.10.9

package/src/providers/attitude/EkfAttitude.ts

@@ -1,233 +0,0 @@
-import { Matrix_t } from '@wemap/maths';
-import {
-    Matrix, Matrix3, Matrix3_t, Matrix4, Matrix4_t,
-    Quaternion, Quaternion_t, Vector, Vector3, Vector3_t,
-    Vector4_t
-} from '@wemap/maths';
-
-type RelativeNoises = { acc: number, gyr: number };
-type AbsoluteNoises = { acc: number, gyr: number, yc: number };
-
-type RelativeNoisesMat = { acc: Matrix3_t, gyr: Matrix3_t };
-type AbsoluteNoisesMat = { acc: Matrix3_t, gyr: Matrix3_t, yc: Matrix3_t };
-
-const DEFAULT_RELATIVE_NOISES = {
-    acc: 0.5,
-    gyr: 0.3
-};
-
-const DEFAULT_ABSOLUTE_NOISES = {
-    acc: 0.5,
-    gyr: 0.3,
-    yc: 2
-};
-
-class EkfAttitude {
-    P: Matrix4_t;
-    accRef: Vector3_t;
-    cRef: Vector3_t;
-    noises!: { relative: RelativeNoisesMat, absolute: AbsoluteNoisesMat };
-    quaternion: Quaternion_t | null;
-
-    constructor(accRef: Vector3_t = [0, 0, 1], ycRef: Vector3_t = [-1, 0, 0]) {
-
-        this.accRef = accRef;
-        this.cRef = ycRef;
-
-        this.P = Matrix4.fromDiagVector(Array(4).fill(0.1 ** 2) as Vector4_t);
-
-        this.quaternion = null;
-        
-        this.noises = {} as never;
-        this.setRelativeNoises(DEFAULT_RELATIVE_NOISES);
-        this.setAbsoluteNoises(DEFAULT_ABSOLUTE_NOISES);
-    }
-
-    setRelativeNoises(relativeNoises: RelativeNoises) {
-        this.noises.relative = {
-            acc: Matrix3.diag(Array(3).fill(relativeNoises.acc ** 2) as Vector3_t),
-            gyr: Matrix3.diag(Array(3).fill(relativeNoises.gyr ** 2) as Vector3_t)
-        };
-    }
-
-
-    setAbsoluteNoises(absoluteNoises: AbsoluteNoises) {
-        this.noises.absolute = {
-            acc: Matrix3.diag(Array(3).fill(absoluteNoises.acc ** 2) as Vector3_t),
-            gyr: Matrix3.diag(Array(3).fill(absoluteNoises.gyr ** 2) as Vector3_t),
-            yc: Matrix3.diag(Array(3).fill(absoluteNoises.yc ** 2) as Vector3_t)
-        };
-    }
-
-    /**
-     * Try to initialize filter.
-     * To initialize, we need at least current acceleration (acc)
-     */
-    tryInitialize(acc: Vector3_t, mag?: Vector3_t) {
-
-        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: Matrix3_t = [
-                [H[0], M[0], accNormalized[0]],
-                [H[1], M[1], accNormalized[1]],
-                [H[2], M[2], accNormalized[2]]
-            ];
-
-            this.quaternion = Quaternion.fromMatrix3Matlab(R);
-
-        } else {
-
-            const r = Vector3.dot(accNormalized, this.accRef) + 1;
-            const v = Vector3.cross(accNormalized, this.accRef);
-
-            let quaternion: Quaternion_t = [r, v[0], v[1], v[2]];
-            quaternion = Quaternion.normalize(quaternion);
-
-            this.quaternion = quaternion;
-        }
-
-        return this.quaternion;
-    }
-
-    update(diffTime: number, acc: Vector3_t, gyr: Vector3_t, mag?: Vector3_t) {
-
-        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 = Matrix4.multiplyVector(F, q);
-        const E1 = Matrix3.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.sum(eSkew, E1));
-
-        const Qk = Matrix4.multiplyScalar(
-            Matrix.multiply(
-                Matrix.multiply(E, this.noises[mag ? 'absolute' : 'relative'].gyr),
-                Matrix.transpose(E)
-            ) as Matrix4_t,
-            (diffTime / 2) ** 2
-        );
-
-        const pAPriori = Matrix4.sum(
-            Matrix4.multiply(
-                Matrix4.multiply(F, this.P),
-                Matrix4.transpose(F)
-            ),
-            Qk
-        );
-
-        /* ------------
-         *   CORRECTION
-         * ------------*/
-
-        const accNormalized = Vector3.normalize(acc);
-        let dz, K: Matrix_t, 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.rotateMatlab(qAPriori, this.cRef));
-            const dzAcc = Vector3.subtract(accNormalized, Quaternion.rotateMatlab(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.acc);
-            const R = Matrix.concatRow(RYc, RAcc);
-
-            K = Matrix.multiply(
-                Matrix.multiply(pAPriori, Matrix.transpose(H)),
-                Matrix.inverse(
-                    Matrix.sum(
-                        Matrix.multiply(
-                            Matrix.multiply(H, pAPriori),
-                            Matrix.transpose(H)
-                        ),
-                        R
-                    )
-                ) as Matrix_t
-            );
-        } else {
-            dz = Vector3.subtract(accNormalized, Quaternion.rotateMatlab(qAPriori, this.accRef));
-            H = this.jacobianES(qAPriori, this.accRef);
-            const R = this.noises.relative.acc;
-
-            K = Matrix.multiply(
-                Matrix.multiply(pAPriori, Matrix.transpose(H)),
-                Matrix3.inverse(
-                    Matrix3.sum(
-                        Matrix.multiply(
-                            Matrix.multiply(H, pAPriori),
-                            Matrix.transpose(H)
-                        ) as Matrix3_t,
-                        R
-                    )
-                ) as Matrix_t
-            );
-        }
-
-        q = Quaternion.sum(
-            qAPriori,
-            Matrix.multiplyVector(K, dz) as Quaternion_t
-        );
-        const P = Matrix4.multiply(
-            Matrix4.subtract(
-                Matrix4.identity,
-                Matrix.multiply(K, H) as Matrix4_t
-            ),
-            pAPriori
-        );
-
-        q = Quaternion.normalize(q);
-        this.quaternion = q;
-        this.P = P;
-
-        return q;
-    }
-
-    computeC(b: Vector4_t): Matrix4_t {
-        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: Quaternion_t, v: Vector3_t) {
-
-        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/TurnProvider.ts

@@ -1,58 +0,0 @@
-import { std } from '@wemap/maths';
-
-import Provider from '../Provider.js';
-import RelativeAttitudeProvider from './relative/RelativeAttitudeProvider.js';
-import { RelativeAttitude, Turn } from '../../events/Types.js';
-
-class TurnProvider extends Provider<Turn> {
-
-    static SLIDING_WINDOW_TIME = 0.3;  // in seconds
-    static STD_THRESHOLD = 0.075;
-    static CONSIDER_TURN_UNTIL = 1; // in seconds
-
-    providerId?: number;
-
-    slidingWindow: [number, number][] = [];
-
-    getName = () => 'Turn';
-
-    availability = () => RelativeAttitudeProvider.getAvailability();
-
-    start() {
-        this.providerId = RelativeAttitudeProvider.addEventListener(
-            this._parseRelativeAttitude,
-            this.notifyError
-        );
-    }
-
-    stop() {
-        RelativeAttitudeProvider.removeEventListener(this.providerId);
-    }
-
-    isTurning() {
-        if (!this.lastEvent || !RelativeAttitudeProvider.lastEvent) {
-            return false;
-        }
-
-        const diffTime = RelativeAttitudeProvider.lastEvent.time as number - this.lastEvent.timestamp;
-        return diffTime < TurnProvider.CONSIDER_TURN_UNTIL;
-    }
-
-
-    private _parseRelativeAttitude = (relativeAttitudeEvent: RelativeAttitude) => {
-
-        const { heading } = relativeAttitudeEvent;
-        const timestamp = relativeAttitudeEvent.time as number;
-
-        this.slidingWindow = this.slidingWindow.filter(item => item[0] >= timestamp - TurnProvider.SLIDING_WINDOW_TIME);
-        this.slidingWindow.push([timestamp, heading]);
-
-        const stdVal = std(this.slidingWindow.map(item => item[1]));
-        if (stdVal > TurnProvider.STD_THRESHOLD) {
-            this.notify({ timestamp });
-        }
-
-    }
-}
-
-export default new TurnProvider();

package/src/providers/attitude/absolute/AbsoluteAttitudeFromBrowserProvider.ts

@@ -1,267 +0,0 @@
-import * as geomag from 'geomag';
-
-import { deg2rad, rad2deg, Quaternion, Rotations, Quaternion_t } from '@wemap/maths';
-import { Browser, BrowserUtils } from '@wemap/utils';
-
-import Provider from '../../Provider.js';
-import AskImuOnDesktopError from '../../../errors/AskImuOnDesktopError.js';
-import MissingMagnetometerError from '../../../errors/MissingMagnetometerError.js';
-import MissingSensorError from '../../../errors/MissingSensorError.js';
-import AbsolutePositionProvider from '../../position/absolute/AbsolutePositionProvider.js';
-import { AbsoluteAttitude, AbsolutePosition } from '../../../events/Types.js';
-
-
-// https://stackoverflow.com/a/73369838/2239938
-export interface DeviceOrientationEventiOS extends DeviceOrientationEvent {
-    webkitCompassHeading: number | null;
-    webkitCompassAccuracy: number | null;
-    requestPermission?: () => Promise<'granted' | 'denied'>;
-}
-
-
-
-declare global {
-    interface Window {
-        addEventListener(evtName: 'deviceorientationabsolute', cb: (evt: DeviceOrientationEvent) => void, options?: boolean): void;
-        addEventListener(evtName: 'deviceorientation', cb: (evt: DeviceOrientationEventiOS) => void, options?: boolean): void;
-        removeEventListener(evtName: 'deviceorientationabsolute', cb: (evt: DeviceOrientationEvent) => void, options?: boolean): void;
-        removeEventListener(evtName: 'deviceorientation', cb: (evt: DeviceOrientationEventiOS) => void, options?: boolean): void;
-    }
-}
-
-
-
-/**
- * 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 AbsoluteAttitudeFromBrowser extends Provider<AbsoluteAttitude> {
-
-    // from http://tyrex.inria.fr/mobile/benchmarks-attitude/
-    DEFAULT_ACCURACY = deg2rad(15);
-
-    absolutePositionProviderId?: number;
-    absolutePositionEvent?: AbsolutePosition;
-
-    declinationQuaternion?: Quaternion_t;
-    magQuaternion?: Quaternion_t;
-    magQuaternionTimestamp?: number;
-
-    iosPreviousQuat?: Quaternion_t;
-    iosIsSkyMode: boolean | null = null;
-
-
-    getName = () => 'AbsoluteAttitudeFromBrowser';
-
-    availability() {
-        return BrowserUtils.isMobile
-            ? Promise.resolve()
-            : Promise.resolve(new AskImuOnDesktopError());
-    }
-
-    start() {
-
-
-        const subscribe = () => {
-            switch (BrowserUtils.getName()) {
-                case Browser.CHROME:
-                    window.addEventListener('deviceorientationabsolute',
-                        this.onDeviceOrientationChromeEvent, true);
-                    break;
-
-                case Browser.SAFARI:
-                case Browser.IOS_WEBVIEW:
-                    window.addEventListener('deviceorientation',
-                        this.onDeviceOrientationSafariEvent, true);
-                    break;
-            }
-        };
-
-        const requestPermission = (DeviceOrientationEvent as unknown as DeviceOrientationEventiOS).requestPermission || undefined;
-
-        if (requestPermission) {
-            requestPermission()
-                .then(response => {
-                    if (response !== 'granted') {
-                        throw new Error('Permission not granted');
-                    }
-                    subscribe();
-                })
-                .catch(this.notifyError);
-        } else {
-            subscribe();
-        }
-
-
-        const lastAbsolutePosition = AbsolutePositionProvider.lastEvent;
-        if (lastAbsolutePosition) {
-            this.onAbsolutePositionEvent(lastAbsolutePosition);
-        } else {
-            this.absolutePositionProviderId = AbsolutePositionProvider.addEventListener(
-                this.onAbsolutePositionEvent,
-                this.notifyError,
-                false
-            );
-        }
-
-    }
-
-    stop() {
-        switch (BrowserUtils.getName()) {
-            case Browser.CHROME:
-                window.removeEventListener('deviceorientationabsolute',
-                    this.onDeviceOrientationChromeEvent, true);
-                break;
-
-            case Browser.SAFARI:
-            case Browser.IOS_WEBVIEW:
-                window.removeEventListener('deviceorientation',
-                    this.onDeviceOrientationSafariEvent, true);
-                break;
-        }
-
-        AbsolutePositionProvider.removeEventListener(this.absolutePositionProviderId);
-    }
-
-
-    onDeviceOrientationChromeEvent = (e: DeviceOrientationEvent) => {
-
-        this.magQuaternionTimestamp = e.timeStamp / 1e3;
-
-        if (typeof e.alpha !== 'number' || typeof e.beta !== 'number' || typeof e.gamma !== 'number') {
-            this.notifyError(new MissingSensorError().from('deviceorientationabsolute'));
-            return;
-        }
-
-        this.magQuaternion = Rotations.eulerToQuaternionZXYDegrees(
-            [e.alpha, e.beta, e.gamma]);
-
-        this.compute();
-    };
-
-
-    onDeviceOrientationSafariEvent = (e: DeviceOrientationEventiOS) => {
-
-        this.magQuaternionTimestamp = e.timeStamp / 1e3;
-
-        if (typeof e.beta !== 'number' || typeof e.gamma !== 'number') {
-            this.notifyError(new MissingSensorError().from('deviceorientation'));
-            return;
-        }
-
-        if (typeof e.webkitCompassHeading !== 'number') {
-            this.notifyError(new MissingMagnetometerError().from('deviceorientation'));
-            return;
-        }
-
-        /**
-         * Trying the best to retrieve a good quaternion from devicemotion event.
-         */
-
-        let alpha;
-        const [qw, qx, qy, qz] = Rotations.eulerToQuaternionZXYDegrees([e.webkitCompassHeading, e.beta, e.gamma]);
-        const groundAngle = rad2deg(Math.acos(qw ** 2 - qx ** 2 - qy ** 2 + qz ** 2));
-
-        let isSkyMode = null;
-        if (groundAngle > 136) {
-            isSkyMode = true;
-        } else if (groundAngle < 134) {
-            isSkyMode = false;
-        } else if (this.iosPreviousQuat && this.iosIsSkyMode !== null) {
-            /**
-             * This condition is true only if :
-             * - we are in the [134°; 136°] ground angle range
-             * - we know the previous quaternion
-             * - one of the both previous condition has been reached during this session
-             *
-             * In this case, we define a threshold to detect if there is a "jump" in the quaternion calculation,
-             * then, the mode is switched.
-             */
-            isSkyMode = Quaternion.distance([qw, qx, qy, qz], this.iosPreviousQuat) < 0.5
-                ? this.iosIsSkyMode
-                : !this.iosIsSkyMode;
-        }
-        this.iosPreviousQuat = [qw, qx, qy, qz];
-        this.iosIsSkyMode = isSkyMode;
-
-        if (typeof (isSkyMode) === 'undefined') {
-            // Algorithm uncertainity
-            return;
-        }
-
-        if (isSkyMode) {
-            alpha = 180 - e.webkitCompassHeading;
-        } else {
-            alpha = AbsoluteAttitudeFromBrowser.webkitCompassToHeading(
-                e.webkitCompassHeading, e.beta, e.gamma);
-        }
-
-        this.magQuaternion = Rotations.eulerToQuaternionZXYDegrees([alpha, e.beta, e.gamma]);
-
-        this.compute();
-    };
-
-
-    compute() {
-
-        if (!this.declinationQuaternion || !this.magQuaternion || !this.absolutePositionEvent) {
-            return;
-        }
-
-        const trueQuaternion = Quaternion.multiply(this.declinationQuaternion, this.magQuaternion);
-        const attitude = new AbsoluteAttitude(trueQuaternion, this.magQuaternionTimestamp, this.DEFAULT_ACCURACY);
-        this.notify(attitude);
-    }
-
-    /**
-     * 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.
-     */
-    onAbsolutePositionEvent = (position: AbsolutePosition) => {
-
-        this.absolutePositionEvent = position;
-
-        const wmmResult = geomag.field(position.lat, position.lng);
-
-        // Declination is given in NED frame and our code use ENU, that is why we have: "-declination"
-        this.declinationQuaternion = Quaternion.fromAxisAngle([0, 0, 1], - deg2rad(wmmResult.declination));
-
-        AbsolutePositionProvider.removeEventListener(this.absolutePositionProviderId);
-        delete this.absolutePositionProviderId;
-        this.compute();
-    }
-
-    static webkitCompassToHeading(_webkitCompassHeading: number, _beta: number, _gamma: number) {
-        const webkitCompassHeading = deg2rad(_webkitCompassHeading);
-        const beta = deg2rad(_beta);
-        const gamma = deg2rad(_gamma);
-
-        const c1 = Math.cos(webkitCompassHeading / 2);
-        const c2 = Math.cos(beta / 2);
-        const c3 = Math.cos(gamma / 2);
-        const s1 = Math.sin(webkitCompassHeading / 2);
-        const s2 = Math.sin(beta / 2);
-        const s3 = Math.sin(gamma / 2);
-
-        const qw = c1 * c2 * c3 - s1 * s2 * s3;
-        const qz = s1 * c2 * c3 + c1 * s2 * s3;
-
-        return rad2deg(-2 * Math.atan(qz / qw));
-    }
-}
-
-export default new AbsoluteAttitudeFromBrowser();