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