Revert "Revert "Air Mouse""

This reverts commit bbc0888b45.

applications/plugins/airmouse/tracking/calibration_data.cc

@@ -0,0 +1,85 @@
+#include <furi.h>
+#include <furi_hal.h>
+
+#define TAG "tracker"
+
+#include "calibration_data.h"
+
+#include <cmath>
+#include <algorithm>
+
+// Student's distribution T value for 95% (two-sided) confidence interval.
+static const double Tn = 1.960;
+
+// Number of samples (degrees of freedom) for the corresponding T values.
+static const int Nn = 200;
+
+void CalibrationData::reset()
+{
+    complete = false;
+    count = 0;
+    sum = Vector::Zero();
+    sumSq = Vector::Zero();
+    mean = Vector::Zero();
+    median = Vector::Zero();
+    sigma = Vector::Zero();
+    delta = Vector::Zero();
+    xData.clear();
+    yData.clear();
+    zData.clear();
+}
+
+bool CalibrationData::add(Vector& data)
+{
+    if (complete) {
+        return true;
+    }
+
+    xData.push_back(data[0]);
+    yData.push_back(data[1]);
+    zData.push_back(data[2]);
+
+    sum += data;
+    sumSq += data * data;
+    count++;
+
+    if (count >= Nn) {
+        calcDelta();
+        complete = true;
+    }
+
+    return complete;
+}
+
+static inline double medianOf(std::vector<double>& list)
+{
+    std::sort(list.begin(), list.end());
+    int count = list.size();
+    int middle = count / 2;
+    return (count % 2 == 1) ? list[middle] : (list[middle - 1] + list[middle]) / 2.0l;
+}
+
+void CalibrationData::calcDelta()
+{
+    median.Set(medianOf(xData), medianOf(yData), medianOf(zData));
+
+    mean = sum / count;
+    Vector m2 = mean * mean;
+    Vector d = sumSq / count - m2;
+    Vector s2 = (d * count) / (count - 1);
+    sigma = Vector(std::sqrt(d[0]), std::sqrt(d[1]), std::sqrt(d[2]));
+    Vector s = Vector(std::sqrt(s2[0]), std::sqrt(s2[1]), std::sqrt(s2[2]));
+    delta = s * Tn / std::sqrt((double)count);
+    Vector low = mean - delta;
+    Vector high = mean + delta;
+
+    FURI_LOG_I(TAG,
+        "M[x] = { %f ... %f }  //  median = %f  //  avg = %f  //  delta = %f  //  sigma = %f",
+        low[0], high[0], median[0], mean[0], delta[0], sigma[0]);
+    FURI_LOG_I(TAG,
+        "M[y] = { %f ... %f }  //  median = %f  //  avg = %f  //  delta = %f  //  sigma = %f",
+        low[1], high[1], median[1], mean[1], delta[1], sigma[1]);
+    FURI_LOG_I(TAG,
+        "M[z] = { %f ... %f }  //  median = %f  //  avg = %f  //  delta = %f  //  sigma = %f",
+        low[2], high[2], median[2], mean[2], delta[2], sigma[2]);
+}

applications/plugins/airmouse/tracking/calibration_data.h

@@ -0,0 +1,117 @@
+#pragma once
+
+#include <toolbox/saved_struct.h>
+#include <storage/storage.h>
+#include <vector>
+
+#include "util/vector.h"
+
+#define CALIBRATION_DATA_VER (1)
+#define CALIBRATION_DATA_FILE_NAME ".calibration.data"
+#define CALIBRATION_DATA_PATH INT_PATH(CALIBRATION_DATA_FILE_NAME)
+#define CALIBRATION_DATA_MAGIC (0x23)
+
+#define CALIBRATION_DATA_SAVE(x)   \
+    saved_struct_save(             \
+        CALIBRATION_DATA_PATH,     \
+        (x),                       \
+        sizeof(CalibrationMedian), \
+        CALIBRATION_DATA_MAGIC,    \
+        CALIBRATION_DATA_VER)
+
+#define CALIBRATION_DATA_LOAD(x)   \
+    saved_struct_load(             \
+        CALIBRATION_DATA_PATH,     \
+        (x),                       \
+        sizeof(CalibrationMedian), \
+        CALIBRATION_DATA_MAGIC,    \
+        CALIBRATION_DATA_VER)
+
+typedef struct {
+    double x;
+    double y;
+    double z;
+} CalibrationMedian;
+
+typedef cardboard::Vector3 Vector;
+
+/**
+ * Helper class to gather some stats and store the calibration data. Right now it calculates a lot
+ * more stats than actually needed. Some of them are used for logging the sensors quality (and
+ * filing bugs), other may be required in the future, e.g. for bias.
+ */
+class CalibrationData {
+public:
+    /**
+     * Check if the sensors were calibrated before.
+     *
+     * @return {@code true} if calibration data is available, or {@code false} otherwise.
+     */
+    bool isComplete() {
+        return complete;
+    }
+
+    /** Prepare to collect new calibration data. */
+    void reset();
+
+    /**
+     * Retrieve the median gyroscope readings.
+     *
+     * @return Three-axis median vector.
+     */
+    Vector getMedian() {
+        return median;
+    }
+
+    /**
+     * Retrieve the mean gyroscope readings.
+     *
+     * @return Three-axis mean vector.
+     */
+    Vector getMean() {
+        return mean;
+    }
+
+    /**
+     * Retrieve the standard deviation of gyroscope readings.
+     *
+     * @return Three-axis standard deviation vector.
+     */
+    Vector getSigma() {
+        return sigma;
+    }
+
+    /**
+     * Retrieve the confidence interval size of gyroscope readings.
+     *
+     * @return Three-axis confidence interval size vector.
+     */
+    Vector getDelta() {
+        return delta;
+    }
+
+    /**
+     * Add a new gyroscope reading to the stats.
+     *
+     * @param data gyroscope values vector.
+     * @return {@code true} if we now have enough data for calibration, or {@code false} otherwise.
+     */
+    bool add(Vector& data);
+
+private:
+    // Calculates the confidence interval (mean +- delta) and some other related values, like
+    // standard deviation, etc. See https://en.wikipedia.org/wiki/Student%27s_t-distribution
+    void calcDelta();
+
+    int count;
+    bool complete;
+    Vector sum;
+    Vector sumSq;
+    Vector mean;
+    Vector median;
+    Vector sigma;
+    Vector delta;
+    std::vector<double> xData;
+    std::vector<double> yData;
+    std::vector<double> zData;
+};

applications/plugins/airmouse/tracking/imu/bmi160.h

