Revert "Revert "Air Mouse""

This reverts commit bbc0888b45.

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_