nuclear@1: /************************************************************************************
nuclear@1: 
nuclear@1: PublicHeader:   OVR.h
nuclear@1: Filename    :   OVR_SensorFilter.h
nuclear@1: Content     :   Basic filtering of sensor data
nuclear@1: Created     :   March 7, 2013
nuclear@1: Authors     :   Steve LaValle, Anna Yershova
nuclear@1: 
nuclear@1: Copyright   :   Copyright 2012 Oculus VR, Inc. All Rights reserved.
nuclear@1: 
nuclear@1: Use of this software is subject to the terms of the Oculus license
nuclear@1: agreement provided at the time of installation or download, or which
nuclear@1: otherwise accompanies this software in either electronic or hard copy form.
nuclear@1: 
nuclear@1: *************************************************************************************/
nuclear@1: 
nuclear@1: #ifndef OVR_SensorFilter_h
nuclear@1: #define OVR_SensorFilter_h
nuclear@1: 
nuclear@1: #include "Kernel/OVR_Math.h"
nuclear@1: 
nuclear@1: 
nuclear@1: namespace OVR {
nuclear@1: 
nuclear@1: // This class maintains a sliding window of sensor data taken over time and implements
nuclear@1: // various simple filters, most of which are linear functions of the data history.
nuclear@1: class SensorFilter
nuclear@1: {
nuclear@1:     enum
nuclear@1:     {
nuclear@1:         MaxFilterSize     = 100,
nuclear@1:         DefaultFilterSize = 20
nuclear@1:     };
nuclear@1: 
nuclear@1: private:
nuclear@1:     int         LastIdx;                    // The index of the last element that was added to the array
nuclear@1:     int         Size;                       // The window size (number of elements)
nuclear@1:     Vector3f    Elements[MaxFilterSize]; 
nuclear@1: 
nuclear@1: public:
nuclear@1:     // Create a new filter with default size
nuclear@1:     SensorFilter() 
nuclear@1:     {
nuclear@1:         LastIdx = -1;
nuclear@1:         Size = DefaultFilterSize;
nuclear@1:     };
nuclear@1: 
nuclear@1:     // Create a new filter with size i
nuclear@1:     SensorFilter(int i) 
nuclear@1:     {
nuclear@1:         OVR_ASSERT(i <= MaxFilterSize);
nuclear@1:         LastIdx = -1;
nuclear@1:         Size = i;
nuclear@1:     };
nuclear@1: 
nuclear@1: 
nuclear@1:     // Create a new element to the filter
nuclear@1:     void AddElement (const Vector3f &e) 
nuclear@1:     {
nuclear@1:         if (LastIdx == Size - 1) 
nuclear@1:             LastIdx = 0;
nuclear@1:         else                            
nuclear@1:             LastIdx++;
nuclear@1: 
nuclear@1:         Elements[LastIdx] = e;
nuclear@1:     };
nuclear@1: 
nuclear@1:     // Get element i.  0 is the most recent, 1 is one step ago, 2 is two steps ago, ...
nuclear@1:     Vector3f GetPrev(int i) const
nuclear@1:     {
nuclear@1: 		OVR_ASSERT(i >= 0); // 
nuclear@1:         int idx = (LastIdx - i);
nuclear@1:         if (idx < 0) // Fix the wraparound case
nuclear@1:             idx += Size;
nuclear@1: 		OVR_ASSERT(idx >= 0); // Multiple wraparounds not allowed
nuclear@1:         return Elements[idx];
nuclear@1:     };
nuclear@1: 
nuclear@1:     // Simple statistics
nuclear@1:     Vector3f Total() const;
nuclear@1:     Vector3f Mean() const;
nuclear@1:     Vector3f Median() const;
nuclear@1:     Vector3f Variance() const; // The diagonal of covariance matrix
nuclear@1:     Matrix4f Covariance() const;
nuclear@1:     Vector3f PearsonCoefficient() const;
nuclear@1: 
nuclear@1:     // A popular family of smoothing filters and smoothed derivatives
nuclear@1:     Vector3f SavitzkyGolaySmooth8() const;
nuclear@1:     Vector3f SavitzkyGolayDerivative4() const;
nuclear@1:     Vector3f SavitzkyGolayDerivative5() const;
nuclear@1:     Vector3f SavitzkyGolayDerivative12() const; 
nuclear@1:     Vector3f SavitzkyGolayDerivativeN(int n) const;
nuclear@1: 
nuclear@1:     ~SensorFilter() {};
nuclear@1: };
nuclear@1: 
nuclear@1: } //namespace OVR
nuclear@1: 
nuclear@1: #endif // OVR_SensorFilter_h