@wemap/positioning 2.1.0 → 2.2.0

package/src.old/attitude/AttitudeHandler.js

@@ -1,342 +0,0 @@
-import geomagnetism from 'geomagnetism';
-
-import { Attitude } from '@wemap/geo';
-import {
-    deg2rad, Quaternion
-} from '@wemap/maths';
-
-import EkfAttitude from './EkfAttitude';
-import SensorsCompatibility from '../sensors/SensorsCompatibility';
-
-
-const RelativeMethod = {
-    AUTOMATIC: 0,
-    BROWSER: 1,
-    INTERNAL_EKF: 2
-};
-
-const AbsoluteMethod = {
-    AUTOMATIC: 0,
-    BROWSER: 1,
-    INTERNAL_EKF: 2,
-    INTERNAL_CUSTOM: 3
-};
-
-
-class AttitudeHandler {
-
-    isRelativeStarted = false;
-
-    constructor() {
-        this.sensorsCompatibility = new SensorsCompatibility();
-        this.relativeOffsetQuaternion = [1, 0, 0, 0];
-        this.ekfAttitude = new EkfAttitude();
-    }
-
-    static get RelativeMethod() {
-        return RelativeMethod;
-    }
-
-    static get AbsoluteMethod() {
-        return AbsoluteMethod;
-    }
-
-    setLogger(logger) {
-        this.sensorsCompatibility.setLogger(logger);
-    }
-
-    startRelative(callback, method = RelativeMethod.INTERNAL_EKF) {
-
-        if (method === RelativeMethod.AUTOMATIC) {
-            throw new Error('Do not use automatic method for the moment');
-        }
-
-        let promise;
-
-        this.relativeMethodAnswer = false;
-
-        const fn = (quaternion, timestamp) => {
-            if (quaternion) {
-                callback(new Attitude(quaternion), timestamp);
-            }
-        };
-
-        this.relativeMethod = method;
-
-        if (method === RelativeMethod.AUTOMATIC) {
-
-            promise = new Promise((resolve, reject) => {
-                // Try internal EKF first
-                this.startRelative(callback, RelativeMethod.INTERNAL_EKF)
-                    .then(resolve)
-                    .catch(() => {
-                        this.stopRelative();
-                        // If EKF did not work, try Browser method
-                        this.startRelative(callback, RelativeMethod.BROWSER)
-                            .then(resolve)
-                            .catch(() => {
-                                this.stopRelative();
-                                reject();
-                            });
-                    });
-            });
-
-        } else if (method === RelativeMethod.BROWSER) {
-
-            promise = this.sensorsCompatibility.startRelativeOrientation((quaternion, timestamp) => {
-                quaternion = Quaternion.multiply(this.relativeOffsetQuaternion, quaternion);
-                fn(quaternion, timestamp);
-            });
-
-        } else if (method === RelativeMethod.INTERNAL_EKF) {
-
-            let lastTimestamp = 0;
-            promise = this.sensorsCompatibility.startImu(accGyrEvent => {
-                const timestamp = accGyrEvent.timestamp;
-                const acc = accGyrEvent.acc;
-                const gyr = accGyrEvent.gyr;
-
-                // Handle timestamps and dt
-                if (lastTimestamp === 0) {
-                    lastTimestamp = timestamp;
-                    return;
-                }
-                const diffTime = timestamp - lastTimestamp;
-                lastTimestamp = timestamp;
-                const quaternion = this.ekfAttitude.update(diffTime, acc, gyr);
-                fn(quaternion, timestamp);
-            }, {
-                accelerometer: true,
-                gyroscope: true
-            });
-        } else {
-            return Promise.reject();
-        }
-
-        this.isRelativeStarted = true;
-
-        return promise;
-    }
-
-
-    stopRelative() {
-        this.isRelativeStarted = false;
-
-        if (this.relativeMethod === RelativeMethod.BROWSER) {
-            this.sensorsCompatibility.stopRelativeOrientation();
-        } else if (this.relativeMethod === RelativeMethod.INTERNAL_EKF) {
-            this.sensorsCompatibility.stopImu();
-        }
-        this.relativeMethod = null;
-    }
-
-
-    /**
-     *
-     * Set yaw offset, this value will be used as the filter does not use magnetometer
-     * @param {Number} heading heading offset in radians and clockwise
-     */
-    setRelativeHeading(heading) {
-
-        // Minus before "heading" is here because ENU attitude is counter-clockwise whereas WGS84 heading is clockwise.
-        heading = -heading;
-
-        // Offset from window orientation
-        heading += deg2rad(window.orientation || 0);
-
-        // // We don't know the method used, so we set both
-        this.relativeOffsetQuaternion = Quaternion.fromAxisAngle([0, 0, 1], heading);
-
-        // We don't know the method used, so we set both
-        this.ekfAttitude.setOrientationYaw(heading);
-
-        if (this.relativeMethod === RelativeMethod.BROWSER && this.relativeMethodAnswer) {
-            this.stopRelative();
-            return this.startRelative(this.relativeCallback, RelativeMethod.BROWSER);
-        }
-
-        return Promise.resolve();
-    }
-
-
-    startAbsolute(callback, method = AbsoluteMethod.BROWSER) {
-
-        if (method === AbsoluteMethod.AUTOMATIC) {
-            throw new Error('Do not use automatic method for the moment');
-        }
-
-        let promise;
-
-        this.absoluteMethodAnswer = false;
-
-        const fn = (quaternion, timestamp, externalData) => {
-            if (!quaternion || !this.declinationQuaternion) {
-                return;
-            }
-            const trueQuaternion = Quaternion.multiply(this.declinationQuaternion, quaternion);
-            callback(new Attitude(trueQuaternion), timestamp, externalData);
-        };
-
-        this.absoluteMethod = method;
-
-        if (method === AbsoluteMethod.AUTOMATIC) {
-
-            promise = new Promise((resolve, reject) => {
-
-                // Try internal EKF first
-                this.startAbsolute(callback, AbsoluteMethod.INTERNAL_EKF).catch(() => {
-                    this.stopAbsolute();
-
-                    // If EKF did not work, try internal custom method
-                    this.startAbsolute(callback, AbsoluteMethod.INTERNAL_CUSTOM).catch(() => {
-                        this.stopAbsolute();
-
-                        // If internal custom did not work, try Browser method
-                        this.startAbsolute(callback, AbsoluteMethod.BROWSER).catch(() => {
-
-                            this.stopAbsolute();
-                            reject();
-
-                        }).then(resolve);
-
-                    }).then(resolve);
-
-                }).then(resolve);
-            });
-
-        } else if (method === AbsoluteMethod.INTERNAL_CUSTOM) {
-
-            let lastAttitude;
-            let isFirst = true;
-
-            promise = new Promise((resolve, reject) => {
-
-                const startRelativeFn = (heading) => {
-                    this.stopAbsolute();
-
-                    // Mandatory, otherwise this.absoluteMethod is null and cannot be stopped
-                    this.absoluteMethod = AbsoluteMethod.INTERNAL_CUSTOM;
-
-                    this.startRelative(callback)
-                        .then(resolve)
-                        .catch(() => {
-                            this.stopRelative();
-                            reject();
-                        });
-                    this.setRelativeHeading(heading);
-                };
-
-                const customCallback = (attitude, timestamp, externalData) => {
-                    lastAttitude = attitude;
-
-                    if (isFirst) {
-                        this.timeoutAbsoluteFirstMillis = setTimeout(() => {
-                            startRelativeFn(externalData && externalData.webkitCompassHeading
-                                ? deg2rad(externalData.webkitCompassHeading)
-                                : lastAttitude.heading);
-                        }, 300);
-
-                        isFirst = false;
-                    }
-
-                };
-
-                this.startAbsolute(customCallback, AbsoluteMethod.BROWSER)
-                    .catch(() => {
-                        this.stopAbsolute();
-                        reject();
-                    });
-            });
-
-        } else if (method === AbsoluteMethod.BROWSER) {
-            promise = this.sensorsCompatibility.startAbsoluteOrientation((quaternion, timestamp, externalData) => {
-                fn(quaternion, timestamp, externalData);
-            });
-
-        } else if (method === AbsoluteMethod.INTERNAL_EKF) {
-            let lastTimestamp = 0;
-            promise = this.sensorsCompatibility.startImu(accGyrMagEvent => {
-                const timestamp = accGyrMagEvent.timestamp;
-                const acc = accGyrMagEvent.acc;
-                const gyr = accGyrMagEvent.gyr;
-                const mag = accGyrMagEvent.mag;
-
-                // Handle timestamps and dt
-                if (lastTimestamp === 0) {
-                    lastTimestamp = timestamp;
-                    return;
-                }
-                const diffTime = timestamp - lastTimestamp;
-                lastTimestamp = timestamp;
-                const quaternion = this.ekfAttitude.update(diffTime, acc, gyr, mag);
-                fn(quaternion, timestamp);
-            });
-        } else {
-            promise = Promise.reject();
-        }
-
-        return promise;
-    }
-
-    stopAbsolute() {
-        if (this.absoluteMethod === AbsoluteMethod.BROWSER) {
-            this.sensorsCompatibility.stopAbsoluteOrientation();
-        } else if (this.absoluteMethod === AbsoluteMethod.INTERNAL_EKF) {
-            this.sensorsCompatibility.stopImu();
-        } else if (this.absoluteMethod === AbsoluteMethod.INTERNAL_CUSTOM) {
-            this.stopRelative();
-        }
-        if (this.timeoutAbsoluteFirstMillis) {
-            clearTimeout(this.timeoutAbsoluteFirstMillis);
-            this.timeoutAbsoluteFirstMillis = null;
-        }
-        this.absoluteMethod = null;
-    }
-
-    // This method should be theoretically called every time the user moves.
-    // But in reality declination does not change as much.
-    setUserLocationForAbsolute(userLocation) {
-        const wmmResult = geomagnetism.model().point([userLocation.lat, userLocation.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));
-    }
-
-    startMonitoringInclination(callback) {
-
-        this.sensorsCompatibilityInclination = new SensorsCompatibility();
-
-        return this.sensorsCompatibility.startImu(({ acc }) => {
-
-            const screenOrientation = window.orientation || 0;
-
-            const sizeAcc = Math.sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
-            const accNormalized = [acc[0] / sizeAcc, acc[1] / sizeAcc, acc[2] / sizeAcc];
-
-            const q = [accNormalized[2] + 1, accNormalized[1], -accNormalized[0], 0];
-            const qSize = Math.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]);
-            const qNormalized = [q[0] / qSize, q[1] / qSize, q[2] / qSize, 0];
-
-            let inclination;
-            if (screenOrientation === 0) {
-                inclination = Math.asin(2 * qNormalized[1] * qNormalized[0]);
-            } else if (screenOrientation === 90) {
-                inclination = -Math.asin(2 * qNormalized[2] * qNormalized[0]);
-            } else if (screenOrientation === -90) {
-                inclination = Math.asin(2 * qNormalized[2] * qNormalized[0]);
-            } else if (screenOrientation === 180) {
-                inclination = -Math.asin(2 * qNormalized[1] * qNormalized[0]);
-            }
-
-            callback(inclination);
-
-        }, { accelerometer: true });
-    }
-
-    stopMonitoringInclination() {
-        if (this.sensorsCompatibilityInclination) {
-            this.sensorsCompatibilityInclination.stopImu();
-        }
-    }
-}
-
-export default AttitudeHandler;

package/src.old/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(
-                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.old/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);
-        }
-    });
-});