1 /* Copyright 2016, Ableton AG, Berlin. All rights reserved.
2  *
3  *  This program is free software: you can redistribute it and/or modify
4  *  it under the terms of the GNU General Public License as published by
5  *  the Free Software Foundation, either version 2 of the License, or
6  *  (at your option) any later version.
7  *
8  *  This program is distributed in the hope that it will be useful,
9  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
10  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11  *  GNU General Public License for more details.
12  *
13  *  You should have received a copy of the GNU General Public License
14  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
15  *
16  *  If you would like to incorporate Link into a proprietary software application,
17  *  please contact <link-devs@ableton.com>.
18  */
19 
20 #pragma once
21 
22 #include <array>
23 #include <cfloat>
24 #include <cmath>
25 
26 
27 namespace ableton
28 {
29 namespace link
30 {
31 
32 template <std::size_t n>
33 struct Kalman
34 {
Kalmanableton::link::Kalman35   Kalman()
36     : mValue(0)
37     , mCoVariance(1)
38     , mVarianceLength(n)
39     , mCounter(mVarianceLength)
40   {
41   }
42 
getValueableton::link::Kalman43   double getValue()
44   {
45     return mValue;
46   }
47 
calculateVVarianceableton::link::Kalman48   double calculateVVariance()
49   {
50     auto vVar = 0.;
51     auto meanOfDiffs = 0.;
52 
53     for (size_t k = 0; k < (mVarianceLength); k++)
54     {
55       meanOfDiffs += (mMeasuredValues[k] - mFilterValues[k]);
56     }
57 
58     meanOfDiffs /= (mVarianceLength);
59 
60     for (size_t i = 0; i < (mVarianceLength); i++)
61     {
62       vVar += (pow(mMeasuredValues[i] - mFilterValues[i] - meanOfDiffs, 2.0));
63     }
64 
65     vVar /= (mVarianceLength - 1);
66 
67     return vVar;
68   }
69 
calculateWVarianceableton::link::Kalman70   double calculateWVariance()
71   {
72     auto wVar = 0.;
73     auto meanOfDiffs = 0.;
74 
75     for (size_t k = 0; k < (mVarianceLength); k++)
76     {
77       meanOfDiffs += (mFilterValues[(mCounter - k - 1) % mVarianceLength]
78                       - mFilterValues[(mCounter - k - 2) % mVarianceLength]);
79     }
80 
81     meanOfDiffs /= (mVarianceLength);
82 
83     for (size_t i = 0; i < (mVarianceLength); i++)
84     {
85       wVar += (pow(mFilterValues[(mCounter - i - 1) % mVarianceLength]
86                      - mFilterValues[(mCounter - i - 2) % mVarianceLength] - meanOfDiffs,
87         2.0));
88     }
89 
90     wVar /= (mVarianceLength - 1);
91 
92     return wVar;
93   }
94 
iterateableton::link::Kalman95   void iterate(const double value)
96   {
97     const std::size_t currentIndex = mCounter % mVarianceLength;
98     mMeasuredValues[currentIndex] = value;
99 
100     if (mCounter < (mVarianceLength + mVarianceLength))
101     {
102       if (mCounter == mVarianceLength)
103       {
104         mValue = value;
105       }
106       else
107       {
108         mValue = (mValue + value) / 2;
109       }
110     }
111     else
112     {
113       // prediction equations
114       const double prevFilterValue = mFilterValues[(mCounter - 1) % mVarianceLength];
115       mFilterValues[currentIndex] = prevFilterValue;
116       const auto wVariance = calculateWVariance();
117       const double coVarianceEstimation = mCoVariance + wVariance;
118 
119       // update equations
120       const auto vVariance = calculateVVariance();
121       // Gain defines how easily the filter will adjust to a new condition
122       // With gain = 1 the output equals the input, with gain = 0 the input
123       // is ignored and the output equals the last filtered value
124       const auto divisor = coVarianceEstimation + vVariance;
125       const auto gain = divisor != 0. ? coVarianceEstimation / divisor : 0.7;
126       mValue = prevFilterValue + gain * (value - prevFilterValue);
127       mCoVariance = (1 - gain) * coVarianceEstimation;
128     }
129     mFilterValues[currentIndex] = mValue;
130 
131     ++mCounter;
132   }
133 
134   double mValue;
135   double mCoVariance;
136   size_t mVarianceLength;
137   size_t mCounter;
138   std::array<double, n> mFilterValues;
139   std::array<double, n> mMeasuredValues;
140 };
141 
142 } // namespace link
143 } // namespace ableton
144