Fixed Initial State Bug

As soon as I plugged the kalman filter to the new sin wave tests I noticed that the output was way off. I put off fixing it until first fixing the error function first. To find the bug I first tried asking my ai and it halucinated something that had nothing to do with the problem. I started to look for the bug myself and noticed that for the initial state the y axis had the wrong sign. When it should have had about 57 degrees it had -57 ish. So I went to the code that calculates that and couldn’t find an obvious bug. Since I couldn’t fix the existing code I replaced it with a different method to do the same thing. The old code calculated a rotation matrix with the accelerometer and magnetometer and the new code calculates the euler angles. I’m hoping that because it starts with a static state I won’t run into gimble lock but that’s a future me problem.