@@ -0,0 +1,992 @@
+/**
+* Copyright (c) 2021 Bosch Sensortec GmbH. All rights reserved.
+*
+* BSD-3-Clause
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+*
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in the
+*    documentation and/or other materials provided with the distribution.
+*
+* 3. Neither the name of the copyright holder nor the names of its
+*    contributors may be used to endorse or promote products derived from
+*    this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* @file       bmi160.h
+* @date       2021-10-05
+* @version    v3.9.2
+*
+*/
+
+/*!
+ * @defgroup bmi160 BMI160
+ */
+
+#ifndef BMI160_H_
+#define BMI160_H_
+
+/*************************** C++ guard macro *****************************/
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "bmi160_defs.h"
+#ifdef __KERNEL__
+#include <bmi160_math.h>
+#else
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+#endif
+
+/*********************** User function prototypes ************************/
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiInit Initialization
+ * @brief Initialize the sensor and device structure
+ */
+
+/*!
+ * \ingroup bmi160ApiInit
+ * \page bmi160_api_bmi160_init bmi160_init
+ * \code
+ * int8_t bmi160_init(struct bmi160_dev *dev);
+ * \endcode
+ * @details This API is the entry point for sensor.It performs
+ *  the selection of I2C/SPI read mechanism according to the
+ *  selected interface and reads the chip-id of bmi160 sensor.
+ *
+ *  @param[in,out] dev : Structure instance of bmi160_dev
+ *  @note : Refer user guide for detailed info.
+ *
+ *  @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_init(struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiRegs Registers
+ * @brief Read data from the given register address of sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiRegs
+ * \page bmi160_api_bmi160_get_regs bmi160_get_regs
+ * \code
+ * int8_t bmi160_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API reads the data from the given register address of sensor.
+ *
+ * @param[in] reg_addr  : Register address from where the data to be read
+ * @param[out] data     : Pointer to data buffer to store the read data.
+ * @param[in] len       : No of bytes of data to be read.
+ * @param[in] dev       : Structure instance of bmi160_dev.
+ *
+ * @note For most of the registers auto address increment applies, with the
+ * exception of a few special registers, which trap the address. For e.g.,
+ * Register address - 0x24(BMI160_FIFO_DATA_ADDR)
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t
+    bmi160_get_regs(uint8_t reg_addr, uint8_t* data, uint16_t len, const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiRegs
+ * \page bmi160_api_bmi160_set_regs bmi160_set_regs
+ * \code
+ * int8_t bmi160_set_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API writes the given data to the register address
+ * of sensor.
+ *
+ * @param[in] reg_addr  : Register address from where the data to be written.
+ * @param[in] data      : Pointer to data buffer which is to be written
+ * in the sensor.
+ * @param[in] len       : No of bytes of data to write..
+ * @param[in] dev       : Structure instance of bmi160_dev.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t
+    bmi160_set_regs(uint8_t reg_addr, uint8_t* data, uint16_t len, const struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiSoftreset Soft reset
+ * @brief Perform soft reset of the sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiSoftreset
+ * \page bmi160_api_bmi160_soft_reset bmi160_soft_reset
+ * \code
+ * int8_t bmi160_soft_reset(struct bmi160_dev *dev);
+ * \endcode
+ * @details This API resets and restarts the device.
+ * All register values are overwritten with default parameters.
+ *
+ * @param[in] dev  : Structure instance of bmi160_dev.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_soft_reset(struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiConfig Configuration
+ * @brief Configuration of the sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiConfig
+ * \page bmi160_api_bmi160_set_sens_conf bmi160_set_sens_conf
+ * \code
+ * int8_t bmi160_set_sens_conf(struct bmi160_dev *dev);
+ * \endcode
+ * @details This API configures the power mode, range and bandwidth
+ * of sensor.
+ *
+ * @param[in] dev    : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_set_sens_conf(struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiConfig
+ * \page bmi160_api_bmi160_get_sens_conf bmi160_get_sens_conf
+ * \code
+ * int8_t bmi160_get_sens_conf(struct bmi160_dev *dev);
+ * \endcode
+ * @details This API gets accel and gyro configurations.
+ *
+ * @param[out] dev    : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_get_sens_conf(struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiPowermode Power mode
+ * @brief Set / Get power mode of the sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiPowermode
+ * \page bmi160_api_bmi160_set_power_mode bmi160_set_power_mode
+ * \code
+ * int8_t bmi160_set_power_mode(struct bmi160_dev *dev);
+ * \endcode
+ * @details This API sets the power mode of the sensor.
+ *
+ * @param[in] dev  : Structure instance of bmi160_dev.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_set_power_mode(struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiPowermode
+ * \page bmi160_api_bmi160_get_power_mode bmi160_get_power_mode
+ * \code
+ * int8_t bmi160_get_power_mode(struct bmi160_dev *dev);
+ * \endcode
+ * @details This API gets the power mode of the sensor.
+ *
+ * @param[in] dev         : Structure instance of bmi160_dev
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_get_power_mode(struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiData Sensor Data
+ * @brief Read sensor data
+ */
+
+/*!
+ * \ingroup bmi160ApiData
+ * \page bmi160_api_bmi160_get_sensor_data bmi160_get_sensor_data
+ * \code
+ * int8_t bmi160_get_sensor_data(uint8_t select_sensor,
+ *                             struct bmi160_sensor_data *accel,
+ *                             struct bmi160_sensor_data *gyro,
+ *                             const struct bmi160_dev *dev);
+ *
+ * \endcode
+ * @details This API reads sensor data, stores it in
+ * the bmi160_sensor_data structure pointer passed by the user.
+ * The user can ask for accel data ,gyro data or both sensor
+ * data using bmi160_select_sensor enum
+ *
+ * @param[in] select_sensor    : enum to choose accel,gyro or both sensor data
+ * @param[out] accel    : Structure pointer to store accel data
+ * @param[out] gyro     : Structure pointer to store gyro data
+ * @param[in] dev       : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_get_sensor_data(
+    uint8_t select_sensor,
+    struct bmi160_sensor_data* accel,
+    struct bmi160_sensor_data* gyro,
+    const struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiInt Interrupt configuration
+ * @brief Set interrupt configuration of the sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiInt
+ * \page bmi160_api_bmi160_set_int_config bmi160_set_int_config
+ * \code
+ * int8_t bmi160_set_int_config(struct bmi160_int_settg *int_config, struct bmi160_dev *dev);
+ * \endcode
+ * @details This API configures the necessary interrupt based on
+ *  the user settings in the bmi160_int_settg structure instance.
+ *
+ * @param[in] int_config  : Structure instance of bmi160_int_settg.
+ * @param[in] dev         : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_set_int_config(struct bmi160_int_settg* int_config, struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiStepC Step counter
+ * @brief Step counter operations
+ */
+
+/*!
+ * \ingroup bmi160ApiStepC
+ * \page bmi160_api_bmi160_set_step_counter bmi160_set_step_counter
+ * \code
+ * int8_t bmi160_set_step_counter(uint8_t step_cnt_enable, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API enables the step counter feature.
+ *
+ * @param[in] step_cnt_enable   : value to enable or disable
+ * @param[in] dev       : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_set_step_counter(uint8_t step_cnt_enable, const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiStepC
+ * \page bmi160_api_bmi160_read_step_counter bmi160_read_step_counter
+ * \code
+ * int8_t bmi160_read_step_counter(uint16_t *step_val, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API reads the step counter value.
+ *
+ * @param[in] step_val    : Pointer to store the step counter value.
+ * @param[in] dev         : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_read_step_counter(uint16_t* step_val, const struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiAux Auxiliary sensor
+ * @brief Auxiliary sensor operations
+ */
+
+/*!
+ * \ingroup bmi160ApiAux
+ * \page bmi160_api_bmi160_aux_read bmi160_aux_read
+ * \code
+ * int8_t bmi160_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API reads the mention no of byte of data from the given
+ * register address of auxiliary sensor.
+ *
+ * @param[in] reg_addr    : Address of register to read.
+ * @param[in] aux_data    : Pointer to store the read data.
+ * @param[in] len     : No of bytes to read.
+ * @param[in] dev         : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_aux_read(
+    uint8_t reg_addr,
+    uint8_t* aux_data,
+    uint16_t len,
+    const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiAux
+ * \page bmi160_api_bmi160_aux_write bmi160_aux_write
+ * \code
+ * int8_t bmi160_aux_write(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API writes the mention no of byte of data to the given
+ * register address of auxiliary sensor.
+ *
+ * @param[in] reg_addr    : Address of register to write.
+ * @param[in] aux_data    : Pointer to write data.
+ * @param[in] len     : No of bytes to write.
+ * @param[in] dev         : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_aux_write(
+    uint8_t reg_addr,
+    uint8_t* aux_data,
+    uint16_t len,
+    const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiAux
+ * \page bmi160_api_bmi160_aux_init bmi160_aux_init
+ * \code
+ * int8_t bmi160_aux_init(const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API initialize the auxiliary sensor
+ * in order to access it.
+ *
+ * @param[in] dev         : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_aux_init(const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiAux
+ * \page bmi160_api_bmi160_set_aux_auto_mode bmi160_set_aux_auto_mode
+ * \code
+ * int8_t bmi160_set_aux_auto_mode(uint8_t *data_addr, struct bmi160_dev *dev);
+ * \endcode
+ * @details This API is used to setup the auxiliary sensor of bmi160 in auto mode
+ * Thus enabling the auto update of 8 bytes of data from auxiliary sensor
+ * to BMI160 register address 0x04 to 0x0B
+ *
+ * @param[in] data_addr    : Starting address of aux. sensor's data register
+ *                           (BMI160 registers 0x04 to 0x0B will be updated
+ *                           with 8 bytes of data from auxiliary sensor
+ *                           starting from this register address.)
+ * @param[in] dev      : Structure instance of bmi160_dev.
+ *
+ * @note : Set the value of auxiliary polling rate by setting
+ *         dev->aux_cfg.aux_odr to the required value from the table
+ *         before calling this API
+ *
+ *@verbatim
+ *   dev->aux_cfg.aux_odr  |   Auxiliary ODR (Hz)
+ *  -----------------------|-----------------------
+ *  BMI160_AUX_ODR_0_78HZ  |        25/32
+ *  BMI160_AUX_ODR_1_56HZ  |        25/16
+ *  BMI160_AUX_ODR_3_12HZ  |        25/8
+ *  BMI160_AUX_ODR_6_25HZ  |        25/4
+ *  BMI160_AUX_ODR_12_5HZ  |        25/2
+ *  BMI160_AUX_ODR_25HZ    |        25
+ *  BMI160_AUX_ODR_50HZ    |        50
+ *  BMI160_AUX_ODR_100HZ   |        100
+ *  BMI160_AUX_ODR_200HZ   |        200
+ *  BMI160_AUX_ODR_400HZ   |        400
+ *  BMI160_AUX_ODR_800HZ   |        800
+ *@endverbatim
+ *
+ * @note : Other values of  dev->aux_cfg.aux_odr are reserved and not for use
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_set_aux_auto_mode(uint8_t* data_addr, struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiAux
+ * \page bmi160_api_bmi160_config_aux_mode bmi160_config_aux_mode
+ * \code
+ * int8_t bmi160_config_aux_mode(const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API configures the 0x4C register and settings like
+ * Auxiliary sensor manual enable/ disable and aux burst read length.
+ *
+ * @param[in] dev    : Structure instance of bmi160_dev.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_config_aux_mode(const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiAux
+ * \page bmi160_api_bmi160_read_aux_data_auto_mode bmi160_read_aux_data_auto_mode
+ * \code
+ * int8_t bmi160_read_aux_data_auto_mode(uint8_t *aux_data, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API is used to read the raw uncompensated auxiliary sensor
+ * data of 8 bytes from BMI160 register address 0x04 to 0x0B
+ *
+ * @param[in] aux_data       : Pointer to user array of length 8 bytes
+ *                             Ensure that the aux_data array is of
+ *                             length 8 bytes
+ * @param[in] dev        : Structure instance of bmi160_dev
+ *
+ * @retval zero -> Success  / -ve value -> Error
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_read_aux_data_auto_mode(uint8_t* aux_data, const struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiSelfTest Self test
+ * @brief Perform self test of the sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiSelfTest
+ * \page bmi160_api_bmi160_perform_self_test bmi160_perform_self_test
+ * \code
+ * int8_t bmi160_perform_self_test(uint8_t select_sensor, struct bmi160_dev *dev);
+ * \endcode
+ * @details This is used to perform self test of accel/gyro of the BMI160 sensor
+ *
+ * @param[in] select_sensor  : enum to choose accel or gyro for self test
+ * @param[in] dev            : Structure instance of bmi160_dev
+ *
+ * @note self test can be performed either for accel/gyro at any instant.
+ *
+ *@verbatim
+ *     value of select_sensor       |  Inference
+ *----------------------------------|--------------------------------
+ *   BMI160_ACCEL_ONLY              | Accel self test enabled
+ *   BMI160_GYRO_ONLY               | Gyro self test enabled
+ *   BMI160_BOTH_ACCEL_AND_GYRO     | NOT TO BE USED
+ *@endverbatim
+ *
+ * @note The return value of this API gives us the result of self test.
+ *
+ * @note Performing self test does soft reset of the sensor, User can
+ * set the desired settings after performing the self test.
+ *
+ * @return Result of API execution status
+ * @retval  BMI160_OK                       Self test success
+ * @retval  BMI160_W_GYRO_SELF_TEST_FAIL    Gyro self test fail
+ * @retval  BMI160_W_ACCEl_SELF_TEST_FAIL   Accel self test fail
+ */
+int8_t bmi160_perform_self_test(uint8_t select_sensor, struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiFIFO FIFO
+ * @brief FIFO operations of the sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_get_fifo_data bmi160_get_fifo_data
+ * \code
+ * int8_t bmi160_get_fifo_data(struct bmi160_dev const *dev);
+ * \endcode
+ * @details This API reads data from the fifo buffer.
+ *
+ *  @note User has to allocate the FIFO buffer along with
+ *  corresponding fifo length from his side before calling this API
+ *  as mentioned in the readme.md
+ *
+ *  @note User must specify the number of bytes to read from the FIFO in
+ *  dev->fifo->length , It will be updated by the number of bytes actually
+ *  read from FIFO after calling this API
+ *
+ *  @param[in] dev     : Structure instance of bmi160_dev.
+ *
+ *  @return Result of API execution status
+ *  @retval Zero Success
+ *  @retval Negative Error
+ */
+int8_t bmi160_get_fifo_data(struct bmi160_dev const* dev);
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_set_fifo_flush bmi160_set_fifo_flush
+ * \code
+ * int8_t bmi160_set_fifo_flush(const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API writes fifo_flush command to command register.This
+ *  action clears all data in the Fifo without changing fifo configuration
+ *  settings.
+ *
+ *  @param[in] dev     : Structure instance of bmi160_dev
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ *
+ */
+int8_t bmi160_set_fifo_flush(const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_set_fifo_config bmi160_set_fifo_config
+ * \code
+ * int8_t bmi160_set_fifo_config(uint8_t config, uint8_t enable, struct bmi160_dev const *dev);
+ * \endcode
+ * @details This API sets the FIFO configuration in the sensor.
+ *
+ *  @param[in] config : variable used to specify the FIFO
+ *  configurations which are to be enabled or disabled in the sensor.
+ *
+ *  @note : User can set either set one or more or all FIFO configurations
+ *  by ORing the below mentioned macros.
+ *
+ *@verbatim
+ *      config                  |   Value
+ *      ------------------------|---------------------------
+ *      BMI160_FIFO_TIME        |   0x02
+ *      BMI160_FIFO_TAG_INT2    |   0x04
+ *      BMI160_FIFO_TAG_INT1    |   0x08
+ *      BMI160_FIFO_HEADER      |   0x10
+ *      BMI160_FIFO_AUX         |   0x20
+ *      BMI160_FIFO_ACCEL       |   0x40
+ *      BMI160_FIFO_GYRO        |   0x80
+ *@endverbatim
+ *
+ *  @param[in] enable : Parameter used to enable or disable the above
+ *  FIFO configuration
+ *  @param[in] dev : Structure instance of bmi160_dev.
+ *
+ *  @return status of bus communication result
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ *
+ */
+int8_t bmi160_set_fifo_config(uint8_t config, uint8_t enable, struct bmi160_dev const* dev);
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_set_fifo_down bmi160_set_fifo_down
+ * \code
+ * int8_t bmi160_set_fifo_down(uint8_t fifo_down, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API is used to configure the down sampling ratios of
+ *  the accel and gyro data for FIFO.Also, it configures filtered or
+ *  pre-filtered data for the fifo for accel and gyro.
+ *
+ *  @param[in] fifo_down : variable used to specify the FIFO down
+ *  configurations which are to be enabled or disabled in the sensor.
+ *
+ *  @note The user must select one among the following macros to
+ *  select down-sampling ratio for accel
+ *
+ *@verbatim
+ *      config                               |   Value
+ *      -------------------------------------|---------------------------
+ *      BMI160_ACCEL_FIFO_DOWN_ZERO          |   0x00
+ *      BMI160_ACCEL_FIFO_DOWN_ONE           |   0x10
+ *      BMI160_ACCEL_FIFO_DOWN_TWO           |   0x20
+ *      BMI160_ACCEL_FIFO_DOWN_THREE         |   0x30
+ *      BMI160_ACCEL_FIFO_DOWN_FOUR          |   0x40
+ *      BMI160_ACCEL_FIFO_DOWN_FIVE          |   0x50
+ *      BMI160_ACCEL_FIFO_DOWN_SIX           |   0x60
+ *      BMI160_ACCEL_FIFO_DOWN_SEVEN         |   0x70
+ *@endverbatim
+ *
+ *  @note The user must select one among the following macros to
+ *  select down-sampling ratio for gyro
+ *
+ *@verbatim
+ *      config                               |   Value
+ *      -------------------------------------|---------------------------
+ *      BMI160_GYRO_FIFO_DOWN_ZERO           |   0x00
+ *      BMI160_GYRO_FIFO_DOWN_ONE            |   0x01
+ *      BMI160_GYRO_FIFO_DOWN_TWO            |   0x02
+ *      BMI160_GYRO_FIFO_DOWN_THREE          |   0x03
+ *      BMI160_GYRO_FIFO_DOWN_FOUR           |   0x04
+ *      BMI160_GYRO_FIFO_DOWN_FIVE           |   0x05
+ *      BMI160_GYRO_FIFO_DOWN_SIX            |   0x06
+ *      BMI160_GYRO_FIFO_DOWN_SEVEN          |   0x07
+ *@endverbatim
+ *
+ *  @note The user can enable filtered accel data by the following macro
+ *
+ *@verbatim
+ *      config                               |   Value
+ *      -------------------------------------|---------------------------
+ *      BMI160_ACCEL_FIFO_FILT_EN            |   0x80
+ *@endverbatim
+ *
+ *  @note The user can enable filtered gyro data by the following macro
+ *
+ *@verbatim
+ *      config                               |   Value
+ *      -------------------------------------|---------------------------
+ *      BMI160_GYRO_FIFO_FILT_EN             |   0x08
+ *@endverbatim
+ *
+ *  @note : By ORing the above mentioned macros, the user can select
+ *  the required FIFO down config settings
+ *
+ *  @param[in] dev : Structure instance of bmi160_dev.
+ *
+ *  @return status of bus communication result
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ *
+ */
+int8_t bmi160_set_fifo_down(uint8_t fifo_down, const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_set_fifo_wm bmi160_set_fifo_wm
+ * \code
+ * int8_t bmi160_set_fifo_wm(uint8_t fifo_wm, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API sets the FIFO watermark level in the sensor.
+ *
+ *  @note The FIFO watermark is issued when the FIFO fill level is
+ *  equal or above the watermark level and units of watermark is 4 bytes.
+ *
+ *  @param[in]  fifo_wm        : Variable used to set the FIFO water mark level
+ *  @param[in]  dev            : Structure instance of bmi160_dev
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ *
+ */
+int8_t bmi160_set_fifo_wm(uint8_t fifo_wm, const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_extract_accel bmi160_extract_accel
+ * \code
+ * int8_t bmi160_extract_accel(struct bmi160_sensor_data *accel_data, uint8_t *accel_length, struct bmi160_dev const
+ **dev);
+ * \endcode
+ * @details This API parses and extracts the accelerometer frames from
+ *  FIFO data read by the "bmi160_get_fifo_data" API and stores it in
+ *  the "accel_data" structure instance.
+ *
+ *  @note The bmi160_extract_accel API should be called only after
+ *  reading the FIFO data by calling the bmi160_get_fifo_data() API.
+ *
+ *  @param[out] accel_data    : Structure instance of bmi160_sensor_data
+ *                              where the accelerometer data in FIFO is stored.
+ *  @param[in,out] accel_length  : Number of valid accelerometer frames
+ *                              (x,y,z axes data) read out from fifo.
+ *  @param[in] dev            : Structure instance of bmi160_dev.
+ *
+ *  @note accel_length is updated with the number of valid accelerometer
+ *  frames extracted from fifo (1 accel frame   = 6 bytes) at the end of
+ *  execution of this API.
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ *
+ */
+int8_t bmi160_extract_accel(
+    struct bmi160_sensor_data* accel_data,
+    uint8_t* accel_length,
+    struct bmi160_dev const* dev);
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_extract_gyro bmi160_extract_gyro
+ * \code
+ * int8_t bmi160_extract_gyro(struct bmi160_sensor_data *gyro_data, uint8_t *gyro_length, struct bmi160_dev const *dev);
+ * \endcode
+ * @details This API parses and extracts the gyro frames from
+ *  FIFO data read by the "bmi160_get_fifo_data" API and stores it in
+ *  the "gyro_data" structure instance.
+ *
+ *  @note The bmi160_extract_gyro API should be called only after
+ *  reading the FIFO data by calling the bmi160_get_fifo_data() API.
+ *
+ *  @param[out] gyro_data    : Structure instance of bmi160_sensor_data
+ *                             where the gyro data in FIFO is stored.
+ *  @param[in,out] gyro_length  : Number of valid gyro frames
+ *                             (x,y,z axes data) read out from fifo.
+ *  @param[in] dev           : Structure instance of bmi160_dev.
+ *
+ *  @note gyro_length is updated with the number of valid gyro
+ *  frames extracted from fifo (1 gyro frame   = 6 bytes) at the end of
+ *  execution of this API.
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ *
+ */
+int8_t bmi160_extract_gyro(
+    struct bmi160_sensor_data* gyro_data,
+    uint8_t* gyro_length,
+    struct bmi160_dev const* dev);
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_extract_aux bmi160_extract_aux
+ * \code
+ * int8_t bmi160_extract_aux(struct bmi160_aux_data *aux_data, uint8_t *aux_len, struct bmi160_dev const *dev);
+ * \endcode
+ * @details This API parses and extracts the aux frames from
+ *  FIFO data read by the "bmi160_get_fifo_data" API and stores it in
+ *  the bmi160_aux_data structure instance.
+ *
+ *  @note The bmi160_extract_aux API should be called only after
+ *  reading the FIFO data by calling the bmi160_get_fifo_data() API.
+ *
+ *  @param[out] aux_data    : Structure instance of bmi160_aux_data
+ *                            where the aux data in FIFO is stored.
+ *  @param[in,out] aux_len  : Number of valid aux frames (8bytes)
+ *                            read out from FIFO.
+ *  @param[in] dev          : Structure instance of bmi160_dev.
+ *
+ *  @note aux_len is updated with the number of valid aux
+ *  frames extracted from fifo (1 aux frame = 8 bytes) at the end of
+ *  execution of this API.
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ *
+ */
+int8_t bmi160_extract_aux(
+    struct bmi160_aux_data* aux_data,
+    uint8_t* aux_len,
+    struct bmi160_dev const* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiFOC FOC
+ * @brief Start FOC of accel and gyro sensors
+ */
+
+/*!
+ * \ingroup bmi160ApiFOC
+ * \page bmi160_api_bmi160_start_foc bmi160_start_foc
+ * \code
+ * int8_t bmi160_start_foc(const struct bmi160_foc_conf *foc_conf,
+ * \endcode
+ * @details This API starts the FOC of accel and gyro
+ *
+ *  @note FOC should not be used in low-power mode of sensor
+ *
+ *  @note Accel FOC targets values of +1g , 0g , -1g
+ *  Gyro FOC always targets value of 0 dps
+ *
+ *  @param[in] foc_conf    : Structure instance of bmi160_foc_conf which
+ *                                   has the FOC configuration
+ *  @param[in,out] offset  : Structure instance to store Offset
+ *                                   values read from sensor
+ *  @param[in] dev         : Structure instance of bmi160_dev.
+ *
+ *  @note Pre-requisites for triggering FOC in accel , Set the following,
+ *   Enable the acc_off_en
+ *       Ex :  foc_conf.acc_off_en = BMI160_ENABLE;
+ *
+ *   Set the desired target values of FOC to each axes (x,y,z) by using the
+ *   following macros
+ *       - BMI160_FOC_ACCEL_DISABLED
+ *       - BMI160_FOC_ACCEL_POSITIVE_G
+ *       - BMI160_FOC_ACCEL_NEGATIVE_G
+ *       - BMI160_FOC_ACCEL_0G
+ *
+ *   Ex : foc_conf.foc_acc_x  = BMI160_FOC_ACCEL_0G;
+ *        foc_conf.foc_acc_y  = BMI160_FOC_ACCEL_0G;
+ *        foc_conf.foc_acc_z  = BMI160_FOC_ACCEL_POSITIVE_G;
+ *
+ *  @note Pre-requisites for triggering FOC in gyro ,
+ *  Set the following parameters,
+ *
+ *   Ex : foc_conf.foc_gyr_en = BMI160_ENABLE;
+ *        foc_conf.gyro_off_en = BMI160_ENABLE;
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ */
+int8_t bmi160_start_foc(
+    const struct bmi160_foc_conf* foc_conf,
+    struct bmi160_offsets* offset,
+    struct bmi160_dev const* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiOffsets Offsets
+ * @brief Set / Get offset values of accel and gyro sensors
+ */
+
+/*!
+ * \ingroup bmi160ApiOffsets
+ * \page bmi160_api_bmi160_get_offsets bmi160_get_offsets
+ * \code
+ * int8_t bmi160_get_offsets(struct bmi160_offsets *offset, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API reads and stores the offset values of accel and gyro
+ *
+ *  @param[in,out] offset : Structure instance of bmi160_offsets in which
+ *                          the offset values are read and stored
+ *  @param[in] dev        : Structure instance of bmi160_dev.
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ */
+int8_t bmi160_get_offsets(struct bmi160_offsets* offset, const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiOffsets
+ * \page bmi160_api_bmi160_set_offsets bmi160_set_offsets
+ * \code
+ * int8_t bmi160_set_offsets(const struct bmi160_foc_conf *foc_conf,
+ *                         const struct bmi160_offsets *offset,
+ *                         struct bmi160_dev const *dev);
+ * \endcode
+ * @details This API writes the offset values of accel and gyro to
+ *  the sensor but these values will be reset on POR or soft reset.
+ *
+ *  @param[in] foc_conf    : Structure instance of bmi160_foc_conf which
+ *                                   has the FOC configuration
+ *  @param[in] offset      : Structure instance in which user updates offset
+ *                            values which are to be written in the sensor
+ *  @param[in] dev         : Structure instance of bmi160_dev.
+ *
+ *  @note Offsets can be set by user like offset->off_acc_x = 10;
+ *  where 1LSB = 3.9mg and for gyro 1LSB = 0.061degrees/second
+ *
+ * @note BMI160 offset values for xyz axes of accel should be within range of
+ *  BMI160_ACCEL_MIN_OFFSET (-128) to BMI160_ACCEL_MAX_OFFSET (127)
+ *
+ * @note BMI160 offset values for xyz axes of gyro should be within range of
+ *  BMI160_GYRO_MIN_OFFSET (-512) to BMI160_GYRO_MAX_OFFSET (511)
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ */
+int8_t bmi160_set_offsets(
+    const struct bmi160_foc_conf* foc_conf,
+    const struct bmi160_offsets* offset,
+    struct bmi160_dev const* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiNVM NVM
+ * @brief Write image registers values to NVM
+ */
+
+/*!
+ * \ingroup bmi160ApiNVM
+ * \page bmi160_api_bmi160_update_nvm bmi160_update_nvm
+ * \code
+ * int8_t bmi160_update_nvm(struct bmi160_dev const *dev);
+ * \endcode
+ * @details This API writes the image registers values to NVM which is
+ *  stored even after POR or soft reset
+ *
+ *  @param[in] dev         : Structure instance of bmi160_dev.
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ */
+int8_t bmi160_update_nvm(struct bmi160_dev const* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiInts Interrupt status
+ * @brief Read interrupt status from the sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiInts
+ * \page bmi160_api_bmi160_get_int_status bmi160_get_int_status
+ * \code
+ * int8_t bmi160_get_int_status(enum bmi160_int_status_sel int_status_sel,
+ *                            union bmi160_int_status *int_status,
+ *                            struct bmi160_dev const *dev);
+ * \endcode
+ * @details This API gets the interrupt status from the sensor.
+ *
+ *  @param[in] int_status_sel       : Enum variable to select either individual or all the
+ *  interrupt status bits.
+ *  @param[in] int_status           : pointer variable to get the interrupt status
+ *  from the sensor.
+ *  param[in] dev                   : Structure instance of bmi160_dev.
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ */
+int8_t bmi160_get_int_status(
+    enum bmi160_int_status_sel int_status_sel,
+    union bmi160_int_status* int_status,
+    struct bmi160_dev const* dev);
+
+/*************************** C++ guard macro *****************************/
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* BMI160_H_ */

applications/plugins/airmouse/tracking/imu/imu.c

@@ -0,0 +1,29 @@
+#include "imu.h"
+#include <furi_hal.h>
+
+bool bmi160_begin();
+int bmi160_read(double* vec);
+
+bool lsm6ds3trc_begin();
+void lsm6ds3trc_end();
+int lsm6ds3trc_read(double* vec);
+
+bool imu_begin() {
+    furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
+    bool ret = bmi160_begin(); // lsm6ds3trc_begin();
+    furi_hal_i2c_release(&furi_hal_i2c_handle_external);
+    return ret;
+}
+
+void imu_end() {
+    // furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
+    // lsm6ds3trc_end();
+    // furi_hal_i2c_release(&furi_hal_i2c_handle_external);
+}
+
+int imu_read(double* vec) {
+    furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
+    int ret = bmi160_read(vec); // lsm6ds3trc_read(vec);
+    furi_hal_i2c_release(&furi_hal_i2c_handle_external);
+    return ret;
+}

applications/plugins/airmouse/tracking/imu/imu.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define ACC_DATA_READY (1 << 0)
+#define GYR_DATA_READY (1 << 1)
+
+bool imu_begin();
+void imu_end();
+int imu_read(double* vec);
+
+#ifdef __cplusplus
+}
+#endif

applications/plugins/airmouse/tracking/imu/imu_bmi160.c

@@ -0,0 +1,88 @@
+#include "bmi160.h"
+
+#include <furi_hal.h>
+
+#include "imu.h"
+
+#define TAG "BMI160"
+
+#define BMI160_DEV_ADDR (0x69 << 1)
+
+static const double DEG_TO_RAD = 0.017453292519943295769236907684886;
+static const double G = 9.81;
+
+struct bmi160_dev bmi160dev;
+struct bmi160_sensor_data bmi160_accel;
+struct bmi160_sensor_data bmi160_gyro;
+
+int8_t bmi160_write_i2c(uint8_t dev_addr, uint8_t reg_addr, uint8_t* data, uint16_t len) {
+    if(furi_hal_i2c_write_mem(&furi_hal_i2c_handle_external, dev_addr, reg_addr, data, len, 50))
+        return BMI160_OK;
+    return BMI160_E_COM_FAIL;
+}
+
+int8_t bmi160_read_i2c(uint8_t dev_addr, uint8_t reg_addr, uint8_t* read_data, uint16_t len) {
+    if(furi_hal_i2c_read_mem(&furi_hal_i2c_handle_external, dev_addr, reg_addr, read_data, len, 50))
+        return BMI160_OK;
+    return BMI160_E_COM_FAIL;
+}
+
+bool bmi160_begin() {
+    FURI_LOG_I(TAG, "Init BMI160");
+
+    if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, BMI160_DEV_ADDR, 50)) {
+        FURI_LOG_E(TAG, "Device not ready!");
+        return false;
+    }
+
+    FURI_LOG_I(TAG, "Device ready!");
+
+    bmi160dev.id = BMI160_DEV_ADDR;
+    bmi160dev.intf = BMI160_I2C_INTF;
+    bmi160dev.read = bmi160_read_i2c;
+    bmi160dev.write = bmi160_write_i2c;
+    bmi160dev.delay_ms = furi_delay_ms;
+
+    if(bmi160_init(&bmi160dev) != BMI160_OK) {
+        FURI_LOG_E(TAG, "Initialization failure!");
+        FURI_LOG_E(TAG, "Chip ID 0x%X", bmi160dev.chip_id);
+        return false;
+    }
+
+    bmi160dev.accel_cfg.odr = BMI160_ACCEL_ODR_400HZ;
+    bmi160dev.accel_cfg.range = BMI160_ACCEL_RANGE_4G;
+    bmi160dev.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4;
+    bmi160dev.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;
+    bmi160dev.gyro_cfg.odr = BMI160_GYRO_ODR_400HZ;
+    bmi160dev.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS;
+    bmi160dev.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE;
+    bmi160dev.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;
+
+    if(bmi160_set_sens_conf(&bmi160dev) != BMI160_OK) {
+        FURI_LOG_E(TAG, "Initialization failure!");
+        FURI_LOG_E(TAG, "Chip ID 0x%X", bmi160dev.chip_id);
+        return false;
+    }
+
+    FURI_LOG_I(TAG, "Initialization success!");
+    FURI_LOG_I(TAG, "Chip ID 0x%X", bmi160dev.chip_id);
+
+    return true;
+}
+
+int bmi160_read(double* vec) {
+    if(bmi160_get_sensor_data(
+           (BMI160_ACCEL_SEL | BMI160_GYRO_SEL), &bmi160_accel, &bmi160_gyro, &bmi160dev) !=
+       BMI160_OK) {
+        return 0;
+    }
+
+    vec[0] = ((double)bmi160_accel.x * 4 / 32768) * G;
+    vec[1] = ((double)bmi160_accel.y * 4 / 32768) * G;
+    vec[2] = ((double)bmi160_accel.z * 4 / 32768) * G;
+    vec[3] = ((double)bmi160_gyro.x * 2000 / 32768) * DEG_TO_RAD;
+    vec[4] = ((double)bmi160_gyro.y * 2000 / 32768) * DEG_TO_RAD;
+    vec[5] = ((double)bmi160_gyro.z * 2000 / 32768) * DEG_TO_RAD;
+
+    return ACC_DATA_READY | GYR_DATA_READY;
+}

applications/plugins/airmouse/tracking/imu/imu_lsm6ds3trc.c

@@ -0,0 +1,94 @@
+#include "lsm6ds3tr_c_reg.h"
+
+#include <furi_hal.h>
+
+#include "imu.h"
+
+#define TAG "LSM6DS3TR-C"
+
+#define LSM6DS3_ADDRESS (0x6A << 1)
+
+static const double DEG_TO_RAD = 0.017453292519943295769236907684886;
+
+stmdev_ctx_t lsm6ds3trc_ctx;
+
+int32_t lsm6ds3trc_write_i2c(void* handle, uint8_t reg_addr, const uint8_t* data, uint16_t len) {
+    if(furi_hal_i2c_write_mem(handle, LSM6DS3_ADDRESS, reg_addr, (uint8_t*)data, len, 50))
+        return 0;
+    return -1;
+}
+
+int32_t lsm6ds3trc_read_i2c(void* handle, uint8_t reg_addr, uint8_t* read_data, uint16_t len) {
+    if(furi_hal_i2c_read_mem(handle, LSM6DS3_ADDRESS, reg_addr, read_data, len, 50)) return 0;
+    return -1;
+}
+
+bool lsm6ds3trc_begin() {
+    FURI_LOG_I(TAG, "Init LSM6DS3TR-C");
+
+    if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, LSM6DS3_ADDRESS, 50)) {
+        FURI_LOG_E(TAG, "Not ready");
+        return false;
+    }
+
+    lsm6ds3trc_ctx.write_reg = lsm6ds3trc_write_i2c;
+    lsm6ds3trc_ctx.read_reg = lsm6ds3trc_read_i2c;
+    lsm6ds3trc_ctx.mdelay = furi_delay_ms;
+    lsm6ds3trc_ctx.handle = &furi_hal_i2c_handle_external;
+
+    uint8_t whoami;
+    lsm6ds3tr_c_device_id_get(&lsm6ds3trc_ctx, &whoami);
+    if(whoami != LSM6DS3TR_C_ID) {
+        FURI_LOG_I(TAG, "Unknown model: %x", (int)whoami);
+        return false;
+    }
+
+    lsm6ds3tr_c_reset_set(&lsm6ds3trc_ctx, PROPERTY_ENABLE);
+    uint8_t rst = PROPERTY_ENABLE;
+    while(rst) lsm6ds3tr_c_reset_get(&lsm6ds3trc_ctx, &rst);
+
+    lsm6ds3tr_c_block_data_update_set(&lsm6ds3trc_ctx, PROPERTY_ENABLE);
+    lsm6ds3tr_c_fifo_mode_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_BYPASS_MODE);
+
+    lsm6ds3tr_c_xl_data_rate_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_XL_ODR_104Hz);
+    lsm6ds3tr_c_xl_full_scale_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_4g);
+    lsm6ds3tr_c_xl_lp1_bandwidth_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_XL_LP1_ODR_DIV_4);
+
+    lsm6ds3tr_c_gy_data_rate_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_GY_ODR_104Hz);
+    lsm6ds3tr_c_gy_full_scale_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_2000dps);
+    lsm6ds3tr_c_gy_power_mode_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_GY_HIGH_PERFORMANCE);
+    lsm6ds3tr_c_gy_band_pass_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_LP2_ONLY);
+
+    FURI_LOG_I(TAG, "Init OK");
+    return true;
+}
+
+void lsm6ds3trc_end() {
+    lsm6ds3tr_c_xl_data_rate_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_XL_ODR_OFF);
+    lsm6ds3tr_c_gy_data_rate_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_GY_ODR_OFF);
+}
+
+int lsm6ds3trc_read(double* vec) {
+    int ret = 0;
+    int16_t data[3];
+    lsm6ds3tr_c_reg_t reg;
+    lsm6ds3tr_c_status_reg_get(&lsm6ds3trc_ctx, &reg.status_reg);
+
+    if(reg.status_reg.xlda) {
+        lsm6ds3tr_c_acceleration_raw_get(&lsm6ds3trc_ctx, data);
+        vec[2] = (double)lsm6ds3tr_c_from_fs2g_to_mg(data[0]) / 1000;
+        vec[0] = (double)lsm6ds3tr_c_from_fs2g_to_mg(data[1]) / 1000;
+        vec[1] = (double)lsm6ds3tr_c_from_fs2g_to_mg(data[2]) / 1000;
+        ret |= ACC_DATA_READY;
+    }
+
+    if(reg.status_reg.gda) {
+        lsm6ds3tr_c_angular_rate_raw_get(&lsm6ds3trc_ctx, data);
+        vec[5] = (double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[0]) * DEG_TO_RAD / 1000;
+        vec[3] = (double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[1]) * DEG_TO_RAD / 1000;
+        vec[4] = (double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[2]) * DEG_TO_RAD / 1000;
+        ret |= GYR_DATA_READY;
+    }
+
+    return ret;
+}

