@wemap/positioning 1.2.0

package/src/attitude/AttitudeHandler.js

@@ -0,0 +1,343 @@
+import geomagnetism from 'geomagnetism';
+
+import {
+    Quaternion, Utils as MathUtils
+} from '@wemap/maths';
+
+import Attitude from './Attitude';
+import EkfAttitude from './EkfAttitude';
+import SensorsCompatibility from '../sensors/SensorsCompatibility';
+
+const { deg2rad } = MathUtils;
+
+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/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/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);
+        }
+    });
+});