public Matrix<float> FilterPoints(Kalman kalman, float x, float y, float z)
{
Matrix<float> prediction = kalman.Predict();
Matrix<float> estimated = kalman.Correct(new Matrix<float>(new[] { x, y, z }));
Matrix<float> results = new Matrix<float>(new[,]
{
{ prediction[0, 0], prediction[1, 0], prediction[2, 0] },
{ estimated[0, 0], estimated[1, 0], estimated[2, 0] }
});
return results;
}