applications/plugins/airmouse/tracking/main_loop.cc

@@ -0,0 +1,189 @@
+#include "main_loop.h"
+
+#include <furi.h>
+#include <furi_hal.h>
+
+#include "imu/imu.h"
+#include "orientation_tracker.h"
+#include "calibration_data.h"
+
+#define TAG "tracker"
+
+static const float CURSOR_SPEED = 1024.0 / (M_PI / 4);
+static const float STABILIZE_BIAS = 16.0;
+
+float g_yaw = 0;
+float g_pitch = 0;
+float g_dYaw = 0;
+float g_dPitch = 0;
+bool firstRead = true;
+bool stabilize = true;
+CalibrationData calibration;
+cardboard::OrientationTracker tracker(10000000l); // 10 ms / 100 Hz
+uint64_t ippms, ippms2;
+
+static inline float clamp(float val)
+{
+    while (val <= -M_PI) {
+        val += 2 * M_PI;
+    }
+    while (val >= M_PI) {
+        val -= 2 * M_PI;
+    }
+    return val;
+}
+
+static inline float highpass(float oldVal, float newVal)
+{
+    if (!stabilize) {
+        return newVal;
+    }
+    float delta = clamp(oldVal - newVal);
+    float alpha = (float) std::max(0.0, 1 - std::pow(std::fabs(delta) * CURSOR_SPEED / STABILIZE_BIAS, 3.0));
+    return newVal + alpha * delta;
+}
+
+void sendCurrentState(MouseMoveCallback mouse_move)
+{
+    float dX = g_dYaw * CURSOR_SPEED;
+    float dY = g_dPitch * CURSOR_SPEED;
+
+    // Scale the shift down to fit the protocol.
+    if (dX > 127) {
+        dY *= 127.0 / dX;
+        dX = 127;
+    }
+    if (dX < -127) {
+        dY *= -127.0 / dX;
+        dX = -127;
+    }
+    if (dY > 127) {
+        dX *= 127.0 / dY;
+        dY = 127;
+    }
+    if (dY < -127) {
+        dX *= -127.0 / dY;
+        dY = -127;
+    }
+
+    const int8_t x = (int8_t)std::floor(dX + 0.5);
+    const int8_t y = (int8_t)std::floor(dY + 0.5);
+
+    mouse_move(x, y);
+
+    // Only subtract the part of the error that was already sent.
+    if (x != 0) {
+        g_dYaw -= x / CURSOR_SPEED;
+    }
+    if (y != 0) {
+        g_dPitch -= y / CURSOR_SPEED;
+    }
+}
+
+void onOrientation(cardboard::Vector4& quaternion)
+{
+    float q1 = quaternion[0]; // X * sin(T/2)
+    float q2 = quaternion[1]; // Y * sin(T/2)
+    float q3 = quaternion[2]; // Z * sin(T/2)
+    float q0 = quaternion[3]; // cos(T/2)
+
+    float yaw = std::atan2(2 * (q0 * q3 - q1 * q2), (1 - 2 * (q1 * q1 + q3 * q3)));
+    float pitch = std::asin(2 * (q0 * q1 + q2 * q3));
+    // float roll = std::atan2(2 * (q0 * q2 - q1 * q3), (1 - 2 * (q1 * q1 + q2 * q2)));
+
+    if (yaw == NAN || pitch == NAN) {
+        // NaN case, skip it
+        return;
+    }
+
+    if (firstRead) {
+        g_yaw = yaw;
+        g_pitch = pitch;
+        firstRead = false;
+    } else {
+        const float newYaw = highpass(g_yaw, yaw);
+        const float newPitch = highpass(g_pitch, pitch);
+
+        float dYaw = clamp(g_yaw - newYaw);
+        float dPitch = g_pitch - newPitch;
+        g_yaw = newYaw;
+        g_pitch = newPitch;
+
+        // Accumulate the error locally.
+        g_dYaw += dYaw;
+        g_dPitch += dPitch;
+    }
+}
+
+extern "C" {
+
+void calibration_begin() {
+    calibration.reset();
+    FURI_LOG_I(TAG, "Calibrating");
+}
+
+bool calibration_step() {
+    if (calibration.isComplete())
+        return true;
+
+    double vec[6];
+    if (imu_read(vec) & GYR_DATA_READY) {
+        cardboard::Vector3 data(vec[3], vec[4], vec[5]);
+        furi_delay_ms(9); // Artificially limit to ~100Hz
+        return calibration.add(data);
+    }
+
+    return false;
+}
+
+void calibration_end() {
+    CalibrationMedian store;
+    cardboard::Vector3 median = calibration.getMedian();
+    store.x = median[0];
+    store.y = median[1];
+    store.z = median[2];
+    CALIBRATION_DATA_SAVE(&store);
+}
+
+void tracking_begin() {
+    CalibrationMedian store;
+    cardboard::Vector3 median = calibration.getMedian();
+    if (CALIBRATION_DATA_LOAD(&store)) {
+        median[0] = store.x;
+        median[1] = store.y;
+        median[2] = store.z;
+    }
+
+    ippms = furi_hal_cortex_instructions_per_microsecond();
+    ippms2 = ippms / 2;
+    tracker.SetCalibration(median);
+    tracker.Resume();
+}
+
+void tracking_step(MouseMoveCallback mouse_move) {
+    double vec[6];
+    int ret = imu_read(vec);
+    if (ret != 0) {
+        uint64_t t = (DWT->CYCCNT * 1000llu + ippms2) / ippms;
+        if (ret & ACC_DATA_READY) {
+            cardboard::AccelerometerData adata
+                = { .system_timestamp = t, .sensor_timestamp_ns = t,
+                    .data = cardboard::Vector3(vec[0], vec[1], vec[2]) };
+            tracker.OnAccelerometerData(adata);
+        }
+        if (ret & GYR_DATA_READY) {
+            cardboard::GyroscopeData gdata
+                = { .system_timestamp = t, .sensor_timestamp_ns = t,
+                    .data = cardboard::Vector3(vec[3], vec[4], vec[5]) };
+            cardboard::Vector4 pose = tracker.OnGyroscopeData(gdata);
+            onOrientation(pose);
+            sendCurrentState(mouse_move);
+        }
+    }
+}
+
+void tracking_end() {
+    tracker.Pause();
+}
+
+}

applications/plugins/airmouse/tracking/main_loop.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef bool (*MouseMoveCallback)(int8_t x, int8_t y);
+
+void calibration_begin();
+bool calibration_step();
+void calibration_end();
+
+void tracking_begin();
+void tracking_step(MouseMoveCallback mouse_move);
+void tracking_end();
+
+#ifdef __cplusplus
+}
+#endif

applications/plugins/airmouse/tracking/orientation_tracker.cc

@@ -0,0 +1,95 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "orientation_tracker.h"
+
+#include "sensors/pose_prediction.h"
+#include "util/logging.h"
+#include "util/vector.h"
+#include "util/vectorutils.h"
+
+namespace cardboard {
+
+OrientationTracker::OrientationTracker(const long sampling_period_ns)
+    : sampling_period_ns_(sampling_period_ns)
+    , calibration_(Vector3::Zero())
+    , is_tracking_(false)
+    , sensor_fusion_(new SensorFusionEkf())
+    , latest_gyroscope_data_({ 0, 0, Vector3::Zero() })
+{
+    sensor_fusion_->SetBiasEstimationEnabled(/*kGyroBiasEstimationEnabled*/ true);
+}
+
+void OrientationTracker::SetCalibration(const Vector3& calibration) {
+    calibration_ = calibration;
+}
+
+void OrientationTracker::Pause()
+{
+    if (!is_tracking_) {
+        return;
+    }
+
+    // Create a gyro event with zero velocity. This effectively stops the prediction.
+    GyroscopeData event = latest_gyroscope_data_;
+    event.data = Vector3::Zero();
+
+    OnGyroscopeData(event);
+
+    is_tracking_ = false;
+}
+
+void OrientationTracker::Resume() { is_tracking_ = true; }
+
+Vector4 OrientationTracker::GetPose(int64_t timestamp_ns) const
+{
+    Rotation predicted_rotation;
+    const PoseState pose_state = sensor_fusion_->GetLatestPoseState();
+    if (sensor_fusion_->IsFullyInitialized()) {
+        predicted_rotation = pose_state.sensor_from_start_rotation;
+    } else {
+        CARDBOARD_LOGI("Tracker not fully initialized yet. Using pose prediction only.");
+        predicted_rotation = pose_prediction::PredictPose(timestamp_ns, pose_state);
+    }
+
+    return (-predicted_rotation).GetQuaternion();
+}
+
+void OrientationTracker::OnAccelerometerData(const AccelerometerData& event)
+{
+    if (!is_tracking_) {
+        return;
+    }
+    sensor_fusion_->ProcessAccelerometerSample(event);
+}
+
+Vector4 OrientationTracker::OnGyroscopeData(const GyroscopeData& event)
+{
+    if (!is_tracking_) {
+        return Vector4();
+    }
+
+    const GyroscopeData data = { .system_timestamp = event.system_timestamp,
+        .sensor_timestamp_ns = event.sensor_timestamp_ns,
+        .data = event.data - calibration_ };
+
+    latest_gyroscope_data_ = data;
+
+    sensor_fusion_->ProcessGyroscopeSample(data);
+
+    return OrientationTracker::GetPose(data.sensor_timestamp_ns + sampling_period_ns_);
+}
+
+} // namespace cardboard

applications/plugins/airmouse/tracking/orientation_tracker.h

