public void TestDiscreteKalmanFilter(double r, double T, double q, int set)
{
if (set == 1) //Stable value
{
double[] zs1 = { 555, 561, 554, 563, 555, 558, 559, 557, 557, 553, 558, 564, 556, 559, 559, 556, 556, 557, 554, 564, 556, 555, 564, 563, 559, 553, 560, 556, 559, 556, 556, 559, 557, 561, 559, 560, 562, 556, 557, 556, 557, 557, 565, 558, 558, 554, 554, 558, 557, 559, 554 };
zs = zs1;
}
if (set == 2) //Unstable value 774
{
double[] zs2 = { 585, 581, 587, 588, 605, 631, 663, 679, 707, 724, 735, 752, 757, 774, 776, 784, 789, 796, 814, 810, 811, 810, 810, 809, 813, 808, 814, 810, 809, 810, 808, 811, 812, 808, 812, 808, 769, 704, 671, 616, 587, 583, 577, 565, 558, 558, 554, 554, 558, 557, 559, 554, 558, 558, 554, 554, 558, 557, 559, 554 };
zs = zs2;
}
if (set == 3) //Unstable value 774
{
double[] zs3 = { 571, 574, 574, 571, 579, 581, 571, 572, 574, 578, 578, 579, 577, 577, 581, 575, 579, 584, 561, 554, 550, 548, 536, 525, 514, 517, 507, 490, 486, 483, 472, 460, 451, 446, 437, 419, 421, 395, 396, 396, 342, 387, 380, 372, 366, 353, 345, 335, 335, 327, 328, 324, 329, 327, 324, 329, 324, 323, 323, 321, 329, 316, 325, 320, 328, 333, 335, 345, 348, 356, 367, 368, 380, 388, 394, 407, 423, 447, 479, 493, 503, 516, 526, 542, 548, 563, 576, 583, 579, 583, 576, 573, 584, 584, 578, 584, 579, 585, 575, 580, 591, 599, 603, 609, 614, 631, 634, 643, 661, 669, 684, 693, 697, 710, 716, 733, 755, 757, 775, 786, 792, 799, 803, 803, 811, 808, 809, 810, 813, 806, 809, 807, 808, 807, 815, 810, 808, 799, 799, 795, 780, 767, 754, 758, 743, 732, 716, 705, 694, 680, 661, 649, 627, 611, 598, 583, 577, 578, 574, 574, 575, 573, 573, 577, 577, 579, 575, 574, 576, 575, 577, 572, 574, 577, 571, 570, 571, 571, 575 };
zs = zs3;
}
if (set == 4)
{
double[] zs4 = { 573, 577, 577, 574, 574, 575, 578, 575, 572, 576, 576, 574, 575, 575, 578, 573, 575, 574, 581, 581, 618, 724, 752, 791, 810, 812, 810, 812, 813, 800, 781, 675, 578, 577, 581, 579, 578, 575, 579, 576, 470, 422, 363, 358, 339, 337, 331, 326, 331, 334, 340, 348, 480, 573, 574, 575, 574, 572, 576, 576 };
zs = zs4;
}
// Test constants
//double r = 30; // Measurement covariance 30.0
//double T = 5; // Time interval between measurements 20.0
//double q = 0.1; // Plant noise constant 0.1
double tol = 0.0001; // Accuracy tolerance 0.0001
// Reference values to test against (generated from a known filter
// Reference Measurements
//double[] zs = { 290.16851039,654.55633793,968.97141280,1325.09197161,1636.35947675,1974.39053148,2260.80770553,2574.36119750,2901.32285462,3259.14709098};
// Expected position estimates (predictions)
double[] posp = {1018.94416547,1237.00029618,1754.97092716,1855.62596430,2400.27521403,2446.47067625,2978.94381631,3173.63724675};
// Expected velocity estimates (predictions)
double[] velp = { 18.21939138,13.38351136,21.52280841,10.92729947,21.32868461,9.24370334,20.26482836,13.59419761 };
// Expected position estimates (after measurement update)
double[] posu = { 969.33006892,1324.51475894,1637.07997492,1973.70152187,2261.59660945,2573.64724909,2901.75329465,3258.67447647 };
// Expected velocity estimates (after measurement update)
double[] velu = { 13.38351136,21.52280841,10.92729947,21.32868461,9.24370334,20.26482836,13.59419761,20.93270702 };
// Initial estimate based on two point differencing
double z0 = zs[0];
double z1 = zs[1];
Matrix x0 = new Matrix(2,1);
x0[0,0] = z1;
x0[1,0] = (z1 - z0)/T;
Matrix P0 = new Matrix(2,2);
P0[0,0] = r;
P0[1,0] = r/T;
P0[0,1] = r/T;
P0[1,1] = 2 * r / (T * T);
// Setup a DiscreteKalmanFilter to filter
dkf = new DiscreteKalmanFilter(x0, P0);
//SquareRootFilter dkf = new SquareRootFilter(x0, P0);
zsres[0] = zs[0];
zsres[1] = (double)(dkf.State[0, 0]);
double[] aF = { 1d, 0d, T, 1 };
double[] aG = { (T * T) / 2d, T };
F = new Matrix(aF,2); // State transition matrix
G = new Matrix(aG,2); // Plant noise matrix
Q = new Matrix(1,1); Q[0,0] = q; // Plant noise variance
R = new Matrix(1,1); R[0,0] = r; // Measurement variance matrix
H = new Matrix(1,2); H[0,0] = 1d; H[0,1] = 0d; // Measurement matrix
}
public void TestSquareRootFilter()
{
System.Console.WriteLine("Filter 1 - DiscreteKalmanFilter, Filter 2 - SquareRootFilter");
Matrix x0 = RangeBearingTracker.TwoPointDifferenceState(rM[0], rM[1], bM[0], bM[1], T);
Matrix P0 = RangeBearingTracker.TwoPointDifferenceCovariance(rM[0], rM[1], bM[0], bM[1], re, the, T);
DiscreteKalmanFilter dkf = new DiscreteKalmanFilter(x0, P0);
SquareRootFilter sqrf = new SquareRootFilter(x0, P0);
Assert.IsTrue(RunTest(dkf, sqrf, DefaultTolerance));
}
public void TestDiscreteKalmanFilter()
{
// Test constants
double r = 30.0; // Measurement covariance
double T = 20.0; // Time interval between measurements
double q = 0.1; // Plant noise constant
double tol = 0.0001; // Accuracy tolerance
// Reference values to test against (generated from a known filter)
// Reference Measurements
double[] zs = { 290.16851039,654.55633793,968.97141280,1325.09197161,1636.35947675,1974.39053148,2260.80770553,2574.36119750,2901.32285462,3259.14709098};
// Expected position estimates (predictions)
double[] posp = {1018.94416547,1237.00029618,1754.97092716,1855.62596430,2400.27521403,2446.47067625,2978.94381631,3173.63724675};
// Expected velocity estimates (predictions)
double[] velp = { 18.21939138,13.38351136,21.52280841,10.92729947,21.32868461,9.24370334,20.26482836,13.59419761 };
// Expected position estimates (after measurement update)
double[] posu = { 969.33006892,1324.51475894,1637.07997492,1973.70152187,2261.59660945,2573.64724909,2901.75329465,3258.67447647 };
// Expected velocity estimates (after measurement update)
double[] velu = { 13.38351136,21.52280841,10.92729947,21.32868461,9.24370334,20.26482836,13.59419761,20.93270702 };
// Initial estimate based on two point differencing
double z0 = zs[0];
double z1 = zs[1];
Matrix x0 = new Matrix(2,1);
x0[0,0] = z1;
x0[1,0] = (z1 - z0)/T;
Matrix P0 = new Matrix(2,2);
P0[0,0] = r; P0[1,0] = r/T; P0[0,1] = r/T; P0[1,1] = 2 * r / (T * T);
// Setup a DiscreteKalmanFilter to filter
DiscreteKalmanFilter dkf = new DiscreteKalmanFilter(x0, P0);
double[] aF = { 1d, 0d, T, 1 };
double[] aG = { (T * T) / 2d, T };
Matrix F = new Matrix(aF,2); // State transition matrix
Matrix G = new Matrix(aG,2); // Plant noise matrix
Matrix Q = new Matrix(1,1); Q[0,0] = q; // Plant noise variance
Matrix R = new Matrix(1,1); R[0,0] = r; // Measurement variance matrix
Matrix H = new Matrix(1,2); H[0,0] = 1d; H[0,1] = 0d; // Measurement matrix
// Test the performance of this filter against the stored data from a proven
// Kalman Filter of a one dimenional tracker.
for (int i = 2; i < zs.Length; i++)
{
// Perform the prediction
dkf.Predict(F, G, Q);
// Test against the prediction state/covariance
Assert.IsTrue(Number.AlmostEqual(dkf.State[0,0], posp[i-2], tol), "State Prediction (" + i + ")");
Assert.IsTrue(Number.AlmostEqual(dkf.State[1,0], velp[i-2], tol), "Covariance Prediction (" + i + ")");
// Perform the update
Matrix z = new Matrix(1,1,zs[i]);
dkf.Update(z, H, R);
// Test against the update state/covariance
// Test against the prediction state/covariance
Assert.IsTrue(Number.AlmostEqual(dkf.State[0,0], posu[i-2], tol), "State Prediction (" + i + ")");
Assert.IsTrue(Number.AlmostEqual(dkf.State[1,0], velu[i-2], tol), "Covariance Prediction (" + i + ")");
}
}
