Added Kalman Filter and Math libraries

[DrewBrandt]

Added the Linear Kalman Filter folder from the KF repo to this repo.

Also added the Matrix files and imported Adafruit's "imumaths" library into our namespace.

[mallamacimj]

Also, lets unit test the math library. That is very low hanging fruit, plus I think it is already written somewhere.

[DrewBrandt]

Also, lets unit test the math library. That is very low hanging fruit, plus I think it is already written somewhere.

I'll look into this. Should I bother with the Vector and Quat files? they are taken very nearly verbatim from imumaths.

[DrewBrandt]

Inverse fails testing, but I don't know if I am testing improperly:

Matrix m1(2, 2, new double[4]{1, 2, 3, 4});
void test_inverse()
{
    Matrix m4 = m1.inv();
    TEST_ASSERT_EQUAL(2, m4.getRows());
    TEST_ASSERT_EQUAL(2, m4.getCols());
    TEST_ASSERT_EQUAL(-2, m4.getArr()[0]);
    TEST_ASSERT_EQUAL(1, m4.getArr()[1]);
    TEST_ASSERT_EQUAL(1.5, m4.getArr()[2]);
    TEST_ASSERT_EQUAL(-0.5, m4.getArr()[3]);
}

I think it should be	M1			M2
1	2	-2	1
3	4	1.5	-.05

But I got the inverse from an online calculator and definitely don't remember how to take an inverse to check it.

test_inverse: Expected -2 Was -1       [FAILED]

[mallamacimj]

Also, lets unit test the math library. That is very low hanging fruit, plus I think it is already written somewhere.

I'll look into this. Should I bother with the Vector and Quat files? they are taken very nearly verbatim from imumaths.

Nah. Just the matrix library (since we wrote it). You can also just look at them and see they are correct for the most part.

[DrewBrandt]

Ok Matrix passes unit tests. It was a bad test checking "equals" on floating point numbers. Changed test to be "within 10^-6" and passes.

Remaining questions:

Can someone take a look at the values I chose for the "initialize" function in KalmanInterface.cpp? (pv and qv and other magic numbers) I don't remember what each value does or how I chose them, but I suspect there are better tuned values available. Also, should those values be passed to initialize via a parameter?

Is KalmanInterface a good name? That's what it is, but it's also not strictly an interface in the CS sense, as it has an implementation by default.

These are the tests I ran on Matrix:

    RUN_TEST(test_copy_constructor);
    RUN_TEST(test_assignment_operator);
    RUN_TEST(test_multiply_operator_matrix);
    RUN_TEST(test_multiply_operator_scalar);
    RUN_TEST(test_add_operator_matrix);
    RUN_TEST(test_subtract_operator_matrix);
    RUN_TEST(test_transpose);
    RUN_TEST(test_inverse_small); //2x2
    RUN_TEST(test_inverse_large); //4x4

Are there any other functions that should be tested?

[DrewBrandt]

I also don't think its methods should be virtual if they have a default implementation, it would just be a regular method and anyone who wants a custom implementation is responsible for overriding the functions in a child class.

@varun-un I believe having virtual here is correct. It indicates that the method can be overridden instead of shadowed, which means that the derived method can be called using a reference to the parent object (i.e. if you call KalmanInterface.func() but you call it on a derived type of KalmanInterface with an overridden func(), then the overridden version will be called. IDK if that makes sense...) At least that's how I understand it.

Otherwise I agree with everything you said. It is currently quite awkward and should be changed in the way you said.

[varun-un]

Merging with KF logic in the State class