@@ -0,0 +1,68 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#pragma once
+
+#include <array>
+#include <memory>
+#include <mutex> // NOLINT
+
+#include "sensors/accelerometer_data.h"
+#include "sensors/gyroscope_data.h"
+#include "sensors/sensor_fusion_ekf.h"
+#include "util/rotation.h"
+
+namespace cardboard {
+
+// OrientationTracker encapsulates pose tracking by connecting sensors
+// to SensorFusion.
+// This pose tracker reports poses in display space.
+class OrientationTracker {
+public:
+    OrientationTracker(const long sampling_period_ns);
+
+    void SetCalibration(const Vector3& calibration);
+
+    // Pauses tracking and sensors.
+    void Pause();
+
+    // Resumes tracking ans sensors.
+    void Resume();
+
+    // Gets the predicted pose for a given timestamp.
+    Vector4 GetPose(int64_t timestamp_ns) const;
+
+    // Function called when receiving AccelerometerData.
+    //
+    // @param event sensor event.
+    void OnAccelerometerData(const AccelerometerData& event);
+
+    // Function called when receiving GyroscopeData.
+    //
+    // @param event sensor event.
+    Vector4 OnGyroscopeData(const GyroscopeData& event);
+
+private:
+    long sampling_period_ns_;
+    Vector3 calibration_;
+
+    std::atomic<bool> is_tracking_;
+    // Sensor Fusion object that stores the internal state of the filter.
+    std::unique_ptr<SensorFusionEkf> sensor_fusion_;
+    // Latest gyroscope data.
+    GyroscopeData latest_gyroscope_data_;
+};
+
+} // namespace cardboard

applications/plugins/airmouse/tracking/sensors/accelerometer_data.h

