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 (!UtilMain.IsNumericType(typeof(T)))
{
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();
}
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 (!UtilMain.IsNumericType(typeof(T)))
{
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.
/// Kalman Filter Constructor
/// <summary>
/// Handles construction of the
/// Kalman filter with default
/// filter coefficients.
/// See class comment for more
/// information regarding
/// implementation.</summary>
public Kalman()
{
if (!UtilMain.IsNumericType(typeof(T))) // Can now assert that T is a numeric
{
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;
}
