ITS ALMOST WORKING! So much better that before.

After I wrote the code to abstract the kalman filter logic, there was still the rest of the algorithm to reimplement. I copied over what I could and for some of the functions I changed the way they worked. The guide I am following went through most of the calculations with the general matrix math and then at the end gave the full calculations to get the final matrix with single numbers. Following the spirit of my rewrite I decided to instead use the matrix math where I could. After this I hoked it up to my graphing program and it almost sorta kinda works! I have no idea what changed that makes it work but I’m not complaining.