Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Implements Wrapping feature and associated test. #27

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Implements Wrapping feature and associated test. #27

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
53 changes: 53 additions & 0 deletions src/Kalman.Tests/KalmanFilterTest.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,59 @@ public void TestDiscreteKalmanFilter()
}
}

[Test]
public void TestDiscreteKalmanFilterWrap()
{
// 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 = { 0, (Math.PI + 0.1), 2*(Math.PI + 0.1), 3*(Math.PI + 0.1), 4*(Math.PI + 0.1), 5*(Math.PI + 0.1),6*(Math.PI + 0.1)};
// Expected position estimates (predictions)
double[] posp = {0.2, 0.3-Math.PI, 0.4, 0.5-Math.PI, 0.6};
// Expected velocity estimates (predictions)
double[] velp = { (Math.PI + 0.1)/T, (Math.PI + 0.1)/T, (Math.PI + 0.1)/T, (Math.PI + 0.1)/T, (Math.PI + 0.1)/T };
// Expected position estimates (after measurement update)
double[] posu = { 0.2, 0.3-Math.PI, 0.4, 0.5-Math.PI, 0.6 };
// Expected velocity estimates (after measurement update)
double[] velu = { (Math.PI + 0.1)/T, (Math.PI + 0.1)/T, (Math.PI + 0.1)/T, (Math.PI + 0.1)/T, (Math.PI + 0.1)/T };

// Initial estimate based on two point differencing
double z0 = zs[0];
double z1 = zs[1];
Matrix<double> x0 = Matrix<double>.Build.Dense(2, 1, new[] { z1, (z1 - z0)/T });
Matrix<double> P0 = Matrix<double>.Build.Dense(2, 2, new[] { r, r/T, r/T, 2*r/(T*T) });
// Setup a DiscreteKalmanFilter to filter
DiscreteKalmanFilter dkf = new DiscreteKalmanFilter(x0, P0, new int[] {0}, new int[] {0});
Matrix<double> F = Matrix<double>.Build.Dense(2, 2, new[] { 1d, 0d, T, 1 }); // State transition matrix
Matrix<double> G = Matrix<double>.Build.Dense(2, 1, new[] { (T * T) / 2d, T }); // Plant noise matrix
Matrix<double> Q = Matrix<double>.Build.Dense(1, 1, new[] { q }); // Plant noise variance
Matrix<double> R = Matrix<double>.Build.Dense(1, 1, new[] { r }); // Measurement variance matrix
Matrix<double> H = Matrix<double>.Build.Dense(1, 2, new[] { 1d, 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(dkf.State[0,0].AlmostEqual(posp[i-2], tol), "State Prediction (" + i + ")");
Assert.IsTrue(dkf.State[1, 0].AlmostEqual(velp[i-2], tol), "Covariance Prediction (" + i + ")");
// Perform the update
Matrix<double> z = Matrix<double>.Build.Dense(1, 1, new[] { zs[i] });
dkf.Update(z, H, R);
// Test against the update state/covariance
// Test against the prediction state/covariance
Assert.IsTrue(dkf.State[0, 0].AlmostEqual(posu[i-2], tol), "State Prediction (" + i + ")");
Assert.IsTrue(dkf.State[1, 0].AlmostEqual(velu[i-2], tol), "Covariance Prediction (" + i + ")");
}
}

[Test]
public void TestInformationFilter()
{
Expand Down
49 changes: 45 additions & 4 deletions src/Kalman/DiscreteKalmanFilter.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#endregion

using MathNet.Numerics.LinearAlgebra;
using System;

namespace MathNet.Filtering.Kalman
{
Expand Down Expand Up @@ -71,6 +72,16 @@ public Matrix<double> State
/// The current covariance of the estimated state of the system.
/// </summary>
protected Matrix<double> P;

/// <summary>
/// Array of index of states that need to be wrapped between +/- Pi
/// </summary>
protected int[] wrapStateIndex = new int[0];

/// <summary>
/// Array of index of measurements that need to be wrapped between +/- Pi
/// </summary>
protected int[] wrapMeasurementIndex = new int[0];

/// <summary>
/// Creates a new Discrete Time Kalman Filter with the given values for
Expand All @@ -87,6 +98,36 @@ public DiscreteKalmanFilter(Matrix<double> x0, Matrix<double> P0)
P = P0;
}

/// <summary>
/// Creates a new Discrete Time Kalman Filter with the given values for
/// the initial state and the covariance of that state.
/// </summary>
/// <param name="x0">The best estimate of the initial state of the estimate.</param>
/// <param name="P0">The covariance of the initial state estimate. If unsure
/// about initial state, set to a large value</param>
/// <param name="wrapStateIndex">Array of index of states that need to be wrapped
/// between +/- Pi</param>
/// <param name="wrapMeasurementIndex">Array of index of measurements that need to be wrapped
/// between +/- Pi</param>
public DiscreteKalmanFilter(Matrix<double> x0, Matrix<double> P0, int[] wrapStateIndex, int[] wrapMeasurementIndex) : this (x0, P0)
{
this.wrapStateIndex = wrapStateIndex;
this.wrapMeasurementIndex = wrapMeasurementIndex;
}

/// <summary>
/// Wrap input vector along Index in wrapIndex array
/// </summary>
protected Matrix<double> Wrap(Matrix<double> input, int[] wrapIndex)
{
var output = input;
foreach(int i in wrapIndex)
{
output[i,0] = ((input[i,0] + Math.PI) % (2*Math.PI)) - Math.PI;
}
return output;
}

/// <summary>
/// Perform a discrete time prediction of the system state.
/// </summary>
Expand All @@ -98,7 +139,7 @@ public void Predict(Matrix<double> F)
{
KalmanFilter.CheckPredictParameters(F, this);

x = F*x;
x = Wrap(F*x, wrapStateIndex);
P = F*P*F.Transpose();
}

Expand All @@ -119,7 +160,7 @@ public void Predict(Matrix<double> F, Matrix<double> Q)
KalmanFilter.CheckPredictParameters(F, Q, this);

// Predict the state
x = F*x;
x = Wrap(F*x, wrapStateIndex);
P = (F*P*F.Transpose()) + Q;
}

Expand All @@ -142,7 +183,7 @@ public void Predict(Matrix<double> F, Matrix<double> G, Matrix<double> Q)
KalmanFilter.CheckPredictParameters(F, G, Q, this);

// State prediction
x = F*x;
x = Wrap(F*x, wrapStateIndex);

// Covariance update
P = (F*P*F.Transpose()) + (G*Q*G.Transpose());
Expand All @@ -169,7 +210,7 @@ public void Update(Matrix<double> z, Matrix<double> H, Matrix<double> R)
Matrix<double> S = (H*P*Ht) + R; // Measurement covariance
Matrix<double> K = P*Ht*S.Inverse(); // Kalman Gain
P = (I - (K*H))*P; // Covariance update
x = x + (K*(z - (H*x))); // State update
x = Wrap(x + (K*Wrap(z - (H*x), wrapMeasurementIndex)), wrapStateIndex); // State update
}
}
}