@@ -0,0 +1,38 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_ACCELEROMETER_DATA_H_
+#define CARDBOARD_SDK_SENSORS_ACCELEROMETER_DATA_H_
+
+#include "../util/vector.h"
+
+namespace cardboard {
+
+struct AccelerometerData {
+    // System wall time.
+    uint64_t system_timestamp;
+
+    // Sensor clock time in nanoseconds.
+    uint64_t sensor_timestamp_ns;
+
+    // Acceleration force along the x,y,z axes in m/s^2. This follows android
+    // specification
+    // (https://developer.android.com/guide/topics/sensors/sensors_overview.html#sensors-coords).
+    Vector3 data;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_ACCELEROMETER_DATA_H_

applications/plugins/airmouse/tracking/sensors/gyroscope_bias_estimator.cc

@@ -0,0 +1,313 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "gyroscope_bias_estimator.h"
+
+#include <algorithm>
+#include <chrono> // NOLINT
+
+#include "../util/rotation.h"
+#include "../util/vector.h"
+
+namespace {
+
+// Cutoff frequencies in Hertz applied to our various signals, and their
+// corresponding filters.
+const float kAccelerometerLowPassCutOffFrequencyHz = 1.0f;
+const float kRotationVelocityBasedAccelerometerLowPassCutOffFrequencyHz = 0.15f;
+const float kGyroscopeLowPassCutOffFrequencyHz = 1.0f;
+const float kGyroscopeBiasLowPassCutOffFrequencyHz = 0.15f;
+
+// Note that MEMS IMU are not that precise.
+const double kEpsilon = 1.0e-8;
+
+// Size of the filtering window for the mean and median filter. The larger the
+// windows the larger the filter delay.
+const int kFilterWindowSize = 5;
+
+// Threshold used to compare rotation computed from the accelerometer and the
+// gyroscope bias.
+const double kRatioBetweenGyroBiasAndAccel = 1.5;
+
+// The minimum sum of weights we need to acquire before returning a bias
+// estimation.
+const float kMinSumOfWeightsGyroBiasThreshold = 25.0f;
+
+// Amount of change in m/s^3 we allow on the smoothed accelerometer values to
+// consider the phone static.
+const double kAccelerometerDeltaStaticThreshold = 0.5;
+
+// Amount of change in radians/s^2 we allow on the smoothed gyroscope values to
+// consider the phone static.
+const double kGyroscopeDeltaStaticThreshold = 0.03;
+
+// If the gyroscope value is above this threshold, don't update the gyroscope
+// bias estimation. This threshold is applied to the magnitude of gyroscope
+// vectors in radians/s.
+const float kGyroscopeForBiasThreshold = 0.30f;
+
+// Used to monitor if accelerometer and gyroscope have been static for a few
+// frames.
+const int kStaticFrameDetectionThreshold = 50;
+
+// Minimum time step between sensor updates.
+const double kMinTimestep = 1; // std::chrono::nanoseconds(1);
+} // namespace
+
+namespace cardboard {
+
+// A helper class to keep track of whether some signal can be considered static
+// over specified number of frames.
+class GyroscopeBiasEstimator::IsStaticCounter {
+public:
+    // Initializes a counter with the number of consecutive frames we require
+    // the signal to be static before IsRecentlyStatic returns true.
+    //
+    // @param min_static_frames_threshold number of consecutive frames we
+    //     require the signal to be static before IsRecentlyStatic returns true.
+    explicit IsStaticCounter(int min_static_frames_threshold)
+        : min_static_frames_threshold_(min_static_frames_threshold)
+        , consecutive_static_frames_(0)
+    {
+    }
+
+    // Specifies whether the current frame is considered static.
+    //
+    // @param is_static static flag for current frame.
+    void AppendFrame(bool is_static)
+    {
+        if (is_static) {
+            ++consecutive_static_frames_;
+        } else {
+            consecutive_static_frames_ = 0;
+        }
+    }
+
+    // Returns if static movement is assumed.
+    bool IsRecentlyStatic() const
+    {
+        return consecutive_static_frames_ >= min_static_frames_threshold_;
+    }
+    // Resets counter.
+    void Reset() { consecutive_static_frames_ = 0; }
+
+private:
+    const int min_static_frames_threshold_;
+    int consecutive_static_frames_;
+};
+
+GyroscopeBiasEstimator::GyroscopeBiasEstimator()
+    : accelerometer_lowpass_filter_(kAccelerometerLowPassCutOffFrequencyHz)
+    , simulated_gyroscope_from_accelerometer_lowpass_filter_(
+          kRotationVelocityBasedAccelerometerLowPassCutOffFrequencyHz)
+    , gyroscope_lowpass_filter_(kGyroscopeLowPassCutOffFrequencyHz)
+    , gyroscope_bias_lowpass_filter_(kGyroscopeBiasLowPassCutOffFrequencyHz)
+    , accelerometer_static_counter_(new IsStaticCounter(kStaticFrameDetectionThreshold))
+    , gyroscope_static_counter_(new IsStaticCounter(kStaticFrameDetectionThreshold))
+    , current_accumulated_weights_gyroscope_bias_(0.f)
+    , mean_filter_(kFilterWindowSize)
+    , median_filter_(kFilterWindowSize)
+    , last_mean_filtered_accelerometer_value_({ 0, 0, 0 })
+{
+    Reset();
+}
+
+GyroscopeBiasEstimator::~GyroscopeBiasEstimator() { }
+
+void GyroscopeBiasEstimator::Reset()
+{
+    accelerometer_lowpass_filter_.Reset();
+    gyroscope_lowpass_filter_.Reset();
+    gyroscope_bias_lowpass_filter_.Reset();
+    accelerometer_static_counter_->Reset();
+    gyroscope_static_counter_->Reset();
+}
+
+void GyroscopeBiasEstimator::ProcessGyroscope(
+    const Vector3& gyroscope_sample, uint64_t timestamp_ns)
+{
+    // Update gyroscope and gyroscope delta low-pass filters.
+    gyroscope_lowpass_filter_.AddSample(gyroscope_sample, timestamp_ns);
+
+    const auto smoothed_gyroscope_delta
+        = gyroscope_sample - gyroscope_lowpass_filter_.GetFilteredData();
+
+    gyroscope_static_counter_->AppendFrame(
+        Length(smoothed_gyroscope_delta) < kGyroscopeDeltaStaticThreshold);
+
+    // Only update the bias if the gyroscope and accelerometer signals have been
+    // relatively static recently.
+    if (gyroscope_static_counter_->IsRecentlyStatic()
+        && accelerometer_static_counter_->IsRecentlyStatic()) {
+        // Reset static counter when updating the bias fails.
+        if (!UpdateGyroscopeBias(gyroscope_sample, timestamp_ns)) {
+            // Bias update fails because of large motion, thus reset the static
+            // counter.
+            gyroscope_static_counter_->AppendFrame(false);
+        }
+    } else {
+        // Reset weights, if not static.
+        current_accumulated_weights_gyroscope_bias_ = 0;
+    }
+}
+
+void GyroscopeBiasEstimator::ProcessAccelerometer(
+    const Vector3& accelerometer_sample, uint64_t timestamp_ns)
+{
+    // Get current state of the filter.
+    const uint64_t previous_accel_timestamp_ns
+        = accelerometer_lowpass_filter_.GetMostRecentTimestampNs();
+    const bool is_low_pass_filter_init = accelerometer_lowpass_filter_.IsInitialized();
+
+    // Update accel and accel delta low-pass filters.
+    accelerometer_lowpass_filter_.AddSample(accelerometer_sample, timestamp_ns);
+
+    const auto smoothed_accelerometer_delta
+        = accelerometer_sample - accelerometer_lowpass_filter_.GetFilteredData();
+
+    accelerometer_static_counter_->AppendFrame(
+        Length(smoothed_accelerometer_delta) < kAccelerometerDeltaStaticThreshold);
+
+    // Rotation from accel cannot be differentiated with only one sample.
+    if (!is_low_pass_filter_init) {
+        simulated_gyroscope_from_accelerometer_lowpass_filter_.AddSample({ 0, 0, 0 }, timestamp_ns);
+        return;
+    }
+
+    // No need to update the simulated gyroscope at this point because the motion
+    // is too large.
+    if (!accelerometer_static_counter_->IsRecentlyStatic()) {
+        return;
+    }
+
+    median_filter_.AddSample(accelerometer_lowpass_filter_.GetFilteredData());
+
+    // This processing can only be started if the buffer is fully initialized.
+    if (!median_filter_.IsValid()) {
+        mean_filter_.AddSample(accelerometer_lowpass_filter_.GetFilteredData());
+
+        // Update the last filtered accelerometer value.
+        last_mean_filtered_accelerometer_value_ = accelerometer_lowpass_filter_.GetFilteredData();
+        return;
+    }
+
+    mean_filter_.AddSample(median_filter_.GetFilteredData());
+
+    // Compute a mock gyroscope value from accelerometer.
+    const int64_t diff = timestamp_ns - previous_accel_timestamp_ns;
+    const double timestep = static_cast<double>(diff);
+
+    simulated_gyroscope_from_accelerometer_lowpass_filter_.AddSample(
+        ComputeAngularVelocityFromLatestAccelerometer(timestep), timestamp_ns);
+    last_mean_filtered_accelerometer_value_ = mean_filter_.GetFilteredData();
+}
+
+Vector3 GyroscopeBiasEstimator::ComputeAngularVelocityFromLatestAccelerometer(double timestep) const
+{
+    if (timestep < kMinTimestep) {
+        return { 0, 0, 0 };
+    }
+
+    const auto mean_of_median = mean_filter_.GetFilteredData();
+
+    // Compute an incremental rotation between the last state and the current
+    // state.
+    //
+    // Note that we switch to double precision here because of precision problem
+    // with small rotation.
+    const auto incremental_rotation = Rotation::RotateInto(
+        Vector3(last_mean_filtered_accelerometer_value_[0],
+            last_mean_filtered_accelerometer_value_[1], last_mean_filtered_accelerometer_value_[2]),
+        Vector3(mean_of_median[0], mean_of_median[1], mean_of_median[2]));
+
+    // We use axis angle here because this is how gyroscope values are stored.
+    Vector3 incremental_rotation_axis;
+    double incremental_rotation_angle;
+    incremental_rotation.GetAxisAndAngle(&incremental_rotation_axis, &incremental_rotation_angle);
+
+    incremental_rotation_axis *= incremental_rotation_angle / timestep;
+
+    return { static_cast<float>(incremental_rotation_axis[0]),
+        static_cast<float>(incremental_rotation_axis[1]),
+        static_cast<float>(incremental_rotation_axis[2]) };
+}
+
+bool GyroscopeBiasEstimator::UpdateGyroscopeBias(
+    const Vector3& gyroscope_sample, uint64_t timestamp_ns)
+{
+    // Gyroscope values that are too big are potentially dangerous as they could
+    // originate from slow and steady head rotations.
+    //
+    // Therefore we compute an update weight which:
+    // * favors gyroscope values that are closer to 0
+    // * is set to zero if gyroscope values are greater than a threshold.
+    //
+    // This way, the gyroscope bias estimation converges faster if the phone is
+    // flat on a table, as opposed to held up somewhat stationary in the user's
+    // hands.
+
+    // If magnitude is too big, don't update the filter at all so that we don't
+    // artificially increase the number of samples accumulated by the filter.
+    const float gyroscope_sample_norm2 = Length(gyroscope_sample);
+    if (gyroscope_sample_norm2 >= kGyroscopeForBiasThreshold) {
+        return false;
+    }
+
+    float update_weight
+        = std::max(0.0f, 1 - gyroscope_sample_norm2 / kGyroscopeForBiasThreshold);
+    update_weight *= update_weight;
+    gyroscope_bias_lowpass_filter_.AddWeightedSample(
+        gyroscope_lowpass_filter_.GetFilteredData(), timestamp_ns, update_weight);
+
+    // This counter is only partially valid as the low pass filter drops large
+    // samples.
+    current_accumulated_weights_gyroscope_bias_ += update_weight;
+
+    return true;
+}
+
+Vector3 GyroscopeBiasEstimator::GetGyroscopeBias() const
+{
+    return gyroscope_bias_lowpass_filter_.GetFilteredData();
+}
+
+bool GyroscopeBiasEstimator::IsCurrentEstimateValid() const
+{
+    // Remove any bias component along the gravity because they cannot be
+    // evaluated from accelerometer.
+    const auto current_gravity_dir = Normalized(last_mean_filtered_accelerometer_value_);
+    const auto gyro_bias_lowpass = gyroscope_bias_lowpass_filter_.GetFilteredData();
+
+    const auto off_gravity_gyro_bias
+        = gyro_bias_lowpass - current_gravity_dir * Dot(gyro_bias_lowpass, current_gravity_dir);
+
+    // Checks that the current bias estimate is not correlated with the
+    // rotation computed from accelerometer.
+    const auto gyro_from_accel
+        = simulated_gyroscope_from_accelerometer_lowpass_filter_.GetFilteredData();
+    const bool isGyroscopeBiasCorrelatedWithSimulatedGyro
+        = (Length(gyro_from_accel) * kRatioBetweenGyroBiasAndAccel
+            > (Length(off_gravity_gyro_bias) + kEpsilon));
+    const bool hasEnoughSamples
+        = current_accumulated_weights_gyroscope_bias_ > kMinSumOfWeightsGyroBiasThreshold;
+    const bool areCountersStatic = gyroscope_static_counter_->IsRecentlyStatic()
+        && accelerometer_static_counter_->IsRecentlyStatic();
+
+    const bool isStatic
+        = hasEnoughSamples && areCountersStatic && !isGyroscopeBiasCorrelatedWithSimulatedGyro;
+    return isStatic;
+}
+
+} // namespace cardboard

applications/plugins/airmouse/tracking/sensors/gyroscope_bias_estimator.h

@@ -0,0 +1,134 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_GYROSCOPE_BIAS_ESTIMATOR_H_
+#define CARDBOARD_SDK_SENSORS_GYROSCOPE_BIAS_ESTIMATOR_H_
+
+#include <chrono> // NOLINT
+#include <cstdint>
+#include <list>
+#include <memory>
+#include <vector>
+
+#include "lowpass_filter.h"
+#include "mean_filter.h"
+#include "median_filter.h"
+#include "../util/vector.h"
+
+namespace cardboard {
+
+// Class that attempts to estimate the gyroscope's bias.
+// Its main idea is that it averages the gyroscope values when the phone is
+// considered stationary.
+// Usage: A client should call the ProcessGyroscope and ProcessAccelerometer
+// methods for every accelerometer and gyroscope sensor sample. This class
+// expects these calls to be frequent, i.e., at least at 10 Hz. The client can
+// then call GetGyroBias to retrieve the current estimate of the gyroscope bias.
+// For best results, the fastest available delay option should be used when
+// registering to sensors. Note that this class is not thread-safe.
+//
+// The filtering applied to the accelerometer to estimate a rotation
+// from it follows :
+// Baptiste Delporte, Laurent Perroton, Thierry Grandpierre, Jacques Trichet.
+// Accelerometer and Magnetometer Based Gyroscope Emulation on Smart Sensor
+// for a Virtual Reality Application. Sensor and Transducers Journal, 2012.
+//
+// which is a combination of a IIR filter, a median and a mean filter.
+class GyroscopeBiasEstimator {
+public:
+    GyroscopeBiasEstimator();
+    virtual ~GyroscopeBiasEstimator();
+
+    // Updates the estimator with a gyroscope event.
+    //
+    // @param gyroscope_sample the angular speed around the x, y, z axis in
+    //     radians/sec.
+    // @param timestamp_ns the nanosecond at which the event occurred. Only
+    //     guaranteed to be comparable with timestamps from other PocessGyroscope
+    //     invocations.
+    virtual void ProcessGyroscope(const Vector3& gyroscope_sample, uint64_t timestamp_ns);
+
+    // Processes accelerometer samples to estimate if device is
+    // stable or not.
+    //
+    // First we filter the accelerometer. This is done with 3 filters.
+    //  - A IIR low-pass filter
+    //  - A median filter
+    //  - A mean filter.
+    // Then a rotation is computed between consecutive filtered accelerometer
+    // samples.
+    // Finally this is converted to a velocity to emulate a gyroscope.
+    //
+    // @param accelerometer_sample the acceleration (including gravity) on the x,
+    //     y, z axis in meters/s^2.
+    // @param timestamp_ns the nanosecond at which the event occurred. Only
+    //     guaranteed to be comparable with timestamps from other
+    //     ProcessAccelerometer invocations.
+    virtual void ProcessAccelerometer(const Vector3& accelerometer_sample, uint64_t timestamp_ns);
+
+    // Returns the estimated gyroscope bias.
+    //
+    // @return Estimated gyroscope bias. A vector with zeros is returned if no
+    //     estimate has been computed.
+    virtual Vector3 GetGyroscopeBias() const;
+
+    // Resets the estimator state.
+    void Reset();
+
+    // Returns true if the current estimate returned by GetGyroscopeBias is
+    // correct. The device (measured using the sensors) has to be static for this
+    // function to return true.
+    virtual bool IsCurrentEstimateValid() const;
+
+private:
+    // A helper class to keep track of whether some signal can be considered
+    // static over specified number of frames.
+    class IsStaticCounter;
+
+    // Updates gyroscope bias estimation.
+    //
+    // @return false if the current sample is too large.
+    bool UpdateGyroscopeBias(const Vector3& gyroscope_sample, uint64_t timestamp_ns);
+
+    // Returns device angular velocity (rad/s) from the latest accelerometer data.
+    //
+    // @param timestep in seconds between the last two samples.
+    // @return rotation velocity from latest accelerometer. This can be
+    // interpreted as an gyroscope.
+    Vector3 ComputeAngularVelocityFromLatestAccelerometer(double timestep) const;
+
+    LowpassFilter accelerometer_lowpass_filter_;
+    LowpassFilter simulated_gyroscope_from_accelerometer_lowpass_filter_;
+    LowpassFilter gyroscope_lowpass_filter_;
+    LowpassFilter gyroscope_bias_lowpass_filter_;
+
+    std::unique_ptr<IsStaticCounter> accelerometer_static_counter_;
+    std::unique_ptr<IsStaticCounter> gyroscope_static_counter_;
+
+    // Sum of the weight of sample used for gyroscope filtering.
+    float current_accumulated_weights_gyroscope_bias_;
+
+    // Set of filters for accelerometer data to estimate a rotation
+    // based only on accelerometer.
+    MeanFilter mean_filter_;
+    MedianFilter median_filter_;
+
+    // Last computed filter accelerometer value used for finite differences.
+    Vector3 last_mean_filtered_accelerometer_value_;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_GYROSCOPE_BIAS_ESTIMATOR_H_

applications/plugins/airmouse/tracking/sensors/gyroscope_data.h

@@ -0,0 +1,38 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_GYROSCOPE_DATA_H_
+#define CARDBOARD_SDK_SENSORS_GYROSCOPE_DATA_H_
+
+#include "../util/vector.h"
+
+namespace cardboard {
+
+struct GyroscopeData {
+    // System wall time.
+    uint64_t system_timestamp;
+
+    // Sensor clock time in nanoseconds.
+    uint64_t sensor_timestamp_ns;
+
+    // Rate of rotation around the x,y,z axes in rad/s. This follows android
+    // specification
+    // (https://developer.android.com/guide/topics/sensors/sensors_overview.html#sensors-coords).
+    Vector3 data;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_GYROSCOPE_DATA_H_

applications/plugins/airmouse/tracking/sensors/lowpass_filter.cc

@@ -0,0 +1,84 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "lowpass_filter.h"
+
+#include <cmath>
+
+namespace {
+
+const double kSecondsFromNanoseconds = 1.0e-9;
+
+// Minimum time step between sensor updates. This corresponds to 1000 Hz.
+const double kMinTimestepS = 0.001f;
+
+// Maximum time step between sensor updates. This corresponds to 1 Hz.
+const double kMaxTimestepS = 1.00f;
+
+} // namespace
+
+namespace cardboard {
+
+LowpassFilter::LowpassFilter(double cutoff_freq_hz)
+    : cutoff_time_constant_(1 / (2 * (double)M_PI * cutoff_freq_hz))
+    , initialized_(false)
+{
+    Reset();
+}
+
+void LowpassFilter::AddSample(const Vector3& sample, uint64_t timestamp_ns)
+{
+    AddWeightedSample(sample, timestamp_ns, 1.0);
+}
+
+void LowpassFilter::AddWeightedSample(const Vector3& sample, uint64_t timestamp_ns, double weight)
+{
+    if (!initialized_) {
+        // Initialize filter state
+        filtered_data_ = { sample[0], sample[1], sample[2] };
+        timestamp_most_recent_update_ns_ = timestamp_ns;
+        initialized_ = true;
+        return;
+    }
+
+    if (timestamp_ns < timestamp_most_recent_update_ns_) {
+        timestamp_most_recent_update_ns_ = timestamp_ns;
+        return;
+    }
+
+    const double delta_s = static_cast<double>(timestamp_ns - timestamp_most_recent_update_ns_)
+        * kSecondsFromNanoseconds;
+    if (delta_s <= kMinTimestepS || delta_s > kMaxTimestepS) {
+        timestamp_most_recent_update_ns_ = timestamp_ns;
+        return;
+    }
+
+    const double weighted_delta_secs = weight * delta_s;
+
+    const double alpha = weighted_delta_secs / (cutoff_time_constant_ + weighted_delta_secs);
+
+    for (int i = 0; i < 3; ++i) {
+        filtered_data_[i] = (1 - alpha) * filtered_data_[i] + alpha * sample[i];
+    }
+    timestamp_most_recent_update_ns_ = timestamp_ns;
+}
+
+void LowpassFilter::Reset()
+{
+    initialized_ = false;
+    filtered_data_ = { 0, 0, 0 };
+}
+
+} // namespace cardboard

applications/plugins/airmouse/tracking/sensors/lowpass_filter.h

@@ -0,0 +1,81 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_LOWPASS_FILTER_H_
+#define CARDBOARD_SDK_SENSORS_LOWPASS_FILTER_H_
+
+#include <array>
+#include <memory>
+
+#include "../util/vector.h"
+
+namespace cardboard {
+
+// Implements an IIR, first order, low pass filter over vectors of the given
+// dimension = 3.
+// See http://en.wikipedia.org/wiki/Low-pass_filter
+class LowpassFilter {
+public:
+    // Initializes a filter with the given cutoff frequency in Hz.
+    explicit LowpassFilter(double cutoff_freq_hz);
+
+    // Updates the filter with the given sample. Note that samples with
+    // non-monotonic timestamps and successive samples with a time steps below 1
+    // ms or above 1 s are ignored.
+    //
+    // @param sample current sample data.
+    // @param timestamp_ns timestamp associated to this sample in nanoseconds.
+    void AddSample(const Vector3& sample, uint64_t timestamp_ns);
+
+    // Updates the filter with the given weighted sample.
+    //
+    // @param sample current sample data.
+    // @param timestamp_ns timestamp associated to this sample in nanoseconds.
+    // @param weight typically a [0, 1] weight factor used when applying a new
+    //     sample. A weight of 1 corresponds to calling AddSample. A weight of 0
+    //     makes the update no-op. The first initial sample is not affected by
+    //     this.
+    void AddWeightedSample(const Vector3& sample, uint64_t timestamp_ns, double weight);
+
+    // Returns the filtered value. A vector with zeros is returned if no samples
+    // have been added.
+    Vector3 GetFilteredData() const {
+        return filtered_data_;
+    }
+
+    // Returns the most recent update timestamp in ns.
+    uint64_t GetMostRecentTimestampNs() const {
+        return timestamp_most_recent_update_ns_;
+    }
+
+    // Returns true when the filter is initialized.
+    bool IsInitialized() const {
+        return initialized_;
+    }
+
+    // Resets filter state.
+    void Reset();
+
+private:
+    const double cutoff_time_constant_;
+    uint64_t timestamp_most_recent_update_ns_;
+    bool initialized_;
+
+    Vector3 filtered_data_;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_LOWPASS_FILTER_H_

applications/plugins/airmouse/tracking/sensors/mean_filter.cc

@@ -0,0 +1,46 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "mean_filter.h"
+
+namespace cardboard {
+
+MeanFilter::MeanFilter(size_t filter_size)
+    : filter_size_(filter_size)
+{
+}
+
+void MeanFilter::AddSample(const Vector3& sample)
+{
+    buffer_.push_back(sample);
+    if (buffer_.size() > filter_size_) {
+        buffer_.pop_front();
+    }
+}
+
+bool MeanFilter::IsValid() const { return buffer_.size() == filter_size_; }
+
+Vector3 MeanFilter::GetFilteredData() const
+{
+    // Compute mean of the samples stored in buffer_.
+    Vector3 mean = Vector3::Zero();
+    for (auto sample : buffer_) {
+        mean += sample;
+    }
+
+    return mean / static_cast<double>(filter_size_);
+}
+
+} // namespace cardboard

applications/plugins/airmouse/tracking/sensors/mean_filter.h

@@ -0,0 +1,48 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_MEAN_FILTER_H_
+#define CARDBOARD_SDK_SENSORS_MEAN_FILTER_H_
+
+#include <deque>
+
+#include "../util/vector.h"
+
+namespace cardboard {
+
+// Fixed window FIFO mean filter for vectors of the given dimension.
+class MeanFilter {
+public:
+    // Create a mean filter of size filter_size.
+    // @param filter_size size of the internal filter.
+    explicit MeanFilter(size_t filter_size);
+
+    // Add sample to buffer_ if buffer_ is full it drop the oldest sample.
+    void AddSample(const Vector3& sample);
+
+    // Returns true if buffer has filter_size_ sample, false otherwise.
+    bool IsValid() const;
+
+    // Returns the mean of values stored in the internal buffer.
+    Vector3 GetFilteredData() const;
+
+private:
+    const size_t filter_size_;
+    std::deque<Vector3> buffer_;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_MEAN_FILTER_H_

applications/plugins/airmouse/tracking/sensors/median_filter.cc

@@ -0,0 +1,69 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "median_filter.h"
+
+#include <algorithm>
+#include <vector>
+
+#include "../util/vector.h"
+#include "../util/vectorutils.h"
+
+namespace cardboard {
+
+MedianFilter::MedianFilter(size_t filter_size)
+    : filter_size_(filter_size)
+{
+}
+
+void MedianFilter::AddSample(const Vector3& sample)
+{
+    buffer_.push_back(sample);
+    norms_.push_back(Length(sample));
+    if (buffer_.size() > filter_size_) {
+        buffer_.pop_front();
+        norms_.pop_front();
+    }
+}
+
+bool MedianFilter::IsValid() const { return buffer_.size() == filter_size_; }
+
+Vector3 MedianFilter::GetFilteredData() const
+{
+    std::vector<float> norms(norms_.begin(), norms_.end());
+
+    // Get median of value of the norms.
+    std::nth_element(norms.begin(), norms.begin() + filter_size_ / 2, norms.end());
+    const float median_norm = norms[filter_size_ / 2];
+
+    // Get median value based on their norm.
+    auto median_it = buffer_.begin();
+    for (const auto norm : norms_) {
+        if (norm == median_norm) {
+            break;
+        }
+        ++median_it;
+    }
+
+    return *median_it;
+}
+
+void MedianFilter::Reset()
+{
+    buffer_.clear();
+    norms_.clear();
+}
+
+} // namespace cardboard

applications/plugins/airmouse/tracking/sensors/median_filter.h

@@ -0,0 +1,53 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_MEDIAN_FILTER_H_
+#define CARDBOARD_SDK_SENSORS_MEDIAN_FILTER_H_
+
+#include <deque>
+
+#include "../util/vector.h"
+
+namespace cardboard {
+
+// Fixed window FIFO median filter for vectors of the given dimension = 3.
+class MedianFilter {
+public:
+    // Creates a median filter of size filter_size.
+    // @param filter_size size of the internal filter.
+    explicit MedianFilter(size_t filter_size);
+
+    // Adds sample to buffer_ if buffer_ is full it drops the oldest sample.
+    void AddSample(const Vector3& sample);
+
+    // Returns true if buffer has filter_size_ sample, false otherwise.
+    bool IsValid() const;
+
+    // Returns the median of values store in the internal buffer.
+    Vector3 GetFilteredData() const;
+
+    // Resets the filter, removing all samples that have been added.
+    void Reset();
+
+private:
+    const size_t filter_size_;
+    std::deque<Vector3> buffer_;
+    // Contains norms of the elements stored in buffer_.
+    std::deque<float> norms_;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_MEDIAN_FILTER_H_

applications/plugins/airmouse/tracking/sensors/pose_prediction.cc

@@ -0,0 +1,71 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "pose_prediction.h"
+
+#include <chrono> // NOLINT
+
+#include "../util/logging.h"
+#include "../util/vectorutils.h"
+
+namespace cardboard {
+
+namespace {
+    const double kEpsilon = 1.0e-15;
+} // namespace
+
+namespace pose_prediction {
+
+    Rotation GetRotationFromGyroscope(const Vector3& gyroscope_value, double timestep_s)
+    {
+        const double velocity = Length(gyroscope_value);
+
+        // When there is no rotation data return an identity rotation.
+        if (velocity < kEpsilon) {
+            CARDBOARD_LOGI("PosePrediction::GetRotationFromGyroscope: Velocity really small, "
+                           "returning identity rotation.");
+            return Rotation::Identity();
+        }
+        // Since the gyroscope_value is a start from sensor transformation we need to
+        // invert it to have a sensor from start transformation, hence the minus sign.
+        // For more info:
+        // http://developer.android.com/guide/topics/sensors/sensors_motion.html#sensors-motion-gyro
+        return Rotation::FromAxisAndAngle(gyroscope_value / velocity, -timestep_s * velocity);
+    }
+
+    Rotation PredictPose(int64_t requested_pose_timestamp, const PoseState& current_state)
+    {
+        // Subtracting unsigned numbers is bad when the result is negative.
+        const int64_t diff = requested_pose_timestamp - current_state.timestamp;
+        const double timestep_s = diff * 1.0e-9;
+
+        const Rotation update = GetRotationFromGyroscope(
+            current_state.sensor_from_start_rotation_velocity, timestep_s);
+        return update * current_state.sensor_from_start_rotation;
+    }
+
+    Rotation PredictPoseInv(int64_t requested_pose_timestamp, const PoseState& current_state)
+    {
+        // Subtracting unsigned numbers is bad when the result is negative.
+        const int64_t diff = requested_pose_timestamp - current_state.timestamp;
+        const double timestep_s = diff * 1.0e-9;
+
+        const Rotation update = GetRotationFromGyroscope(
+            current_state.sensor_from_start_rotation_velocity, timestep_s);
+        return current_state.sensor_from_start_rotation * (-update);
+    }
+
+} // namespace pose_prediction
+} // namespace cardboard

applications/plugins/airmouse/tracking/sensors/pose_prediction.h

@@ -0,0 +1,55 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_POSE_PREDICTION_H_
+#define CARDBOARD_SDK_SENSORS_POSE_PREDICTION_H_
+
+#include <cstdint>
+
+#include "pose_state.h"
+#include "../util/rotation.h"
+
+namespace cardboard {
+namespace pose_prediction {
+
+// Returns a rotation matrix based on the integration of the gyroscope_value
+// over the timestep_s in seconds.
+// TODO(pfg): Document the space better here.
+//
+// @param gyroscope_value gyroscope sensor values.
+// @param timestep_s integration period in seconds.
+// @return Integration of the gyroscope value the rotation is from Start to
+//         Sensor Space.
+Rotation GetRotationFromGyroscope(const Vector3& gyroscope_value, double timestep_s);
+
+// Gets a predicted pose for a given time in the future (e.g. rendering time)
+// based on a linear prediction model. This uses the system current state
+// (position, velocity, etc) from the past to extrapolate a position in the
+// future.
+//
+// @param requested_pose_timestamp time at which you want the pose.
+// @param current_state current state that stores the pose and linear model at a
+//        given time prior to requested_pose_timestamp_ns.
+// @return pose from Start to Sensor Space.
+Rotation PredictPose(int64_t requested_pose_timestamp, const PoseState& current_state);
+
+// Equivalent to PredictPose, but for use with poses relative to Start Space
+// rather than sensor space.
+Rotation PredictPoseInv(int64_t requested_pose_timestamp, const PoseState& current_state);
+
+} // namespace pose_prediction
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_POSE_PREDICTION_H_

applications/plugins/airmouse/tracking/sensors/pose_state.h

@@ -0,0 +1,56 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_POSE_STATE_H_
+#define CARDBOARD_SDK_SENSORS_POSE_STATE_H_
+
+#include "../util/rotation.h"
+#include "../util/vector.h"
+
+namespace cardboard {
+
+enum {
+    kPoseStateFlagInvalid = 1U << 0,
+    kPoseStateFlagInitializing = 1U << 1,
+    kPoseStateFlagHas6DoF = 1U << 2,
+};
+
+// Stores a head pose pose plus derivatives. This can be used for prediction.
+struct PoseState {
+    // System wall time.
+    int64_t timestamp;
+
+    // Rotation from Sensor Space to Start Space.
+    Rotation sensor_from_start_rotation;
+
+    // First derivative of the rotation.
+    Vector3 sensor_from_start_rotation_velocity;
+
+    // Current gyroscope bias in rad/s.
+    Vector3 bias;
+
+    // The position of the headset.
+    Vector3 position = Vector3(0, 0, 0);
+
+    // In the same coordinate frame as the position.
+    Vector3 velocity = Vector3(0, 0, 0);
+
+    // Flags indicating the status of the pose.
+    uint64_t flags = 0U;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_POSE_STATE_H_

applications/plugins/airmouse/tracking/sensors/sensor_fusion_ekf.cc

@@ -0,0 +1,333 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "sensor_fusion_ekf.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include "accelerometer_data.h"
+#include "gyroscope_data.h"
+#include "pose_prediction.h"
+#include "../util/matrixutils.h"
+
+namespace cardboard {
+
+namespace {
+
+    const double kFiniteDifferencingEpsilon = 1.0e-7;
+    const double kEpsilon = 1.0e-15;
+    // Default gyroscope frequency. This corresponds to 100 Hz.
+    const double kDefaultGyroscopeTimestep_s = 0.01f;
+    // Maximum time between gyroscope before we start limiting the integration.
+    const double kMaximumGyroscopeSampleDelay_s = 0.04f;
+    // Compute a first-order exponential moving average of changes in accel norm per
+    // frame.
+    const double kSmoothingFactor = 0.5;
+    // Minimum and maximum values used for accelerometer noise covariance matrix.
+    // The smaller the sigma value, the more weight is given to the accelerometer
+    // signal.
+    const double kMinAccelNoiseSigma = 0.75;
+    const double kMaxAccelNoiseSigma = 7.0;
+    // Initial value for the diagonal elements of the different covariance matrices.
+    const double kInitialStateCovarianceValue = 25.0;
+    const double kInitialProcessCovarianceValue = 1.0;
+    // Maximum accelerometer norm change allowed before capping it covariance to a
+    // large value.
+    const double kMaxAccelNormChange = 0.15;
+    // Timestep IIR filtering coefficient.
+    const double kTimestepFilterCoeff = 0.95;
+    // Minimum number of sample for timestep filtering.
+    const int kTimestepFilterMinSamples = 10;
+
+    // Z direction in start space.
+    const Vector3 kCanonicalZDirection(0.0, 0.0, 1.0);
+
+    // Computes an axis-angle rotation from the input vector.
+    // angle = norm(a)
+    // axis = a.normalized()
+    // If norm(a) == 0, it returns an identity rotation.
+    static inline Rotation RotationFromVector(const Vector3& a)
+    {
+        const double norm_a = Length(a);
+        if (norm_a < kEpsilon) {
+            return Rotation::Identity();
+        }
+        return Rotation::FromAxisAndAngle(a / norm_a, norm_a);
+    }
+
+} // namespace
+
+SensorFusionEkf::SensorFusionEkf()
+    : execute_reset_with_next_accelerometer_sample_(false)
+    , bias_estimation_enabled_(true)
+    , gyroscope_bias_estimate_({ 0, 0, 0 })
+{
+    ResetState();
+}
+
+void SensorFusionEkf::Reset() { execute_reset_with_next_accelerometer_sample_ = true; }
+
+void SensorFusionEkf::ResetState()
+{
+    current_state_.sensor_from_start_rotation = Rotation::Identity();
+    current_state_.sensor_from_start_rotation_velocity = Vector3::Zero();
+
+    current_gyroscope_sensor_timestamp_ns_ = 0;
+    current_accelerometer_sensor_timestamp_ns_ = 0;
+
+    state_covariance_ = Matrix3x3::Identity() * kInitialStateCovarianceValue;
+    process_covariance_ = Matrix3x3::Identity() * kInitialProcessCovarianceValue;
+    accelerometer_measurement_covariance_
+        = Matrix3x3::Identity() * kMinAccelNoiseSigma * kMinAccelNoiseSigma;
+    innovation_covariance_ = Matrix3x3::Identity();
+
+    accelerometer_measurement_jacobian_ = Matrix3x3::Zero();
+    kalman_gain_ = Matrix3x3::Zero();
+    innovation_ = Vector3::Zero();
+    accelerometer_measurement_ = Vector3::Zero();
+    prediction_ = Vector3::Zero();
+    control_input_ = Vector3::Zero();
+    state_update_ = Vector3::Zero();
+
+    moving_average_accelerometer_norm_change_ = 0.0;
+
+    is_timestep_filter_initialized_ = false;
+    is_gyroscope_filter_valid_ = false;
+    is_aligned_with_gravity_ = false;
+
+    // Reset biases.
+    gyroscope_bias_estimator_.Reset();
+    gyroscope_bias_estimate_ = { 0, 0, 0 };
+}
+
+// Here I am doing something wrong relative to time stamps. The state timestamps
+// always correspond to the gyrostamps because it would require additional
+// extrapolation if I wanted to do otherwise.
+PoseState SensorFusionEkf::GetLatestPoseState() const { return current_state_; }
+
+void SensorFusionEkf::ProcessGyroscopeSample(const GyroscopeData& sample)
+{
+    // Don't accept gyroscope sample when waiting for a reset.
+    if (execute_reset_with_next_accelerometer_sample_) {
+        return;
+    }
+
+    // Discard outdated samples.
+    if (current_gyroscope_sensor_timestamp_ns_ >= sample.sensor_timestamp_ns) {
+        current_gyroscope_sensor_timestamp_ns_ = sample.sensor_timestamp_ns;
+        return;
+    }
+
+    // Checks that we received at least one gyroscope sample in the past.
+    if (current_gyroscope_sensor_timestamp_ns_ != 0) {
+        double current_timestep_s = std::chrono::duration_cast<std::chrono::duration<double>>(
+            std::chrono::nanoseconds(
+                sample.sensor_timestamp_ns - current_gyroscope_sensor_timestamp_ns_))
+                                        .count();
+        if (current_timestep_s > kMaximumGyroscopeSampleDelay_s) {
+            if (is_gyroscope_filter_valid_) {
+                // Replaces the delta timestamp by the filtered estimates of the delta time.
+                current_timestep_s = filtered_gyroscope_timestep_s_;
+            } else {
+                current_timestep_s = kDefaultGyroscopeTimestep_s;
+            }
+        } else {
+            FilterGyroscopeTimestep(current_timestep_s);
+        }
+
+        if (bias_estimation_enabled_) {
+            gyroscope_bias_estimator_.ProcessGyroscope(sample.data, sample.sensor_timestamp_ns);
+
+            if (gyroscope_bias_estimator_.IsCurrentEstimateValid()) {
+                // As soon as the device is considered to be static, the bias estimator
+                // should have a precise estimate of the gyroscope bias.
+                gyroscope_bias_estimate_ = gyroscope_bias_estimator_.GetGyroscopeBias();
+            }
+        }
+
+        // Only integrate after receiving an accelerometer sample.
+        if (is_aligned_with_gravity_) {
+            const Rotation rotation_from_gyroscope = pose_prediction::GetRotationFromGyroscope(
+                { sample.data[0] - gyroscope_bias_estimate_[0],
+                    sample.data[1] - gyroscope_bias_estimate_[1],
+                    sample.data[2] - gyroscope_bias_estimate_[2] },
+                current_timestep_s);
+            current_state_.sensor_from_start_rotation
+                = rotation_from_gyroscope * current_state_.sensor_from_start_rotation;
+            UpdateStateCovariance(RotationMatrixNH(rotation_from_gyroscope));
+            state_covariance_ = state_covariance_
+                + ((current_timestep_s * current_timestep_s) * process_covariance_);
+        }
+    }
+
+    // Saves gyroscope event for future prediction.
+    current_state_.timestamp = sample.system_timestamp;
+    current_gyroscope_sensor_timestamp_ns_ = sample.sensor_timestamp_ns;
+    current_state_.sensor_from_start_rotation_velocity.Set(
+        sample.data[0] - gyroscope_bias_estimate_[0], sample.data[1] - gyroscope_bias_estimate_[1],
+        sample.data[2] - gyroscope_bias_estimate_[2]);
+}
+
+Vector3 SensorFusionEkf::ComputeInnovation(const Rotation& pose)
+{
+    const Vector3 predicted_down_direction = pose * kCanonicalZDirection;
+
+    const Rotation rotation
+        = Rotation::RotateInto(predicted_down_direction, accelerometer_measurement_);
+    Vector3 axis;
+    double angle;
+    rotation.GetAxisAndAngle(&axis, &angle);
+    return axis * angle;
+}
+
+void SensorFusionEkf::ComputeMeasurementJacobian()
+{
+    for (int dof = 0; dof < 3; dof++) {
+        Vector3 delta = Vector3::Zero();
+        delta[dof] = kFiniteDifferencingEpsilon;
+
+        const Rotation epsilon_rotation = RotationFromVector(delta);
+        const Vector3 delta_rotation
+            = ComputeInnovation(epsilon_rotation * current_state_.sensor_from_start_rotation);
+
+        const Vector3 col = (innovation_ - delta_rotation) / kFiniteDifferencingEpsilon;
+        accelerometer_measurement_jacobian_(0, dof) = col[0];
+        accelerometer_measurement_jacobian_(1, dof) = col[1];
+        accelerometer_measurement_jacobian_(2, dof) = col[2];
+    }
+}
+
+void SensorFusionEkf::ProcessAccelerometerSample(const AccelerometerData& sample)
+{
+    // Discard outdated samples.
+    if (current_accelerometer_sensor_timestamp_ns_ >= sample.sensor_timestamp_ns) {
+        current_accelerometer_sensor_timestamp_ns_ = sample.sensor_timestamp_ns;
+        return;
+    }
+
+    // Call reset state if required.
+    if (execute_reset_with_next_accelerometer_sample_.exchange(false)) {
+        ResetState();
+    }
+
+    accelerometer_measurement_.Set(sample.data[0], sample.data[1], sample.data[2]);
+    current_accelerometer_sensor_timestamp_ns_ = sample.sensor_timestamp_ns;
+
+    if (bias_estimation_enabled_) {
+        gyroscope_bias_estimator_.ProcessAccelerometer(sample.data, sample.sensor_timestamp_ns);
+    }
+
+    if (!is_aligned_with_gravity_) {
+        // This is the first accelerometer measurement so it initializes the
+        // orientation estimate.
+        current_state_.sensor_from_start_rotation
+            = Rotation::RotateInto(kCanonicalZDirection, accelerometer_measurement_);
+        is_aligned_with_gravity_ = true;
+
+        previous_accelerometer_norm_ = Length(accelerometer_measurement_);
+        return;
+    }
+
+    UpdateMeasurementCovariance();
+
+    innovation_ = ComputeInnovation(current_state_.sensor_from_start_rotation);
+    ComputeMeasurementJacobian();
+
+    // S = H * P * H' + R
+    innovation_covariance_ = accelerometer_measurement_jacobian_ * state_covariance_
+            * Transpose(accelerometer_measurement_jacobian_)
+        + accelerometer_measurement_covariance_;
+
+    // K = P * H' * S^-1
+    kalman_gain_ = state_covariance_ * Transpose(accelerometer_measurement_jacobian_)
+        * Inverse(innovation_covariance_);
+
+    // x_update = K*nu
+    state_update_ = kalman_gain_ * innovation_;
+
+    // P = (I - K * H) * P;
+    state_covariance_ = (Matrix3x3::Identity() - kalman_gain_ * accelerometer_measurement_jacobian_)
+        * state_covariance_;
+
+    // Updates pose and associate covariance matrix.
+    const Rotation rotation_from_state_update = RotationFromVector(state_update_);
+
+    current_state_.sensor_from_start_rotation
+        = rotation_from_state_update * current_state_.sensor_from_start_rotation;
+    UpdateStateCovariance(RotationMatrixNH(rotation_from_state_update));
+}
+
+void SensorFusionEkf::UpdateStateCovariance(const Matrix3x3& motion_update)
+{
+    state_covariance_ = motion_update * state_covariance_ * Transpose(motion_update);
+}
+
+void SensorFusionEkf::FilterGyroscopeTimestep(double gyroscope_timestep_s)
+{
+    if (!is_timestep_filter_initialized_) {
+        // Initializes the filter.
+        filtered_gyroscope_timestep_s_ = gyroscope_timestep_s;
+        num_gyroscope_timestep_samples_ = 1;
+        is_timestep_filter_initialized_ = true;
+        return;
+    }
+
+    // Computes the IIR filter response.
+    filtered_gyroscope_timestep_s_ = kTimestepFilterCoeff * filtered_gyroscope_timestep_s_
+        + (1 - kTimestepFilterCoeff) * gyroscope_timestep_s;
+    ++num_gyroscope_timestep_samples_;
+
+    if (num_gyroscope_timestep_samples_ > kTimestepFilterMinSamples) {
+        is_gyroscope_filter_valid_ = true;
+    }
+}
+
+void SensorFusionEkf::UpdateMeasurementCovariance()
+{
+    const double current_accelerometer_norm = Length(accelerometer_measurement_);
+    // Norm change between current and previous accel readings.
+    const double current_accelerometer_norm_change
+        = std::abs(current_accelerometer_norm - previous_accelerometer_norm_);
+    previous_accelerometer_norm_ = current_accelerometer_norm;
+
+    moving_average_accelerometer_norm_change_ = kSmoothingFactor * current_accelerometer_norm_change
+        + (1 - kSmoothingFactor) * moving_average_accelerometer_norm_change_;
+
+    // If we hit the accel norm change threshold, we use the maximum noise sigma
+    // for the accel covariance. For anything below that, we use a linear
+    // combination between min and max sigma values.
+    const double norm_change_ratio
+        = moving_average_accelerometer_norm_change_ / kMaxAccelNormChange;
+    const double accelerometer_noise_sigma = std::min(kMaxAccelNoiseSigma,
+        kMinAccelNoiseSigma + norm_change_ratio * (kMaxAccelNoiseSigma - kMinAccelNoiseSigma));
+
+    // Updates the accel covariance matrix with the new sigma value.
+    accelerometer_measurement_covariance_
+        = Matrix3x3::Identity() * accelerometer_noise_sigma * accelerometer_noise_sigma;
+}
+
+bool SensorFusionEkf::IsBiasEstimationEnabled() const { return bias_estimation_enabled_; }
+
+void SensorFusionEkf::SetBiasEstimationEnabled(bool enable)
+{
+    if (bias_estimation_enabled_ != enable) {
+        bias_estimation_enabled_ = enable;
+        gyroscope_bias_estimate_ = { 0, 0, 0 };
+        gyroscope_bias_estimator_.Reset();
+    }
+}
+
+} // namespace cardboard

applications/plugins/airmouse/tracking/sensors/sensor_fusion_ekf.h

@@ -0,0 +1,188 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_SENSOR_FUSION_EKF_H_
+#define CARDBOARD_SDK_SENSORS_SENSOR_FUSION_EKF_H_
+
+#include <array>
+#include <atomic>
+#include <cstdint>
+
+#include "accelerometer_data.h"
+#include "gyroscope_bias_estimator.h"
+#include "gyroscope_data.h"
+#include "pose_state.h"
+#include "../util/matrix_3x3.h"
+#include "../util/rotation.h"
+#include "../util/vector.h"
+
+namespace cardboard {
+
+// Sensor fusion class that implements an Extended Kalman Filter (EKF) to
+// estimate a 3D rotation from a gyroscope and an accelerometer.
+// This system only has one state, the pose. It does not estimate any velocity
+// or acceleration.
+//
+// To learn more about Kalman filtering one can read this article which is a
+// good introduction: https://en.wikipedia.org/wiki/Kalman_filter
+class SensorFusionEkf {
+public:
+    SensorFusionEkf();
+
+    // Resets the state of the sensor fusion. It sets the velocity for
+    // prediction to zero. The reset will happen with the next
+    // accelerometer sample. Gyroscope sample will be discarded until a new
+    // accelerometer sample arrives.
+    void Reset();
+
+    // Gets the PoseState representing the latest pose and  derivatives at a
+    // particular timestamp as estimated by SensorFusion.
+    PoseState GetLatestPoseState() const;
+
+    // Processes one gyroscope sample event. This updates the pose of the system
+    // and the prediction model. The gyroscope data is assumed to be in axis angle
+    // form. Angle = ||v|| and Axis = v / ||v||, with v = [v_x, v_y, v_z]^T.
+    //
+    // @param sample gyroscope sample data.
+    void ProcessGyroscopeSample(const GyroscopeData& sample);
+
+    // Processes one accelerometer sample event. This updates the pose of the
+    // system. If the Accelerometer norm changes too much between sample it is not
+    // trusted as much.
+    //
+    // @param sample accelerometer sample data.
+    void ProcessAccelerometerSample(const AccelerometerData& sample);
+
+    // Enables or disables the drift correction by estimating the gyroscope bias.
+    //
+    // @param enable Enable drift correction.
+    void SetBiasEstimationEnabled(bool enable);
+
+    // Returns a boolean that indicates if bias estimation is enabled or disabled.
+    //
+    // @return true if bias estimation is enabled, false otherwise.
+    bool IsBiasEstimationEnabled() const;
+
+    // Returns the current gyroscope bias estimate from GyroscopeBiasEstimator.
+    Vector3 GetGyroscopeBias() const {
+        return {
+            gyroscope_bias_estimate_[0], gyroscope_bias_estimate_[1], gyroscope_bias_estimate_[2]};
+    }
+
+    // Returns true after receiving the first accelerometer measurement.
+    bool IsFullyInitialized() const {
+        return is_aligned_with_gravity_;
+    }
+
+private:
+    // Estimates the average timestep between gyroscope event.
+    void FilterGyroscopeTimestep(double gyroscope_timestep);
+
+    // Updates the state covariance with an incremental motion. It changes the
+    // space of the quadric.
+    void UpdateStateCovariance(const Matrix3x3& motion_update);
+
+    // Computes the innovation vector of the Kalman based on the input pose.
+    // It uses the latest measurement vector (i.e. accelerometer data), which must
+    // be set prior to calling this function.
+    Vector3 ComputeInnovation(const Rotation& pose);
+
+    // This computes the measurement_jacobian_ via numerical differentiation based
+    // on the current value of sensor_from_start_rotation_.
+    void ComputeMeasurementJacobian();
+
+    // Updates the accelerometer covariance matrix.
+    //
+    // This looks at the norm of recent accelerometer readings. If it has changed
+    // significantly, it means the phone receives additional acceleration than
+    // just gravity, and so the down vector information gravity signal is noisier.
+    void UpdateMeasurementCovariance();
+
+    // Reset all internal states. This is not thread safe. Lock should be acquired
+    // outside of it. This function is called in ProcessAccelerometerSample.
+    void ResetState();
+
+    // Current transformation from Sensor Space to Start Space.
+    // x_sensor = sensor_from_start_rotation_ * x_start;
+    PoseState current_state_;
+
+    // Filtering of the gyroscope timestep started?
+    bool is_timestep_filter_initialized_;
+    // Filtered gyroscope timestep valid?
+    bool is_gyroscope_filter_valid_;
+    // Sensor fusion currently aligned with gravity? After initialization
+    // it will requires a couple of accelerometer data for the system to get
+    // aligned.
+    std::atomic<bool> is_aligned_with_gravity_;
+
+    // Covariance of Kalman filter state (P in common formulation).
+    Matrix3x3 state_covariance_;
+    // Covariance of the process noise (Q in common formulation).
+    Matrix3x3 process_covariance_;
+    // Covariance of the accelerometer measurement (R in common formulation).
+    Matrix3x3 accelerometer_measurement_covariance_;
+    // Covariance of innovation (S in common formulation).
+    Matrix3x3 innovation_covariance_;
+    // Jacobian of the measurements (H in common formulation).
+    Matrix3x3 accelerometer_measurement_jacobian_;
+    // Gain of the Kalman filter (K in common formulation).
+    Matrix3x3 kalman_gain_;
+    // Parameter update a.k.a. innovation vector. (\nu in common formulation).
+    Vector3 innovation_;
+    // Measurement vector (z in common formulation).
+    Vector3 accelerometer_measurement_;
+    // Current prediction vector (g in common formulation).
+    Vector3 prediction_;
+    // Control input, currently this is only the gyroscope data (\mu in common
+    // formulation).
+    Vector3 control_input_;
+    // Update of the state vector. (x in common formulation).
+    Vector3 state_update_;
+
+    // Sensor time of the last gyroscope processed event.
+    uint64_t current_gyroscope_sensor_timestamp_ns_;
+    // Sensor time of the last accelerometer processed event.
+    uint64_t current_accelerometer_sensor_timestamp_ns_;
+
+    // Estimates of the timestep between gyroscope event in seconds.
+    double filtered_gyroscope_timestep_s_;
+    // Number of timestep samples processed so far by the filter.
+    uint32_t num_gyroscope_timestep_samples_;
+    // Norm of the accelerometer for the previous measurement.
+    double previous_accelerometer_norm_;
+    // Moving average of the accelerometer norm changes. It is computed for every
+    // sensor datum.
+    double moving_average_accelerometer_norm_change_;
+
+    // Flag indicating if a state reset should be executed with the next
+    // accelerometer sample.
+    std::atomic<bool> execute_reset_with_next_accelerometer_sample_;
+
+    // Flag indicating if bias estimation is enabled (enabled by default).
+    std::atomic<bool> bias_estimation_enabled_;
+
+    // Bias estimator and static device detector.
+    GyroscopeBiasEstimator gyroscope_bias_estimator_;
+
+    // Current bias estimate_;
+    Vector3 gyroscope_bias_estimate_;
+
+    SensorFusionEkf(const SensorFusionEkf&) = delete;
+    SensorFusionEkf& operator=(const SensorFusionEkf&) = delete;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_SENSOR_FUSION_EKF_H_

applications/plugins/airmouse/tracking/util/logging.h

@@ -0,0 +1,38 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_UTIL_LOGGING_H_
+#define CARDBOARD_SDK_UTIL_LOGGING_H_
+
+#include <furi.h>
+#include <furi_hal.h>
+
+#if defined(__ANDROID__)
+
+#include <android/log.h>
+
+// Uncomment these to enable debug logging from native code
+
+#define CARDBOARD_LOGI(...) // __android_log_print(ANDROID_LOG_INFO, "CardboardSDK", __VA_ARGS__)
+#define CARDBOARD_LOGE(...) // __android_log_print(ANDROID_LOG_ERROR, "CardboardSDK", __VA_ARGS__)
+
+#else
+
+#define CARDBOARD_LOGI(...) // FURI_LOG_I("CardboardSDK", __VA_ARGS__)
+#define CARDBOARD_LOGE(...) // FURI_LOG_E("CardboardSDK", __VA_ARGS__)
+
+#endif
+
+#endif // CARDBOARD_SDK_UTIL_LOGGING_H_

applications/plugins/airmouse/tracking/util/matrix_3x3.cc

@@ -0,0 +1,121 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "matrix_3x3.h"
+
+namespace cardboard {
+
+Matrix3x3::Matrix3x3(double m00, double m01, double m02, double m10, double m11, double m12,
+    double m20, double m21, double m22)
+    : elem_ { { { m00, m01, m02 }, { m10, m11, m12 }, { m20, m21, m22 } } }
+{
+}
+
+Matrix3x3::Matrix3x3()
+{
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            elem_[row][col] = 0;
+    }
+}
+
+Matrix3x3 Matrix3x3::Zero()
+{
+    Matrix3x3 result;
+    return result;
+}
+
+Matrix3x3 Matrix3x3::Identity()
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        result.elem_[row][row] = 1;
+    }
+    return result;
+}
+
+void Matrix3x3::MultiplyScalar(double s)
+{
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            elem_[row][col] *= s;
+    }
+}
+
+Matrix3x3 Matrix3x3::Negation() const
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            result.elem_[row][col] = -elem_[row][col];
+    }
+    return result;
+}
+
+Matrix3x3 Matrix3x3::Scale(const Matrix3x3& m, double s)
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            result.elem_[row][col] = m.elem_[row][col] * s;
+    }
+    return result;
+}
+
+Matrix3x3 Matrix3x3::Addition(const Matrix3x3& lhs, const Matrix3x3& rhs)
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            result.elem_[row][col] = lhs.elem_[row][col] + rhs.elem_[row][col];
+    }
+    return result;
+}
+
+Matrix3x3 Matrix3x3::Subtraction(const Matrix3x3& lhs, const Matrix3x3& rhs)
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            result.elem_[row][col] = lhs.elem_[row][col] - rhs.elem_[row][col];
+    }
+    return result;
+}
+
+Matrix3x3 Matrix3x3::Product(const Matrix3x3& m0, const Matrix3x3& m1)
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col) {
+            result.elem_[row][col] = 0;
+            for (int i = 0; i < 3; ++i)
+                result.elem_[row][col] += m0.elem_[row][i] * m1.elem_[i][col];
+        }
+    }
+    return result;
+}
+
+bool Matrix3x3::AreEqual(const Matrix3x3& m0, const Matrix3x3& m1)
+{
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col) {
+            if (m0.elem_[row][col] != m1.elem_[row][col])
+                return false;
+        }
+    }
+    return true;
+}
+
+} // namespace cardboard

