INS cleanups

This cleans up the INS code a bit based on some comments Kenn made

1. removes usage of global variables
2. makes variables that can and should be static
3. creates a GetState and GetVariance (which is really diagonal of covariance) method
4. updates attitude accordingly to use this cleaned up API