It's been a while since I posted an update, so here I go with a huge one.

Over the last few weeks, I have experimented with different kinds of filtering and I strayed away from Kalman filtering entirely. I have decided on using a Madgwick or Mahony Filter instead. The great news in this, through these many weeks I have worked hard and I finally got stable output using Madgwick and Mahony filtering. It took a lot of work to get it working without a single library other than the Arduino Wire library. It works great! Also, it seems to be running around 200 times a second, which I believe will be more than enough for what I need. My next step is to implement a PID control based on those 3 axes and the error that it strays away from perfect flight. Godspeed.

(I will be posting code on Github soon)