Abstracted Kalman filter logic

I decided that since the code I already wrote wasn’t working, why not just try again? In all seriousness there was a few reasons for the rewrite, that other parts of the program might need their own kalman filter and it would be easier to understand and maintain but mostly I was sick of the code not working and not knowing why. So I found the basic equations for the extended kalman filter and went to implement that. At first I was using a struct of function pointers to get the input for the ekf (extended kalman filter) but was able to rewrite it to input static matrices instead which was way less clunky.