Found bug in simulation, now results are a lot better

After a little debugging I noticed a bug in the function in the simulation that calculates the rate of change between orientations used to simulate gyro measurements. After fixing that the kalman filter works almost perfectly from the graph!