applications/plugins/airmouse/tracking/util/matrix_3x3.h

@@ -0,0 +1,138 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_UTIL_MATRIX_3X3_H_
+#define CARDBOARD_SDK_UTIL_MATRIX_3X3_H_
+
+#include <array>
+#include <cstring> // For memcpy().
+#include <istream> // NOLINT
+#include <ostream> // NOLINT
+
+namespace cardboard {
+
+// The Matrix3x3 class defines a square 3-dimensional matrix. Elements are
+// stored in row-major order.
+// TODO(b/135461889): Make this class consistent with Matrix4x4.
+class Matrix3x3 {
+public:
+    // The default constructor zero-initializes all elements.
+    Matrix3x3();
+
+    // Dimension-specific constructors that are passed individual element values.
+    Matrix3x3(
+        double m00,
+        double m01,
+        double m02,
+        double m10,
+        double m11,
+        double m12,
+        double m20,
+        double m21,
+        double m22);
+
+    // Constructor that reads elements from a linear array of the correct size.
+    explicit Matrix3x3(const double array[3 * 3]);
+
+    // Returns a Matrix3x3 containing all zeroes.
+    static Matrix3x3 Zero();
+
+    // Returns an identity Matrix3x3.
+    static Matrix3x3 Identity();
+
+    // Mutable element accessors.
+    double& operator()(int row, int col) {
+        return elem_[row][col];
+    }
+    std::array<double, 3>& operator[](int row) {
+        return elem_[row];
+    }
+
+    // Read-only element accessors.
+    const double& operator()(int row, int col) const {
+        return elem_[row][col];
+    }
+    const std::array<double, 3>& operator[](int row) const {
+        return elem_[row];
+    }
+
+    // Return a pointer to the data for interfacing with libraries.
+    double* Data() {
+        return &elem_[0][0];
+    }
+    const double* Data() const {
+        return &elem_[0][0];
+    }
+
+    // Self-modifying multiplication operators.
+    void operator*=(double s) {
+        MultiplyScalar(s);
+    }
+    void operator*=(const Matrix3x3& m) {
+        *this = Product(*this, m);
+    }
+
+    // Unary operators.
+    Matrix3x3 operator-() const {
+        return Negation();
+    }
+
+    // Binary scale operators.
+    friend Matrix3x3 operator*(const Matrix3x3& m, double s) {
+        return Scale(m, s);
+    }
+    friend Matrix3x3 operator*(double s, const Matrix3x3& m) {
+        return Scale(m, s);
+    }
+
+    // Binary matrix addition.
+    friend Matrix3x3 operator+(const Matrix3x3& lhs, const Matrix3x3& rhs) {
+        return Addition(lhs, rhs);
+    }
+
+    // Binary matrix subtraction.
+    friend Matrix3x3 operator-(const Matrix3x3& lhs, const Matrix3x3& rhs) {
+        return Subtraction(lhs, rhs);
+    }
+
+    // Binary multiplication operator.
+    friend Matrix3x3 operator*(const Matrix3x3& m0, const Matrix3x3& m1) {
+        return Product(m0, m1);
+    }
+
+    // Exact equality and inequality comparisons.
+    friend bool operator==(const Matrix3x3& m0, const Matrix3x3& m1) {
+        return AreEqual(m0, m1);
+    }
+    friend bool operator!=(const Matrix3x3& m0, const Matrix3x3& m1) {
+        return !AreEqual(m0, m1);
+    }
+
+private:
+    // These private functions implement most of the operators.
+    void MultiplyScalar(double s);
+    Matrix3x3 Negation() const;
+    static Matrix3x3 Addition(const Matrix3x3& lhs, const Matrix3x3& rhs);
+    static Matrix3x3 Subtraction(const Matrix3x3& lhs, const Matrix3x3& rhs);
+    static Matrix3x3 Scale(const Matrix3x3& m, double s);
+    static Matrix3x3 Product(const Matrix3x3& m0, const Matrix3x3& m1);
+    static bool AreEqual(const Matrix3x3& m0, const Matrix3x3& m1);
+
+    std::array<std::array<double, 3>, 3> elem_;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_UTIL_MATRIX_3X3_H_

applications/plugins/airmouse/tracking/util/matrix_4x4.cc

@@ -0,0 +1,87 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "matrix_4x4.h"
+
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+
+namespace cardboard {
+
+Matrix4x4 Matrix4x4::Identity()
+{
+    Matrix4x4 ret;
+    for (int j = 0; j < 4; ++j) {
+        for (int i = 0; i < 4; ++i) {
+            ret.m[j][i] = (i == j) ? 1 : 0;
+        }
+    }
+
+    return ret;
+}
+
+Matrix4x4 Matrix4x4::Zeros()
+{
+    Matrix4x4 ret;
+    for (int j = 0; j < 4; ++j) {
+        for (int i = 0; i < 4; ++i) {
+            ret.m[j][i] = 0;
+        }
+    }
+
+    return ret;
+}
+
+Matrix4x4 Matrix4x4::Translation(float x, float y, float z)
+{
+    Matrix4x4 ret = Matrix4x4::Identity();
+    ret.m[3][0] = x;
+    ret.m[3][1] = y;
+    ret.m[3][2] = z;
+
+    return ret;
+}
+
+Matrix4x4 Matrix4x4::Perspective(const std::array<float, 4>& fov, float zNear, float zFar)
+{
+    Matrix4x4 ret = Matrix4x4::Zeros();
+
+    const float xLeft = -std::tan(fov[0] * M_PI / 180.0f) * zNear;
+    const float xRight = std::tan(fov[1] * M_PI / 180.0f) * zNear;
+    const float yBottom = -std::tan(fov[2] * M_PI / 180.0f) * zNear;
+    const float yTop = std::tan(fov[3] * M_PI / 180.0f) * zNear;
+
+    const float X = (2 * zNear) / (xRight - xLeft);
+    const float Y = (2 * zNear) / (yTop - yBottom);
+    const float A = (xRight + xLeft) / (xRight - xLeft);
+    const float B = (yTop + yBottom) / (yTop - yBottom);
+    const float C = (zNear + zFar) / (zNear - zFar);
+    const float D = (2 * zNear * zFar) / (zNear - zFar);
+
+    ret.m[0][0] = X;
+    ret.m[2][0] = A;
+    ret.m[1][1] = Y;
+    ret.m[2][1] = B;
+    ret.m[2][2] = C;
+    ret.m[3][2] = D;
+    ret.m[2][3] = -1;
+
+    return ret;
+}
+
+void Matrix4x4::ToArray(float* array) const { std::memcpy(array, &m[0][0], 16 * sizeof(float)); }
+
+} // namespace cardboard

applications/plugins/airmouse/tracking/util/matrix_4x4.h

@@ -0,0 +1,37 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_UTIL_MATRIX_4X4_H_
+#define CARDBOARD_SDK_UTIL_MATRIX_4X4_H_
+
+#include <array>
+
+namespace cardboard {
+
+class Matrix4x4 {
+public:
+    static Matrix4x4 Identity();
+    static Matrix4x4 Zeros();
+    static Matrix4x4 Translation(float x, float y, float z);
+    static Matrix4x4 Perspective(const std::array<float, 4>& fov, float zNear, float zFar);
+    void ToArray(float* array) const;
+
+private:
+    std::array<std::array<float, 4>, 4> m;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_UTIL_MATRIX4X4_H_

applications/plugins/airmouse/tracking/util/matrixutils.cc

@@ -0,0 +1,148 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "matrixutils.h"
+
+#include "vectorutils.h"
+
+namespace cardboard {
+
+namespace {
+
+    // Returns true if the cofactor for a given row and column should be negated.
+    static bool IsCofactorNegated(int row, int col)
+    {
+        // Negated iff (row + col) is odd.
+        return ((row + col) & 1) != 0;
+    }
+
+    static double CofactorElement3(const Matrix3x3& m, int row, int col)
+    {
+        static const int index[3][2] = { { 1, 2 }, { 0, 2 }, { 0, 1 } };
+        const int i0 = index[row][0];
+        const int i1 = index[row][1];
+        const int j0 = index[col][0];
+        const int j1 = index[col][1];
+        const double cofactor = m(i0, j0) * m(i1, j1) - m(i0, j1) * m(i1, j0);
+        return IsCofactorNegated(row, col) ? -cofactor : cofactor;
+    }
+
+    // Multiplies a matrix and some type of column vector to
+    // produce another column vector of the same type.
+    Vector3 MultiplyMatrixAndVector(const Matrix3x3& m, const Vector3& v)
+    {
+        Vector3 result = Vector3::Zero();
+        for (int row = 0; row < 3; ++row) {
+            for (int col = 0; col < 3; ++col)
+                result[row] += m(row, col) * v[col];
+        }
+        return result;
+    }
+
+    // Sets the upper 3x3 of a Matrix to represent a 3D rotation.
+    void RotationMatrix3x3(const Rotation& r, Matrix3x3* matrix)
+    {
+        //
+        // Given a quaternion (a,b,c,d) where d is the scalar part, the 3x3 rotation
+        // matrix is:
+        //
+        //   a^2 - b^2 - c^2 + d^2         2ab - 2cd               2ac + 2bd
+        //         2ab + 2cd        -a^2 + b^2 - c^2 + d^2         2bc - 2ad
+        //         2ac - 2bd               2bc + 2ad        -a^2 - b^2 + c^2 + d^2
+        //
+        const Vector<4>& quat = r.GetQuaternion();
+        const double aa = quat[0] * quat[0];
+        const double bb = quat[1] * quat[1];
+        const double cc = quat[2] * quat[2];
+        const double dd = quat[3] * quat[3];
+
+        const double ab = quat[0] * quat[1];
+        const double ac = quat[0] * quat[2];
+        const double bc = quat[1] * quat[2];
+
+        const double ad = quat[0] * quat[3];
+        const double bd = quat[1] * quat[3];
+        const double cd = quat[2] * quat[3];
+
+        Matrix3x3& m = *matrix;
+        m[0][0] = aa - bb - cc + dd;
+        m[0][1] = 2 * ab - 2 * cd;
+        m[0][2] = 2 * ac + 2 * bd;
+        m[1][0] = 2 * ab + 2 * cd;
+        m[1][1] = -aa + bb - cc + dd;
+        m[1][2] = 2 * bc - 2 * ad;
+        m[2][0] = 2 * ac - 2 * bd;
+        m[2][1] = 2 * bc + 2 * ad;
+        m[2][2] = -aa - bb + cc + dd;
+    }
+
+} // anonymous namespace
+
+Vector3 operator*(const Matrix3x3& m, const Vector3& v) { return MultiplyMatrixAndVector(m, v); }
+
+Matrix3x3 CofactorMatrix(const Matrix3x3& m)
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            result(row, col) = CofactorElement3(m, row, col);
+    }
+    return result;
+}
+
+Matrix3x3 AdjugateWithDeterminant(const Matrix3x3& m, double* determinant)
+{
+    const Matrix3x3 cofactor_matrix = CofactorMatrix(m);
+    if (determinant) {
+        *determinant = m(0, 0) * cofactor_matrix(0, 0) + m(0, 1) * cofactor_matrix(0, 1)
+            + m(0, 2) * cofactor_matrix(0, 2);
+    }
+    return Transpose(cofactor_matrix);
+}
+
+// Returns the transpose of a matrix.
+Matrix3x3 Transpose(const Matrix3x3& m)
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            result(row, col) = m(col, row);
+    }
+    return result;
+}
+
+Matrix3x3 InverseWithDeterminant(const Matrix3x3& m, double* determinant)
+{
+    // The inverse is the adjugate divided by the determinant.
+    double det;
+    Matrix3x3 adjugate = AdjugateWithDeterminant(m, &det);
+    if (determinant)
+        *determinant = det;
+    if (det == 0)
+        return Matrix3x3::Zero();
+    else
+        return adjugate * (1 / det);
+}
+
+Matrix3x3 Inverse(const Matrix3x3& m) { return InverseWithDeterminant(m, nullptr); }
+
+Matrix3x3 RotationMatrixNH(const Rotation& r)
+{
+    Matrix3x3 m;
+    RotationMatrix3x3(r, &m);
+    return m;
+}
+
+} // namespace cardboard

