Skip to content

Instantly share code, notes, and snippets.

@mikasjp
Created March 23, 2017 17:55
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save mikasjp/0248dde9c7277ed8efa4e7b0d1d58aa3 to your computer and use it in GitHub Desktop.
Save mikasjp/0248dde9c7277ed8efa4e7b0d1d58aa3 to your computer and use it in GitHub Desktop.
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