Looks like the Kalman Filter in my simulation can correct for a bias in the gyro, but I have the R on the gyro set very high.
Adding a fairly simple start to a ground station. I have a main, which just launches two threads. One thread listens to the keyboard and accepts user commands. The user can quit by pressing q, or change the stick inputs with the keyboard. Eventually this might be upgraded to an Xbox controller interface.

The other simply displays the those commands. They have separate cycle times, but both set to 100 ms for now. Next step would be to start a thread that just grabs the latest command and upload them to aircraft, and one that recieves from the aircraft.

Maybe my Commands class should be called "GetCommands" to differentiate from "SendCommands", and then we can have a class that is "GetData" that is just running a thread. Feel free to modify this as you see fit. I couldn't get it to run nicely in Eclipse, but it builds well if you have WindowsSDK.
Apparently I wasn't noticing my errors during compile, but glad I noticed them after I committed. This should take care of them. However, I now notice that pressing "q" only kills the Commands thread, and not the Display thread, so it keeps running until it timesout (in the future, we'll get rid of the time-out of course, but need to figure out how to have all threads kill with the "q").

Or, just get rid of the thread concept and have everything run in series at one rate. I don't know why I even spent time on this, but it was interesting to me.