1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
|
#pragma once
#include "base/math.hpp"
#include "base/assert.hpp"
#include "std/deque.hpp"
#include "std/array.hpp"
#include "std/cstring.hpp"
namespace math
{
template <class T, size_t Dim> class AvgVector
{
typedef deque<array<T, Dim> > ContT;
typedef typename ContT::value_type ValueT;
ContT m_vectors;
size_t m_count;
static T Distance(ValueT const & a1, ValueT const & a2)
{
T res = 0;
for (size_t i = 0; i < Dim; ++i)
res += my::sq(a1[i] - a2[i]);
return sqrt(res);
}
static void Average(ValueT const & a1, ValueT const & a2, T * res)
{
for (size_t i = 0; i < Dim; ++i)
res[i] = (a1[i] + a2[i]) / 2.0;
}
void CalcAverage(T * res) const
{
T minD = numeric_limits<T>::max();
size_t I = 0, J = 1;
size_t const count = m_vectors.size();
ASSERT_GREATER ( count, 1, () );
for (size_t i = 0; i < count - 1; ++i)
for (size_t j = i+1; j < count; ++j)
{
T const d = Distance(m_vectors[i], m_vectors[j]);
if (d < minD)
{
I = i;
J = j;
minD = d;
}
}
Average(m_vectors[I], m_vectors[J], res);
}
public:
AvgVector(size_t count = 1) : m_count(count)
{
static_assert(is_floating_point<T>::value, "");
}
void SetCount(size_t count) { m_count = count; }
/// @param[in] Next measurement.
/// @param[out] Average value.
void Next(T * arr)
{
if (m_vectors.size() == m_count)
m_vectors.pop_front();
m_vectors.push_back(ValueT());
memcpy(m_vectors.back().data(), arr, Dim*sizeof(T));
if (m_vectors.size() > 1)
CalcAverage(arr);
}
};
// Compass smoothing parameters
// We're using technique described in
// http://windowsteamblog.com/windows_phone/b/wpdev/archive/2010/09/08/using-the-accelerometer-on-windows-phone-7.aspx
// In short it's a combination of low-pass filter to smooth the
// small orientation changes and a threshold filter to get big changes fast.
// k in the following formula: O(n) = O(n-1) + k * (I - O(n - 1));
// smoothed heading angle. doesn't always correspond to the m_headingAngle
// as we change the heading angle only if the delta between
// smoothedHeadingRad and new heading value is bigger than smoothingThreshold.
template <class T, size_t Dim> class LowPassVector
{
typedef array<T, Dim> ArrayT;
ArrayT m_val;
T m_factor;
public:
LowPassVector() : m_factor(0.15)
{
m_val.fill(T());
}
void SetFactor(T t) { m_factor = t; }
/// @param[in] Next measurement.
/// @param[out] Average value.
void Next(T * arr)
{
for (size_t i = 0; i < Dim; ++i)
{
m_val[i] = m_val[i] + m_factor * (arr[i] - m_val[i]);
arr[i] = m_val[i];
}
}
};
}
|
