Created
March 23, 2017 17:55
-
-
Save mikasjp/0248dde9c7277ed8efa4e7b0d1d58aa3 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
public class AlphaBetaFilter: IFilter | |
{ | |
private double Alpha { get; set; } | |
private double Beta { get; set; } | |
private Sample LastSample { get; set; } | |
double LastDerivative { get; set; } = 0; | |
public AlphaBetaFilter(double alpha, double beta, double InitialTime = 0) | |
{ | |
Alpha = alpha; | |
Beta = beta; | |
LastSample = new Sample(InitialTime, 0); | |
} | |
public Sample AddSample(Sample sample) | |
{ | |
double dt = sample.Time - LastSample.Time; | |
if (dt == 0) | |
return sample; | |
Sample estSample = new Sample(sample.Time,LastSample.Value); | |
estSample.Value += LastDerivative * dt; | |
double error = sample.Value - estSample.Value; | |
estSample.Value += Alpha * error; | |
LastDerivative += (Beta * error) / dt; | |
LastSample = estSample; | |
return estSample; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment