/// <summary>
/// Checks the state transition matrix is the correct dimension.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <param name="filter">Filter being predicted.</param>
/// <exception cref="System.ArgumentException">Thrown when the transition
/// matrix is non-square or does not have the same number of rows/cols as there
/// are state variables in the given filter.</exception>
public static void CheckPredictParameters(Matrix <double> F, IKalmanFilter filter)
{
// State transition should be n-by-n matrix (n is number of state variables)
if ((F.ColumnCount != F.RowCount) || (F.ColumnCount != filter.State.RowCount))
{
throw new ArgumentException("KFStateTransitionMalformed", "F");
}
}
/// <summary>
/// Creates an Information Filter from a given Kalman Filter.
/// </summary>
/// <param name="kf">The filter used to derive the information filter.</param>
public InformationFilter(
IKalmanFilter kf
)
{
this.J = kf.Cov.Inverse();
this.y = this.J * kf.State;
this.I = Matrix.Identity(this.y.RowCount, this.y.RowCount);
}
InformationFilter(
IKalmanFilter kf
)
{
this.J = kf.Cov.Inverse();
this.y = this.J * kf.State;
this.I = Matrix.Identity(this.y.RowCount, this.y.RowCount);
}
public OneDimensionTracker(IKalmanFilter filter)
{
this.filter = filter;
this.F = Matrix <double> .Build.Dense(2, 2, new[] { 1d, 0d, -1d, 1d });
this.Q = Matrix <double> .Build.Dense(1, 1);
this.G = Matrix <double> .Build.Dense(2, 1);
this.H = Matrix <double> .Build.Dense(1, 2, new[] { 1d, 0d });
this.R = Matrix <double> .Build.Dense(1, 1);
}
public OneDimensionTracker(IKalmanFilter filter)
{
this.filter = filter;
this.F = new Matrix(2, 2);
this.F[0, 0] = 1d;
this.F[1, 0] = 0d;
this.F[0, 1] = -1d;
this.F[1, 1] = 1d;
this.Q = new Matrix(1, 1);
this.G = new Matrix(2, 1);
this.H = new Matrix(1, 2);
this.H[0, 0] = 1d;
this.H[0, 1] = 0d;
this.R = new Matrix(1, 1);
}
public LinearFilter()
{
var initStateEstimate = Matrix(new[, ]
{
{ 0.0 },
{ 0.0 }
});
var initStateCov = Matrix(new[, ]
{
{ 1.0, 0.0 },
{ 0.0, 1.0 }
});
_kalmanFilter = new SquareRootFilter(initStateEstimate, initStateCov);
}
CheckPredictParameters(
Matrix F,
Matrix Q,
IKalmanFilter filter
)
{
// State transition should be n-by-n matrix (n is number of state variables)
if ((F.ColumnCount != F.RowCount) || (F.ColumnCount != filter.State.RowCount))
{
throw new ArgumentException(Resources.KFStateTransitionMalformed, "F");
}
if ((Q.ColumnCount != Q.RowCount) || (Q.ColumnCount != filter.State.RowCount))
{
throw new ArgumentException(Resources.KFSquareNoiseCouplingMalformed, "Q");
}
}
CheckUpdateParameters(
Matrix z,
Matrix H,
Matrix R,
IKalmanFilter filter
)
{
if (z.ColumnCount != 1)
{
throw new ArgumentException(Resources.KFMeasurementVectorMalformed, "z");
}
if ((H.RowCount != z.RowCount) || (H.ColumnCount != filter.State.RowCount))
{
throw new ArgumentException(Resources.KFMeasureSensitivityMalformed, "H");
}
if ((R.ColumnCount != R.RowCount) || (R.ColumnCount != z.RowCount))
{
throw new ArgumentException(Resources.KFMeasureCovarainceMalformed, "R");
}
}
CheckPredictParameters(
Matrix F,
Matrix G,
Matrix Q,
IKalmanFilter filter
)
{
// State transition should be n-by-n matrix (n is number of state variables)
if ((F.ColumnCount != F.RowCount) || (F.ColumnCount != filter.State.RowCount))
{
throw new ArgumentException(Resources.KFStateTransitionMalformed, "F");
}
// Noise coupling should be n-by-p (p is rows in Q)
if ((G.RowCount != filter.State.RowCount) || (G.ColumnCount != Q.RowCount))
{
throw new ArgumentException(Resources.KFNoiseCouplingMalformed, "G");
}
// Noise covariance should be p-by-p
if (Q.ColumnCount != Q.RowCount)
{
throw new ArgumentException(Resources.KFNoiseCovarianceMalformed, "Q");
}
}
private bool RunTest(IKalmanFilter filter1, IKalmanFilter filter2, double tolerance)
{
List <double> xf1 = new List <double>();
List <double> yf1 = new List <double>();
List <double> xf2 = new List <double>();
List <double> yf2 = new List <double>();
Matrix ZeroCov = new Matrix(filter1.Cov.RowCount, filter1.Cov.RowCount);
Matrix ZeroState = new Matrix(filter1.State.RowCount, 1);
RangeBearingTracker rbt1 = new RangeBearingTracker(filter1);
RangeBearingTracker rbt2 = new RangeBearingTracker(filter2);
bool withinTolerance = true;
// Predict the filters
rbt1.Predict(this.T, this.q);
rbt2.Predict(this.T, this.q);
for (int i = 2; i < this.bM.Length; i++)
{
rbt1.Update(this.rM[i], this.bM[i], this.re, this.the);
rbt2.Update(this.rM[i], this.bM[i], this.re, this.the);
xf1.Add(rbt1.State[0, 0]);
yf1.Add(rbt1.State[2, 0]);
xf2.Add(rbt2.State[0, 0]);
yf2.Add(rbt2.State[2, 0]);
if (!Matrix.AlmostEqual(ZeroCov, (rbt2.Cov - rbt1.Cov), tolerance))
{
withinTolerance = false;
}
else if (!Matrix.AlmostEqual(ZeroState, (rbt2.State - rbt1.State), tolerance))
{
withinTolerance = false;
}
rbt1.Predict(this.T, this.q);
rbt2.Predict(this.T, this.q);
}
// Create strings
string x1s = ""; string y1s = ""; string x2s = ""; string y2s = "";
for (int i = 0; i < xf1.Count; i++)
{
x1s += xf1[i].ToString("#00.00") + "\t";
y1s += yf1[i].ToString("#00.00") + "\t";
x2s += xf2[i].ToString("#00.00") + "\t";
y2s += yf2[i].ToString("#00.00") + "\t";
}
System.Console.WriteLine("Filter 1 Estimates");
System.Console.WriteLine(x1s);
System.Console.WriteLine(y1s);
System.Console.WriteLine("Filter 2 Estimates");
System.Console.WriteLine(x2s);
System.Console.WriteLine(y2s);
return(withinTolerance);
}
/// <summary>
/// Creates an Information Filter from a given Kalman Filter.
/// </summary>
/// <param name="kf">The filter used to derive the information filter.</param>
public InformationFilter(IKalmanFilter kf)
{
J = kf.Cov.Inverse();
y = J*kf.State;
I = Matrix<double>.Build.DenseIdentity(y.RowCount, y.RowCount);
}
public OneDimensionTracker(IKalmanFilter filter)
{
this.filter = filter;
this.F = Matrix<double>.Build.Dense(2, 2, new[] { 1d, 0d, -1d, 1d });
this.Q = Matrix<double>.Build.Dense(1, 1);
this.G = Matrix<double>.Build.Dense(2, 1);
this.H = Matrix<double>.Build.Dense(1, 2, new[] { 1d, 0d });
this.R = Matrix<double>.Build.Dense(1, 1);
}
/// <summary>
/// Checks the given update parameters are of the correct dimension.
/// </summary>
/// <param name="z">Measurement matrix.</param>
/// <param name="H">Measurement sensitivity matrix.</param>
/// <param name="R">Measurement covariance matrix.</param>
/// <param name="filter">Filter being updated.</param>
/// <exception cref="System.ArgumentException">Thrown when:
/// <list type="bullet"><item>z is not a column vector.</item>
/// <item>H does not have same number of rows as z and columns as R.</item>
/// <item>R is non square.</item></list></exception>
public static void CheckUpdateParameters(Matrix <double> z, Matrix <double> H, Matrix <double> R, IKalmanFilter filter)
{
if (z.ColumnCount != 1)
{
throw new ArgumentException("KFMeasurementVectorMalformed", "z");
}
if ((H.RowCount != z.RowCount) || (H.ColumnCount != filter.State.RowCount))
{
throw new ArgumentException("KFMeasureSensitivityMalformed", "H");
}
if ((R.ColumnCount != R.RowCount) || (R.ColumnCount != z.RowCount))
{
throw new ArgumentException("KFMeasureCovarainceMalformed", "R");
}
}
/// <summary>
/// Checks that the given matrices for prediction are the correct dimensions.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <param name="G">Noise coupling matrix.</param>
/// <param name="Q">Noise process covariance.</param>
/// <param name="filter">Filter being predicted.</param>
/// <exception cref="System.ArgumentException">Thrown when:
/// <list type="bullet"><item>F is non-square with same rows/cols as state
/// the number of state variables.</item>
/// <item>G does not have same number of columns as number of state variables
/// and rows as Q.</item>
/// <item>Q is non-square.</item>
/// </list></exception>
public static void CheckPredictParameters(Matrix<double> F, Matrix<double> G, Matrix<double> Q, IKalmanFilter filter)
{
// State transition should be n-by-n matrix (n is number of state variables)
if ((F.ColumnCount != F.RowCount) || (F.ColumnCount != filter.State.RowCount))
{
throw new ArgumentException(Resources.KFStateTransitionMalformed, "F");
}
// Noise coupling should be n-by-p (p is rows in Q)
if ((G.RowCount != filter.State.RowCount) || (G.ColumnCount != Q.RowCount))
{
throw new ArgumentException(Resources.KFNoiseCouplingMalformed, "G");
}
// Noise covariance should be p-by-p
if (Q.ColumnCount != Q.RowCount)
{
throw new ArgumentException(Resources.KFNoiseCovarianceMalformed, "Q");
}
}
/// <summary>
/// Checks the state transition matrix is the correct dimension.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <param name="filter">Filter being predicted.</param>
/// <exception cref="System.ArgumentException">Thrown when the transition
/// matrix is non-square or does not have the same number of rows/cols as there
/// are state variables in the given filter.</exception>
public static void CheckPredictParameters(Matrix<double> F, IKalmanFilter filter)
{
// State transition should be n-by-n matrix (n is number of state variables)
if ((F.ColumnCount != F.RowCount) || (F.ColumnCount != filter.State.RowCount))
{
throw new ArgumentException(Resources.KFStateTransitionMalformed, "F");
}
}
public OneDimensionTracker(IKalmanFilter filter)
{
this.filter = filter;
this.F = new Matrix(2,2);
this.F[0,0] = 1d;
this.F[1,0] = 0d;
this.F[0,1] = -1d;
this.F[1,1] = 1d;
this.Q = new Matrix(1,1);
this.G = new Matrix(2,1);
this.H = new Matrix(1,2);
this.H[0,0] = 1d;
this.H[0,1] = 0d;
this.R = new Matrix(1,1);
}
public RangeBearingTracker(IKalmanFilter kf)
{
this.kf = kf;
}
/// <summary>
/// Checks the given update parameters are of the correct dimension.
/// </summary>
/// <param name="z">Measurement matrix.</param>
/// <param name="H">Measurement sensitivity matrix.</param>
/// <param name="R">Measurement covariance matrix.</param>
/// <param name="filter">Filter being updated.</param>
/// <exception cref="System.ArgumentException">Thrown when:
/// <list type="bullet"><item>z is not a column vector.</item>
/// <item>H does not have same number of rows as z and columns as R.</item>
/// <item>R is non square.</item></list></exception>
public static void CheckUpdateParameters(Matrix<double> z, Matrix<double> H, Matrix<double> R, IKalmanFilter filter)
{
if (z.ColumnCount != 1)
{
throw new ArgumentException(Resources.KFMeasurementVectorMalformed, "z");
}
if ((H.RowCount != z.RowCount) || (H.ColumnCount != filter.State.RowCount))
{
throw new ArgumentException(Resources.KFMeasureSensitivityMalformed, "H");
}
if ((R.ColumnCount != R.RowCount) || (R.ColumnCount != z.RowCount))
{
throw new ArgumentException(Resources.KFMeasureCovarainceMalformed, "R");
}
}
private bool RunTest(IKalmanFilter filter1, IKalmanFilter filter2, double tolerance)
{
List<double> xf1 = new List<double>();
List<double> yf1 = new List<double>();
List<double> xf2 = new List<double>();
List<double> yf2 = new List<double>();
Matrix ZeroCov = new Matrix(filter1.Cov.RowCount, filter1.Cov.RowCount);
Matrix ZeroState = new Matrix(filter1.State.RowCount,1);
RangeBearingTracker rbt1 = new RangeBearingTracker(filter1);
RangeBearingTracker rbt2 = new RangeBearingTracker(filter2);
bool withinTolerance = true;
// Predict the filters
rbt1.Predict(this.T, this.q);
rbt2.Predict(this.T, this.q);
for (int i = 2; i < this.bM.Length; i++)
{
rbt1.Update(this.rM[i], this.bM[i], this.re, this.the);
rbt2.Update(this.rM[i], this.bM[i], this.re, this.the);
xf1.Add(rbt1.State[0,0]);
yf1.Add(rbt1.State[2,0]);
xf2.Add(rbt2.State[0,0]);
yf2.Add(rbt2.State[2,0]);
if (!Matrix.AlmostEqual(ZeroCov, (rbt2.Cov - rbt1.Cov), tolerance))
withinTolerance = false;
else if (!Matrix.AlmostEqual(ZeroState, (rbt2.State - rbt1.State), tolerance))
withinTolerance = false;
rbt1.Predict(this.T, this.q);
rbt2.Predict(this.T, this.q);
}
// Create strings
string x1s = ""; string y1s = ""; string x2s = ""; string y2s = "";
for (int i=0; i < xf1.Count; i++)
{
x1s += xf1[i].ToString("#00.00") + "\t";
y1s += yf1[i].ToString("#00.00") + "\t";
x2s += xf2[i].ToString("#00.00") + "\t";
y2s += yf2[i].ToString("#00.00") + "\t";
}
System.Console.WriteLine("Filter 1 Estimates");
System.Console.WriteLine(x1s);
System.Console.WriteLine(y1s);
System.Console.WriteLine("Filter 2 Estimates");
System.Console.WriteLine(x2s);
System.Console.WriteLine(y2s);
return withinTolerance;
}
/// <summary>
/// Creates an Information Filter from a given Kalman Filter.
/// </summary>
/// <param name="kf">The filter used to derive the information filter.</param>
public InformationFilter(IKalmanFilter kf)
{
J = kf.Cov.Inverse();
y = J * kf.State;
I = Matrix <double> .Build.DenseIdentity(y.RowCount, y.RowCount);
}
public RangeBearingTracker(IKalmanFilter kf)
{
this.kf = kf;
}
/// <summary>
/// Checks that the given prediction matrices are the correct dimension.
/// </summary>
/// <param name="F">State transition matrix.</param>
/// <param name="Q">Noise covariance matrix.</param>
/// <param name="filter">Filter being predicted.</param>
/// <exception cref="System.ArgumentException">Thrown when either transition
/// or process noise matrices are non-square and/or have a number of rows/cols not
/// equal to the number of state variables for the filter.</exception>
public static void CheckPredictParameters(
Matrix F,
Matrix Q,
IKalmanFilter filter
)
{
// State transition should be n-by-n matrix (n is number of state variables)
if((F.ColumnCount != F.RowCount) || (F.ColumnCount != filter.State.RowCount))
throw new ArgumentException(Resources.KFStateTransitionMalformed, "F");
if((Q.ColumnCount != Q.RowCount) || (Q.ColumnCount != filter.State.RowCount))
throw new ArgumentException(Resources.KFSquareNoiseCouplingMalformed, "Q");
}
