private TrackingState GetJointTrackingState(EigenWrapper.Matrix covariance, DateTime lastTrackedTime) { double logNorm = Math.Log(covariance.Norm()); TrackingState state = TrackingState.NotTracked; TimeSpan trackingAge = DateTime.UtcNow - lastTrackedTime; //For debugging only //System.Diagnostics.Debug.WriteLine("Joint has age {0} ms with norm {1}", trackingAge.TotalMilliseconds, logNorm); //TODO: Test to see if these thresholds need to be modified if (trackingAge.TotalMilliseconds > 1000) { //If the tracking age is old, but it still has a decent prediction, the joint is inferred if (logNorm < 2) { state = TrackingState.Inferred; } } else { //If the tracking age is new, determine if it is tracked, inferred, or not tracked (default) based on how good the prediction is if (logNorm < 0.75) { state = TrackingState.Tracked; } else if (logNorm < 2) { state = TrackingState.Inferred; } } return(state); }
static void TestMatrixNormCorrectness() { double[,] A = { { 51, 634, 70 }, { 2, 57, 8 }, { 53, 63, 91 } }; //Det = 0 EigenWrapper.Matrix AMat = new EigenWrapper.Matrix(3, 3); //Set the values for the matrix validation tests AMat[0, 0] = 51; AMat[0, 1] = 2; AMat[0, 2] = 53; AMat[1, 0] = 634; AMat[1, 1] = 57; AMat[1, 2] = 63; AMat[2, 0] = 70; AMat[2, 1] = 8; AMat[2, 2] = 91; //Test the C# matrix norm Console.WriteLine("C# matrix norm correctness test:"); Console.WriteLine("The test matrix A is:"); PrintMatrix(A); Console.WriteLine("The matrix norm of A is:"); double[,] Atrans = MatrixMathCS.Transpose(A); Console.WriteLine(MatrixMathCS.MatrixNorm(Atrans).ToString()); Console.WriteLine(); //Test the Eigen matrix norm Console.WriteLine("P/Invoke Eigen 2D double array matrix norm correctness test:"); Console.WriteLine("The test matrix A is:"); PrintMatrix(A); Console.WriteLine("The matrix norm of A is:"); Console.WriteLine(EigenWrapper.MatrixMath.MatrixNorm(A)); Console.WriteLine(); //Test the Matrix class norm Console.WriteLine("P/Invoke Eigen matrix class matrix norm correctness test:"); Console.WriteLine("The test matrix A is:"); PrintMatrix(AMat); Console.WriteLine("The matrix norm of A is:"); Console.WriteLine(AMat.Norm().ToString()); }