Fixed memory leaks (mostly)

This is a bit of a tangent from running on my rp2040 but it’s important. When I did run it on my board it threw an out of memory panic so I began to look for ways to fix that. I found valgrind which is a tool to check for memory leaks in c/c++ programs which was exactly what I need. Running it revealed that my test suite alone ate up ~600 kilobytes of extra memory which is very not good. I began to go through all the errors valgrind gave me and finding ways to free memory. I created a few functions to help with this, mainly for the core functions such as add/sub_matrix and mul_matrix, that run the base function but then free the first argument to make the code a little cleaner. In the end I was able to solve all but two memory leaks in the kinetic code. One I think is a false positive where the error is when the kalman filter state is updated. There were a few more in the simulation code. I tried to fix some but gave up since it doesn’t really mater if that leaks memory.