EKF3: structural improvements

[rmackay9]

This issue has some ideas from @rmackay9, @tridge and perhaps others re structural/architectural improvements we could make to the EKF3 to improve its usability, reliability and extensibility.

Some issues with EKF3's structure are:

• Class level variables are used instead of methods accepting arguments
• Repeated/Duplicate logic tests in different methods. This opens up the possibility that they could become inconsistent after enhancements are made.
• Definitions are used instead of enums
• The parameters to define the source of position, velocity and yaw are not straightforward. They are sometimes mixed with other features (i.e. EK2_MAG_CAL) or they change based on what sensor data arrives in a particular loop.
• Coding style does not conform to the ArduPilot style guide (we should not fix this)
• "read" functions have unexpected side effects. For example NavEKF3_core::readGpsData() has the side effect of also setting the origin and getting the magnetic earth field tables (which hold expected compass vector lengths). This means that non-GPS navigation is at a disadvantage because it will not have access to the earth magnetic field strength when using a compass.

Some specific suggestions:

• [ ] Sensor data buffers (like storedExtNav) should be allocated when sensor data arrives to save memory in cases where the sensor is not used
• [ ] Firmware size could be reduced by splitting AP_NavEKF3_Buffer.h into a .h and .cpp file with the implementations in the .cpp file
• [ ] Add a pre-arm check and log an error if the sensor data buffer is not large enough to handle the update rate of the sensor. In these cases the new sensor data arrives and ends up pushing out an old piece of data from the buffer. The time on the pushed-out data will be less than the delayed time horizon
• [ ] Convert all remaining float arrays and matrices to use the method that MATH_CHECK_INDEXES can check
• [ ] move setting of origin out of NavEKF3_core::readGpsData()
• [ ] move calls to AP_Declination::get_earth_field_ga out of NavEKF3_core::readGpsData()
• [ ] _originHgtMode should have an enum for the various param values. Parameter description could be improved. rename parameter to ORGN_ALT_COR (i.e. origin altitude correction)
• [ ] rename "height" to "alt" everywhere
• [ ] add CorrectRangeFinderForSensorOffset function based on code at top of selectHeightForFusion() function. This non-functional change will make the already long selectHeightForFusion() a little shorter and make handling consistent with GPS and ExtNav sensors

[priseborough]

An item worth mentioning in this context is that the auto-generated code for the EKF3 prediction and update equations which was generated using the Matlab symbolic toolbox (see https://github.com/PX4/ecl/tree/master/EKF/matlab/scripts/Inertial%20Nav%20EKF), can now be generated using a SymPy script (see https://github.com/PX4/ecl/tree/master/EKF/python/ekf_derivation) which takes a fraction of the tine to run (<2min vs 3 hours) .

When we compared PX4/ecl EKF before and after the conversion from Matlab to SymPy generated equations, it saved over 15K of flash space.