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
115
116
117
118
|
#pragma once
#include "base/assert.hpp"
#include "base/math.hpp"
#include <array>
#include <cmath>
#include <cstddef>
#include <cstring>
#include <deque>
#include <limits>
#include <type_traits>
namespace math
{
template <class T, size_t Dim>
class AvgVector
{
public:
explicit AvgVector(size_t count = 1) : m_count(count)
{
static_assert(std::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({});
std::memcpy(m_vectors.back().data(), arr, Dim * sizeof(T));
if (m_vectors.size() > 1)
CalcAverage(arr);
}
private:
using Cont = std::deque<std::array<T, Dim>>;
using Value = typename Cont::value_type;
Cont m_vectors;
size_t m_count;
static T Distance(Value const & a1, Value const & a2)
{
T res = 0;
for (size_t i = 0; i < Dim; ++i)
res += std::pow(a1[i] - a2[i], 2);
return std::sqrt(res);
}
static void Average(Value const & a1, Value 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 = std::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);
}
};
// 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
{
public:
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];
}
}
private:
std::array<T, Dim> m_val{};
T m_factor = 0.15;
};
} // namespace math
|
