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));
}