private int NumCyclesAverageSame; // Number of cycles that the average has stayed the same
/// <summary> Construct an average filter with given roll-length; give null for continuous mode. </summary>
/// <remarks> To use this as a continuous (non-rolling) average filter, set <see cref="FilterCount"/> to null </remarks>
/// <param name="FilterCount">
/// Roll length for the average filter.
/// If this value is null, the filter is set to continuous mode (as opposed to a roll mode).
/// </param>
public Average(int?FilterCount = 10)
{
// Assert that T is a numeric type
if (!UtilData.IsNumericType(typeof(T)))
{
throw new ArgumentException("Cannot create filter of non-numeric type: " + typeof(T).ToString());
}
this.Output = default(T);
this.CurSum = 0;
this.Index = 0;
this.Iterations = 0;
this.FilterCount = FilterCount;
// Use array if in roll-mode
if (this.FilterCount != null)
{
this.AverageArray = new dynamic[Math.Abs((int)this.FilterCount)];
// Initialize average array to defaults
this.InitializeArray();
}
this.NumCyclesAverageSame = 0;
this.Feed(default(T));
}
private T LastValue; // Last filter value (internal use)
/// <summary> Constructs a low pass filter with time constant <c>LPFk</c>. </summary>
/// <param name="LPFk"> Low Pass Filter Time Constant. </param>
public LowPass(double LPFk = 0.25)
{
if (!UtilData.IsNumericType(typeof(T)))
{
Log.Output(Log.Severity.ERROR, Log.Source.OTHER, "Low-pass filter cannot be instantiated with non-numeric type.");
throw new ArgumentException("Cannot create filter of non-numeric type: " + typeof(T).ToString());
} // We can now assert that T is a numeric
this.Output = default(T);
this.LPFk = LPFk;
this.Reset();
}
} // Time constant for the Low Pass Filter from 0 to 1
/// <summary> Constructs a low pass filter with time constant <see cref="LPFk"/>. </summary>
/// <param name="LPFk"> Low Pass Filter Time Constant. </param>
/// <param name="SteadyStateEpsilon"> Allowable difference in output to be considered a steady state system. </param>
public LowPass(double LPFk = 0.25, double SteadyStateEpsilon = 0)
{
// Assert that T is numeric
if (!UtilData.IsNumericType(typeof(T)))
{
throw new ArgumentException("Cannot create filter of non-numeric type: " + typeof(T).ToString());
}
this.Output = default(T);
this.LPFk = LPFk;
this.SteadyStateEpsilon = SteadyStateEpsilon;
this.Reset();
this.Feed(default(T));
}
Iterations; // Number of iterations in the filter
/// <summary> Construct an average filter with given roll-length. </summary>
/// <param name="FilterCount"> Roll length for the average filter. </param>
public Average(int FilterCount = 10)
{
if (!UtilData.IsNumericType(typeof(T)))
{
Log.Output(Log.Severity.ERROR, Log.Source.OTHER, "Average filter cannot be instantiated with non-numeric type.");
throw new ArgumentException("Cannot create filter of non-numeric type: " + typeof(T).ToString());
} // We can now assert that T is a numeric type
this.Output = default(T);
this.CurSum = 0;
this.Index = 0;
this.Iterations = 0;
this.FilterCount = FilterCount;
this.AverageArray = new dynamic[this.FilterCount];
this.InitializeArray(); // Initialize average array to defaults
}
public double Qbias; // Process noise variance for the signal rate bias.
/// <summary>
/// Handles construction of the Kalman filter with default filter coefficients.
/// See class comment for more information regarding implementation.
/// </summary>
public Kalman()
{
if (!UtilData.IsNumericType(typeof(T))) // Can now assert that T is a numeric
{
Log.Output(Log.Severity.ERROR, Log.Source.OTHER, "Kalman filter cannot be instantiated with non-numeric type.");
throw new ArgumentException("Cannot create filter of non-numeric type: " + typeof(T).ToString());
}
this.Output = default(T);
this.LastRate = default(T);
this.StopWatch = new Stopwatch();
this.Initialized = false;
this.Q = 0.001; // Default Q value (arbitrary), set Q to necessary value
this.Qbias = 0.003; // Default Qbias value (arbitrary), set Qbias to necessary value
this.Rmeasure = 0.03; // Default Rmeasure value (arbitrary), set Rmeasure to necessary value
this.CalcMeasure = 0.0;
this.Bias = 0.0;
this.P = new double[2, 2];
this.K = new double[2];
this.P[0, 0] = 0.0; // Since we assume that the bias is 0 and we know the starting position,
this.P[0, 1] = 0.0; // the error covariance matrix is set like so.
this.P[1, 0] = 0.0;
this.P[1, 1] = 0.0;
}
