Inertial Navigation for PX4 and Pixhawk

This is an implementation and further development of the Extended Kalman Filter  algorithm presented in  https://github.com/priseborough/InertialNav. The filter currently utilises ~10% of the PX4/Pixhawk processing power.

It has had some flight testing on PX4 installed in a Skywalker X-8 and a Pixhawk installed in a X-5 airframe with good results. Link to data from Pixhawk test in an X-5 airframe below:

https://drive.google.com/file/d/0B6YbG6fDf0m4X01Vd0RrWEpBM00/edit?usp=sharing

It has also had very limited flight testing on an Iris quadcopter running a Pixhawk. This testing has indicated that the tuning of parameters for best copter performance will likely be different from plane.

There are 22 filter states in the version currently implemented:

4 x quaternions
3 x NED velocities
3 x NED positions
3 x gyro delta angle bias
1 x Z accel delta velocity bias
2 x NE wind velocity
3 x earth magnetic field vector
3 x body magnetic field vector

Short term development needs are:

- Add correction for AHRS_TRIM_X and Y to outputs
- Tuning parameters need to be brought out to be adjustable via MAVlink
- The test harness nee4s to be updated to include arming events and airspeed measurements

Planned future developments include:

- Addition of optical flow measurements from a PX4FLOW sensor.
- Support for redundant sensors with fault detection
- Further numerical optimisation