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<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); 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 + ")"); } }
public void UpDateVelocity(float[] acceleration) { float interval = sw.ElapsedMilliseconds / 100.00f; KFilter.Update(getZ(acceleration), getH(interval), getR(acceleration)); KFilter.Predict(getStateUpdateMat(interval, acceleration), getStateCov()); state = KFilter.State; updateState(); if (interval > 0) { //Use the Pythagoras for the velocity. V = Root(A*A + B*B + C*C) velocity = (float)Math.Sqrt((velocityXYZ[0] * velocityXYZ[0] + velocityXYZ[1] * velocityXYZ[1] + velocityXYZ[2] * velocityXYZ[2])); string text = "State = "; for (int i = 0; i < 9; i++) { text += string.Format("{0:f}, ", state[i, 0]); } VelText.Text = text; // VelText.Text = string.Format("STATE = {0:f}m/s", velocity); xVel.Text = string.Format("x - Velocity = {0:f}m/s", velocityXYZ[0]); yVel.Text = string.Format("y - Velocity = {0:f}m/s", velocityXYZ[1]); zVel.Text = string.Format("z - Velocity = {0:f}m/s", velocityXYZ[2]); UpdatePosition(); sw.Restart(); } }
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 + ")"); } }
/// <summary> /// This step is to update the Kalman filter postior to the location measurement /// </summary> private void UpdateKalmanEstimate() { _location.R = _location.Cov.Multiply(0.3); // 0.3 is an abitrary magic number _location.UpdateMeasurement(); dkf.Update(_location.Measured, _location.H, _location.R); _location.Current = dkf.State; _location.Cov = dkf.Cov; }
static void Main(string[] args) { double[] IndividualValues = { 0, 0, 0, 0, 0, 0 }; String[] SubStrings; float[] q = { 0, 0, 0, 0 }; double x = 0, xq, y = 0, z = 0, t2 = 0, x1, y1, z1, time, velx, vely, velz, accx, accy, accz, temp; bool correctRead; int comaCount = 0; char[] test; SerialPort port = new SerialPort("COM3", 19200); String CurrentReading = "ABCD"; Matrix initial = Matrix.Build.DenseOfArray(new double[, ] { { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 } }); Matrix covariance = Matrix.Build.DenseOfArray(new double[, ] { { 0.1, 0, 0, 0, 0, 0, 0, 0, 0 }, { 0, 0.1, 0, 0, 0, 0, 0, 0, 0 }, { 0, 0, 0.1, 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0.1, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0.1, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0.1, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0, 0.1, 0, 0 }, { 0, 0, 0, 0, 0, 0, 0, 0.1, 0 }, { 0, 0, 0, 0, 0, 0, 0, 0, 0.1 } }); DiscreteKalmanFilter kalman = new DiscreteKalmanFilter(initial, covariance); port.ReadTimeout = 2000; port.Close(); if (!port.IsOpen) { port.Open(); } while (t2 <= 20000000000) { CurrentReading = port.ReadLine(); test = CurrentReading.ToCharArray(); correctRead = true; comaCount = 0; for (int i = test.Length - 1; i >= 0; i--) { if (Char.IsNumber(test[i]) == false & test[i] != ',' & test[i] != '.' & Char.IsWhiteSpace(test[i]) == false & test[i] != '-') { correctRead = false; } if (test[i] == ',') { comaCount = comaCount + 1; } } if (correctRead == true & comaCount == 9) { SubStrings = CurrentReading.Split(','); q[0] = Convert.ToSingle(SubStrings[0]); q[1] = Convert.ToSingle(SubStrings[1]); q[2] = Convert.ToSingle(SubStrings[2]); q[3] = Convert.ToSingle(SubStrings[3]); Quaternion quaternion = new Quaternion(q[0], q[1], q[2], q[3]); x1 = Convert.ToDouble(SubStrings[4]); y1 = Convert.ToDouble(SubStrings[5]); z1 = Convert.ToDouble(SubStrings[6]); temp = Convert.ToDouble(SubStrings[7]); time = Convert.ToSingle(SubStrings[8]); velx = (x1 - x) / (time - t2); vely = (y1 - y) / (time - t2); velz = (z1 - z) / (time - t2); accx = velx / (time - t2); accy = vely / (time - t2); accz = velz / (time - t2); //Dummy Data : Generate a very noisy position estimate Matrix Meassurement = Matrix.Build.DenseOfArray(new double[, ] { { 0.1 }, { 0.1 }, { 0.1 }, { 0.1 }, { 0.1 }, { 0.1 }, { 0.1 }, { 0.1 }, { 0.1 } }); Matrix Noise = Matrix.Build.DenseOfArray(new double[, ] { { 50.94324709, 0, 0, 0, 0, 0, 0, 0, 0 }, { 0, 345.3342043, 0, 0, 0, 0, 0, 0, 0 }, { 0, 0, 184.5258328, 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0.000105439, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0.000107133, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0.000586316, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0, 5.7569E-06, 0, 0 }, { 0, 0, 0, 0, 0, 0, 0, 1.20007E-05, 0 }, { 0, 0, 0, 0, 0, 0, 0, 0, 3.71021E-052 } }); //veryfie Matrix startetrans = Matrix.Build.DenseOfArray(new double[, ] { { x1, 0, 0, 0, 0, 0, 0, 0, 0 }, { 0, y1, 0, 0, 0, 0, 0, 0, 0 }, { 0, 0, z1, 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, velx, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, vely, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, velz, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0, accx, 0, 0 }, { 0, 0, 0, 0, 0, 0, 0, accy, 0 }, { 0, 0, 0, 0, 0, 0, 0, 0, accz } }); kalman.Predict(startetrans, kalman.Cov); kalman.Update(Meassurement, Noise, covariance); x1 += kalman.State.At(0, 0); y1 += kalman.State.At(1, 0); z1 += kalman.State.At(2, 0); Console.WriteLine("x: " + quaternion.ToEulerAngles().Alpha.Degrees + ", y: " + quaternion.ToEulerAngles().Beta.Degrees + ", z: " + z1 + ", temperature: " + temp + ", time: " + t2); t2 = time; x = x1; y = y1; z = z1; } } }
public static Tuple <Matrix <double>, Matrix <double> > UpdateState(this DiscreteKalmanFilter dFilterSharp, Matrix <double> z, Matrix <double> H, Matrix <double> R) { dFilterSharp.Update(z, H, R); return(Tuple.Create(dFilterSharp.State, dFilterSharp.Cov)); }