applications/plugins/airmouse/tracking/util/matrixutils.h

@@ -0,0 +1,65 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_UTIL_MATRIXUTILS_H_
+#define CARDBOARD_SDK_UTIL_MATRIXUTILS_H_
+
+//
+// This file contains operators and free functions that define generic Matrix
+// operations.
+//
+
+#include "matrix_3x3.h"
+#include "rotation.h"
+#include "vector.h"
+
+namespace cardboard {
+
+// Returns the transpose of a matrix.
+Matrix3x3 Transpose(const Matrix3x3& m);
+
+// Multiplies a Matrix and a column Vector of the same Dimension to produce
+// another column Vector.
+Vector3 operator*(const Matrix3x3& m, const Vector3& v);
+
+// Returns the determinant of the matrix. This function is defined for all the
+// typedef'ed Matrix types.
+double Determinant(const Matrix3x3& m);
+
+// Returns the adjugate of the matrix, which is defined as the transpose of the
+// cofactor matrix. This function is defined for all the typedef'ed Matrix
+// types.  The determinant of the matrix is computed as a side effect, so it is
+// returned in the determinant parameter if it is not null.
+Matrix3x3 AdjugateWithDeterminant(const Matrix3x3& m, double* determinant);
+
+// Returns the inverse of the matrix. This function is defined for all the
+// typedef'ed Matrix types.  The determinant of the matrix is computed as a
+// side effect, so it is returned in the determinant parameter if it is not
+// null. If the determinant is 0, the returned matrix has all zeroes.
+Matrix3x3 InverseWithDeterminant(const Matrix3x3& m, double* determinant);
+
+// Returns the inverse of the matrix. This function is defined for all the
+// typedef'ed Matrix types. If the determinant of the matrix is 0, the returned
+// matrix has all zeroes.
+Matrix3x3 Inverse(const Matrix3x3& m);
+
+// Returns a 3x3 Matrix representing a 3D rotation. This creates a Matrix that
+// does not work with homogeneous coordinates, so the function name ends in
+// "NH".
+Matrix3x3 RotationMatrixNH(const Rotation& r);
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_UTIL_MATRIXUTILS_H_

applications/plugins/airmouse/tracking/util/rotation.cc

@@ -0,0 +1,117 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "rotation.h"
+
+#include <cmath>
+#include <limits>
+
+#include "vectorutils.h"
+
+namespace cardboard {
+
+void Rotation::SetAxisAndAngle(const VectorType& axis, double angle)
+{
+    VectorType unit_axis = axis;
+    if (!Normalize(&unit_axis)) {
+        *this = Identity();
+    } else {
+        double a = angle / 2;
+        const double s = sin(a);
+        const VectorType v(unit_axis * s);
+        SetQuaternion(QuaternionType(v[0], v[1], v[2], cos(a)));
+    }
+}
+
+Rotation Rotation::FromRotationMatrix(const Matrix3x3& mat)
+{
+    static const double kOne = 1.0;
+    static const double kFour = 4.0;
+
+    const double d0 = mat(0, 0), d1 = mat(1, 1), d2 = mat(2, 2);
+    const double ww = kOne + d0 + d1 + d2;
+    const double xx = kOne + d0 - d1 - d2;
+    const double yy = kOne - d0 + d1 - d2;
+    const double zz = kOne - d0 - d1 + d2;
+
+    const double max = std::max(ww, std::max(xx, std::max(yy, zz)));
+    if (ww == max) {
+        const double w4 = sqrt(ww * kFour);
+        return Rotation::FromQuaternion(QuaternionType((mat(2, 1) - mat(1, 2)) / w4,
+            (mat(0, 2) - mat(2, 0)) / w4, (mat(1, 0) - mat(0, 1)) / w4, w4 / kFour));
+    }
+
+    if (xx == max) {
+        const double x4 = sqrt(xx * kFour);
+        return Rotation::FromQuaternion(QuaternionType(x4 / kFour, (mat(0, 1) + mat(1, 0)) / x4,
+            (mat(0, 2) + mat(2, 0)) / x4, (mat(2, 1) - mat(1, 2)) / x4));
+    }
+
+    if (yy == max) {
+        const double y4 = sqrt(yy * kFour);
+        return Rotation::FromQuaternion(QuaternionType((mat(0, 1) + mat(1, 0)) / y4, y4 / kFour,
+            (mat(1, 2) + mat(2, 1)) / y4, (mat(0, 2) - mat(2, 0)) / y4));
+    }
+
+    // zz is the largest component.
+    const double z4 = sqrt(zz * kFour);
+    return Rotation::FromQuaternion(QuaternionType((mat(0, 2) + mat(2, 0)) / z4,
+        (mat(1, 2) + mat(2, 1)) / z4, z4 / kFour, (mat(1, 0) - mat(0, 1)) / z4));
+}
+
+void Rotation::GetAxisAndAngle(VectorType* axis, double* angle) const
+{
+    VectorType vec(quat_[0], quat_[1], quat_[2]);
+    if (Normalize(&vec)) {
+        *angle = 2 * acos(quat_[3]);
+        *axis = vec;
+    } else {
+        *axis = VectorType(1, 0, 0);
+        *angle = 0.0;
+    }
+}
+
+Rotation Rotation::RotateInto(const VectorType& from, const VectorType& to)
+{
+    static const double kTolerance = std::numeric_limits<double>::epsilon() * 100;
+
+    // Directly build the quaternion using the following technique:
+    // http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final
+    const double norm_u_norm_v = sqrt(LengthSquared(from) * LengthSquared(to));
+    double real_part = norm_u_norm_v + Dot(from, to);
+    VectorType w;
+    if (real_part < kTolerance * norm_u_norm_v) {
+        // If |from| and |to| are exactly opposite, rotate 180 degrees around an
+        // arbitrary orthogonal axis. Axis normalization can happen later, when we
+        // normalize the quaternion.
+        real_part = 0.0;
+        w = (abs(from[0]) > abs(from[2])) ? VectorType(-from[1], from[0], 0)
+                                          : VectorType(0, -from[2], from[1]);
+    } else {
+        // Otherwise, build the quaternion the standard way.
+        w = Cross(from, to);
+    }
+
+    // Build and return a normalized quaternion.
+    // Note that Rotation::FromQuaternion automatically performs normalization.
+    return Rotation::FromQuaternion(QuaternionType(w[0], w[1], w[2], real_part));
+}
+
+Rotation::VectorType Rotation::operator*(const Rotation::VectorType& v) const
+{
+    return ApplyToVector(v);
+}
+
+} // namespace cardboard

applications/plugins/airmouse/tracking/util/rotation.h

@@ -0,0 +1,156 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_UTIL_ROTATION_H_
+#define CARDBOARD_SDK_UTIL_ROTATION_H_
+
+#include "matrix_3x3.h"
+#include "vector.h"
+#include "vectorutils.h"
+
+namespace cardboard {
+
+// The Rotation class represents a rotation around a 3-dimensional axis. It
+// uses normalized quaternions internally to make the math robust.
+class Rotation {
+public:
+    // Convenience typedefs for vector of the correct type.
+    typedef Vector<3> VectorType;
+    typedef Vector<4> QuaternionType;
+
+    // The default constructor creates an identity Rotation, which has no effect.
+    Rotation() {
+        quat_.Set(0, 0, 0, 1);
+    }
+
+    // Returns an identity Rotation, which has no effect.
+    static Rotation Identity() {
+        return Rotation();
+    }
+
+    // Sets the Rotation from a quaternion (4D vector), which is first normalized.
+    void SetQuaternion(const QuaternionType& quaternion) {
+        quat_ = Normalized(quaternion);
+    }
+
+    // Returns the Rotation as a normalized quaternion (4D vector).
+    const QuaternionType& GetQuaternion() const {
+        return quat_;
+    }
+
+    // Sets the Rotation to rotate by the given angle around the given axis,
+    // following the right-hand rule. The axis does not need to be unit
+    // length. If it is zero length, this results in an identity Rotation.
+    void SetAxisAndAngle(const VectorType& axis, double angle);
+
+    // Returns the right-hand rule axis and angle corresponding to the
+    // Rotation. If the Rotation is the identity rotation, this returns the +X
+    // axis and an angle of 0.
+    void GetAxisAndAngle(VectorType* axis, double* angle) const;
+
+    // Convenience function that constructs and returns a Rotation given an axis
+    // and angle.
+    static Rotation FromAxisAndAngle(const VectorType& axis, double angle) {
+        Rotation r;
+        r.SetAxisAndAngle(axis, angle);
+        return r;
+    }
+
+    // Convenience function that constructs and returns a Rotation given a
+    // quaternion.
+    static Rotation FromQuaternion(const QuaternionType& quat) {
+        Rotation r;
+        r.SetQuaternion(quat);
+        return r;
+    }
+
+    // Convenience function that constructs and returns a Rotation given a
+    // rotation matrix R with $R^\top R = I && det(R) = 1$.
+    static Rotation FromRotationMatrix(const Matrix3x3& mat);
+
+    // Convenience function that constructs and returns a Rotation given Euler
+    // angles that are applied in the order of rotate-Z by roll, rotate-X by
+    // pitch, rotate-Y by yaw (same as GetRollPitchYaw).
+    static Rotation FromRollPitchYaw(double roll, double pitch, double yaw) {
+        VectorType x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
+        return FromAxisAndAngle(z, roll) * (FromAxisAndAngle(x, pitch) * FromAxisAndAngle(y, yaw));
+    }
+
+    // Convenience function that constructs and returns a Rotation given Euler
+    // angles that are applied in the order of rotate-Y by yaw, rotate-X by
+    // pitch, rotate-Z by roll (same as GetYawPitchRoll).
+    static Rotation FromYawPitchRoll(double yaw, double pitch, double roll) {
+        VectorType x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
+        return FromAxisAndAngle(y, yaw) * (FromAxisAndAngle(x, pitch) * FromAxisAndAngle(z, roll));
+    }
+
+    // Constructs and returns a Rotation that rotates one vector to another along
+    // the shortest arc. This returns an identity rotation if either vector has
+    // zero length.
+    static Rotation RotateInto(const VectorType& from, const VectorType& to);
+
+    // The negation operator returns the inverse rotation.
+    friend Rotation operator-(const Rotation& r) {
+        // Because we store normalized quaternions, the inverse is found by
+        // negating the vector part.
+        return Rotation(-r.quat_[0], -r.quat_[1], -r.quat_[2], r.quat_[3]);
+    }
+
+    // Appends a rotation to this one.
+    Rotation& operator*=(const Rotation& r) {
+        const QuaternionType& qr = r.quat_;
+        QuaternionType& qt = quat_;
+        SetQuaternion(QuaternionType(
+            qr[3] * qt[0] + qr[0] * qt[3] + qr[2] * qt[1] - qr[1] * qt[2],
+            qr[3] * qt[1] + qr[1] * qt[3] + qr[0] * qt[2] - qr[2] * qt[0],
+            qr[3] * qt[2] + qr[2] * qt[3] + qr[1] * qt[0] - qr[0] * qt[1],
+            qr[3] * qt[3] - qr[0] * qt[0] - qr[1] * qt[1] - qr[2] * qt[2]));
+        return *this;
+    }
+
+    // Binary multiplication operator - returns a composite Rotation.
+    friend const Rotation operator*(const Rotation& r0, const Rotation& r1) {
+        Rotation r = r0;
+        r *= r1;
+        return r;
+    }
+
+    // Multiply a Rotation and a Vector to get a Vector.
+    VectorType operator*(const VectorType& v) const;
+
+private:
+    // Private constructor that builds a Rotation from quaternion components.
+    Rotation(double q0, double q1, double q2, double q3)
+        : quat_(q0, q1, q2, q3) {
+    }
+
+    // Applies a Rotation to a Vector to rotate the Vector. Method borrowed from:
+    // http://blog.molecular-matters.com/2013/05/24/a-faster-quaternion-vector-multiplication/
+    VectorType ApplyToVector(const VectorType& v) const {
+        VectorType im(quat_[0], quat_[1], quat_[2]);
+        VectorType temp = 2.0 * Cross(im, v);
+        return v + quat_[3] * temp + Cross(im, temp);
+    }
+
+    // The rotation represented as a normalized quaternion. (Unit quaternions are
+    // required for constructing rotation matrices, so it makes sense to always
+    // store them that way.) The vector part is in the first 3 elements, and the
+    // scalar part is in the last element.
+    QuaternionType quat_;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_UTIL_ROTATION_H_

applications/plugins/airmouse/tracking/util/vector.h

@@ -0,0 +1,251 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_UTIL_VECTOR_H_
+#define CARDBOARD_SDK_UTIL_VECTOR_H_
+
+#include <array>
+
+namespace cardboard {
+
+// Geometric N-dimensional Vector class.
+template <int Dimension>
+class Vector {
+public:
+    // The default constructor zero-initializes all elements.
+    Vector();
+
+    // Dimension-specific constructors that are passed individual element values.
+    constexpr Vector(double e0, double e1, double e2);
+    constexpr Vector(double e0, double e1, double e2, double e3);
+
+    // Constructor for a Vector of dimension N from a Vector of dimension N-1 and
+    // a scalar of the correct type, assuming N is at least 2.
+    // constexpr Vector(const Vector<Dimension - 1>& v, double s);
+
+    void Set(double e0, double e1, double e2); // Only when Dimension == 3.
+    void Set(double e0, double e1, double e2,
+             double e3); // Only when Dimension == 4.
+
+    // Mutable element accessor.
+    double& operator[](int index) {
+        return elem_[index];
+    }
+
+    // Element accessor.
+    double operator[](int index) const {
+        return elem_[index];
+    }
+
+    // Returns a Vector containing all zeroes.
+    static Vector Zero();
+
+    // Self-modifying operators.
+    void operator+=(const Vector& v) {
+        Add(v);
+    }
+    void operator-=(const Vector& v) {
+        Subtract(v);
+    }
+    void operator*=(double s) {
+        Multiply(s);
+    }
+    void operator/=(double s) {
+        Divide(s);
+    }
+
+    // Unary negation operator.
+    Vector operator-() const {
+        return Negation();
+    }
+
+    // Binary operators.
+    friend Vector operator+(const Vector& v0, const Vector& v1) {
+        return Sum(v0, v1);
+    }
+    friend Vector operator-(const Vector& v0, const Vector& v1) {
+        return Difference(v0, v1);
+    }
+    friend Vector operator*(const Vector& v, double s) {
+        return Scale(v, s);
+    }
+    friend Vector operator*(double s, const Vector& v) {
+        return Scale(v, s);
+    }
+    friend Vector operator*(const Vector& v, const Vector& s) {
+        return Product(v, s);
+    }
+    friend Vector operator/(const Vector& v, double s) {
+        return Divide(v, s);
+    }
+
+    // Self-modifying addition.
+    void Add(const Vector& v);
+    // Self-modifying subtraction.
+    void Subtract(const Vector& v);
+    // Self-modifying multiplication by a scalar.
+    void Multiply(double s);
+    // Self-modifying division by a scalar.
+    void Divide(double s);
+
+    // Unary negation.
+    Vector Negation() const;
+
+    // Binary component-wise multiplication.
+    static Vector Product(const Vector& v0, const Vector& v1);
+    // Binary component-wise addition.
+    static Vector Sum(const Vector& v0, const Vector& v1);
+    // Binary component-wise subtraction.
+    static Vector Difference(const Vector& v0, const Vector& v1);
+    // Binary multiplication by a scalar.
+    static Vector Scale(const Vector& v, double s);
+    // Binary division by a scalar.
+    static Vector Divide(const Vector& v, double s);
+
+private:
+    std::array<double, Dimension> elem_;
+};
+//------------------------------------------------------------------------------
+
+template <int Dimension>
+Vector<Dimension>::Vector() {
+    for(int i = 0; i < Dimension; i++) {
+        elem_[i] = 0;
+    }
+}
+
+template <int Dimension>
+constexpr Vector<Dimension>::Vector(double e0, double e1, double e2)
+    : elem_{e0, e1, e2} {
+}
+
+template <int Dimension>
+constexpr Vector<Dimension>::Vector(double e0, double e1, double e2, double e3)
+    : elem_{e0, e1, e2, e3} {
+}
+/*
+template <>
+constexpr Vector<4>::Vector(const Vector<3>& v, double s)
+    : elem_{v[0], v[1], v[2], s} {}
+*/
+template <int Dimension>
+void Vector<Dimension>::Set(double e0, double e1, double e2) {
+    elem_[0] = e0;
+    elem_[1] = e1;
+    elem_[2] = e2;
+}
+
+template <int Dimension>
+void Vector<Dimension>::Set(double e0, double e1, double e2, double e3) {
+    elem_[0] = e0;
+    elem_[1] = e1;
+    elem_[2] = e2;
+    elem_[3] = e3;
+}
+
+template <int Dimension>
+Vector<Dimension> Vector<Dimension>::Zero() {
+    Vector<Dimension> v;
+    return v;
+}
+
+template <int Dimension>
+void Vector<Dimension>::Add(const Vector& v) {
+    for(int i = 0; i < Dimension; i++) {
+        elem_[i] += v[i];
+    }
+}
+
+template <int Dimension>
+void Vector<Dimension>::Subtract(const Vector& v) {
+    for(int i = 0; i < Dimension; i++) {
+        elem_[i] -= v[i];
+    }
+}
+
+template <int Dimension>
+void Vector<Dimension>::Multiply(double s) {
+    for(int i = 0; i < Dimension; i++) {
+        elem_[i] *= s;
+    }
+}
+
+template <int Dimension>
+void Vector<Dimension>::Divide(double s) {
+    for(int i = 0; i < Dimension; i++) {
+        elem_[i] /= s;
+    }
+}
+
+template <int Dimension>
+Vector<Dimension> Vector<Dimension>::Negation() const {
+    Vector<Dimension> ret;
+    for(int i = 0; i < Dimension; i++) {
+        ret.elem_[i] = -elem_[i];
+    }
+    return ret;
+}
+
+template <int Dimension>
+Vector<Dimension> Vector<Dimension>::Product(const Vector& v0, const Vector& v1) {
+    Vector<Dimension> ret;
+    for(int i = 0; i < Dimension; i++) {
+        ret.elem_[i] = v0[i] * v1[i];
+    }
+    return ret;
+}
+
+template <int Dimension>
+Vector<Dimension> Vector<Dimension>::Sum(const Vector& v0, const Vector& v1) {
+    Vector<Dimension> ret;
+    for(int i = 0; i < Dimension; i++) {
+        ret.elem_[i] = v0[i] + v1[i];
+    }
+    return ret;
+}
+
+template <int Dimension>
+Vector<Dimension> Vector<Dimension>::Difference(const Vector& v0, const Vector& v1) {
+    Vector<Dimension> ret;
+    for(int i = 0; i < Dimension; i++) {
+        ret.elem_[i] = v0[i] - v1[i];
+    }
+    return ret;
+}
+
+template <int Dimension>
+Vector<Dimension> Vector<Dimension>::Scale(const Vector& v, double s) {
+    Vector<Dimension> ret;
+    for(int i = 0; i < Dimension; i++) {
+        ret.elem_[i] = v[i] * s;
+    }
+    return ret;
+}
+
+template <int Dimension>
+Vector<Dimension> Vector<Dimension>::Divide(const Vector& v, double s) {
+    Vector<Dimension> ret;
+    for(int i = 0; i < Dimension; i++) {
+        ret.elem_[i] = v[i] / s;
+    }
+    return ret;
+}
+
+typedef Vector<3> Vector3;
+typedef Vector<4> Vector4;
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_UTIL_VECTOR_H_

applications/plugins/airmouse/tracking/util/vectorutils.cc

@@ -0,0 +1,40 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "vectorutils.h"
+
+namespace cardboard {
+
+// Returns the dot (inner) product of two Vectors.
+double Dot(const Vector<3>& v0, const Vector<3>& v1)
+{
+    return v0[0] * v1[0] + v0[1] * v1[1] + v0[2] * v1[2];
+}
+
+// Returns the dot (inner) product of two Vectors.
+double Dot(const Vector<4>& v0, const Vector<4>& v1)
+{
+    return v0[0] * v1[0] + v0[1] * v1[1] + v0[2] * v1[2] + v0[3] * v1[3];
+}
+
+// Returns the 3-dimensional cross product of 2 Vectors. Note that this is
+// defined only for 3-dimensional Vectors.
+Vector<3> Cross(const Vector<3>& v0, const Vector<3>& v1)
+{
+    return Vector<3>(v0[1] * v1[2] - v0[2] * v1[1], v0[2] * v1[0] - v0[0] * v1[2],
+        v0[0] * v1[1] - v0[1] * v1[0]);
+}
+
+} // namespace cardboard

applications/plugins/airmouse/tracking/util/vectorutils.h

@@ -0,0 +1,76 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_UTIL_VECTORUTILS_H_
+#define CARDBOARD_SDK_UTIL_VECTORUTILS_H_
+
+//
+// This file contains free functions that operate on Vector instances.
+//
+
+#include <cmath>
+
+#include "vector.h"
+
+namespace cardboard {
+
+// Returns the dot (inner) product of two Vectors.
+double Dot(const Vector<3>& v0, const Vector<3>& v1);
+
+// Returns the dot (inner) product of two Vectors.
+double Dot(const Vector<4>& v0, const Vector<4>& v1);
+
+// Returns the 3-dimensional cross product of 2 Vectors. Note that this is
+// defined only for 3-dimensional Vectors.
+Vector<3> Cross(const Vector<3>& v0, const Vector<3>& v1);
+
+// Returns the square of the length of a Vector.
+template <int Dimension>
+double LengthSquared(const Vector<Dimension>& v) {
+    return Dot(v, v);
+}
+
+// Returns the geometric length of a Vector.
+template <int Dimension>
+double Length(const Vector<Dimension>& v) {
+    return sqrt(LengthSquared(v));
+}
+
+// the Vector untouched and returns false.
+template <int Dimension>
+bool Normalize(Vector<Dimension>* v) {
+    const double len = Length(*v);
+    if(len == 0) {
+        return false;
+    } else {
+        (*v) /= len;
+        return true;
+    }
+}
+
+// Returns a unit-length version of a Vector. If the given Vector has no
+// length, this returns a Zero() Vector.
+template <int Dimension>
+Vector<Dimension> Normalized(const Vector<Dimension>& v) {
+    Vector<Dimension> result = v;
+    if(Normalize(&result))
+        return result;
+    else
+        return Vector<Dimension>::Zero();
+}
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_UTIL_VECTORUTILS_H_