From a08efce2eafd1e687e5a2b714d6f03f6e682bc85 Mon Sep 17 00:00:00 2001 From: masavoyat Date: Wed, 4 May 2022 09:16:17 +0200 Subject: [PATCH] Implements Wrapping feature and associated test. --- src/Kalman.Tests/KalmanFilterTest.cs | 53 ++++++++++++++++++++++++++++ src/Kalman/DiscreteKalmanFilter.cs | 49 ++++++++++++++++++++++--- 2 files changed, 98 insertions(+), 4 deletions(-) diff --git a/src/Kalman.Tests/KalmanFilterTest.cs b/src/Kalman.Tests/KalmanFilterTest.cs index 7ff293ee..7480f634 100644 --- a/src/Kalman.Tests/KalmanFilterTest.cs +++ b/src/Kalman.Tests/KalmanFilterTest.cs @@ -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 x0 = Matrix.Build.Dense(2, 1, new[] { z1, (z1 - z0)/T }); + Matrix P0 = Matrix.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 F = Matrix.Build.Dense(2, 2, new[] { 1d, 0d, T, 1 }); // State transition matrix + Matrix G = Matrix.Build.Dense(2, 1, new[] { (T * T) / 2d, T }); // Plant noise matrix + Matrix Q = Matrix.Build.Dense(1, 1, new[] { q }); // Plant noise variance + Matrix R = Matrix.Build.Dense(1, 1, new[] { r }); // Measurement variance matrix + Matrix H = Matrix.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 z = Matrix.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() { diff --git a/src/Kalman/DiscreteKalmanFilter.cs b/src/Kalman/DiscreteKalmanFilter.cs index 9ab9d00d..3f1908fb 100644 --- a/src/Kalman/DiscreteKalmanFilter.cs +++ b/src/Kalman/DiscreteKalmanFilter.cs @@ -20,6 +20,7 @@ #endregion using MathNet.Numerics.LinearAlgebra; +using System; namespace MathNet.Filtering.Kalman { @@ -71,6 +72,16 @@ public Matrix State /// The current covariance of the estimated state of the system. /// protected Matrix P; + + /// + /// Array of index of states that need to be wrapped between +/- Pi + /// + protected int[] wrapStateIndex = new int[0]; + + /// + /// Array of index of measurements that need to be wrapped between +/- Pi + /// + protected int[] wrapMeasurementIndex = new int[0]; /// /// Creates a new Discrete Time Kalman Filter with the given values for @@ -87,6 +98,36 @@ public DiscreteKalmanFilter(Matrix x0, Matrix P0) P = P0; } + /// + /// Creates a new Discrete Time Kalman Filter with the given values for + /// the initial state and the covariance of that state. + /// + /// The best estimate of the initial state of the estimate. + /// The covariance of the initial state estimate. If unsure + /// about initial state, set to a large value + /// Array of index of states that need to be wrapped + /// between +/- Pi + /// Array of index of measurements that need to be wrapped + /// between +/- Pi + public DiscreteKalmanFilter(Matrix x0, Matrix P0, int[] wrapStateIndex, int[] wrapMeasurementIndex) : this (x0, P0) + { + this.wrapStateIndex = wrapStateIndex; + this.wrapMeasurementIndex = wrapMeasurementIndex; + } + + /// + /// Wrap input vector along Index in wrapIndex array + /// + protected Matrix Wrap(Matrix 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; + } + /// /// Perform a discrete time prediction of the system state. /// @@ -98,7 +139,7 @@ public void Predict(Matrix F) { KalmanFilter.CheckPredictParameters(F, this); - x = F*x; + x = Wrap(F*x, wrapStateIndex); P = F*P*F.Transpose(); } @@ -119,7 +160,7 @@ public void Predict(Matrix F, Matrix Q) KalmanFilter.CheckPredictParameters(F, Q, this); // Predict the state - x = F*x; + x = Wrap(F*x, wrapStateIndex); P = (F*P*F.Transpose()) + Q; } @@ -142,7 +183,7 @@ public void Predict(Matrix F, Matrix G, Matrix 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()); @@ -169,7 +210,7 @@ public void Update(Matrix z, Matrix H, Matrix R) Matrix S = (H*P*Ht) + R; // Measurement covariance Matrix 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 } } }