From 39969e7d8ef9f363db10838eb99596925a69e8c1 Mon Sep 17 00:00:00 2001 From: priseborough Date: Thu, 14 Jul 2016 15:08:43 +1000 Subject: [PATCH] AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 1404 ++++++++++++++ libraries/AP_NavEKF3/AP_NavEKF3.h | 433 +++++ .../AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp | 440 +++++ libraries/AP_NavEKF3/AP_NavEKF3_Buffer.h | 188 ++ libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 502 +++++ libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp | 37 + libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 1133 +++++++++++ .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 760 ++++++++ .../AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 717 +++++++ libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 588 ++++++ .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 819 ++++++++ .../AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp | 583 ++++++ .../AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp | 449 +++++ libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 1649 +++++++++++++++++ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 1135 ++++++++++++ 15 files changed, 10837 insertions(+) create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3.cpp create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3.h create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3_Buffer.h create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3_core.cpp create mode 100644 libraries/AP_NavEKF3/AP_NavEKF3_core.h diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp new file mode 100644 index 0000000000..873757eaa0 --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -0,0 +1,1404 @@ +#include +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 + +#include "AP_NavEKF3_core.h" +#include +#include +#include + +/* + parameter defaults for different types of vehicle. The + APM_BUILD_DIRECTORY is taken from the main vehicle directory name + where the code is built. + */ +#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_Replay) +// copter defaults +#define VELNE_M_NSE_DEFAULT 0.5f +#define VELD_M_NSE_DEFAULT 0.7f +#define POSNE_M_NSE_DEFAULT 0.5f +#define ALT_M_NSE_DEFAULT 2.0f +#define MAG_M_NSE_DEFAULT 0.05f +#define GYRO_P_NSE_DEFAULT 1.5E-02f +#define ACC_P_NSE_DEFAULT 3.5E-01f +#define GBIAS_P_NSE_DEFAULT 1.0E-03f +#define ABIAS_P_NSE_DEFAULT 3.0E-03f +#define MAGB_P_NSE_DEFAULT 1.0E-04f +#define MAGE_P_NSE_DEFAULT 1.0E-03f +#define VEL_I_GATE_DEFAULT 500 +#define POS_I_GATE_DEFAULT 500 +#define HGT_I_GATE_DEFAULT 500 +#define MAG_I_GATE_DEFAULT 300 +#define MAG_CAL_DEFAULT 3 +#define GLITCH_RADIUS_DEFAULT 25 +#define FLOW_MEAS_DELAY 10 +#define FLOW_M_NSE_DEFAULT 0.25f +#define FLOW_I_GATE_DEFAULT 300 +#define CHECK_SCALER_DEFAULT 100 + +#elif APM_BUILD_TYPE(APM_BUILD_APMrover2) +// rover defaults +#define VELNE_M_NSE_DEFAULT 0.5f +#define VELD_M_NSE_DEFAULT 0.7f +#define POSNE_M_NSE_DEFAULT 0.5f +#define ALT_M_NSE_DEFAULT 2.0f +#define MAG_M_NSE_DEFAULT 0.05f +#define GYRO_P_NSE_DEFAULT 1.5E-02f +#define ACC_P_NSE_DEFAULT 3.5E-01f +#define GBIAS_P_NSE_DEFAULT 1.0E-03f +#define ABIAS_P_NSE_DEFAULT 3.0E-03f +#define MAGB_P_NSE_DEFAULT 1.0E-04f +#define MAGE_P_NSE_DEFAULT 1.0E-03f +#define VEL_I_GATE_DEFAULT 500 +#define POS_I_GATE_DEFAULT 500 +#define HGT_I_GATE_DEFAULT 500 +#define MAG_I_GATE_DEFAULT 300 +#define MAG_CAL_DEFAULT 2 +#define GLITCH_RADIUS_DEFAULT 25 +#define FLOW_MEAS_DELAY 10 +#define FLOW_M_NSE_DEFAULT 0.25f +#define FLOW_I_GATE_DEFAULT 300 +#define CHECK_SCALER_DEFAULT 100 + +#elif APM_BUILD_TYPE(APM_BUILD_ArduPlane) +// plane defaults +#define VELNE_M_NSE_DEFAULT 0.5f +#define VELD_M_NSE_DEFAULT 0.7f +#define POSNE_M_NSE_DEFAULT 0.5f +#define ALT_M_NSE_DEFAULT 3.0f +#define MAG_M_NSE_DEFAULT 0.05f +#define GYRO_P_NSE_DEFAULT 1.5E-02f +#define ACC_P_NSE_DEFAULT 3.5E-01f +#define GBIAS_P_NSE_DEFAULT 1.0E-03f +#define ABIAS_P_NSE_DEFAULT 3.0E-03f +#define MAGB_P_NSE_DEFAULT 1.0E-04f +#define MAGE_P_NSE_DEFAULT 1.0E-03f +#define VEL_I_GATE_DEFAULT 500 +#define POS_I_GATE_DEFAULT 500 +#define HGT_I_GATE_DEFAULT 500 +#define MAG_I_GATE_DEFAULT 300 +#define MAG_CAL_DEFAULT 0 +#define GLITCH_RADIUS_DEFAULT 25 +#define FLOW_MEAS_DELAY 10 +#define FLOW_M_NSE_DEFAULT 0.25f +#define FLOW_I_GATE_DEFAULT 300 +#define CHECK_SCALER_DEFAULT 100 + +#else +// build type not specified, use copter defaults +#define VELNE_M_NSE_DEFAULT 0.5f +#define VELD_M_NSE_DEFAULT 0.7f +#define POSNE_M_NSE_DEFAULT 0.5f +#define ALT_M_NSE_DEFAULT 2.0f +#define MAG_M_NSE_DEFAULT 0.05f +#define GYRO_P_NSE_DEFAULT 1.5E-02f +#define ACC_P_NSE_DEFAULT 3.5E-01f +#define GBIAS_P_NSE_DEFAULT 1.0E-03f +#define ABIAS_P_NSE_DEFAULT 3.0E-03f +#define MAGB_P_NSE_DEFAULT 1.0E-04f +#define MAGE_P_NSE_DEFAULT 1.0E-03f +#define VEL_I_GATE_DEFAULT 500 +#define POS_I_GATE_DEFAULT 500 +#define HGT_I_GATE_DEFAULT 500 +#define MAG_I_GATE_DEFAULT 300 +#define MAG_CAL_DEFAULT 3 +#define GLITCH_RADIUS_DEFAULT 25 +#define FLOW_MEAS_DELAY 10 +#define FLOW_M_NSE_DEFAULT 0.25f +#define FLOW_I_GATE_DEFAULT 300 +#define CHECK_SCALER_DEFAULT 100 + +#endif // APM_BUILD_DIRECTORY + +extern const AP_HAL::HAL& hal; + +// Define tuning parameters +const AP_Param::GroupInfo NavEKF3::var_info[] = { + + // @Param: ENABLE + // @DisplayName: Enable EKF3 + // @Description: This enables EKF3. Enabling EKF3 only makes the maths run, it does not mean it will be used for flight control. To use it for flight control set AHRS_EKF_TYPE=3. A reboot or restart will need to be performed after changing the value of EK2_ENABLE for it to take effect. + // @Values: 0:Disabled, 1:Enabled + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 0, NavEKF3, _enable, 1, AP_PARAM_FLAG_ENABLE), + + // GPS measurement parameters + + // @Param: GPS_TYPE + // @DisplayName: GPS mode control + // @Description: This controls use of GPS measurements : 0 = use 3D velocity & 2D position, 1 = use 2D velocity and 2D position, 2 = use 2D position, 3 = Inhibit GPS use - this can be useful when flying with an optical flow sensor in an environment where GPS quality is poor and subject to large multipath errors. + // @Values: 0:GPS 3D Vel and 2D Pos, 1:GPS 2D vel and 2D pos, 2:GPS 2D pos, 3:No GPS + // @User: Advanced + AP_GROUPINFO("GPS_TYPE", 1, NavEKF3, _fusionModeGPS, 0), + + // @Param: VELNE_M_NSE + // @DisplayName: GPS horizontal velocity measurement noise (m/s) + // @Description: This sets a lower limit on the speed accuracy reported by the GPS receiver that is used to set horizontal velocity observation noise. If the model of receiver used does not provide a speed accurcy estimate, then the parameter value will be used. Increasing it reduces the weighting of the GPS horizontal velocity measurements. + // @Range: 0.05 5.0 + // @Increment: 0.05 + // @User: Advanced + // @Units: m/s + AP_GROUPINFO("VELNE_M_NSE", 2, NavEKF3, _gpsHorizVelNoise, VELNE_M_NSE_DEFAULT), + + // @Param: VELD_M_NSE + // @DisplayName: GPS vertical velocity measurement noise (m/s) + // @Description: This sets a lower limit on the speed accuracy reported by the GPS receiver that is used to set vertical velocity observation noise. If the model of receiver used does not provide a speed accurcy estimate, then the parameter value will be used. Increasing it reduces the weighting of the GPS vertical velocity measurements. + // @Range: 0.05 5.0 + // @Increment: 0.05 + // @User: Advanced + // @Units: m/s + AP_GROUPINFO("VELD_M_NSE", 3, NavEKF3, _gpsVertVelNoise, VELD_M_NSE_DEFAULT), + + // @Param: VEL_I_GATE + // @DisplayName: GPS velocity innovation gate size + // @Description: This sets the percentage number of standard deviations applied to the GPS velocity measurement innovation consistency check. Decreasing it makes it more likely that good measurements willbe rejected. Increasing it makes it more likely that bad measurements will be accepted. + // @Range: 100 1000 + // @Increment: 25 + // @User: Advanced + AP_GROUPINFO("VEL_I_GATE", 4, NavEKF3, _gpsVelInnovGate, VEL_I_GATE_DEFAULT), + + // @Param: POSNE_M_NSE + // @DisplayName: GPS horizontal position measurement noise (m) + // @Description: This sets the GPS horizontal position observation noise. Increasing it reduces the weighting of GPS horizontal position measurements. + // @Range: 0.1 10.0 + // @Increment: 0.1 + // @User: Advanced + // @Units: m + AP_GROUPINFO("POSNE_M_NSE", 5, NavEKF3, _gpsHorizPosNoise, POSNE_M_NSE_DEFAULT), + + // @Param: POS_I_GATE + // @DisplayName: GPS position measurement gate size + // @Description: This sets the percentage number of standard deviations applied to the GPS position measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. + // @Range: 100 1000 + // @Increment: 25 + // @User: Advanced + AP_GROUPINFO("POS_I_GATE", 6, NavEKF3, _gpsPosInnovGate, POS_I_GATE_DEFAULT), + + // @Param: GLITCH_RAD + // @DisplayName: GPS glitch radius gate size (m) + // @Description: This controls the maximum radial uncertainty in position between the value predicted by the filter and the value measured by the GPS before the filter position and velocity states are reset to the GPS. Making this value larger allows the filter to ignore larger GPS glitches but also means that non-GPS errors such as IMU and compass can create a larger error in position before the filter is forced back to the GPS position. + // @Range: 10 100 + // @Increment: 5 + // @User: Advanced + // @Units: m + AP_GROUPINFO("GLITCH_RAD", 7, NavEKF3, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT), + + // @Param: GPS_DELAY + // @DisplayName: GPS measurement delay (msec) + // @Description: This is the number of msec that the GPS measurements lag behind the inertial measurements. + // @Range: 0 250 + // @Increment: 10 + // @User: Advanced + // @Units: msec + AP_GROUPINFO("GPS_DELAY", 8, NavEKF3, _gpsDelay_ms, 220), + + // Height measurement parameters + + // @Param: ALT_SOURCE + // @Description: This parameter controls the primary height sensor used by the EKF. If the selected option cannot be used, it will default to Baro as the primary height source. Setting 0 will use the baro altitude at all times. Setting 1 uses the range finder and is only available in combination with optical flow navigation (EK2_GPS_TYPE = 3). Setting 2 uses GPS. Setting 3 uses the range beacon data. NOTE - the EK2_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground. + // @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon + // @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS + // @User: Advanced + AP_GROUPINFO("ALT_SOURCE", 9, NavEKF3, _altSource, 0), + + // @Param: ALT_M_NSE + // @DisplayName: Altitude measurement noise (m) + // @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting of the baro measurement and will make the filter respond more slowly to baro measurement errors, but will make it more sensitive to GPS and accelerometer errors. + // @Range: 0.1 10.0 + // @Increment: 0.1 + // @User: Advanced + // @Units: m + AP_GROUPINFO("ALT_M_NSE", 10, NavEKF3, _baroAltNoise, ALT_M_NSE_DEFAULT), + + // @Param: HGT_I_GATE + // @DisplayName: Height measurement gate size + // @Description: This sets the percentage number of standard deviations applied to the height measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. + // @Range: 100 1000 + // @Increment: 25 + // @User: Advanced + AP_GROUPINFO("HGT_I_GATE", 11, NavEKF3, _hgtInnovGate, HGT_I_GATE_DEFAULT), + + // @Param: HGT_DELAY + // @DisplayName: Height measurement delay (msec) + // @Description: This is the number of msec that the Height measurements lag behind the inertial measurements. + // @Range: 0 250 + // @Increment: 10 + // @User: Advanced + // @Units: msec + AP_GROUPINFO("HGT_DELAY", 12, NavEKF3, _hgtDelay_ms, 60), + + // Magnetometer measurement parameters + + // @Param: MAG_M_NSE + // @DisplayName: Magnetometer measurement noise (Gauss) + // @Description: This is the RMS value of noise in magnetometer measurements. Increasing it reduces the weighting on these measurements. + // @Range: 0.01 0.5 + // @Increment: 0.01 + // @User: Advanced + // @Units: gauss + AP_GROUPINFO("MAG_M_NSE", 13, NavEKF3, _magNoise, MAG_M_NSE_DEFAULT), + + // @Param: MAG_CAL + // @DisplayName: Magnetometer calibration mode + // @Description: EKF_MAG_CAL = 0 enables calibration when airborne and is the default setting for Plane users. EKF_MAG_CAL = 1 enables calibration when manoeuvreing. EKF_MAG_CAL = 2 prevents magnetometer calibration regardless of flight condition, is recommended if the external magnetic field is varying and is the default for rovers. EKF_MAG_CAL = 3 enables calibration when the first in-air field and yaw reset has completed and is the default for copters. EKF_MAG_CAL = 4 enables calibration all the time. This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states. This model is only suitable for use when the external magnetic field environment is stable. + // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always + // @User: Advanced + AP_GROUPINFO("MAG_CAL", 14, NavEKF3, _magCal, MAG_CAL_DEFAULT), + + // @Param: MAG_I_GATE + // @DisplayName: Magnetometer measurement gate size + // @Description: This sets the percentage number of standard deviations applied to the magnetometer measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. + // @Range: 100 1000 + // @Increment: 25 + // @User: Advanced + AP_GROUPINFO("MAG_I_GATE", 15, NavEKF3, _magInnovGate, MAG_I_GATE_DEFAULT), + + // Airspeed measurement parameters + + // @Param: EAS_M_NSE + // @DisplayName: Equivalent airspeed measurement noise (m/s) + // @Description: This is the RMS value of noise in equivalent airspeed measurements used by planes. Increasing it reduces the weighting of airspeed measurements and will make wind speed estimates less noisy and slower to converge. Increasing also increases navigation errors when dead-reckoning without GPS measurements. + // @Range: 0.5 5.0 + // @Increment: 0.1 + // @User: Advanced + // @Units: m/s + AP_GROUPINFO("EAS_M_NSE", 16, NavEKF3, _easNoise, 1.4f), + + // @Param: EAS_I_GATE + // @DisplayName: Airspeed measurement gate size + // @Description: This sets the percentage number of standard deviations applied to the airspeed measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. + // @Range: 100 1000 + // @Increment: 25 + // @User: Advanced + AP_GROUPINFO("EAS_I_GATE", 17, NavEKF3, _tasInnovGate, 400), + + // Rangefinder measurement parameters + + // @Param: RNG_M_NSE + // @DisplayName: Range finder measurement noise (m) + // @Description: This is the RMS value of noise in the range finder measurement. Increasing it reduces the weighting on this measurement. + // @Range: 0.1 10.0 + // @Increment: 0.1 + // @User: Advanced + // @Units: m + AP_GROUPINFO("RNG_M_NSE", 18, NavEKF3, _rngNoise, 0.5f), + + // @Param: RNG_I_GATE + // @DisplayName: Range finder measurement gate size + // @Description: This sets the percentage number of standard deviations applied to the range finder innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. + // @Range: 100 1000 + // @Increment: 25 + // @User: Advanced + AP_GROUPINFO("RNG_I_GATE", 19, NavEKF3, _rngInnovGate, 500), + + // Optical flow measurement parameters + + // @Param: MAX_FLOW + // @DisplayName: Maximum valid optical flow rate + // @Description: This sets the magnitude maximum optical flow rate in rad/sec that will be accepted by the filter + // @Range: 1.0 4.0 + // @Increment: 0.1 + // @User: Advanced + // @Units: rad/s + AP_GROUPINFO("MAX_FLOW", 20, NavEKF3, _maxFlowRate, 2.5f), + + // @Param: FLOW_M_NSE + // @DisplayName: Optical flow measurement noise (rad/s) + // @Description: This is the RMS value of noise and errors in optical flow measurements. Increasing it reduces the weighting on these measurements. + // @Range: 0.05 1.0 + // @Increment: 0.05 + // @User: Advanced + // @Units: rad/s + AP_GROUPINFO("FLOW_M_NSE", 21, NavEKF3, _flowNoise, FLOW_M_NSE_DEFAULT), + + // @Param: FLOW_I_GATE + // @DisplayName: Optical Flow measurement gate size + // @Description: This sets the percentage number of standard deviations applied to the optical flow innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. + // @Range: 100 1000 + // @Increment: 25 + // @User: Advanced + AP_GROUPINFO("FLOW_I_GATE", 22, NavEKF3, _flowInnovGate, FLOW_I_GATE_DEFAULT), + + // @Param: FLOW_DELAY + // @DisplayName: Optical Flow measurement delay (msec) + // @Description: This is the number of msec that the optical flow measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor. + // @Range: 0 250 + // @Increment: 10 + // @User: Advanced + // @Units: msec + AP_GROUPINFO("FLOW_DELAY", 23, NavEKF3, _flowDelay_ms, FLOW_MEAS_DELAY), + + // State and Covariance Predition Parameters + + // @Param: GYRO_P_NSE + // @DisplayName: Rate gyro noise (rad/s) + // @Description: This control disturbance noise controls the growth of estimated error due to gyro measurement errors excluding bias. Increasing it makes the flter trust the gyro measurements less and other measurements more. + // @Range: 0.0001 0.1 + // @Increment: 0.0001 + // @User: Advanced + // @Units: rad/s + AP_GROUPINFO("GYRO_P_NSE", 24, NavEKF3, _gyrNoise, GYRO_P_NSE_DEFAULT), + + // @Param: ACC_P_NSE + // @DisplayName: Accelerometer noise (m/s^2) + // @Description: This control disturbance noise controls the growth of estimated error due to accelerometer measurement errors excluding bias. Increasing it makes the flter trust the accelerometer measurements less and other measurements more. + // @Range: 0.01 1.0 + // @Increment: 0.01 + // @User: Advanced + // @Units: m/s/s + AP_GROUPINFO("ACC_P_NSE", 25, NavEKF3, _accNoise, ACC_P_NSE_DEFAULT), + + // @Param: GBIAS_P_NSE + // @DisplayName: Rate gyro bias stability (rad/s/s) + // @Description: This state process noise controls growth of the gyro delta angle bias state error estimate. Increasing it makes rate gyro bias estimation faster and noisier. + // @Range: 0.00001 0.001 + // @User: Advanced + // @Units: rad/s/s + AP_GROUPINFO("GBIAS_P_NSE", 26, NavEKF3, _gyroBiasProcessNoise, GBIAS_P_NSE_DEFAULT), + + // 27 previously used for EK2_GSCL_P_NSE parameter that has been removed + + // @Param: ABIAS_P_NSE + // @DisplayName: Accelerometer bias stability (m/s^3) + // @Description: This noise controls the growth of the vertical accelerometer delta velocity bias state error estimate. Increasing it makes accelerometer bias estimation faster and noisier. + // @Range: 0.00001 0.001 + // @User: Advanced + // @Units: m/s/s/s + AP_GROUPINFO("ABIAS_P_NSE", 28, NavEKF3, _accelBiasProcessNoise, ABIAS_P_NSE_DEFAULT), + + // 29 previously used for EK2_MAG_P_NSE parameter that has been replaced with EK2_MAGE_P_NSE and EK2_MAGB_P_NSE + + // @Param: WIND_P_NSE + // @DisplayName: Wind velocity process noise (m/s^2) + // @Description: This state process noise controls the growth of wind state error estimates. Increasing it makes wind estimation faster and noisier. + // @Range: 0.01 1.0 + // @Increment: 0.1 + // @User: Advanced + // @Units: m/s/s + AP_GROUPINFO("WIND_P_NSE", 30, NavEKF3, _windVelProcessNoise, 0.1f), + + // @Param: WIND_PSCALE + // @DisplayName: Height rate to wind procss noise scaler + // @Description: This controls how much the process noise on the wind states is increased when gaining or losing altitude to take into account changes in wind speed and direction with altitude. Increasing this parameter increases how rapidly the wind states adapt when changing altitude, but does make wind velocity estimation noiser. + // @Range: 0.0 1.0 + // @Increment: 0.1 + // @User: Advanced + AP_GROUPINFO("WIND_PSCALE", 31, NavEKF3, _wndVarHgtRateScale, 0.5f), + + // @Param: GPS_CHECK + // @DisplayName: GPS preflight check + // @Description: This is a 1 byte bitmap controlling which GPS preflight checks are performed. Set to 0 to bypass all checks. Set to 255 perform all checks. Set to 3 to check just the number of satellites and HDoP. Set to 31 for the most rigorous checks that will still allow checks to pass when the copter is moving, eg launch from a boat. + // @Bitmask: 0:NSats,1:HDoP,2:speed error,3:horiz pos error,4:yaw error,5:pos drift,6:vert speed,7:horiz speed + // @User: Advanced + AP_GROUPINFO("GPS_CHECK", 32, NavEKF3, _gpsCheck, 31), + + // @Param: IMU_MASK + // @DisplayName: Bitmask of active IMUs + // @Description: 1 byte bitmap of IMUs to use in EKF3. A separate instance of EKF3 will be started for each IMU selected. Set to 1 to use the first IMU only (default), set to 2 to use the second IMU only, set to 3 to use the first and second IMU. Additional IMU's can be used up to a maximum of 6 if memory and processing resources permit. There may be insufficient memory and processing resources to run multiple instances. If this occurs EKF3 will fail to start. + // @Range: 1 127 + // @User: Advanced + AP_GROUPINFO("IMU_MASK", 33, NavEKF3, _imuMask, 3), + + // @Param: CHECK_SCALE + // @DisplayName: GPS accuracy check scaler (%) + // @Description: This scales the thresholds that are used to check GPS accuracy before it is used by the EKF. A value of 100 is the default. Values greater than 100 increase and values less than 100 reduce the maximum GPS error the EKF will accept. A value of 200 will double the allowable GPS error. + // @Range: 50 200 + // @User: Advanced + // @Units: % + AP_GROUPINFO("CHECK_SCALE", 34, NavEKF3, _gpsCheckScaler, CHECK_SCALER_DEFAULT), + + // @Param: NOAID_M_NSE + // @DisplayName: Non-GPS operation position uncertainty (m) + // @Description: This sets the amount of position variation that the EKF allows for when operating without external measurements (eg GPS or optical flow). Increasing this parameter makes the EKF attitude estimate less sensitive to vehicle manoeuvres but more sensitive to IMU errors. + // @Range: 0.5 50.0 + // @User: Advanced + // @Units: m/s + AP_GROUPINFO("NOAID_M_NSE", 35, NavEKF3, _noaidHorizNoise, 10.0f), + + // @Param: LOG_MASK + // @DisplayName: EKF sensor logging IMU mask + // @Description: This sets the IMU mask of sensors to do full logging for + // @Values: 0:Disabled,1:FirstIMU,3:FirstAndSecondIMU,7:AllIMUs + // @User: Advanced + AP_GROUPINFO("LOG_MASK", 36, NavEKF3, _logging_mask, 1), + + // control of magentic yaw angle fusion + + // @Param: YAW_M_NSE + // @DisplayName: Yaw measurement noise (rad) + // @Description: This is the RMS value of noise in yaw measurements from the magnetometer. Increasing it reduces the weighting on these measurements. + // @Range: 0.05 1.0 + // @Increment: 0.05 + // @User: Advanced + // @Units: gauss + AP_GROUPINFO("YAW_M_NSE", 37, NavEKF3, _yawNoise, 0.5f), + + // @Param: YAW_I_GATE + // @DisplayName: Yaw measurement gate size + // @Description: This sets the percentage number of standard deviations applied to the magnetometer yaw measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. + // @Range: 100 1000 + // @Increment: 25 + // @User: Advanced + AP_GROUPINFO("YAW_I_GATE", 38, NavEKF3, _yawInnovGate, 300), + + // @Param: TAU_OUTPUT + // @DisplayName: Output complementary filter time constant (centi-sec) + // @Description: Sets the time constant of the output complementary filter/predictor in centi-seconds. + // @Range: 10 50 + // @Increment: 5 + // @User: Advanced + AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF3, _tauVelPosOutput, 25), + + // @Param: MAGE_P_NSE + // @DisplayName: Earth magnetic field process noise (gauss/s) + // @Description: This state process noise controls the growth of earth magnetic field state error estimates. Increasing it makes earth magnetic field estimation faster and noisier. + // @Range: 0.00001 0.01 + // @User: Advanced + // @Units: gauss/s + AP_GROUPINFO("MAGE_P_NSE", 40, NavEKF3, _magEarthProcessNoise, MAGE_P_NSE_DEFAULT), + + // @Param: MAGB_P_NSE + // @DisplayName: Body magnetic field process noise (gauss/s) + // @Description: This state process noise controls the growth of body magnetic field state error estimates. Increasing it makes magnetometer bias error estimation faster and noisier. + // @Range: 0.00001 0.01 + // @User: Advanced + // @Units: gauss/s + AP_GROUPINFO("MAGB_P_NSE", 41, NavEKF3, _magBodyProcessNoise, MAGB_P_NSE_DEFAULT), + + // @Param: RNG_USE_HGT + // @DisplayName: Range finder switch height percentage + // @Description: The range finder will be used as the primary height source when below a specified percentage of the sensor maximum as set by the RNGFND_MAX_CM parameter. Set to -1 to prevent range finder use. + // @Range: -1 70 + // @Increment: 1 + // @User: Advanced + // @Units: % + AP_GROUPINFO("RNG_USE_HGT", 42, NavEKF3, _useRngSwHgt, -1), + + // @Param: TERR_GRAD + // @DisplayName: Maximum terrain gradient + // @Description: Specifies the maxium gradient of the terrain below the vehicle when it is using range finder as a height reference + // @Range: 0 0.2 + // @Increment: 0.01 + // @User: Advanced + AP_GROUPINFO("TERR_GRAD", 43, NavEKF3, _terrGradMax, 0.1f), + + // @Param: BCN_M_NSE + // @DisplayName: Range beacon measurement noise (m) + // @Description: This is the RMS value of noise in the range beacon measurement. Increasing it reduces the weighting on this measurement. + // @Range: 0.1 10.0 + // @Increment: 0.1 + // @User: Advanced + // @Units: m + AP_GROUPINFO("BCN_M_NSE", 44, NavEKF3, _rngBcnNoise, 1.0f), + + // @Param: BCN_I_GTE + // @DisplayName: Range beacon measurement gate size + // @Description: This sets the percentage number of standard deviations applied to the range beacon measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. + // @Range: 100 1000 + // @Increment: 25 + // @User: Advanced + AP_GROUPINFO("BCN_I_GTE", 45, NavEKF3, _rngBcnInnovGate, 500), + + // @Param: BCN_DELAY + // @DisplayName: Range beacon measurement delay (msec) + // @Description: This is the number of msec that the range beacon measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor. + // @Range: 0 250 + // @Increment: 10 + // @User: Advanced + // @Units: msec + AP_GROUPINFO("BCN_DELAY", 46, NavEKF3, _rngBcnDelay_ms, 50), + + AP_GROUPEND +}; + +NavEKF3::NavEKF3(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) : + _ahrs(ahrs), + _baro(baro), + _rng(rng), + gpsNEVelVarAccScale(0.05f), // Scale factor applied to horizontal velocity measurement variance due to manoeuvre acceleration - used when GPS doesn't report speed error + gpsDVelVarAccScale(0.07f), // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration - used when GPS doesn't report speed error + gpsPosVarAccScale(0.05f), // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration + magDelay_ms(60), // Magnetometer measurement delay (msec) + tasDelay_ms(240), // Airspeed measurement delay (msec) + tiltDriftTimeMax_ms(15000), // Maximum number of ms allowed without any form of tilt aiding (GPS, flow, TAS, etc) + posRetryTimeUseVel_ms(10000), // Position aiding retry time with velocity measurements (msec) + posRetryTimeNoVel_ms(7000), // Position aiding retry time without velocity measurements (msec) + hgtRetryTimeMode0_ms(10000), // Height retry time with vertical velocity measurement (msec) + hgtRetryTimeMode12_ms(5000), // Height retry time without vertical velocity measurement (msec) + tasRetryTime_ms(5000), // True airspeed timeout and retry interval (msec) + magFailTimeLimit_ms(10000), // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec) + magVarRateScale(0.005f), // scale factor applied to magnetometer variance due to angular rate and measurement timing jitter. Assume timing jitter of 10msec + gyroBiasNoiseScaler(2.0f), // scale factor applied to imu gyro bias learning before the vehicle is armed + hgtAvg_ms(100), // average number of msec between height measurements + betaAvg_ms(100), // average number of msec between synthetic sideslip measurements + covTimeStepMax(0.1f), // maximum time (sec) between covariance prediction updates + covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates + DCM33FlowMin(0.71f), // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high. + fScaleFactorPnoise(1e-10f), // Process noise added to focal length scale factor state variance at each time step + flowTimeDeltaAvg_ms(100), // average interval between optical flow measurements (msec) + flowIntervalMax_ms(100), // maximum allowable time between flow fusion events + gndEffectTimeout_ms(1000), // time in msec that baro ground effect compensation will timeout after initiation + gndEffectBaroScaler(4.0f), // scaler applied to the barometer observation variance when operating in ground effect + gndGradientSigma(50), // RMS terrain gradient percentage assumed by the terrain height estimation + fusionTimeStep_ms(10) // The minimum number of msec between covariance prediction and fusion operations +{ + AP_Param::setup_object_defaults(this, var_info); +} + +/* + see if we should log some sensor data + */ +void NavEKF3::check_log_write(void) +{ + if (!have_ekf_logging()) { + return; + } + if (logging.log_compass) { + DataFlash_Class::instance()->Log_Write_Compass(*_ahrs->get_compass(), imuSampleTime_us); + logging.log_compass = false; + } + if (logging.log_gps) { + DataFlash_Class::instance()->Log_Write_GPS(_ahrs->get_gps(), 0, imuSampleTime_us); + logging.log_gps = false; + } + if (logging.log_baro) { + DataFlash_Class::instance()->Log_Write_Baro(_baro, imuSampleTime_us); + logging.log_baro = false; + } + if (logging.log_imu) { + const AP_InertialSensor &ins = _ahrs->get_ins(); + DataFlash_Class::instance()->Log_Write_IMUDT(ins, imuSampleTime_us, _logging_mask.get()); + logging.log_imu = false; + } + + // this is an example of an ad-hoc log in EKF + // DataFlash_Class::instance()->Log_Write("NKA", "TimeUS,X", "Qf", AP_HAL::micros64(), (double)2.4f); +} + + +// Initialise the filter +bool NavEKF3::InitialiseFilter(void) +{ + if (_enable == 0) { + return false; + } + + imuSampleTime_us = AP_HAL::micros64(); + + // see if we will be doing logging + DataFlash_Class *dataflash = DataFlash_Class::instance(); + if (dataflash != nullptr) { + logging.enabled = dataflash->log_replay(); + } + + if (core == nullptr) { + + // don't run multiple filters for 1 IMU + const AP_InertialSensor &ins = _ahrs->get_ins(); + uint8_t mask = (1U<available_memory() < sizeof(NavEKF3_core)*num_cores + 4096) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "NavEKF3: not enough memory"); + _enable.set(0); + return false; + } + + core = new NavEKF3_core[num_cores]; + if (core == nullptr) { + _enable.set(0); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "NavEKF3: allocation failed"); + return false; + } + + // set the IMU index for the cores + num_cores = 0; + for (uint8_t i=0; i<7; i++) { + if (_imuMask & (1U<get_ins(); + + bool statePredictEnabled[num_cores]; + for (uint8_t i=0; i 0) && (core[i-1].getFramesSincePredict() < 2) && (ins.get_sample_rate() > 200)) { + statePredictEnabled[i] = false; + } else { + statePredictEnabled[i] = true; + } + core[i].UpdateFilter(statePredictEnabled[i]); + } + + // If the current core selected has a bad error score or is unhealthy, switch to a healthy core with the lowest fault score + float primaryErrorScore = core[primary].errorScore(); + if (primaryErrorScore > 1.0f || !core[primary].healthy()) { + float lowestErrorScore = 0.67f * primaryErrorScore; + uint8_t newPrimaryIndex = primary; // index for new primary + for (uint8_t coreIndex=0; coreIndex= num_cores) instance = primary; + if (!core) { + return false; + } + return core[instance].getPosNE(posNE); +} + +// Write the last calculated D position relative to the reference point (m). +// If a calculated solution is not available, use the best available data and return false +// If false returned, do not use for flight control +bool NavEKF3::getPosD(int8_t instance, float &posD) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (!core) { + return false; + } + return core[instance].getPosD(posD); +} + +// return NED velocity in m/s +void NavEKF3::getVelNED(int8_t instance, Vector3f &vel) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + core[instance].getVelNED(vel); + } +} + +// Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s +float NavEKF3::getPosDownDerivative(int8_t instance) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + // return the value calculated from a complementary filer applied to the EKF height and vertical acceleration + if (core) { + return core[instance].getPosDownDerivative(); + } + return 0.0f; +} + +// This returns the specific forces in the NED frame +void NavEKF3::getAccelNED(Vector3f &accelNED) const +{ + if (core) { + core[primary].getAccelNED(accelNED); + } +} + +// return body axis gyro bias estimates in rad/sec +void NavEKF3::getGyroBias(int8_t instance, Vector3f &gyroBias) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + core[instance].getGyroBias(gyroBias); + } +} + +// return acceerometer bias estimate in m/s/s +void NavEKF3::getAccelBias(int8_t instance, Vector3f &accelBias) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + core[instance].getAccelBias(accelBias); + } +} + +// return tilt error convergence metric for the specified instance +void NavEKF3::getTiltError(int8_t instance, float &ang) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + core[instance].getTiltError(ang); + } +} + +// reset body axis gyro bias estimates +void NavEKF3::resetGyroBias(void) +{ + if (core) { + for (uint8_t i=0; i= num_cores) instance = primary; + if (core) { + core[instance].getWind(wind); + } +} + +// return earth magnetic field estimates in measurement units / 1000 +void NavEKF3::getMagNED(int8_t instance, Vector3f &magNED) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + core[instance].getMagNED(magNED); + } +} + +// return body magnetic field estimates in measurement units / 1000 +void NavEKF3::getMagXYZ(int8_t instance, Vector3f &magXYZ) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + core[instance].getMagXYZ(magXYZ); + } +} + +// return the magnetometer in use for the specified instance +uint8_t NavEKF3::getActiveMag(int8_t instance) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + return core[instance].getActiveMag(); + } else { + return 255; + } +} + +// Return estimated magnetometer offsets +// Return true if magnetometer offsets are valid +bool NavEKF3::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const +{ + if (!core) { + return false; + } + // try the primary first, else loop through all of the cores and return when one has offsets for this mag instance + if (core[primary].getMagOffsets(mag_idx, magOffsets)) { + return true; + } + for (uint8_t i=0; i= num_cores) instance = primary; + if (core) { + core[instance].getEulerAngles(eulers); + } +} + +// return the transformation matrix from XYZ (body) to NED axes +void NavEKF3::getRotationBodyToNED(Matrix3f &mat) const +{ + if (core) { + core[primary].getRotationBodyToNED(mat); + } +} + +// return the quaternions defining the rotation from NED to XYZ (body) axes +void NavEKF3::getQuaternion(Quaternion &quat) const +{ + if (core) { + core[primary].getQuaternion(quat); + } +} + +// return the innovations for the specified instance +void NavEKF3::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + core[instance].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); + } +} + +// publish output observer angular, velocity and position tracking error +void NavEKF3::getOutputTrackingError(int8_t instance, Vector3f &error) const +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + core[instance].getOutputTrackingError(error); + } +} + +// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements +void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + core[instance].getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); + } +} + +// should we use the compass? This is public so it can be used for +// reporting via ahrs.use_compass() +bool NavEKF3::use_compass(void) const +{ + if (!core) { + return false; + } + return core[primary].use_compass(); +} + +// write the raw optical flow measurements +// rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality +// rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes. +// rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro +// The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate +// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor. +// posOffset is the XYZ flow sensor position in the body frame in m +void NavEKF3::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset) +{ + if (core) { + for (uint8_t i=0; i= num_cores) instance = primary; + if (core) { + core[instance].getFlowDebug(varFlow, gndOffset, flowInnovX, flowInnovY, auxInnov, HAGL, rngInnov, range, gndOffsetErr); + } +} + +// return data for debugging range beacon fusion +bool NavEKF3::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + return core[instance].getRangeBeaconDebug(ID, rng, innov, innovVar, testRatio, beaconPosNED, offsetHigh, offsetLow); + } else { + return false; + } +} + +// called by vehicle code to specify that a takeoff is happening +// causes the EKF to compensate for expected barometer errors due to ground effect +void NavEKF3::setTakeoffExpected(bool val) +{ + if (core) { + for (uint8_t i=0; i= num_cores) instance = primary; + if (core) { + core[instance].getFilterFaults(faults); + } else { + faults = 0; + } +} + +/* + return filter timeout status as a bitmasked integer + 0 = position measurement timeout + 1 = velocity measurement timeout + 2 = height measurement timeout + 3 = magnetometer measurement timeout + 5 = unassigned + 6 = unassigned + 7 = unassigned + 7 = unassigned +*/ +void NavEKF3::getFilterTimeouts(int8_t instance, uint8_t &timeouts) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + core[instance].getFilterTimeouts(timeouts); + } else { + timeouts = 0; + } +} + +/* + return filter status flags +*/ +void NavEKF3::getFilterStatus(int8_t instance, nav_filter_status &status) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + core[instance].getFilterStatus(status); + } else { + memset(&status, 0, sizeof(status)); + } +} + +/* +return filter gps quality check status +*/ +void NavEKF3::getFilterGpsStatus(int8_t instance, nav_gps_status &status) +{ + if (instance < 0 || instance >= num_cores) instance = primary; + if (core) { + core[instance].getFilterGpsStatus(status); + } else { + memset(&status, 0, sizeof(status)); + } +} + +// send an EKF_STATUS_REPORT message to GCS +void NavEKF3::send_status_report(mavlink_channel_t chan) +{ + if (core) { + core[primary].send_status_report(chan); + } +} + +// provides the height limit to be observed by the control loops +// returns false if no height limiting is required +// this is needed to ensure the vehicle does not fly too high when using optical flow navigation +bool NavEKF3::getHeightControlLimit(float &height) const +{ + if (!core) { + return false; + } + return core[primary].getHeightControlLimit(height); +} + +// Returns the amount of yaw angle change (in radians) due to the last yaw angle reset or core selection switch +// Returns the time of the last yaw angle reset or 0 if no reset or core switch has ever occurred +// Where there are multiple consumers, they must access this function on the same frame as each other +uint32_t NavEKF3::getLastYawResetAngle(float &yawAngDelta) +{ + if (!core) { + return 0; + } + + yawAngDelta = 0.0f; + + // Do the conversion to msec in one place + uint32_t now_time_ms = imuSampleTime_us / 1000; + + // The last time we switched to the current primary core is the first reset event + uint32_t lastYawReset_ms = yaw_reset_data.last_primary_change; + + // There has been a change notification in the primary core that the controller has not consumed + // or this is a repeated acce + if (yaw_reset_data.core_changed || yaw_reset_data.last_function_call == now_time_ms) { + yawAngDelta = yaw_reset_data.core_delta; + yaw_reset_data.core_changed = false; + } + + // Record last time controller got the yaw reset + yaw_reset_data.last_function_call = now_time_ms; + + // There has been a reset inside the core since we switched so update the time and delta + float temp_yawAng; + uint32_t lastCoreYawReset_ms = core[primary].getLastYawResetAngle(temp_yawAng); + if (lastCoreYawReset_ms > lastYawReset_ms) { + yawAngDelta = wrap_PI(yawAngDelta + temp_yawAng); + lastYawReset_ms = lastCoreYawReset_ms; + } + + return lastYawReset_ms; +} + +// Returns the amount of NE position change due to the last position reset or core switch in metres +// Returns the time of the last reset or 0 if no reset or core switch has ever occurred +// Where there are multiple consumers, they must access this function on the same frame as each other +uint32_t NavEKF3::getLastPosNorthEastReset(Vector2f &posDelta) +{ + if (!core) { + return 0; + } + + posDelta.zero(); + + // Do the conversion to msec in one place + uint32_t now_time_ms = imuSampleTime_us / 1000; + + // The last time we switched to the current primary core is the first reset event + uint32_t lastPosReset_ms = pos_reset_data.last_primary_change; + + // There has been a change in the primary core that the controller has not consumed + // allow for multiple consumers on the same frame + if (pos_reset_data.core_changed || pos_reset_data.last_function_call == now_time_ms) { + posDelta = pos_reset_data.core_delta; + pos_reset_data.core_changed = false; + } + + // Record last time controller got the position reset + pos_reset_data.last_function_call = now_time_ms; + + // There has been a reset inside the core since we switched so update the time and delta + Vector2f tempPosDelta; + uint32_t lastCorePosReset_ms = core[primary].getLastPosNorthEastReset(tempPosDelta); + if (lastCorePosReset_ms > lastPosReset_ms) { + posDelta = posDelta + tempPosDelta; + lastPosReset_ms = lastCorePosReset_ms; + } + + return lastPosReset_ms; +} + +// return the amount of NE velocity change due to the last velocity reset in metres/sec +// returns the time of the last reset or 0 if no reset has ever occurred +uint32_t NavEKF3::getLastVelNorthEastReset(Vector2f &vel) const +{ + if (!core) { + return 0; + } + return core[primary].getLastVelNorthEastReset(vel); +} + +// report the reason for why the backend is refusing to initialise +const char *NavEKF3::prearm_failure_reason(void) const +{ + if (!core) { + return nullptr; + } + return core[primary].prearm_failure_reason(); +} + +// Returns the amount of vertical position change due to the last reset or core switch in metres +// Returns the time of the last reset or 0 if no reset or core switch has ever occurred +// Where there are multiple consumers, they must access this function on the same frame as each other +uint32_t NavEKF3::getLastPosDownReset(float &posDelta) +{ + if (!core) { + return 0; + } + + posDelta = 0.0f; + + // Do the conversion to msec in one place + uint32_t now_time_ms = imuSampleTime_us / 1000; + + // The last time we switched to the current primary core is the first reset event + uint32_t lastPosReset_ms = pos_down_reset_data.last_primary_change; + + // There has been a change in the primary core that the controller has not consumed + // allow for multiple consumers on the same frame + if (pos_down_reset_data.core_changed || pos_down_reset_data.last_function_call == now_time_ms) { + posDelta = pos_down_reset_data.core_delta; + pos_down_reset_data.core_changed = false; + } + + // Record last time controller got the position reset + pos_down_reset_data.last_function_call = now_time_ms; + + // There has been a reset inside the core since we switched so update the time and delta + float tempPosDelta; + uint32_t lastCorePosReset_ms = core[primary].getLastPosDownReset(tempPosDelta); + if (lastCorePosReset_ms > lastPosReset_ms) { + posDelta += tempPosDelta; + lastPosReset_ms = lastCorePosReset_ms; + } + + return lastPosReset_ms; +} + +// update the yaw reset data to capture changes due to a lane switch +void NavEKF3::updateLaneSwitchYawResetData(uint8_t new_primary, uint8_t old_primary) +{ + Vector3f eulers_old_primary, eulers_new_primary; + float old_yaw_delta; + + // If core yaw reset data has been consumed reset delta to zero + if (!yaw_reset_data.core_changed) { + yaw_reset_data.core_delta = 0; + } + + // If current primary has reset yaw after controller got it, add it to the delta + if (core[old_primary].getLastYawResetAngle(old_yaw_delta) > yaw_reset_data.last_function_call) { + yaw_reset_data.core_delta += old_yaw_delta; + } + + // Record the yaw delta between current core and new primary core and the timestamp of the core change + // Add current delta in case it hasn't been consumed yet + core[old_primary].getEulerAngles(eulers_old_primary); + core[new_primary].getEulerAngles(eulers_new_primary); + yaw_reset_data.core_delta = wrap_PI(eulers_new_primary.z - eulers_old_primary.z + yaw_reset_data.core_delta); + yaw_reset_data.last_primary_change = imuSampleTime_us / 1000; + yaw_reset_data.core_changed = true; + +} + +// update the position reset data to capture changes due to a lane switch +void NavEKF3::updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_primary) +{ + Vector2f pos_old_primary, pos_new_primary, old_pos_delta; + + // If core position reset data has been consumed reset delta to zero + if (!pos_reset_data.core_changed) { + pos_reset_data.core_delta.zero(); + } + + // If current primary has reset position after controller got it, add it to the delta + if (core[old_primary].getLastPosNorthEastReset(old_pos_delta) > pos_reset_data.last_function_call) { + pos_reset_data.core_delta += old_pos_delta; + } + + // Record the position delta between current core and new primary core and the timestamp of the core change + // Add current delta in case it hasn't been consumed yet + core[old_primary].getPosNE(pos_old_primary); + core[new_primary].getPosNE(pos_new_primary); + pos_reset_data.core_delta = pos_new_primary - pos_old_primary + pos_reset_data.core_delta; + pos_reset_data.last_primary_change = imuSampleTime_us / 1000; + pos_reset_data.core_changed = true; + +} + +// Update the vertical position reset data to capture changes due to a core switch +// This should be called after the decision to switch cores has been made, but before the +// new primary EKF update has been run +void NavEKF3::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary) +{ + float posDownOldPrimary, posDownNewPrimary, oldPosDownDelta; + + // If core position reset data has been consumed reset delta to zero + if (!pos_down_reset_data.core_changed) { + pos_down_reset_data.core_delta = 0.0f; + } + + // If current primary has reset position after controller got it, add it to the delta + if (core[old_primary].getLastPosDownReset(oldPosDownDelta) > pos_down_reset_data.last_function_call) { + pos_down_reset_data.core_delta += oldPosDownDelta; + } + + // Record the position delta between current core and new primary core and the timestamp of the core change + // Add current delta in case it hasn't been consumed yet + core[old_primary].getPosD(posDownOldPrimary); + core[new_primary].getPosD(posDownNewPrimary); + pos_down_reset_data.core_delta = posDownNewPrimary - posDownOldPrimary + pos_down_reset_data.core_delta; + pos_down_reset_data.last_primary_change = imuSampleTime_us / 1000; + pos_down_reset_data.core_changed = true; + +} + +#endif //HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h new file mode 100644 index 0000000000..3a91ccf4d1 --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -0,0 +1,433 @@ +/* + 24 state EKF based on https://github.com/priseborough/InertialNav + Converted from Matlab to C++ by Paul Riseborough + + EKF Tuning parameters refactored by Tom Cauchois + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +class NavEKF3_core; +class AP_AHRS; + +class NavEKF3 +{ +public: + friend class NavEKF3_core; + static const struct AP_Param::GroupInfo var_info[]; + + NavEKF3(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng); + + // allow logging to determine the number of active cores + uint8_t activeCores(void) const { + return num_cores; + } + + // Initialise the filter + bool InitialiseFilter(void); + + // Update Filter States - this should be called whenever new IMU data is available + void UpdateFilter(void); + + // check if we should write log messages + void check_log_write(void); + + // Check basic filter health metrics and return a consolidated health status + bool healthy(void) const; + + // returns the index of the primary core + // return -1 if no primary core selected + int8_t getPrimaryCoreIndex(void) const; + + // returns the index of the IMU of the primary core + // return -1 if no primary core selected + int8_t getPrimaryCoreIMUIndex(void) const; + + // Write the last calculated NE position relative to the reference point (m) for the specified instance. + // An out of range instance (eg -1) returns data for the the primary instance + // If a calculated solution is not available, use the best available data and return false + // If false returned, do not use for flight control + bool getPosNE(int8_t instance, Vector2f &posNE); + + // Write the last calculated D position relative to the reference point (m) for the specified instance. + // An out of range instance (eg -1) returns data for the the primary instance + // If a calculated solution is not available, use the best available data and return false + // If false returned, do not use for flight control + bool getPosD(int8_t instance, float &posD); + + // return NED velocity in m/s for the specified instance + // An out of range instance (eg -1) returns data for the the primary instance + void getVelNED(int8_t instance, Vector3f &vel); + + // Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s for the specified instance + // An out of range instance (eg -1) returns data for the the primary instance + // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF + // but will always be kinematically consistent with the z component of the EKF position state + float getPosDownDerivative(int8_t instance); + + // This returns the specific forces in the NED frame + void getAccelNED(Vector3f &accelNED) const; + + // return body axis gyro bias estimates in rad/sec for the specified instance + // An out of range instance (eg -1) returns data for the the primary instance + void getGyroBias(int8_t instance, Vector3f &gyroBias); + + // return accelerometer bias estimate in m/s/s + // An out of range instance (eg -1) returns data for the the primary instance + void getAccelBias(int8_t instance, Vector3f &accelBias); + + // return tilt error convergence metric for the specified instance + // An out of range instance (eg -1) returns data for the the primary instance + void getTiltError(int8_t instance, float &ang); + + // reset body axis gyro bias estimates + void resetGyroBias(void); + + // Resets the baro so that it reads zero at the current height + // Resets the EKF height to zero + // Adjusts the EKf origin height so that the EKF height + origin height is the same as before + // Returns true if the height datum reset has been performed + // If using a range finder for height no reset is performed and it returns false + bool resetHeightDatum(void); + + // Commands the EKF to not use GPS. + // This command must be sent prior to arming as it will only be actioned when the filter is in static mode + // This command is forgotten by the EKF each time it goes back into static mode (eg the vehicle disarms) + // Returns 0 if command rejected + // Returns 1 if attitude, vertical velocity and vertical position will be provided + // Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided + uint8_t setInhibitGPS(void); + + // return the horizontal speed limit in m/s set by optical flow sensor limits + // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow + void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const; + + // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) + // An out of range instance (eg -1) returns data for the the primary instance + void getWind(int8_t instance, Vector3f &wind); + + // return earth magnetic field estimates in measurement units / 1000 for the specified instance + // An out of range instance (eg -1) returns data for the the primary instance + void getMagNED(int8_t instance, Vector3f &magNED); + + // return body magnetic field estimates in measurement units / 1000 for the specified instance + // An out of range instance (eg -1) returns data for the the primary instance + void getMagXYZ(int8_t instance, Vector3f &magXYZ); + + // return the magnetometer in use for the specified instance + // An out of range instance (eg -1) returns data for the the primary instance + uint8_t getActiveMag(int8_t instance); + + // Return estimated magnetometer offsets + // Return true if magnetometer offsets are valid + bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const; + + // Return the last calculated latitude, longitude and height in WGS-84 + // If a calculated location isn't available, return a raw GPS measurement + // The status will return true if a calculation or raw measurement is available + // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control + bool getLLH(struct Location &loc) const; + + // return the latitude and longitude and height used to set the NED origin + // All NED positions calculated by the filter are relative to this location + // Returns false if the origin has not been set + bool getOriginLLH(struct Location &loc) const; + + // set the latitude and longitude and height used to set the NED origin + // All NED positions calcualted by the filter will be relative to this location + // The origin cannot be set if the filter is in a flight mode (eg vehicle armed) + // Returns false if the filter has rejected the attempt to set the origin + bool setOriginLLH(struct Location &loc); + + // return estimated height above ground level + // return false if ground height is not being estimated. + bool getHAGL(float &HAGL) const; + + // return the Euler roll, pitch and yaw angle in radians for the specified instance + // An out of range instance (eg -1) returns data for the the primary instance + void getEulerAngles(int8_t instance, Vector3f &eulers); + + // return the transformation matrix from XYZ (body) to NED axes + void getRotationBodyToNED(Matrix3f &mat) const; + + // return the quaternions defining the rotation from NED to XYZ (body) axes + void getQuaternion(Quaternion &quat) const; + + // return the innovations for the specified instance + // An out of range instance (eg -1) returns data for the the primary instance + void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov); + + // publish output observer angular, velocity and position tracking error + void getOutputTrackingError(int8_t instance, Vector3f &error) const; + + // return the innovation consistency test ratios for the specified instance + // An out of range instance (eg -1) returns data for the the primary instance + void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset); + + // should we use the compass? This is public so it can be used for + // reporting via ahrs.use_compass() + bool use_compass(void) const; + + // write the raw optical flow measurements + // rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality + // rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes. + // rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro + // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate + // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor. + // posOffset is the XYZ flow sensor position in the body frame in m + void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset); + + // return data for debugging optical flow fusion for the specified instance + // An out of range instance (eg -1) returns data for the the primary instance + void getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr); + + /* + Returns the following data for debugging range beacon fusion from the specified instance + An out of range instance (eg -1) returns data for the the primary instance + ID : beacon identifier + rng : measured range to beacon (m) + innov : range innovation (m) + innovVar : innovation variance (m^2) + testRatio : innovation consistency test ratio + beaconPosNED : beacon NED position (m) + returns true if data could be found, false if it could not + */ + bool getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow); + + // called by vehicle code to specify that a takeoff is happening + // causes the EKF to compensate for expected barometer errors due to ground effect + void setTakeoffExpected(bool val); + + // called by vehicle code to specify that a touchdown is expected to happen + // causes the EKF to compensate for expected barometer errors due to ground effect + void setTouchdownExpected(bool val); + + // Set to true if the terrain underneath is stable enough to be used as a height reference + // in combination with a range finder. Set to false if the terrain underneath the vehicle + // cannot be used as a height reference + void setTerrainHgtStable(bool val); + + /* + return the filter fault status as a bitmasked integer for the specified instance + An out of range instance (eg -1) returns data for the the primary instance + 0 = quaternions are NaN + 1 = velocities are NaN + 2 = badly conditioned X magnetometer fusion + 3 = badly conditioned Y magnetometer fusion + 5 = badly conditioned Z magnetometer fusion + 6 = badly conditioned airspeed fusion + 7 = badly conditioned synthetic sideslip fusion + 7 = filter is not initialised + */ + void getFilterFaults(int8_t instance, uint16_t &faults); + + /* + return filter timeout status as a bitmasked integer for the specified instance + An out of range instance (eg -1) returns data for the the primary instance + 0 = position measurement timeout + 1 = velocity measurement timeout + 2 = height measurement timeout + 3 = magnetometer measurement timeout + 5 = unassigned + 6 = unassigned + 7 = unassigned + 7 = unassigned + */ + void getFilterTimeouts(int8_t instance, uint8_t &timeouts); + + /* + return filter gps quality check status for the specified instance + An out of range instance (eg -1) returns data for the the primary instance + */ + void getFilterGpsStatus(int8_t instance, nav_gps_status &faults); + + /* + return filter status flags for the specified instance + An out of range instance (eg -1) returns data for the the primary instance + */ + void getFilterStatus(int8_t instance, nav_filter_status &status); + + // send an EKF_STATUS_REPORT message to GCS + void send_status_report(mavlink_channel_t chan); + + // provides the height limit to be observed by the control loops + // returns false if no height limiting is required + // this is needed to ensure the vehicle does not fly too high when using optical flow navigation + bool getHeightControlLimit(float &height) const; + + // return the amount of yaw angle change (in radians) due to the last yaw angle reset or core selection switch + // returns the time of the last yaw angle reset or 0 if no reset has ever occurred + uint32_t getLastYawResetAngle(float &yawAngDelta); + + // return the amount of NE position change due to the last position reset in metres + // returns the time of the last reset or 0 if no reset has ever occurred + uint32_t getLastPosNorthEastReset(Vector2f &posDelta); + + // return the amount of NE velocity change due to the last velocity reset in metres/sec + // returns the time of the last reset or 0 if no reset has ever occurred + uint32_t getLastVelNorthEastReset(Vector2f &vel) const; + + // return the amount of vertical position change due to the last reset in metres + // returns the time of the last reset or 0 if no reset has ever occurred + uint32_t getLastPosDownReset(float &posDelta); + + // report any reason for why the backend is refusing to initialise + const char *prearm_failure_reason(void) const; + + // allow the enable flag to be set by Replay + void set_enable(bool enable) { _enable.set(enable); } + + // are we doing sensor logging inside the EKF? + bool have_ekf_logging(void) const { return logging.enabled && _logging_mask != 0; } + +private: + uint8_t num_cores; // number of allocated cores + uint8_t primary; // current primary core + NavEKF3_core *core = nullptr; + const AP_AHRS *_ahrs; + AP_Baro &_baro; + const RangeFinder &_rng; + + // EKF Mavlink Tuneable Parameters + AP_Int8 _enable; // zero to disable EKF3 + AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s + AP_Float _gpsVertVelNoise; // GPS vertical velocity measurement noise : m/s + AP_Float _gpsHorizPosNoise; // GPS horizontal position measurement noise m + AP_Float _baroAltNoise; // Baro height measurement noise : m + AP_Float _magNoise; // magnetometer measurement noise : gauss + AP_Float _easNoise; // equivalent airspeed measurement noise : m/s + AP_Float _windVelProcessNoise; // wind velocity state process noise : m/s^2 + AP_Float _wndVarHgtRateScale; // scale factor applied to wind process noise due to height rate + AP_Float _magEarthProcessNoise; // Earth magnetic field process noise : gauss/sec + AP_Float _magBodyProcessNoise; // Body magnetic field process noise : gauss/sec + AP_Float _gyrNoise; // gyro process noise : rad/s + AP_Float _accNoise; // accelerometer process noise : m/s^2 + AP_Float _gyroBiasProcessNoise; // gyro bias state process noise : rad/s + AP_Float _accelBiasProcessNoise;// accel bias state process noise : m/s^2 + AP_Int16 _gpsDelay_ms; // effective average delay of GPS measurements relative to inertial measurement (msec) + AP_Int16 _hgtDelay_ms; // effective average delay of Height measurements relative to inertial measurements (msec) + AP_Int8 _fusionModeGPS; // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity + AP_Int16 _gpsVelInnovGate; // Percentage number of standard deviations applied to GPS velocity innovation consistency check + AP_Int16 _gpsPosInnovGate; // Percentage number of standard deviations applied to GPS position innovation consistency check + AP_Int16 _hgtInnovGate; // Percentage number of standard deviations applied to height innovation consistency check + AP_Int16 _magInnovGate; // Percentage number of standard deviations applied to magnetometer innovation consistency check + AP_Int16 _tasInnovGate; // Percentage number of standard deviations applied to true airspeed innovation consistency check + AP_Int8 _magCal; // Sets activation condition for in-flight magnetometer calibration + AP_Int8 _gpsGlitchRadiusMax; // Maximum allowed discrepancy between inertial and GPS Horizontal position before GPS glitch is declared : m + AP_Float _flowNoise; // optical flow rate measurement noise + AP_Int16 _flowInnovGate; // Percentage number of standard deviations applied to optical flow innovation consistency check + AP_Int8 _flowDelay_ms; // effective average delay of optical flow measurements rel to IMU (msec) + AP_Int16 _rngInnovGate; // Percentage number of standard deviations applied to range finder innovation consistency check + AP_Float _maxFlowRate; // Maximum flow rate magnitude that will be accepted by the filter + AP_Int8 _altSource; // Primary alt source during optical flow navigation. 0 = use Baro, 1 = use range finder. + AP_Float _rngNoise; // Range finder noise : m + AP_Int8 _gpsCheck; // Bitmask controlling which preflight GPS checks are bypassed + AP_Int8 _imuMask; // Bitmask of IMUs to instantiate EKF3 for + AP_Int16 _gpsCheckScaler; // Percentage increase to be applied to GPS pre-flight accuracy and drift thresholds + AP_Float _noaidHorizNoise; // horizontal position measurement noise assumed when synthesised zero position measurements are used to constrain attitude drift : m + AP_Int8 _logging_mask; // mask of IMUs to log + AP_Float _yawNoise; // magnetic yaw measurement noise : rad + AP_Int16 _yawInnovGate; // Percentage number of standard deviations applied to magnetic yaw innovation consistency check + AP_Int8 _tauVelPosOutput; // Time constant of output complementary filter : csec (centi-seconds) + AP_Int8 _useRngSwHgt; // Maximum valid range of the range finder in metres + AP_Float _terrGradMax; // Maximum terrain gradient below the vehicle + AP_Float _rngBcnNoise; // Range beacon measurement noise (m) + AP_Int16 _rngBcnInnovGate; // Percentage number of standard deviations applied to range beacon innovation consistency check + AP_Int8 _rngBcnDelay_ms; // effective average delay of range beacon measurements rel to IMU (msec) + + // Tuning parameters + const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration + const float gpsDVelVarAccScale; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration + const float gpsPosVarAccScale; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration + const uint16_t magDelay_ms; // Magnetometer measurement delay (msec) + const uint16_t tasDelay_ms; // Airspeed measurement delay (msec) + const uint16_t tiltDriftTimeMax_ms; // Maximum number of ms allowed without any form of tilt aiding (GPS, flow, TAS, etc) + const uint16_t posRetryTimeUseVel_ms; // Position aiding retry time with velocity measurements (msec) + const uint16_t posRetryTimeNoVel_ms; // Position aiding retry time without velocity measurements (msec) + const uint16_t hgtRetryTimeMode0_ms; // Height retry time with vertical velocity measurement (msec) + const uint16_t hgtRetryTimeMode12_ms; // Height retry time without vertical velocity measurement (msec) + const uint16_t tasRetryTime_ms; // True airspeed timeout and retry interval (msec) + const uint32_t magFailTimeLimit_ms; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec) + const float magVarRateScale; // scale factor applied to magnetometer variance due to angular rate + const float gyroBiasNoiseScaler; // scale factor applied to gyro bias state process noise when on ground + const uint16_t hgtAvg_ms; // average number of msec between height measurements + const uint16_t betaAvg_ms; // average number of msec between synthetic sideslip measurements + const float covTimeStepMax; // maximum time (sec) between covariance prediction updates + const float covDelAngMax; // maximum delta angle between covariance prediction updates + const float DCM33FlowMin; // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high. + const float fScaleFactorPnoise; // Process noise added to focal length scale factor state variance at each time step + const uint8_t flowTimeDeltaAvg_ms; // average interval between optical flow measurements (msec) + const uint32_t flowIntervalMax_ms; // maximum allowable time between flow fusion events + const uint16_t gndEffectTimeout_ms; // time in msec that ground effect mode is active after being activated + const float gndEffectBaroScaler; // scaler applied to the barometer observation variance when ground effect mode is active + const uint8_t gndGradientSigma; // RMS terrain gradient percentage assumed by the terrain height estimation + const uint8_t fusionTimeStep_ms; // The minimum time interval between covariance predictions and measurement fusions in msec + + struct { + bool enabled:1; + bool log_compass:1; + bool log_gps:1; + bool log_baro:1; + bool log_imu:1; + } logging; + + // time at start of current filter update + uint64_t imuSampleTime_us; + + struct { + uint32_t last_function_call; // last time getLastYawYawResetAngle was called + bool core_changed; // true when a core change happened and hasn't been consumed, false otherwise + uint32_t last_primary_change; // last time a primary has changed + float core_delta; // the amount of yaw change between cores when a change happened + } yaw_reset_data; + + struct { + uint32_t last_function_call; // last time getLastPosNorthEastReset was called + bool core_changed; // true when a core change happened and hasn't been consumed, false otherwise + uint32_t last_primary_change; // last time a primary has changed + Vector2f core_delta; // the amount of NE position change between cores when a change happened + } pos_reset_data; + + struct { + uint32_t last_function_call; // last time getLastPosDownReset was called + bool core_changed; // true when a core change happened and hasn't been consumed, false otherwise + uint32_t last_primary_change; // last time a primary has changed + float core_delta; // the amount of D position change between cores when a change happened + } pos_down_reset_data; + + // update the yaw reset data to capture changes due to a lane switch + // new_primary - index of the ekf instance that we are about to switch to as the primary + // old_primary - index of the ekf instance that we are currently using as the primary + void updateLaneSwitchYawResetData(uint8_t new_primary, uint8_t old_primary); + + // update the position reset data to capture changes due to a lane switch + // new_primary - index of the ekf instance that we are about to switch to as the primary + // old_primary - index of the ekf instance that we are currently using as the primary + void updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_primary); + + // update the position down reset data to capture changes due to a lane switch + // new_primary - index of the ekf instance that we are about to switch to as the primary + // old_primary - index of the ekf instance that we are currently using as the primary + void updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary); +}; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp new file mode 100644 index 0000000000..e362515f4a --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -0,0 +1,440 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 + +#include "AP_NavEKF3.h" +#include "AP_NavEKF3_core.h" +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +/******************************************************** +* RESET FUNCTIONS * +********************************************************/ + +/******************************************************** +* FUSE MEASURED_DATA * +********************************************************/ + +/* + * Fuse true airspeed measurements using explicit algebraic equations generated with Matlab symbolic toolbox. + * The script file used to generate these and other equations in this filter can be found here: + * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m +*/ +void NavEKF3_core::FuseAirspeed() +{ + // start performance timer + hal.util->perf_begin(_perf_FuseAirspeed); + + // declarations + float vn; + float ve; + float vd; + float vwn; + float vwe; + float EAS2TAS = _ahrs->get_EAS2TAS(); + const float R_TAS = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(EAS2TAS, 0.9f, 10.0f)); + float SH_TAS[3]; + float SK_TAS[2]; + Vector24 H_TAS = {}; + float VtasPred; + + // health is set bad until test passed + tasHealth = false; + + // copy required states to local variable names + vn = stateStruct.velocity.x; + ve = stateStruct.velocity.y; + vd = stateStruct.velocity.z; + vwn = stateStruct.wind_vel.x; + vwe = stateStruct.wind_vel.y; + + // calculate the predicted airspeed + VtasPred = norm((ve - vwe) , (vn - vwn) , vd); + // perform fusion of True Airspeed measurement + if (VtasPred > 1.0f) + { + // calculate observation jacobians + SH_TAS[0] = 1.0f/VtasPred; + SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f; + SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f; + H_TAS[4] = SH_TAS[2]; + H_TAS[5] = SH_TAS[1]; + H_TAS[6] = vd*SH_TAS[0]; + H_TAS[22] = -SH_TAS[2]; + H_TAS[23] = -SH_TAS[1]; + // calculate Kalman gains + float temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + if (temp >= R_TAS) { + SK_TAS[0] = 1.0f / temp; + faultStatus.bad_airspeed = false; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we reset the covariance matrix and try again next measurement + CovarianceInit(); + faultStatus.bad_airspeed = true; + return; + } + SK_TAS[1] = SH_TAS[1]; + Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]); + Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]); + Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]); + Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]); + Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]); + Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]); + Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]); + Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]); + Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]); + Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]); + Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]); + Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]); + Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]); + Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]); + Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]); + Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]); + Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]); + Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]); + // zero Kalman gains to inhibit magnetic field state estimation + if (!inhibitMagStates) { + Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]); + Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]); + Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]); + Kfusion[19] = SK_TAS[0]*(P[19][4]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][5]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][6]*vd*SH_TAS[0]); + Kfusion[20] = SK_TAS[0]*(P[20][4]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][5]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][6]*vd*SH_TAS[0]); + Kfusion[21] = SK_TAS[0]*(P[21][4]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][5]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][6]*vd*SH_TAS[0]); + } else { + for (uint8_t i=16; i<=21; i++) { + Kfusion[i] = 0.0f; + } + } + + // calculate measurement innovation variance + varInnovVtas = 1.0f/SK_TAS[0]; + + // calculate measurement innovation + innovVtas = VtasPred - tasDataDelayed.tas; + + // calculate the innovation consistency test ratio + tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (float)frontend->_tasInnovGate, 1.0f)) * varInnovVtas); + + // fail if the ratio is > 1, but don't fail if bad IMU data + tasHealth = ((tasTestRatio < 1.0f) || badIMUdata); + tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend->tasRetryTime_ms; + + // test the ratio before fusing data, forcing fusion if airspeed and position are timed out as we have no choice but to try and use airspeed to constrain error growth + if (tasHealth || (tasTimeout && posTimeout)) { + + // restart the counter + lastTasPassTime_ms = imuSampleTime_ms; + + // correct the state vector + for (uint8_t j= 0; j<=stateIndexLim; j++) { + statesArray[j] = statesArray[j] - Kfusion[j] * innovVtas; + } + stateStruct.quat.normalize(); + + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (unsigned i = 0; i<=stateIndexLim; i++) { + for (unsigned j = 0; j<=3; j++) { + KH[i][j] = 0.0f; + } + for (unsigned j = 4; j<=6; j++) { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (unsigned j = 7; j<=21; j++) { + KH[i][j] = 0.0f; + } + for (unsigned j = 22; j<=23; j++) { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + } + for (unsigned j = 0; j<=stateIndexLim; j++) { + for (unsigned i = 0; i<=stateIndexLim; i++) { + ftype res = 0; + res += KH[i][4] * P[4][j]; + res += KH[i][5] * P[5][j]; + res += KH[i][6] * P[6][j]; + res += KH[i][22] * P[22][j]; + res += KH[i][23] * P[23][j]; + KHP[i][j] = res; + } + } + for (unsigned i = 0; i<=stateIndexLim; i++) { + for (unsigned j = 0; j<=stateIndexLim; j++) { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + + // force the covariance matrix to me symmetrical and limit the variances to prevent ill-condiioning. + ForceSymmetry(); + ConstrainVariances(); + + // stop performance timer + hal.util->perf_end(_perf_FuseAirspeed); +} + +// select fusion of true airspeed measurements +void NavEKF3_core::SelectTasFusion() +{ + // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz + // If so, don't fuse measurements on this time step to reduce frame over-runs + // Only allow one time slip to prevent high rate magnetometer data locking out fusion of other measurements + if (magFusePerformed && dtIMUavg < 0.005f && !airSpdFusionDelayed) { + airSpdFusionDelayed = true; + return; + } else { + airSpdFusionDelayed = false; + } + + // get true airspeed measurement + readAirSpdData(); + + // If we haven't received airspeed data for a while, then declare the airspeed data as being timed out + if (imuSampleTime_ms - tasDataNew.time_ms > frontend->tasRetryTime_ms) { + tasTimeout = true; + } + + // if the filter is initialised, wind states are not inhibited and we have data to fuse, then perform TAS fusion + if (tasDataToFuse && statesInitialised && !inhibitWindStates) { + FuseAirspeed(); + prevTasStep_ms = imuSampleTime_ms; + } +} + + +// select fusion of synthetic sideslip measurements +// synthetic sidelip fusion only works for fixed wing aircraft and relies on the average sideslip being close to zero +// it requires a stable wind for best results and should not be used for aerobatic flight with manoeuvres that induce large sidslip angles (eg knife-edge, spins, etc) +void NavEKF3_core::SelectBetaFusion() +{ + // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz + // If so, don't fuse measurements on this time step to reduce frame over-runs + // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements + if (magFusePerformed && dtIMUavg < 0.005f && !sideSlipFusionDelayed) { + sideSlipFusionDelayed = true; + return; + } else { + sideSlipFusionDelayed = false; + } + + // set true when the fusion time interval has triggered + bool f_timeTrigger = ((imuSampleTime_ms - prevBetaStep_ms) >= frontend->betaAvg_ms); + // set true when use of synthetic sideslip fusion is necessary because we have limited sensor data or are dead reckoning position + bool f_required = !(use_compass() && useAirspeed() && ((imuSampleTime_ms - lastPosPassTime_ms) < frontend->posRetryTimeNoVel_ms)); + // set true when sideslip fusion is feasible (requires zero sideslip assumption to be valid and use of wind states) + bool f_feasible = (assume_zero_sideslip() && !inhibitWindStates); + // use synthetic sideslip fusion if feasible, required and enough time has lapsed since the last fusion + if (f_feasible && f_required && f_timeTrigger) { + FuseSideslip(); + prevBetaStep_ms = imuSampleTime_ms; + } +} + +/* + * Fuse sythetic sideslip measurement of zero using explicit algebraic equations generated with Matlab symbolic toolbox. + * The script file used to generate these and other equations in this filter can be found here: + * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m +*/ +void NavEKF3_core::FuseSideslip() +{ + // start performance timer + hal.util->perf_begin(_perf_FuseSideslip); + + // declarations + float q0; + float q1; + float q2; + float q3; + float vn; + float ve; + float vd; + float vwn; + float vwe; + const float R_BETA = 0.03f; // assume a sideslip angle RMS of ~10 deg + Vector13 SH_BETA; + Vector8 SK_BETA; + Vector3f vel_rel_wind; + Vector24 H_BETA; + float innovBeta; + + // copy required states to local variable names + q0 = stateStruct.quat[0]; + q1 = stateStruct.quat[1]; + q2 = stateStruct.quat[2]; + q3 = stateStruct.quat[3]; + vn = stateStruct.velocity.x; + ve = stateStruct.velocity.y; + vd = stateStruct.velocity.z; + vwn = stateStruct.wind_vel.x; + vwe = stateStruct.wind_vel.y; + + // calculate predicted wind relative velocity in NED + vel_rel_wind.x = vn - vwn; + vel_rel_wind.y = ve - vwe; + vel_rel_wind.z = vd; + + // rotate into body axes + vel_rel_wind = prevTnb * vel_rel_wind; + + // perform fusion of assumed sideslip = 0 + if (vel_rel_wind.x > 5.0f) + { + // Calculate observation jacobians + SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2); + if (fabsf(SH_BETA[0]) <= 1e-9f) { + faultStatus.bad_sideslip = true; + return; + } else { + faultStatus.bad_sideslip = false; + } + SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2); + SH_BETA[2] = vn - vwn; + SH_BETA[3] = ve - vwe; + SH_BETA[4] = 1/sq(SH_BETA[0]); + SH_BETA[5] = 1/SH_BETA[0]; + SH_BETA[6] = SH_BETA[5]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); + SH_BETA[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3); + SH_BETA[8] = 2*q0*SH_BETA[3] - 2*q3*SH_BETA[2] + 2*q1*vd; + SH_BETA[9] = 2*q0*SH_BETA[2] + 2*q3*SH_BETA[3] - 2*q2*vd; + SH_BETA[10] = 2*q2*SH_BETA[2] - 2*q1*SH_BETA[3] + 2*q0*vd; + SH_BETA[11] = 2*q1*SH_BETA[2] + 2*q2*SH_BETA[3] + 2*q3*vd; + SH_BETA[12] = 2*q0*q3; + + H_BETA[0] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; + H_BETA[1] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; + H_BETA[2] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; + H_BETA[3] = - SH_BETA[5]*SH_BETA[9] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; + H_BETA[4] = - SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) - SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; + H_BETA[5] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2); + H_BETA[6] = SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3); + for (uint8_t i=7; i<=21; i++) { + H_BETA[i] = 0.0f; + } + H_BETA[22] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; + H_BETA[23] = SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2) - SH_BETA[6]; + + // Calculate Kalman gains + float temp = (R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][4]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][4]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][4]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][4]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][4]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][4]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][4]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][22]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][22]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][22]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][22]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][22]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][22]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][22]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P[22][5]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][5]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][5]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][5]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][5]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][5]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][5]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P[22][23]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][23]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][23]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][23]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][23]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][23]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][23]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P[22][0]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][0]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][0]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][0]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][0]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][0]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][0]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P[22][1]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][1]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][1]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][1]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][1]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][1]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][1]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P[22][2]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][2]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][2]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][2]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][2]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][2]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][2]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][3]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][3]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][3]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][3]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][3]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][3]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][3]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))*(P[22][6]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][6]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][6]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][6]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][6]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][6]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][6]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3)))); + if (temp >= R_BETA) { + SK_BETA[0] = 1.0f / temp; + faultStatus.bad_sideslip = false; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we reset the covariance matrix and try again next measurement + CovarianceInit(); + faultStatus.bad_sideslip = true; + return; + } + SK_BETA[1] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; + SK_BETA[2] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2); + SK_BETA[3] = SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3); + SK_BETA[4] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; + SK_BETA[5] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; + SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; + SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; + + Kfusion[0] = SK_BETA[0]*(P[0][0]*SK_BETA[5] + P[0][1]*SK_BETA[4] - P[0][4]*SK_BETA[1] + P[0][5]*SK_BETA[2] + P[0][2]*SK_BETA[6] + P[0][6]*SK_BETA[3] - P[0][3]*SK_BETA[7] + P[0][22]*SK_BETA[1] - P[0][23]*SK_BETA[2]); + Kfusion[1] = SK_BETA[0]*(P[1][0]*SK_BETA[5] + P[1][1]*SK_BETA[4] - P[1][4]*SK_BETA[1] + P[1][5]*SK_BETA[2] + P[1][2]*SK_BETA[6] + P[1][6]*SK_BETA[3] - P[1][3]*SK_BETA[7] + P[1][22]*SK_BETA[1] - P[1][23]*SK_BETA[2]); + Kfusion[2] = SK_BETA[0]*(P[2][0]*SK_BETA[5] + P[2][1]*SK_BETA[4] - P[2][4]*SK_BETA[1] + P[2][5]*SK_BETA[2] + P[2][2]*SK_BETA[6] + P[2][6]*SK_BETA[3] - P[2][3]*SK_BETA[7] + P[2][22]*SK_BETA[1] - P[2][23]*SK_BETA[2]); + Kfusion[3] = SK_BETA[0]*(P[3][0]*SK_BETA[5] + P[3][1]*SK_BETA[4] - P[3][4]*SK_BETA[1] + P[3][5]*SK_BETA[2] + P[3][2]*SK_BETA[6] + P[3][6]*SK_BETA[3] - P[3][3]*SK_BETA[7] + P[3][22]*SK_BETA[1] - P[3][23]*SK_BETA[2]); + Kfusion[4] = SK_BETA[0]*(P[4][0]*SK_BETA[5] + P[4][1]*SK_BETA[4] - P[4][4]*SK_BETA[1] + P[4][5]*SK_BETA[2] + P[4][2]*SK_BETA[6] + P[4][6]*SK_BETA[3] - P[4][3]*SK_BETA[7] + P[4][22]*SK_BETA[1] - P[4][23]*SK_BETA[2]); + Kfusion[5] = SK_BETA[0]*(P[5][0]*SK_BETA[5] + P[5][1]*SK_BETA[4] - P[5][4]*SK_BETA[1] + P[5][5]*SK_BETA[2] + P[5][2]*SK_BETA[6] + P[5][6]*SK_BETA[3] - P[5][3]*SK_BETA[7] + P[5][22]*SK_BETA[1] - P[5][23]*SK_BETA[2]); + Kfusion[6] = SK_BETA[0]*(P[6][0]*SK_BETA[5] + P[6][1]*SK_BETA[4] - P[6][4]*SK_BETA[1] + P[6][5]*SK_BETA[2] + P[6][2]*SK_BETA[6] + P[6][6]*SK_BETA[3] - P[6][3]*SK_BETA[7] + P[6][22]*SK_BETA[1] - P[6][23]*SK_BETA[2]); + Kfusion[7] = SK_BETA[0]*(P[7][0]*SK_BETA[5] + P[7][1]*SK_BETA[4] - P[7][4]*SK_BETA[1] + P[7][5]*SK_BETA[2] + P[7][2]*SK_BETA[6] + P[7][6]*SK_BETA[3] - P[7][3]*SK_BETA[7] + P[7][22]*SK_BETA[1] - P[7][23]*SK_BETA[2]); + Kfusion[8] = SK_BETA[0]*(P[8][0]*SK_BETA[5] + P[8][1]*SK_BETA[4] - P[8][4]*SK_BETA[1] + P[8][5]*SK_BETA[2] + P[8][2]*SK_BETA[6] + P[8][6]*SK_BETA[3] - P[8][3]*SK_BETA[7] + P[8][22]*SK_BETA[1] - P[8][23]*SK_BETA[2]); + Kfusion[9] = SK_BETA[0]*(P[9][0]*SK_BETA[5] + P[9][1]*SK_BETA[4] - P[9][4]*SK_BETA[1] + P[9][5]*SK_BETA[2] + P[9][2]*SK_BETA[6] + P[9][6]*SK_BETA[3] - P[9][3]*SK_BETA[7] + P[9][22]*SK_BETA[1] - P[9][23]*SK_BETA[2]); + Kfusion[10] = SK_BETA[0]*(P[10][0]*SK_BETA[5] + P[10][1]*SK_BETA[4] - P[10][4]*SK_BETA[1] + P[10][5]*SK_BETA[2] + P[10][2]*SK_BETA[6] + P[10][6]*SK_BETA[3] - P[10][3]*SK_BETA[7] + P[10][22]*SK_BETA[1] - P[10][23]*SK_BETA[2]); + Kfusion[11] = SK_BETA[0]*(P[11][0]*SK_BETA[5] + P[11][1]*SK_BETA[4] - P[11][4]*SK_BETA[1] + P[11][5]*SK_BETA[2] + P[11][2]*SK_BETA[6] + P[11][6]*SK_BETA[3] - P[11][3]*SK_BETA[7] + P[11][22]*SK_BETA[1] - P[11][23]*SK_BETA[2]); + Kfusion[12] = SK_BETA[0]*(P[12][0]*SK_BETA[5] + P[12][1]*SK_BETA[4] - P[12][4]*SK_BETA[1] + P[12][5]*SK_BETA[2] + P[12][2]*SK_BETA[6] + P[12][6]*SK_BETA[3] - P[12][3]*SK_BETA[7] + P[12][22]*SK_BETA[1] - P[12][23]*SK_BETA[2]); + Kfusion[13] = SK_BETA[0]*(P[13][0]*SK_BETA[5] + P[13][1]*SK_BETA[4] - P[13][4]*SK_BETA[1] + P[13][5]*SK_BETA[2] + P[13][2]*SK_BETA[6] + P[13][6]*SK_BETA[3] - P[13][3]*SK_BETA[7] + P[13][22]*SK_BETA[1] - P[13][23]*SK_BETA[2]); + Kfusion[14] = SK_BETA[0]*(P[14][0]*SK_BETA[5] + P[14][1]*SK_BETA[4] - P[14][4]*SK_BETA[1] + P[14][5]*SK_BETA[2] + P[14][2]*SK_BETA[6] + P[14][6]*SK_BETA[3] - P[14][3]*SK_BETA[7] + P[14][22]*SK_BETA[1] - P[14][23]*SK_BETA[2]); + Kfusion[15] = SK_BETA[0]*(P[15][0]*SK_BETA[5] + P[15][1]*SK_BETA[4] - P[15][4]*SK_BETA[1] + P[15][5]*SK_BETA[2] + P[15][2]*SK_BETA[6] + P[15][6]*SK_BETA[3] - P[15][3]*SK_BETA[7] + P[15][22]*SK_BETA[1] - P[15][23]*SK_BETA[2]); + Kfusion[22] = SK_BETA[0]*(P[22][0]*SK_BETA[5] + P[22][1]*SK_BETA[4] - P[22][4]*SK_BETA[1] + P[22][5]*SK_BETA[2] + P[22][2]*SK_BETA[6] + P[22][6]*SK_BETA[3] - P[22][3]*SK_BETA[7] + P[22][22]*SK_BETA[1] - P[22][23]*SK_BETA[2]); + Kfusion[23] = SK_BETA[0]*(P[23][0]*SK_BETA[5] + P[23][1]*SK_BETA[4] - P[23][4]*SK_BETA[1] + P[23][5]*SK_BETA[2] + P[23][2]*SK_BETA[6] + P[23][6]*SK_BETA[3] - P[23][3]*SK_BETA[7] + P[23][22]*SK_BETA[1] - P[23][23]*SK_BETA[2]); + // zero Kalman gains to inhibit magnetic field state estimation + if (!inhibitMagStates) { + Kfusion[16] = SK_BETA[0]*(P[16][0]*SK_BETA[5] + P[16][1]*SK_BETA[4] - P[16][4]*SK_BETA[1] + P[16][5]*SK_BETA[2] + P[16][2]*SK_BETA[6] + P[16][6]*SK_BETA[3] - P[16][3]*SK_BETA[7] + P[16][22]*SK_BETA[1] - P[16][23]*SK_BETA[2]); + Kfusion[17] = SK_BETA[0]*(P[17][0]*SK_BETA[5] + P[17][1]*SK_BETA[4] - P[17][4]*SK_BETA[1] + P[17][5]*SK_BETA[2] + P[17][2]*SK_BETA[6] + P[17][6]*SK_BETA[3] - P[17][3]*SK_BETA[7] + P[17][22]*SK_BETA[1] - P[17][23]*SK_BETA[2]); + Kfusion[18] = SK_BETA[0]*(P[18][0]*SK_BETA[5] + P[18][1]*SK_BETA[4] - P[18][4]*SK_BETA[1] + P[18][5]*SK_BETA[2] + P[18][2]*SK_BETA[6] + P[18][6]*SK_BETA[3] - P[18][3]*SK_BETA[7] + P[18][22]*SK_BETA[1] - P[18][23]*SK_BETA[2]); + Kfusion[19] = SK_BETA[0]*(P[19][0]*SK_BETA[5] + P[19][1]*SK_BETA[4] - P[19][4]*SK_BETA[1] + P[19][5]*SK_BETA[2] + P[19][2]*SK_BETA[6] + P[19][6]*SK_BETA[3] - P[19][3]*SK_BETA[7] + P[19][22]*SK_BETA[1] - P[19][23]*SK_BETA[2]); + Kfusion[20] = SK_BETA[0]*(P[20][0]*SK_BETA[5] + P[20][1]*SK_BETA[4] - P[20][4]*SK_BETA[1] + P[20][5]*SK_BETA[2] + P[20][2]*SK_BETA[6] + P[20][6]*SK_BETA[3] - P[20][3]*SK_BETA[7] + P[20][22]*SK_BETA[1] - P[20][23]*SK_BETA[2]); + Kfusion[21] = SK_BETA[0]*(P[21][0]*SK_BETA[5] + P[21][1]*SK_BETA[4] - P[21][4]*SK_BETA[1] + P[21][5]*SK_BETA[2] + P[21][2]*SK_BETA[6] + P[21][6]*SK_BETA[3] - P[21][3]*SK_BETA[7] + P[21][22]*SK_BETA[1] - P[21][23]*SK_BETA[2]); + } else { + for (uint8_t i=16; i<=21; i++) { + Kfusion[i] = 0.0f; + } + } + + // calculate predicted sideslip angle and innovation using small angle approximation + innovBeta = vel_rel_wind.y / vel_rel_wind.x; + + // reject measurement if greater than 3-sigma inconsistency + if (innovBeta > 0.5f) { + return; + } + + // correct the state vector + for (uint8_t j= 0; j<=stateIndexLim; j++) { + statesArray[j] = statesArray[j] - Kfusion[j] * innovBeta; + } + stateStruct.quat.normalize(); + + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (unsigned i = 0; i<=stateIndexLim; i++) { + for (unsigned j = 0; j<=6; j++) { + KH[i][j] = Kfusion[i] * H_BETA[j]; + } + for (unsigned j = 7; j<=21; j++) { + KH[i][j] = 0.0f; + } + for (unsigned j = 22; j<=23; j++) { + KH[i][j] = Kfusion[i] * H_BETA[j]; + } + } + for (unsigned j = 0; j<=stateIndexLim; j++) { + for (unsigned i = 0; i<=stateIndexLim; i++) { + ftype res = 0; + res += KH[i][0] * P[0][j]; + res += KH[i][1] * P[1][j]; + res += KH[i][2] * P[2][j]; + res += KH[i][3] * P[3][j]; + res += KH[i][4] * P[4][j]; + res += KH[i][5] * P[5][j]; + res += KH[i][6] * P[6][j]; + res += KH[i][22] * P[22][j]; + res += KH[i][23] * P[23][j]; + KHP[i][j] = res; + } + } + for (unsigned i = 0; i<=stateIndexLim; i++) { + for (unsigned j = 0; j<=stateIndexLim; j++) { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + ForceSymmetry(); + ConstrainVariances(); + + // stop the performance timer + hal.util->perf_end(_perf_FuseSideslip); +} + +/******************************************************** +* MISC FUNCTIONS * +********************************************************/ + + +#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Buffer.h b/libraries/AP_NavEKF3/AP_NavEKF3_Buffer.h new file mode 100644 index 0000000000..d9ef1c618d --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Buffer.h @@ -0,0 +1,188 @@ +// EKF Buffer models + +// this buffer model is to be used for observation buffers, +// the data is pushed into buffer like any standard ring buffer +// return is based on the sample time provided +template +class obs_ring_buffer_t +{ +public: + struct element_t{ + element_type element; + } *buffer; + + // initialise buffer, returns false when allocation has failed + bool init(uint32_t size) + { + buffer = new element_t[size]; + if(buffer == nullptr) + { + return false; + } + memset(buffer,0,size*sizeof(element_t)); + _size = size; + _head = 0; + _tail = 0; + _new_data = false; + return true; + } + + /* + * Searches through a ring buffer and return the newest data that is older than the + * time specified by sample_time_ms + * Zeros old data so it cannot not be used again + * Returns false if no data can be found that is less than 100msec old + */ + + bool recall(element_type &element,uint32_t sample_time) + { + if(!_new_data) { + return false; + } + bool success = false; + uint8_t tail = _tail, bestIndex; + + if(_head == tail) { + if (buffer[tail].element.time_ms != 0 && buffer[tail].element.time_ms <= sample_time) { + // if head is equal to tail just check if the data is unused and within time horizon window + if (((sample_time - buffer[tail].element.time_ms) < 100)) { + bestIndex = tail; + success = true; + _new_data = false; + } + } + } else { + while(_head != tail) { + // find a measurement older than the fusion time horizon that we haven't checked before + if (buffer[tail].element.time_ms != 0 && buffer[tail].element.time_ms <= sample_time) { + // Find the most recent non-stale measurement that meets the time horizon criteria + if (((sample_time - buffer[tail].element.time_ms) < 100)) { + bestIndex = tail; + success = true; + } + } else if(buffer[tail].element.time_ms > sample_time){ + break; + } + tail = (tail+1)%_size; + } + } + + if (success) { + element = buffer[bestIndex].element; + _tail = (bestIndex+1)%_size; + //make time zero to stop using it again, + //resolves corner case of reusing the element when head == tail + buffer[bestIndex].element.time_ms = 0; + return true; + } else { + return false; + } + } + + /* + * Writes data and timestamp to a Ring buffer and advances indices that + * define the location of the newest and oldest data + */ + inline void push(element_type element) + { + // Advance head to next available index + _head = (_head+1)%_size; + // New data is written at the head + buffer[_head].element = element; + _new_data = true; + } + // writes the same data to all elements in the ring buffer + inline void reset_history(element_type element, uint32_t sample_time) { + for (uint8_t index=0; index<_size; index++) { + buffer[index].element = element; + } + } + + // zeroes all data in the ring buffer + inline void reset() { + _head = 0; + _tail = 0; + _new_data = false; + memset(buffer,0,_size*sizeof(element_t)); + } + +private: + uint8_t _size,_head,_tail,_new_data; +}; + + +// Following buffer model is for IMU data, +// it achieves a distance of sample size +// between youngest and oldest +template +class imu_ring_buffer_t +{ +public: + struct element_t{ + element_type element; + } *buffer; + + // initialise buffer, returns false when allocation has failed + bool init(uint32_t size) + { + buffer = new element_t[size]; + if(buffer == nullptr) + { + return false; + } + memset(buffer,0,size*sizeof(element_t)); + _size = size; + _youngest = 0; + _oldest = 0; + return true; + } + /* + * Writes data to a Ring buffer and advances indices that + * define the location of the newest and oldest data + */ + inline void push_youngest_element(element_type element) + { + // push youngest to the buffer + _youngest = (_youngest+1)%_size; + buffer[_youngest].element = element; + // set oldest data index + _oldest = (_youngest+1)%_size; + } + + // retrieve the oldest data from the ring buffer tail + inline element_type pop_oldest_element() { + element_type ret = buffer[_oldest].element; + return ret; + } + + // writes the same data to all elements in the ring buffer + inline void reset_history(element_type element) { + for (uint8_t index=0; index<_size; index++) { + buffer[index].element = element; + } + } + + // zeroes all data in the ring buffer + inline void reset() { + _youngest = 0; + _oldest = 0; + memset(buffer,0,_size*sizeof(element_t)); + } + + // retrieves data from the ring buffer at a specified index + inline element_type& operator[](uint32_t index) { + return buffer[index].element; + } + + // returns the index for the ring buffer oldest data + inline uint8_t get_oldest_index(){ + return _oldest; + } + + // returns the index for the ring buffer youngest data + inline uint8_t get_youngest_index(){ + return _youngest; + } +private: + uint8_t _size,_oldest,_youngest; +}; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp new file mode 100644 index 0000000000..2823693366 --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -0,0 +1,502 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 + +#include "AP_NavEKF3.h" +#include "AP_NavEKF3_core.h" +#include +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + + +// Control filter mode transitions +void NavEKF3_core::controlFilterModes() +{ + // Determine motor arm status + prevMotorsArmed = motorsArmed; + motorsArmed = hal.util->get_soft_armed(); + if (motorsArmed && !prevMotorsArmed) { + // set the time at which we arm to assist with checks + timeAtArming_ms = imuSampleTime_ms; + } + + // Detect if we are in flight on or ground + detectFlight(); + + // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to + // avoid unnecessary operations + setWindMagStateLearningMode(); + + // Check the alignmnent status of the tilt and yaw attitude + // Used during initial bootstrap alignment of the filter + checkAttitudeAlignmentStatus(); + + // Set the type of inertial navigation aiding used + setAidingMode(); + +} + +/* + return effective value for _magCal for this core + */ +uint8_t NavEKF3_core::effective_magCal(void) const +{ + // if we are on the 2nd core and _magCal is 3 then treat it as + // 2. This is a workaround for a mag fusion problem + if (frontend->_magCal ==3 && imu_index == 1) { + return 2; + } + return frontend->_magCal; +} + +// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to +// avoid unnecessary operations +void NavEKF3_core::setWindMagStateLearningMode() +{ + // If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states + bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE); + if (!inhibitWindStates && setWindInhibit) { + inhibitWindStates = true; + } else if (inhibitWindStates && !setWindInhibit) { + inhibitWindStates = false; + // set states and variances + if (yawAlignComplete && useAirspeed()) { + // if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading + // which assumes the vehicle has launched into the wind + Vector3f tempEuler; + stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); + float windSpeed = sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; + stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z); + stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z); + + // set the wind sate variances to the measurement uncertainty + for (uint8_t index=22; index<=23; index++) { + P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(_ahrs->get_EAS2TAS(), 0.9f, 10.0f)); + } + } else { + // set the variances using a typical wind speed + for (uint8_t index=22; index<=23; index++) { + P[index][index] = sq(5.0f); + } + } + } + + // determine if the vehicle is manoevring + if (accNavMagHoriz > 0.5f) { + manoeuvring = true; + } else { + manoeuvring = false; + } + + // Determine if learning of magnetic field states has been requested by the user + uint8_t magCal = effective_magCal(); + bool magCalRequested = + ((magCal == 0) && inFlight) || // when flying + ((magCal == 1) && manoeuvring) || // when manoeuvring + ((magCal == 3) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete + (magCal == 4); // all the time + + // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user, + // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user) + bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4); + + // Inhibit the magnetic field calibration if not requested or denied + bool setMagInhibit = !magCalRequested || magCalDenied; + if (!inhibitMagStates && setMagInhibit) { + inhibitMagStates = true; + } else if (inhibitMagStates && !setMagInhibit) { + inhibitMagStates = false; + if (magFieldLearned) { + // if we have already learned the field states, then retain the learned variances + P[16][16] = earthMagFieldVar.x; + P[17][17] = earthMagFieldVar.y; + P[18][18] = earthMagFieldVar.z; + P[19][19] = bodyMagFieldVar.x; + P[20][20] = bodyMagFieldVar.y; + P[21][21] = bodyMagFieldVar.z; + } else { + // set the variances equal to the observation variances + for (uint8_t index=18; index<=21; index++) { + P[index][index] = sq(frontend->_magNoise); + } + + // set the NE earth magnetic field states using the published declination + // and set the corresponding variances and covariances + alignMagStateDeclination(); + + } + // request a reset of the yaw and magnetic field states if not done before + if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) { + magYawResetRequest = true; + } + } + + // inhibit delta velocity bias learning if we have not yet aligned the tilt + if (tiltAlignComplete && inhibitDelVelBiasStates) { + // activate the states + inhibitDelVelBiasStates = false; + // set the initial covariance values + P[13][13] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtEkfAvg); + P[14][14] = P[13][13]; + P[15][15] = P[13][13]; + } + + if (tiltAlignComplete && inhibitDelAngBiasStates) { + // activate the states + inhibitDelAngBiasStates = false; + // set the initial covariance values + P[10][10] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg)); + P[11][11] = P[10][10]; + P[12][12] = P[10][10]; + } + + // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed + // because we want it re-done for each takeoff + if (onGround) { + finalInflightYawInit = false; + finalInflightMagInit = false; + } + + // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations + // if we are not using those states + if (inhibitMagStates && inhibitWindStates && inhibitDelVelBiasStates) { + stateIndexLim = 12; + } else if (inhibitMagStates && !inhibitWindStates) { + stateIndexLim = 15; + } else if (inhibitWindStates) { + stateIndexLim = 21; + } else { + stateIndexLim = 23; + } +} + +// Set inertial navigation aiding mode +void NavEKF3_core::setAidingMode() +{ + // Save the previous status so we can detect when it has changed + PV_AidingModePrev = PV_AidingMode; + + // Determine if we should change aiding mode + if (PV_AidingMode == AID_NONE) { + // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete + // and IMU gyro bias estimates have stabilised + bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus(); + // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present + // GPS aiding is the preferred option unless excluded by the user + bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit); + bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable; + if(canUseGPS || canUseRangeBeacon) { + PV_AidingMode = AID_ABSOLUTE; + } else if (optFlowDataPresent() && filterIsStable) { + PV_AidingMode = AID_RELATIVE; + } + } else if (PV_AidingMode == AID_RELATIVE) { + // Check if the optical flow sensor has timed out + bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000); + // Check if the fusion has timed out (flow measurements have been rejected for too long) + bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000); + // Enable switch to absolute position mode if GPS is available + // If GPS is not available and flow fusion has timed out, then fall-back to no-aiding + if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) { + PV_AidingMode = AID_ABSOLUTE; + } else if (flowSensorTimeout || flowFusionTimeout) { + PV_AidingMode = AID_NONE; + } + } else if (PV_AidingMode == AID_ABSOLUTE) { + // Find the minimum time without data required to trigger any check + uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms)); + + // Check if optical flow data is being used + bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms > minTestTime_ms); + + // Check if airspeed data is being used + bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms > minTestTime_ms); + + // Check if range beacon data is being used + bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms > minTestTime_ms); + + // Check if GPS is being used + bool gpsPosUsed = (imuSampleTime_ms - lastPosPassTime_ms > minTestTime_ms); + bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms > minTestTime_ms); + + // Check if attitude drift has been constrained by a measurement source + bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed; + + // check if velocity drift has been constrained by a measurement source + bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed; + + // check if position drift has been constrained by a measurement source + bool posAiding = gpsPosUsed || rngBcnUsed; + + // Check if the loss of attitude aiding has become critical + bool attAidLossCritical = false; + if (!attAiding) { + attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) && + (imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) && + (imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) && + (imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) && + (imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms); + } + + // Check if the loss of position accuracy has become critical + bool posAidLossCritical = false; + if (!posAiding ) { + uint16_t maxLossTime_ms; + if (!velAiding) { + maxLossTime_ms = frontend->posRetryTimeNoVel_ms; + } else { + maxLossTime_ms = frontend->posRetryTimeUseVel_ms; + } + posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) && + (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms) && + (imuSampleTime_ms - lastVelPassTime_ms > maxLossTime_ms); + } + + if (attAidLossCritical) { + // if the loss of attitude data is critical, then put the filter into a constant position mode + PV_AidingMode = AID_NONE; + posTimeout = true; + velTimeout = true; + rngBcnTimeout = true; + tasTimeout = true; + gpsNotAvailable = true; + } else if (posAidLossCritical) { + // if the loss of position is critical, declare all sources of position aiding as being timed out + posTimeout = true; + velTimeout = true; + rngBcnTimeout = true; + gpsNotAvailable = true; + } + + } + + // check to see if we are starting or stopping aiding and set states and modes as required + if (PV_AidingMode != PV_AidingModePrev) { + // set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped. + if (PV_AidingMode == AID_NONE) { + // We have ceased aiding + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF3 IMU%u has stopped aiding",(unsigned)imu_index); + // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors + posTimeout = true; + velTimeout = true; + // Reset the normalised innovation to avoid false failing bad fusion tests + velTestRatio = 0.0f; + posTestRatio = 0.0f; + // store the current position to be used to keep reporting the last known position + lastKnownPositionNE.x = stateStruct.position.x; + lastKnownPositionNE.y = stateStruct.position.y; + // initialise filtered altitude used to provide a takeoff reference to current baro on disarm + // this reduces the time required for the baro noise filter to settle before the filtered baro data can be used + meaHgtAtTakeOff = baroDataDelayed.hgt; + // reset the vertical position state to faster recover from baro errors experienced during touchdown + stateStruct.position.z = -meaHgtAtTakeOff; + } else if (PV_AidingMode == AID_RELATIVE) { + // We have commenced aiding, but GPS usage has been prohibited so use optical flow only + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using optical flow",(unsigned)imu_index); + posTimeout = true; + velTimeout = true; + // Reset the last valid flow measurement time + flowValidMeaTime_ms = imuSampleTime_ms; + // Reset the last valid flow fusion time + prevFlowFuseTime_ms = imuSampleTime_ms; + } else if (PV_AidingMode == AID_ABSOLUTE) { + bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit); + bool canUseRangeBeacon = readyToUseRangeBeacon(); + // We have commenced aiding and GPS usage is allowed + if (canUseGPS) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using GPS",(unsigned)imu_index); + } + posTimeout = false; + velTimeout = false; + // We have commenced aiding and range beacon usage is allowed + if (canUseRangeBeacon) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using range beacons",(unsigned)imu_index); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset); + } + // reset the last fusion accepted times to prevent unwanted activation of timeout logic + lastPosPassTime_ms = imuSampleTime_ms; + lastVelPassTime_ms = imuSampleTime_ms; + lastRngBcnPassTime_ms = imuSampleTime_ms; + } + + // Always reset the position and velocity when changing mode + ResetVelocity(); + ResetPosition(); + + } + +} + +// Check the tilt and yaw alignmnent status +// Used during initial bootstrap alignment of the filter +void NavEKF3_core::checkAttitudeAlignmentStatus() +{ + // Check for tilt convergence - used during initial alignment + if (norm(P[0][0],P[1][1],P[2][2],P[3][3]) < sq(0.03f) && !tiltAlignComplete) { + tiltAlignComplete = true; + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete\n",(unsigned)imu_index); + } + + // submit yaw and magnetic field reset requests depending on whether we have compass data + if (tiltAlignComplete && !yawAlignComplete) { + if (use_compass()) { + magYawResetRequest = true; + gpsYawResetRequest = false; + } else { + magYawResetRequest = false; + gpsYawResetRequest = true; + } + } +} + +// return true if we should use the airspeed sensor +bool NavEKF3_core::useAirspeed(void) const +{ + return _ahrs->airspeed_sensor_enabled(); +} + +// return true if we should use the range finder sensor +bool NavEKF3_core::useRngFinder(void) const +{ + // TO-DO add code to set this based in setting of optical flow use parameter and presence of sensor + return true; +} + +// return true if optical flow data is available +bool NavEKF3_core::optFlowDataPresent(void) const +{ + return (imuSampleTime_ms - flowMeaTime_ms < 200); +} + +// return true if the filter to be ready to use gps +bool NavEKF3_core::readyToUseGPS(void) const +{ + return validOrigin && tiltAlignComplete && yawAlignComplete && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse; +} + +// return true if the filter to be ready to use the beacon range measurements +bool NavEKF3_core::readyToUseRangeBeacon(void) const +{ + return tiltAlignComplete && yawAlignComplete && rngBcnGoodToAlign && rngBcnDataToFuse; +} + +// return true if we should use the compass +bool NavEKF3_core::use_compass(void) const +{ + return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed; +} + +/* + should we assume zero sideslip? + */ +bool NavEKF3_core::assume_zero_sideslip(void) const +{ + // we don't assume zero sideslip for ground vehicles as EKF could + // be quite sensitive to a rapid spin of the ground vehicle if + // traction is lost + return _ahrs->get_fly_forward() && _ahrs->get_vehicle_class() != AHRS_VEHICLE_GROUND; +} + +// set the LLH location of the filters NED origin +bool NavEKF3_core::setOriginLLH(struct Location &loc) +{ + if (PV_AidingMode == AID_ABSOLUTE) { + return false; + } + EKF_origin = loc; + // define Earth rotation vector in the NED navigation frame at the origin + calcEarthRateNED(earthRateNED, _ahrs->get_home().lat); + validOrigin = true; + return true; +} + +// Set the NED origin to be used until the next filter reset +void NavEKF3_core::setOrigin() +{ + // assume origin at current GPS location (no averaging) + EKF_origin = _ahrs->get_gps().location(); + // define Earth rotation vector in the NED navigation frame at the origin + calcEarthRateNED(earthRateNED, _ahrs->get_home().lat); + validOrigin = true; + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u Origin set to GPS",(unsigned)imu_index); +} + +// record a yaw reset event +void NavEKF3_core::recordYawReset() +{ + yawAlignComplete = true; + if (inFlight) { + finalInflightYawInit = true; + } +} + +// return true and set the class variable true if the delta angle bias has been learned +bool NavEKF3_core::checkGyroCalStatus(void) +{ + // check delta angle bias variances + const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg)); + delAngBiasLearned = (P[10][10] <= delAngBiasVarMax) && + (P[11][11] <= delAngBiasVarMax) && + (P[12][12] <= delAngBiasVarMax); + return delAngBiasLearned; +} + +// Commands the EKF to not use GPS. +// This command must be sent prior to arming +// This command is forgotten by the EKF each time the vehicle disarms +// Returns 0 if command rejected +// Returns 1 if attitude, vertical velocity and vertical position will be provided +// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided +uint8_t NavEKF3_core::setInhibitGPS(void) +{ + if((PV_AidingMode == AID_ABSOLUTE) && motorsArmed) { + return 0; + } else { + gpsInhibit = true; + return 1; + } + // option 2 is not yet implemented as it requires a deeper integration of optical flow and GPS operation +} + +// Update the filter status +void NavEKF3_core::updateFilterStatus(void) +{ + // init return value + filterStatus.value = 0; + bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid; + bool doingWindRelNav = !tasTimeout && assume_zero_sideslip(); + bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE); + bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout; + bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav; + bool optFlowNavPossible = flowDataValid && delAngBiasLearned; + bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned; + bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE))); + // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks + bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin; + + // set individual flags + filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check) + filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid + filterStatus.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid + filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid + filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid + filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid + filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid + filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode + filterStatus.flags.pred_horiz_pos_rel = ((optFlowNavPossible || gpsNavPossible) && filterHealthy) || filterStatus.flags.horiz_pos_rel; // we should be able to estimate a relative position when we enter flight mode + filterStatus.flags.pred_horiz_pos_abs = (gpsNavPossible && filterHealthy) || filterStatus.flags.horiz_pos_abs; // we should be able to estimate an absolute position when we enter flight mode + filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected + filterStatus.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode + filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode + filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE); + filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE); // The GPS is glitching +} + +#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp new file mode 100644 index 0000000000..78b5ca5ded --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp @@ -0,0 +1,37 @@ +#include + +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 + +#include "AP_NavEKF3.h" +#include "AP_NavEKF3_core.h" +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +// reset the body axis gyro bias states to zero and re-initialise the corresponding covariances +// Assume that the calibration is performed to an accuracy of 0.5 deg/sec which will require averaging under static conditions +// WARNING - a non-blocking calibration method must be used +void NavEKF3_core::resetGyroBias(void) +{ + stateStruct.gyro_bias.zero(); + zeroRows(P,10,12); + zeroCols(P,10,12); + + P[10][10] = sq(radians(0.5f * dtIMUavg)); + P[11][11] = P[10][10]; + P[12][12] = P[10][10]; +} + +/* + vehicle specific initial gyro bias uncertainty in deg/sec + */ +float NavEKF3_core::InitialGyroBiasUncertainty(void) const +{ + return 2.5f; +} + + +#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp new file mode 100644 index 0000000000..b309cfdd8b --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -0,0 +1,1133 @@ +#include + +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 + +#include "AP_NavEKF3.h" +#include "AP_NavEKF3_core.h" +#include +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +/******************************************************** +* RESET FUNCTIONS * +********************************************************/ + +// Control reset of yaw and magnetic field states +void NavEKF3_core::controlMagYawReset() +{ + + // Vehicles that can use a zero sideslip assumption (Planes) are a special case + // They can use the GPS velocity to recover from bad initial compass data + // This allows recovery for heading alignment errors due to compass faults + if (assume_zero_sideslip() && !finalInflightYawInit && inFlight ) { + gpsYawResetRequest = true; + return; + } else { + gpsYawResetRequest = false; + } + + // Quaternion and delta rotation vector that are re-used for different calculations + Vector3f deltaRotVecTemp; + Quaternion deltaQuatTemp; + + bool flightResetAllowed = false; + bool initialResetAllowed = false; + if (!finalInflightYawInit) { + // Use a quaternion division to calculate the delta quaternion between the rotation at the current and last time + deltaQuatTemp = stateStruct.quat / prevQuatMagReset; + prevQuatMagReset = stateStruct.quat; + + // convert the quaternion to a rotation vector and find its length + deltaQuatTemp.to_axis_angle(deltaRotVecTemp); + + // check if the spin rate is OK - high spin rates can cause angular alignment errors + bool angRateOK = deltaRotVecTemp.length() < 0.1745f; + + initialResetAllowed = angRateOK; + flightResetAllowed = angRateOK && !onGround; + + } + + // Check if conditions for a interim or final yaw/mag reset are met + bool finalResetRequest = false; + bool interimResetRequest = false; + if (flightResetAllowed && !assume_zero_sideslip()) { + // check that we have reached a height where ground magnetic interference effects are insignificant + // and can perform a final reset of the yaw and field states + finalResetRequest = (stateStruct.position.z - posDownAtTakeoff) < -5.0f; + + // check for increasing height + bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f; + float yawInnovIncrease = fabsf(innovYaw) - fabsf(yawInnovAtLastMagReset); + + // check for increasing yaw innovations + bool yawInnovIncreasing = yawInnovIncrease > 0.25f; + + // check that the yaw innovations haven't been caused by a large change in attitude + deltaQuatTemp = quatAtLastMagReset / stateStruct.quat; + deltaQuatTemp.to_axis_angle(deltaRotVecTemp); + bool largeAngleChange = deltaRotVecTemp.length() > yawInnovIncrease; + + // if yaw innovations and height have increased and we haven't rotated much + // then we are climbing away from a ground based magnetic anomaly and need to reset + interimResetRequest = hgtIncreasing && yawInnovIncreasing && !largeAngleChange; + } + + // an initial reset is required if we have not yet aligned the yaw angle + bool initialResetRequest = initialResetAllowed && !yawAlignComplete; + + // a combined yaw angle and magnetic field reset can be initiated by: + magYawResetRequest = magYawResetRequest || // an external request + initialResetRequest || // an initial alignment performed by all vehicle types using magnetometer + interimResetRequest || // an interim alignment required to recover from ground based magnetic anomaly + finalResetRequest; // the final reset when we have acheived enough height to be in stable magnetic field environment + + // Perform a reset of magnetic field states and reset yaw to corrected magnetic heading + if (magYawResetRequest || magStateResetRequest) { + + // get the euler angles from the current state estimate + Vector3f eulerAngles; + stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); + + // Use the Euler angles and magnetometer measurement to update the magnetic field states + // and get an updated quaternion + Quaternion newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); + + // if a yaw reset has been requested, apply the updated quaternion to the current state + if (magYawResetRequest) { + // previous value used to calculate a reset delta + Quaternion prevQuat = stateStruct.quat; + + // calculate the variance for the rotation estimate expressed as a rotation vector + // this will be used later to reset the quaternion state covariances + Vector3f angleErrVarVec = calcRotVecVariances(); + + // update the quaternion states using the new yaw angle + stateStruct.quat = newQuat; + + // update the yaw angle variance using the variance of the measurement + angleErrVarVec.z = sq(fmaxf(frontend->_yawNoise, 1.0e-2f)); + + // reset the quaternion covariances using the rotation vector variances + initialiseQuatCovariances(angleErrVarVec); + + // calculate the change in the quaternion state and apply it to the ouput history buffer + prevQuat = stateStruct.quat/prevQuat; + StoreQuatRotate(prevQuat); + + // send initial alignment status to console + if (!yawAlignComplete) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u initial yaw alignment complete\n",(unsigned)imu_index); + } + + // send in-flight yaw alignment status to console + if (finalResetRequest) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u in-flight yaw alignment complete\n",(unsigned)imu_index); + } else if (interimResetRequest) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF3 IMU%u ground mag anomaly, yaw re-aligned\n",(unsigned)imu_index); + } + + // update the yaw reset completed status + recordYawReset(); + + // clear the yaw reset request flag + magYawResetRequest = false; + + // clear the complete flags if an interim reset has been performed to allow subsequent + // and final reset to occur + if (interimResetRequest) { + finalInflightYawInit = false; + finalInflightMagInit = false; + } + } + } +} + +// this function is used to do a forced re-alignment of the yaw angle to align with the horizontal velocity +// vector from GPS. It is used to align the yaw angle after launch or takeoff. +void NavEKF3_core::realignYawGPS() +{ + // get quaternion from existing filter states and calculate roll, pitch and yaw angles + Vector3f eulerAngles; + stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); + + if ((sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y)) > 25.0f) { + // calculate course yaw angle + float velYaw = atan2f(stateStruct.velocity.y,stateStruct.velocity.x); + + // calculate course yaw angle from GPS velocity + float gpsYaw = atan2f(gpsDataDelayed.vel.y,gpsDataDelayed.vel.x); + + // Check the yaw angles for consistency + float yawErr = MAX(fabsf(wrap_PI(gpsYaw - velYaw)),fabsf(wrap_PI(gpsYaw - eulerAngles.z))); + + // If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad + badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete; + + // correct yaw angle using GPS ground course if compass yaw bad + if (badMagYaw) { + + // calculate the variance for the rotation estimate expressed as a rotation vector + // this will be used later to reset the quaternion state covariances + Vector3f angleErrVarVec = calcRotVecVariances(); + + // calculate new filter quaternion states from Euler angles + stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw); + + // reset the velocity and posiiton states as they will be inaccurate due to bad yaw + ResetVelocity(); + ResetPosition(); + + // set the yaw angle variance to a larger value to reflect the uncertainty in yaw + angleErrVarVec.z = sq(radians(45.0f)); + + // reset the quaternion covariances using the rotation vector variances + zeroRows(P,0,3); + zeroCols(P,0,3); + initialiseQuatCovariances(angleErrVarVec); + + // send yaw alignment information to console + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index); + + + // record the yaw reset event + recordYawReset(); + + // clear all pending yaw reset requests + gpsYawResetRequest = false; + magYawResetRequest = false; + + if (use_compass()) { + // request a mag field reset which may enable us to use the magnetoemter if the previous fault was due to bad initialisation + magStateResetRequest = true; + // clear the all sensors failed status so that the magnetometers sensors get a second chance now that we are flying + allMagSensorsFailed = false; + } + } + } +} + +/******************************************************** +* FUSE MEASURED_DATA * +********************************************************/ + +// select fusion of magnetometer data +void NavEKF3_core::SelectMagFusion() +{ + // start performance timer + hal.util->perf_begin(_perf_FuseMagnetometer); + + // clear the flag that lets other processes know that the expensive magnetometer fusion operation has been perfomred on that time step + // used for load levelling + magFusePerformed = false; + + // check for and read new magnetometer measurements + readMagData(); + + // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout + if (magHealth) { + magTimeout = false; + lastHealthyMagTime_ms = imuSampleTime_ms; + } else if ((imuSampleTime_ms - lastHealthyMagTime_ms) > frontend->magFailTimeLimit_ms && use_compass()) { + magTimeout = true; + } + + // check for availability of magnetometer data to fuse + magDataToFuse = storedMag.recall(magDataDelayed,imuDataDelayed.time_ms); + + // Control reset of yaw and magnetic field states if we are using compass data + if (magDataToFuse && use_compass()) { + controlMagYawReset(); + } + + // determine if conditions are right to start a new fusion cycle + // wait until the EKF time horizon catches up with the measurement + bool dataReady = (magDataToFuse && statesInitialised && use_compass() && yawAlignComplete); + if (dataReady) { + // use the simple method of declination to maintain heading if we cannot use the magnetic field states + if(inhibitMagStates || magStateResetRequest || !magStateInitComplete) { + fuseEulerYaw(); + // zero the test ratio output from the inactive 3-axis magnetometer fusion + magTestRatio.zero(); + } else { + // if we are not doing aiding with earth relative observations (eg GPS) then the declination is + // maintained by fusing declination as a synthesised observation + if (PV_AidingMode != AID_ABSOLUTE) { + FuseDeclination(0.34f); + } + // fuse the three magnetometer componenents sequentially + for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++) { + hal.util->perf_begin(_perf_test[0]); + FuseMagnetometer(); + hal.util->perf_end(_perf_test[0]); + // don't continue fusion if unhealthy + if (!magHealth) { + break; + } + } + // zero the test ratio output from the inactive simple magnetometer yaw fusion + yawTestRatio = 0.0f; + } + } + + // If we have no magnetometer and are on the ground, fuse in a synthetic heading measurement to prevent the + // filter covariances from becoming badly conditioned + if (!use_compass()) { + if (onGround && (imuSampleTime_ms - lastSynthYawTime_ms > 1000)) { + fuseEulerYaw(); + magTestRatio.zero(); + yawTestRatio = 0.0f; + lastSynthYawTime_ms = imuSampleTime_ms; + } + } + + // If the final yaw reset has been performed and the state variances are sufficiently low + // record that the earth field has been learned. + if (!magFieldLearned && finalInflightMagInit) { + magFieldLearned = (P[16][16] < sq(0.01f)) && (P[17][17] < sq(0.01f)) && (P[18][18] < sq(0.01f)); + } + + // record the last learned field variances + if (magFieldLearned && !inhibitMagStates) { + earthMagFieldVar.x = P[16][16]; + earthMagFieldVar.y = P[17][17]; + earthMagFieldVar.z = P[18][18]; + bodyMagFieldVar.x = P[19][19]; + bodyMagFieldVar.y = P[20][20]; + bodyMagFieldVar.z = P[21][21]; + } + + // stop performance timer + hal.util->perf_end(_perf_FuseMagnetometer); +} + +/* + * Fuse magnetometer measurements using explicit algebraic equations generated with Matlab symbolic toolbox. + * The script file used to generate these and other equations in this filter can be found here: + * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m +*/ +void NavEKF3_core::FuseMagnetometer() +{ + // declarations + ftype &q0 = mag_state.q0; + ftype &q1 = mag_state.q1; + ftype &q2 = mag_state.q2; + ftype &q3 = mag_state.q3; + ftype &magN = mag_state.magN; + ftype &magE = mag_state.magE; + ftype &magD = mag_state.magD; + ftype &magXbias = mag_state.magXbias; + ftype &magYbias = mag_state.magYbias; + ftype &magZbias = mag_state.magZbias; + uint8_t &obsIndex = mag_state.obsIndex; + Matrix3f &DCM = mag_state.DCM; + Vector3f &MagPred = mag_state.MagPred; + ftype &R_MAG = mag_state.R_MAG; + ftype *SH_MAG = &mag_state.SH_MAG[0]; + Vector24 H_MAG; + Vector5 SK_MX; + Vector5 SK_MY; + Vector5 SK_MZ; + + // perform sequential fusion of magnetometer measurements. + // this assumes that the errors in the different components are + // uncorrelated which is not true, however in the absence of covariance + // data fit is the only assumption we can make + // so we might as well take advantage of the computational efficiencies + // associated with sequential fusion + // calculate observation jacobians and Kalman gains + + // copy required states to local variable names + q0 = stateStruct.quat[0]; + q1 = stateStruct.quat[1]; + q2 = stateStruct.quat[2]; + q3 = stateStruct.quat[3]; + magN = stateStruct.earth_magfield[0]; + magE = stateStruct.earth_magfield[1]; + magD = stateStruct.earth_magfield[2]; + magXbias = stateStruct.body_magfield[0]; + magYbias = stateStruct.body_magfield[1]; + magZbias = stateStruct.body_magfield[2]; + + // rotate predicted earth components into body axes and calculate + // predicted measurements + DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3; + DCM[0][1] = 2.0f*(q1*q2 + q0*q3); + DCM[0][2] = 2.0f*(q1*q3-q0*q2); + DCM[1][0] = 2.0f*(q1*q2 - q0*q3); + DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3; + DCM[1][2] = 2.0f*(q2*q3 + q0*q1); + DCM[2][0] = 2.0f*(q1*q3 + q0*q2); + DCM[2][1] = 2.0f*(q2*q3 - q0*q1); + DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3; + MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias; + MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias; + MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; + + // calculate the measurement innovation for each axis + for (uint8_t i = 0; i<=2; i++) { + innovMag[i] = MagPred[i] - magDataDelayed.mag[i]; + } + + // scale magnetometer observation error with total angular rate to allow for timing errors + R_MAG = sq(constrain_float(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*imuDataDelayed.delAng.length() / imuDataDelayed.delAngDT); + + // calculate common expressions used to calculate observation jacobians an innovation variance for each component + SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1; + SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2; + SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3; + SH_MAG[3] = sq(q3); + SH_MAG[4] = sq(q2); + SH_MAG[5] = sq(q1); + SH_MAG[6] = sq(q0); + SH_MAG[7] = 2.0f*magN*q0; + SH_MAG[8] = 2.0f*magE*q3; + + // Calculate the innovation variance for each axis + // X axis + varInnovMag[0] = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] - P[2][19]*SH_MAG[1] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f*q0*q3 + 2.0f*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] - P[2][17]*SH_MAG[1] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][17]*(2.0f*q0*q2 - 2.0f*q1*q3) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q2 - 2.0f*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] - P[2][18]*SH_MAG[1] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][18]*(2.0f*q0*q2 - 2.0f*q1*q3) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] - P[2][0]*SH_MAG[1] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][0]*(2.0f*q0*q2 - 2.0f*q1*q3) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[17][19]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][19]*(2.0f*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] - P[2][1]*SH_MAG[1] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][1]*(2.0f*q0*q2 - 2.0f*q1*q3) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P[19][2] + P[1][2]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][2]*(2.0f*q0*q2 - 2.0f*q1*q3) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] - P[2][3]*SH_MAG[1] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][3]*(2.0f*q0*q2 - 2.0f*q1*q3) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] - P[2][16]*SH_MAG[1] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][16]*(2.0f*q0*q2 - 2.0f*q1*q3) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); + if (varInnovMag[0] >= R_MAG) { + faultStatus.bad_xmag = false; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we reset the covariance matrix and try again next measurement + CovarianceInit(); + faultStatus.bad_xmag = true; + return; + } + + // Y axis + varInnovMag[1] = (P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2.0f*q0*q3 - 2.0f*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][16]*(2.0f*q0*q1 + 2.0f*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (2.0f*q0*q1 + 2.0f*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][18]*(2.0f*q0*q1 + 2.0f*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][3]*(2.0f*q0*q1 + 2.0f*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P[16][20]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][20]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][0]*(2.0f*q0*q1 + 2.0f*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][1]*(2.0f*q0*q1 + 2.0f*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][2]*(2.0f*q0*q1 + 2.0f*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][17]*(2.0f*q0*q1 + 2.0f*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); + if (varInnovMag[1] >= R_MAG) { + faultStatus.bad_ymag = false; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we reset the covariance matrix and try again next measurement + CovarianceInit(); + faultStatus.bad_ymag = true; + return; + } + + // Z axis + varInnovMag[2] = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] - P[1][21]*SH_MAG[2] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2.0f*q0*q2 + 2.0f*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] - P[1][16]*SH_MAG[2] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][16]*(2.0f*q0*q1 - 2.0f*q2*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q1 - 2.0f*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] - P[1][17]*SH_MAG[2] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][17]*(2.0f*q0*q1 - 2.0f*q2*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] - P[1][2]*SH_MAG[2] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][2]*(2.0f*q0*q1 - 2.0f*q2*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[16][21]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][21]*(2.0f*q0*q1 - 2.0f*q2*q3) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] - P[1][0]*SH_MAG[2] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][0]*(2.0f*q0*q1 - 2.0f*q2*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[2]*(P[21][1] + P[0][1]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][1]*(2.0f*q0*q1 - 2.0f*q2*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] - P[1][3]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][3]*(2.0f*q0*q1 - 2.0f*q2*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] - P[1][18]*SH_MAG[2] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][18]*(2.0f*q0*q1 - 2.0f*q2*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); + if (varInnovMag[2] >= R_MAG) { + faultStatus.bad_zmag = false; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we reset the covariance matrix and try again next measurement + CovarianceInit(); + faultStatus.bad_zmag = true; + return; + } + + // calculate the innovation test ratios + for (uint8_t i = 0; i<=2; i++) { + magTestRatio[i] = sq(innovMag[i]) / (sq(MAX(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]); + } + + // check the last values from all components and set magnetometer health accordingly + magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f); + + // if the magnetometer is unhealthy, do not proceed further + if (!magHealth) { + return; + } + + for (obsIndex = 0; obsIndex <= 2; obsIndex++) { + + if (obsIndex == 0) { + + for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f; + H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; + H_MAG[1] = SH_MAG[0]; + H_MAG[2] = -SH_MAG[1]; + H_MAG[3] = SH_MAG[2]; + H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[17] = 2.0f*q0*q3 + 2.0f*q1*q2; + H_MAG[18] = 2.0f*q1*q3 - 2.0f*q0*q2; + H_MAG[19] = 1.0f; + + // calculate Kalman gain + SK_MX[0] = 1.0f / varInnovMag[0]; + SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; + SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; + SK_MX[3] = 2.0f*q0*q2 - 2.0f*q1*q3; + SK_MX[4] = 2.0f*q0*q3 + 2.0f*q1*q2; + Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] - P[0][2]*SH_MAG[1] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[4] - P[0][18]*SK_MX[3]); + Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] - P[1][2]*SH_MAG[1] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[4] - P[1][18]*SK_MX[3]); + Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[4] - P[2][18]*SK_MX[3]); + Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] - P[3][2]*SH_MAG[1] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[4] - P[3][18]*SK_MX[3]); + Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] - P[4][2]*SH_MAG[1] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[4] - P[4][18]*SK_MX[3]); + Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] - P[5][2]*SH_MAG[1] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[4] - P[5][18]*SK_MX[3]); + Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] - P[6][2]*SH_MAG[1] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[4] - P[6][18]*SK_MX[3]); + Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] - P[7][2]*SH_MAG[1] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[4] - P[7][18]*SK_MX[3]); + Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] - P[8][2]*SH_MAG[1] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[4] - P[8][18]*SK_MX[3]); + Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] - P[9][2]*SH_MAG[1] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[4] - P[9][18]*SK_MX[3]); + Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] - P[10][2]*SH_MAG[1] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[4] - P[10][18]*SK_MX[3]); + Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] - P[11][2]*SH_MAG[1] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[4] - P[11][18]*SK_MX[3]); + Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] - P[12][2]*SH_MAG[1] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[4] - P[12][18]*SK_MX[3]); + Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] - P[13][2]*SH_MAG[1] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[4] - P[13][18]*SK_MX[3]); + Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] - P[14][2]*SH_MAG[1] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[4] - P[14][18]*SK_MX[3]); + Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] - P[15][2]*SH_MAG[1] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[4] - P[15][18]*SK_MX[3]); + + // zero Kalman gains to inhibit wind state estimation + if (!inhibitWindStates) { + Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]); + Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]); + } else { + Kfusion[22] = 0.0f; + Kfusion[23] = 0.0f; + } + // zero Kalman gains to inhibit magnetic field state estimation + if (!inhibitMagStates) { + Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] - P[16][2]*SH_MAG[1] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[4] - P[16][18]*SK_MX[3]); + Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] - P[17][2]*SH_MAG[1] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[4] - P[17][18]*SK_MX[3]); + Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] - P[18][2]*SH_MAG[1] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[4] - P[18][18]*SK_MX[3]); + Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] - P[19][2]*SH_MAG[1] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[4] - P[19][18]*SK_MX[3]); + Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] - P[20][2]*SH_MAG[1] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[4] - P[20][18]*SK_MX[3]); + Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] - P[21][2]*SH_MAG[1] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[4] - P[21][18]*SK_MX[3]); + } else { + for (uint8_t i=16; i<=21; i++) { + Kfusion[i] = 0.0f; + } + } + + // set flags to indicate to other processes that fusion has been performed and is required on the next frame + // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step + magFusePerformed = true; + magFuseRequired = true; + + } else if (obsIndex == 1) { // Fuse Y axis + + // calculate observation jacobians + for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f; + H_MAG[0] = SH_MAG[2]; + H_MAG[1] = SH_MAG[1]; + H_MAG[2] = SH_MAG[0]; + H_MAG[3] = 2.0f*magD*q2 - SH_MAG[8] - SH_MAG[7]; + H_MAG[16] = 2.0f*q1*q2 - 2.0f*q0*q3; + H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; + H_MAG[18] = 2.0f*q0*q1 + 2.0f*q2*q3; + H_MAG[20] = 1.0f; + + // calculate Kalman gain + SK_MY[0] = 1.0f / varInnovMag[1]; + SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; + SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; + SK_MY[3] = 2.0f*q0*q3 - 2.0f*q1*q2; + SK_MY[4] = 2.0f*q0*q1 + 2.0f*q2*q3; + + Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]); + Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]); + Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]); + Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]); + Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]); + Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]); + Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]); + Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]); + Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]); + Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]); + Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]); + Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]); + Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); + Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); + Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); + Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); + // zero Kalman gains to inhibit wind state estimation + if (!inhibitWindStates) { + Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); + Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]); + } else { + Kfusion[22] = 0.0f; + Kfusion[23] = 0.0f; + } + // zero Kalman gains to inhibit magnetic field state estimation + if (!inhibitMagStates) { + Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); + Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); + Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); + Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); + Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); + Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); + } else { + for (uint8_t i=16; i<=21; i++) { + Kfusion[i] = 0.0f; + } + } + + // set flags to indicate to other processes that fusion has been performede and is required on the next frame + // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step + magFusePerformed = true; + magFuseRequired = true; + } + else if (obsIndex == 2) // we are now fusing the Z measurement + { + // calculate observation jacobians + for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f; + H_MAG[0] = SH_MAG[1]; + H_MAG[1] = -SH_MAG[2]; + H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; + H_MAG[3] = SH_MAG[0]; + H_MAG[16] = 2.0f*q0*q2 + 2.0f*q1*q3; + H_MAG[17] = 2.0f*q2*q3 - 2.0f*q0*q1; + H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + H_MAG[21] = 1.0f; + + // calculate Kalman gain + SK_MZ[0] = 1.0f / varInnovMag[2]; + SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; + SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3; + SK_MZ[4] = 2.0f*q0*q2 + 2.0f*q1*q3; + + Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] - P[0][1]*SH_MAG[2] + P[0][3]*SH_MAG[0] + P[0][2]*SK_MZ[2] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[4] - P[0][17]*SK_MZ[3]); + Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[1][3]*SH_MAG[0] + P[1][2]*SK_MZ[2] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[4] - P[1][17]*SK_MZ[3]); + Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] - P[2][1]*SH_MAG[2] + P[2][3]*SH_MAG[0] + P[2][2]*SK_MZ[2] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[4] - P[2][17]*SK_MZ[3]); + Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] - P[3][1]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[3][2]*SK_MZ[2] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[4] - P[3][17]*SK_MZ[3]); + Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] - P[4][1]*SH_MAG[2] + P[4][3]*SH_MAG[0] + P[4][2]*SK_MZ[2] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[4] - P[4][17]*SK_MZ[3]); + Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] - P[5][1]*SH_MAG[2] + P[5][3]*SH_MAG[0] + P[5][2]*SK_MZ[2] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[4] - P[5][17]*SK_MZ[3]); + Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] - P[6][1]*SH_MAG[2] + P[6][3]*SH_MAG[0] + P[6][2]*SK_MZ[2] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[4] - P[6][17]*SK_MZ[3]); + Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] - P[7][1]*SH_MAG[2] + P[7][3]*SH_MAG[0] + P[7][2]*SK_MZ[2] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[4] - P[7][17]*SK_MZ[3]); + Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] - P[8][1]*SH_MAG[2] + P[8][3]*SH_MAG[0] + P[8][2]*SK_MZ[2] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[4] - P[8][17]*SK_MZ[3]); + Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] - P[9][1]*SH_MAG[2] + P[9][3]*SH_MAG[0] + P[9][2]*SK_MZ[2] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[4] - P[9][17]*SK_MZ[3]); + Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] - P[10][1]*SH_MAG[2] + P[10][3]*SH_MAG[0] + P[10][2]*SK_MZ[2] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[4] - P[10][17]*SK_MZ[3]); + Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] - P[11][1]*SH_MAG[2] + P[11][3]*SH_MAG[0] + P[11][2]*SK_MZ[2] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[4] - P[11][17]*SK_MZ[3]); + Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] - P[12][1]*SH_MAG[2] + P[12][3]*SH_MAG[0] + P[12][2]*SK_MZ[2] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[4] - P[12][17]*SK_MZ[3]); + Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] - P[13][1]*SH_MAG[2] + P[13][3]*SH_MAG[0] + P[13][2]*SK_MZ[2] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[4] - P[13][17]*SK_MZ[3]); + Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] - P[14][1]*SH_MAG[2] + P[14][3]*SH_MAG[0] + P[14][2]*SK_MZ[2] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[4] - P[14][17]*SK_MZ[3]); + Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] - P[15][1]*SH_MAG[2] + P[15][3]*SH_MAG[0] + P[15][2]*SK_MZ[2] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[4] - P[15][17]*SK_MZ[3]); + // zero Kalman gains to inhibit wind state estimation + if (!inhibitWindStates) { + Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]); + Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]); + } else { + Kfusion[22] = 0.0f; + Kfusion[23] = 0.0f; + } + // zero Kalman gains to inhibit magnetic field state estimation + if (!inhibitMagStates) { + Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] - P[16][1]*SH_MAG[2] + P[16][3]*SH_MAG[0] + P[16][2]*SK_MZ[2] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[4] - P[16][17]*SK_MZ[3]); + Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] - P[17][1]*SH_MAG[2] + P[17][3]*SH_MAG[0] + P[17][2]*SK_MZ[2] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[4] - P[17][17]*SK_MZ[3]); + Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] - P[18][1]*SH_MAG[2] + P[18][3]*SH_MAG[0] + P[18][2]*SK_MZ[2] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[4] - P[18][17]*SK_MZ[3]); + Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] - P[19][1]*SH_MAG[2] + P[19][3]*SH_MAG[0] + P[19][2]*SK_MZ[2] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[4] - P[19][17]*SK_MZ[3]); + Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] - P[20][1]*SH_MAG[2] + P[20][3]*SH_MAG[0] + P[20][2]*SK_MZ[2] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[4] - P[20][17]*SK_MZ[3]); + Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] - P[21][1]*SH_MAG[2] + P[21][3]*SH_MAG[0] + P[21][2]*SK_MZ[2] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[4] - P[21][17]*SK_MZ[3]); + } else { + for (uint8_t i=16; i<=21; i++) { + Kfusion[i] = 0.0f; + } + } + + // set flags to indicate to other processes that fusion has been performede and is required on the next frame + // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step + magFusePerformed = true; + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (unsigned i = 0; i<=stateIndexLim; i++) { + for (unsigned j = 0; j<=3; j++) { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + for (unsigned j = 4; j<=15; j++) { + KH[i][j] = 0.0f; + } + for (unsigned j = 16; j<=21; j++) { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + for (unsigned j = 22; j<=23; j++) { + KH[i][j] = 0.0f; + } + } + for (unsigned j = 0; j<=stateIndexLim; j++) { + for (unsigned i = 0; i<=stateIndexLim; i++) { + ftype res = 0; + res += KH[i][0] * P[0][j]; + res += KH[i][1] * P[1][j]; + res += KH[i][2] * P[2][j]; + res += KH[i][3] * P[3][j]; + res += KH[i][16] * P[16][j]; + res += KH[i][17] * P[17][j]; + res += KH[i][18] * P[18][j]; + res += KH[i][19] * P[19][j]; + res += KH[i][20] * P[20][j]; + res += KH[i][21] * P[21][j]; + KHP[i][j] = res; + } + } + // Check that we are not going to drive any variances negative and skip the update if so + bool healthyFusion = true; + for (uint8_t i= 0; i<=stateIndexLim; i++) { + if (KHP[i][i] > P[i][i]) { + healthyFusion = false; + } + } + if (healthyFusion) { + // update the covariance matrix + for (uint8_t i= 0; i<=stateIndexLim; i++) { + for (uint8_t j= 0; j<=stateIndexLim; j++) { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + ForceSymmetry(); + ConstrainVariances(); + + // correct the state vector + for (uint8_t j= 0; j<=stateIndexLim; j++) { + statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex]; + } + stateStruct.quat.normalize(); + + } else { + // record bad axis + if (obsIndex == 0) { + faultStatus.bad_xmag = true; + } else if (obsIndex == 1) { + faultStatus.bad_ymag = true; + } else if (obsIndex == 2) { + faultStatus.bad_zmag = true; + } + } + } +} + + +/* + * Fuse magnetic heading measurement using explicit algebraic equations generated with Matlab symbolic toolbox. + * The script file used to generate these and other equations in this filter can be found here: + * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m + * This fusion method only modifies the orientation, does not require use of the magnetic field states and is computatonally cheaper. + * It is suitable for use when the external magnetic field environment is disturbed (eg close to metal structures, on ground). + * It is not as robust to magneometer failures. + * It is not suitable for operation where the horizontal magnetic field strength is weak (within 30 degreees latitude of the the magnetic poles) +*/ +void NavEKF3_core::fuseEulerYaw() +{ + float q0 = stateStruct.quat[0]; + float q1 = stateStruct.quat[1]; + float q2 = stateStruct.quat[2]; + float q3 = stateStruct.quat[3]; + + // compass measurement error variance (rad^2) + const float R_YAW = sq(frontend->_yawNoise); + + // calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix + // determine if a 321 or 312 Euler sequence is best + float predicted_yaw; + float H_YAW[4]; + Matrix3f Tbn_zeroYaw; + if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) { + // calculate observation jacobian when we are observing the first rotation in a 321 sequence + float t9 = q0*q3; + float t10 = q1*q2; + float t2 = t9+t10; + float t3 = q0*q0; + float t4 = q1*q1; + float t5 = q2*q2; + float t6 = q3*q3; + float t7 = t3+t4-t5-t6; + float t8 = t7*t7; + if (t8 > 1e-6f) { + t8 = 1.0f/t8; + } else { + return; + } + float t11 = t2*t2; + float t12 = t8*t11*4.0f; + float t13 = t12+1.0f; + float t14; + if (fabsf(t13) > 1e-6f) { + t14 = 1.0f/t13; + } else { + return; + } + + H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f; + H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f; + H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f; + H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f; + + // Get the 321 euler angles + Vector3f euler321; + stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z); + predicted_yaw = euler321.z; + + // set the yaw to zero and calculate the zero yaw rotation from body to earth frame + Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f); + + } else { + // calculate observaton jacobian when we are observing a rotation in a 312 sequence + float t9 = q0*q3; + float t10 = q1*q2; + float t2 = t9-t10; + float t3 = q0*q0; + float t4 = q1*q1; + float t5 = q2*q2; + float t6 = q3*q3; + float t7 = t3-t4+t5-t6; + float t8 = t7*t7; + if (t8 > 1e-6f) { + t8 = 1.0f/t8; + } else { + return; + } + float t11 = t2*t2; + float t12 = t8*t11*4.0f; + float t13 = t12+1.0f; + float t14; + if (fabsf(t13) > 1e-6f) { + t14 = 1.0f/t13; + } else { + return; + } + + H_YAW[0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0f)*-2.0f; + H_YAW[1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0f)*-2.0f; + H_YAW[2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0f)*2.0f; + H_YAW[3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0f)*2.0f; + + // Get the 321 euler angles + Vector3f euler312 = stateStruct.quat.to_vector312(); + predicted_yaw = euler312.z; + + // set the yaw to zero and calculate the zero yaw rotation from body to earth frame + Tbn_zeroYaw.from_euler312(euler312.x, euler312.y, 0.0f); + } + + // rotate measured mag components into earth frame + Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; + + // Use the difference between the horizontal projection and declination to give the measured yaw + // If we can't use compass data, set the meaurement to the predicted + // to prevent uncontrolled variance growth whilst on ground without magnetometer + float measured_yaw; + if (use_compass() && yawAlignComplete && magStateInitComplete) { + measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination()); + } else { + measured_yaw = predicted_yaw; + } + + // Calculate the innovation + float innovation = wrap_PI(predicted_yaw - measured_yaw); + + // Copy raw value to output variable used for data logging + innovYaw = innovation; + + // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero + float PH[4]; + float varInnov = R_YAW; + for (uint8_t rowIndex=0; rowIndex<=3; rowIndex++) { + PH[rowIndex] = 0.0f; + for (uint8_t colIndex=0; colIndex<=3; colIndex++) { + PH[rowIndex] += P[rowIndex][colIndex]*H_YAW[colIndex]; + } + varInnov += H_YAW[rowIndex]*PH[rowIndex]; + } + float varInnovInv; + if (varInnov >= R_YAW) { + varInnovInv = 1.0f / varInnov; + // output numerical health status + faultStatus.bad_yaw = false; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we reset the covariance matrix and try again next measurement + CovarianceInit(); + // output numerical health status + faultStatus.bad_yaw = true; + return; + } + + // calculate Kalman gain + for (uint8_t rowIndex=0; rowIndex<=stateIndexLim; rowIndex++) { + Kfusion[rowIndex] = 0.0f; + for (uint8_t colIndex=0; colIndex<=3; colIndex++) { + Kfusion[rowIndex] += P[rowIndex][colIndex]*H_YAW[colIndex]; + } + Kfusion[rowIndex] *= varInnovInv; + } + + // calculate the innovation test ratio + yawTestRatio = sq(innovation) / (sq(MAX(0.01f * (float)frontend->_yawInnovGate, 1.0f)) * varInnov); + + // Declare the magnetometer unhealthy if the innovation test fails + if (yawTestRatio > 1.0f) { + magHealth = false; + // On the ground a large innovation could be due to large initial gyro bias or magnetic interference from nearby objects + // If we are flying, then it is more likely due to a magnetometer fault and we should not fuse the data + if (inFlight) { + return; + } + } else { + magHealth = true; + } + + // limit the innovation so that initial corrections are not too large + if (innovation > 0.5f) { + innovation = 0.5f; + } else if (innovation < -0.5f) { + innovation = -0.5f; + } + + // correct the covariance using P = P - K*H*P taking advantage of the fact that only the first 3 elements in H are non zero + // calculate K*H*P + for (uint8_t row = 0; row <= stateIndexLim; row++) { + for (uint8_t column = 0; column <= 3; column++) { + KH[row][column] = Kfusion[row] * H_YAW[column]; + } + } + for (uint8_t row = 0; row <= stateIndexLim; row++) { + for (uint8_t column = 0; column <= stateIndexLim; column++) { + float tmp = KH[row][0] * P[0][column]; + tmp += KH[row][1] * P[1][column]; + tmp += KH[row][2] * P[2][column]; + tmp += KH[row][3] * P[3][column]; + KHP[row][column] = tmp; + } + } + + // Check that we are not going to drive any variances negative and skip the update if so + bool healthyFusion = true; + for (uint8_t i= 0; i<=stateIndexLim; i++) { + if (KHP[i][i] > P[i][i]) { + healthyFusion = false; + } + } + if (healthyFusion) { + // update the covariance matrix + for (uint8_t i= 0; i<=stateIndexLim; i++) { + for (uint8_t j= 0; j<=stateIndexLim; j++) { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + ForceSymmetry(); + ConstrainVariances(); + + // correct the state vector + for (uint8_t i=0; i<=stateIndexLim; i++) { + statesArray[i] -= Kfusion[i] * innovation; + } + stateStruct.quat.normalize(); + + // record fusion numerical health status + faultStatus.bad_yaw = false; + + } else { + // record fusion numerical health status + faultStatus.bad_yaw = true; + } +} + +/* + * Fuse declination angle using explicit algebraic equations generated with Matlab symbolic toolbox. + * The script file used to generate these and other equations in this filter can be found here: + * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m + * This is used to prevent the declination of the EKF earth field states from drifting during operation without GPS + * or some other absolute position or velocity reference +*/ +void NavEKF3_core::FuseDeclination(float declErr) +{ + // declination error variance (rad^2) + const float R_DECL = sq(declErr); + + // copy required states to local variables + float magN = stateStruct.earth_magfield.x; + float magE = stateStruct.earth_magfield.y; + + // prevent bad earth field states from causing numerical errors or exceptions + if (magN < 1e-3f) { + return; + } + + // Calculate observation Jacobian and Kalman gains + // Calculate intermediate variables + float t2 = magE*magE; + float t3 = magN*magN; + float t4 = t2+t3; + // if the horizontal magnetic field is too small, this calculation will be badly conditioned + if (t4 < 1e-4f) { + return; + } + float t5 = P[16][16]*t2; + float t6 = P[17][17]*t3; + float t7 = t2*t2; + float t8 = R_DECL*t7; + float t9 = t3*t3; + float t10 = R_DECL*t9; + float t11 = R_DECL*t2*t3*2.0f; + float t14 = P[16][17]*magE*magN; + float t15 = P[17][16]*magE*magN; + float t12 = t5+t6+t8+t10+t11-t14-t15; + float t13; + if (fabsf(t12) > 1e-6f) { + t13 = 1.0f / t12; + } else { + return; + } + float t18 = magE*magE; + float t19 = magN*magN; + float t20 = t18+t19; + float t21; + if (fabsf(t20) > 1e-6f) { + t21 = 1.0f/t20; + } else { + return; + } + + // Calculate the observation Jacobian + // Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost + float H_DECL[24] = {}; + H_DECL[16] = -magE*t21; + H_DECL[17] = magN*t21; + + Kfusion[0] = -t4*t13*(P[0][16]*magE-P[0][17]*magN); + Kfusion[1] = -t4*t13*(P[1][16]*magE-P[1][17]*magN); + Kfusion[2] = -t4*t13*(P[2][16]*magE-P[2][17]*magN); + Kfusion[3] = -t4*t13*(P[3][16]*magE-P[3][17]*magN); + Kfusion[4] = -t4*t13*(P[4][16]*magE-P[4][17]*magN); + Kfusion[5] = -t4*t13*(P[5][16]*magE-P[5][17]*magN); + Kfusion[6] = -t4*t13*(P[6][16]*magE-P[6][17]*magN); + Kfusion[7] = -t4*t13*(P[7][16]*magE-P[7][17]*magN); + Kfusion[8] = -t4*t13*(P[8][16]*magE-P[8][17]*magN); + Kfusion[9] = -t4*t13*(P[9][16]*magE-P[9][17]*magN); + Kfusion[10] = -t4*t13*(P[10][16]*magE-P[10][17]*magN); + Kfusion[11] = -t4*t13*(P[11][16]*magE-P[11][17]*magN); + Kfusion[12] = -t4*t13*(P[12][16]*magE-P[12][17]*magN); + Kfusion[13] = -t4*t13*(P[13][16]*magE-P[13][17]*magN); + Kfusion[14] = -t4*t13*(P[14][16]*magE-P[14][17]*magN); + Kfusion[15] = -t4*t13*(P[15][16]*magE-P[15][17]*magN); + Kfusion[16] = -t4*t13*(P[16][16]*magE-P[16][17]*magN); + Kfusion[17] = -t4*t13*(P[17][16]*magE-P[17][17]*magN); + Kfusion[18] = -t4*t13*(P[18][16]*magE-P[18][17]*magN); + Kfusion[19] = -t4*t13*(P[19][16]*magE-P[19][17]*magN); + Kfusion[20] = -t4*t13*(P[20][16]*magE-P[20][17]*magN); + Kfusion[21] = -t4*t13*(P[21][16]*magE-P[21][17]*magN); + Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN); + Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN); + + // get the magnetic declination + float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0; + + // Calculate the innovation + float innovation = atan2f(magE , magN) - magDecAng; + + // limit the innovation to protect against data errors + if (innovation > 0.5f) { + innovation = 0.5f; + } else if (innovation < -0.5f) { + innovation = -0.5f; + } + + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (unsigned i = 0; i<=stateIndexLim; i++) { + for (unsigned j = 0; j<=15; j++) { + KH[i][j] = 0.0f; + } + KH[i][16] = Kfusion[i] * H_DECL[16]; + KH[i][17] = Kfusion[i] * H_DECL[17]; + for (unsigned j = 18; j<=23; j++) { + KH[i][j] = 0.0f; + } + } + for (unsigned j = 0; j<=stateIndexLim; j++) { + for (unsigned i = 0; i<=stateIndexLim; i++) { + KHP[i][j] = KH[i][16] * P[16][j] + KH[i][17] * P[17][j]; + } + } + + // Check that we are not going to drive any variances negative and skip the update if so + bool healthyFusion = true; + for (uint8_t i= 0; i<=stateIndexLim; i++) { + if (KHP[i][i] > P[i][i]) { + healthyFusion = false; + } + } + + if (healthyFusion) { + // update the covariance matrix + for (uint8_t i= 0; i<=stateIndexLim; i++) { + for (uint8_t j= 0; j<=stateIndexLim; j++) { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + ForceSymmetry(); + ConstrainVariances(); + + // correct the state vector + for (uint8_t j= 0; j<=stateIndexLim; j++) { + statesArray[j] = statesArray[j] - Kfusion[j] * innovation; + } + stateStruct.quat.normalize(); + + // record fusion health status + faultStatus.bad_decl = false; + } else { + // record fusion health status + faultStatus.bad_decl = true; + } +} + +/******************************************************** +* MISC FUNCTIONS * +********************************************************/ + +// align the NE earth magnetic field states with the published declination +void NavEKF3_core::alignMagStateDeclination() +{ + // don't do this if we already have a learned magnetic field + if (magFieldLearned) { + return; + } + + // get the magnetic declination + float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0; + + // rotate the NE values so that the declination matches the published value + Vector3f initMagNED = stateStruct.earth_magfield; + float magLengthNE = norm(initMagNED.x,initMagNED.y); + stateStruct.earth_magfield.x = magLengthNE * cosf(magDecAng); + stateStruct.earth_magfield.y = magLengthNE * sinf(magDecAng); + + if (!inhibitMagStates) { + // zero the corresponding state covariances if magnetic field state learning is active + float var_16 = P[16][16]; + float var_17 = P[17][17]; + zeroRows(P,16,17); + zeroCols(P,16,17); + P[16][16] = var_16; + P[17][17] = var_17; + + // fuse the declination angle to establish covariances and prevent large swings in declination + // during initial fusion + FuseDeclination(0.1f); + + } +} + +// record a magentic field state reset event +void NavEKF3_core::recordMagReset() +{ + magStateInitComplete = true; + if (inFlight) { + finalInflightMagInit = true; + } + // take a snap-shot of the vertical position, quaternion and yaw innovation to use as a reference + // for post alignment checks + posDownAtLastMagReset = stateStruct.position.z; + quatAtLastMagReset = stateStruct.quat; + yawInnovAtLastMagReset = innovYaw; +} + + +#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp new file mode 100644 index 0000000000..45bb18606f --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -0,0 +1,760 @@ +#include + +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 +#include "AP_NavEKF3.h" +#include "AP_NavEKF3_core.h" +#include +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + + +/******************************************************** +* OPT FLOW AND RANGE FINDER * +********************************************************/ + +// Read the range finder and take new measurements if available +// Apply a median filter +void NavEKF3_core::readRangeFinder(void) +{ + uint8_t midIndex; + uint8_t maxIndex; + uint8_t minIndex; + // get theoretical correct range when the vehicle is on the ground + rngOnGnd = frontend->_rng.ground_clearance_cm() * 0.01f; + + // read range finder at 20Hz + // TODO better way of knowing if it has new data + if ((imuSampleTime_ms - lastRngMeasTime_ms) > 50) { + + // reset the timer used to control the measurement rate + lastRngMeasTime_ms = imuSampleTime_ms; + + // store samples and sample time into a ring buffer if valid + // use data from two range finders if available + + for (uint8_t sensorIndex = 0; sensorIndex <= 1; sensorIndex++) { + if (frontend->_rng.status(sensorIndex) == RangeFinder::RangeFinder_Good) { + rngMeasIndex[sensorIndex] ++; + if (rngMeasIndex[sensorIndex] > 2) { + rngMeasIndex[sensorIndex] = 0; + } + storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25; + storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = frontend->_rng.distance_cm(sensorIndex) * 0.01f; + } + + // check for three fresh samples + bool sampleFresh[2][3] = {}; + for (uint8_t index = 0; index <= 2; index++) { + sampleFresh[sensorIndex][index] = (imuSampleTime_ms - storedRngMeasTime_ms[sensorIndex][index]) < 500; + } + + // find the median value if we have three fresh samples + if (sampleFresh[sensorIndex][0] && sampleFresh[sensorIndex][1] && sampleFresh[sensorIndex][2]) { + if (storedRngMeas[sensorIndex][0] > storedRngMeas[sensorIndex][1]) { + minIndex = 1; + maxIndex = 0; + } else { + minIndex = 0; + maxIndex = 1; + } + if (storedRngMeas[sensorIndex][2] > storedRngMeas[sensorIndex][maxIndex]) { + midIndex = maxIndex; + } else if (storedRngMeas[sensorIndex][2] < storedRngMeas[sensorIndex][minIndex]) { + midIndex = minIndex; + } else { + midIndex = 2; + } + + // don't allow time to go backwards + if (storedRngMeasTime_ms[sensorIndex][midIndex] > rangeDataNew.time_ms) { + rangeDataNew.time_ms = storedRngMeasTime_ms[sensorIndex][midIndex]; + } + + // limit the measured range to be no less than the on-ground range + rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex],rngOnGnd); + + // get position in body frame for the current sensor + rangeDataNew.sensor_idx = sensorIndex; + + // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it + storedRange.push(rangeDataNew); + + // indicate we have updated the measurement + rngValidMeaTime_ms = imuSampleTime_ms; + + } else if (!takeOffDetected && ((imuSampleTime_ms - rngValidMeaTime_ms) > 200)) { + // before takeoff we assume on-ground range value if there is no data + rangeDataNew.time_ms = imuSampleTime_ms; + rangeDataNew.rng = rngOnGnd; + rangeDataNew.time_ms = imuSampleTime_ms; + + // don't allow time to go backwards + if (imuSampleTime_ms > rangeDataNew.time_ms) { + rangeDataNew.time_ms = imuSampleTime_ms; + } + + // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it + storedRange.push(rangeDataNew); + + // indicate we have updated the measurement + rngValidMeaTime_ms = imuSampleTime_ms; + + } + } + } +} + +// write the raw optical flow measurements +// this needs to be called externally. +void NavEKF3_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset) +{ + // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update + // The PX4Flow sensor outputs flow rates with the following axis and sign conventions: + // A positive X rate is produced by a positive sensor rotation about the X axis + // A positive Y rate is produced by a positive sensor rotation about the Y axis + // This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a + // negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate + flowMeaTime_ms = imuSampleTime_ms; + // calculate bias errors on flow sensor gyro rates, but protect against spikes in data + // reset the accumulated body delta angle and time + // don't do the calculation if not enough time lapsed for a reliable body rate measurement + if (delTimeOF > 0.01f) { + flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f); + flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f); + delAngBodyOF.zero(); + delTimeOF = 0.0f; + } + // by definition if this function is called, then flow measurements have been provided so we + // need to run the optical flow takeoff detection + detectOptFlowTakeoff(); + + // calculate rotation matrices at mid sample time for flow observations + stateStruct.quat.rotation_matrix(Tbn_flow); + // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data) + if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) { + // correct flow sensor body rates for bias and write + ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x; + ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y; + // the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead + if (delTimeOF > 0.001f) { + // first preference is to use the rate averaged over the same sampling period as the flow sensor + ofDataNew.bodyRadXYZ.z = delAngBodyOF.z / delTimeOF; + } else if (imuDataNew.delAngDT > 0.001f){ + // second preference is to use most recent IMU data + ofDataNew.bodyRadXYZ.z = imuDataNew.delAng.z / imuDataNew.delAngDT; + } else { + // third preference is use zero + ofDataNew.bodyRadXYZ.z = 0.0f; + } + // write uncorrected flow rate measurements + // note correction for different axis and sign conventions used by the px4flow sensor + ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec) + // write the flow sensor position in body frame + ofDataNew.body_offset = &posOffset; + // write flow rate measurements corrected for body rates + ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x; + ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y; + // record time last observation was received so we can detect loss of data elsewhere + flowValidMeaTime_ms = imuSampleTime_ms; + // estimate sample time of the measurement + ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2; + // Correct for the average intersampling delay due to the filter updaterate + ofDataNew.time_ms -= localFilterTimeStep_ms/2; + // Prevent time delay exceeding age of oldest IMU data in the buffer + ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms); + // Save data to buffer + storedOF.push(ofDataNew); + // Check for data at the fusion time horizon + flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms); + } +} + + +/******************************************************** +* MAGNETOMETER * +********************************************************/ + +// check for new magnetometer data and update store measurements if available +void NavEKF3_core::readMagData() +{ + if (!_ahrs->get_compass()) { + allMagSensorsFailed = true; + return; + } + // If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable + // magnetometer, then declare the magnetometers as failed for this flight + uint8_t maxCount = _ahrs->get_compass()->get_count(); + if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) { + allMagSensorsFailed = true; + return; + } + + // do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading + // because magnetometer fusion is an expensive step and we could overflow the FIFO buffer + if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) { + frontend->logging.log_compass = true; + + // If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available + // Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem + // before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets + if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) { + + // search through the list of magnetometers + for (uint8_t i=1; i= maxCount) { + tempIndex -= maxCount; + } + // if the magnetometer is allowed to be used for yaw and has a different index, we start using it + if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) { + magSelectIndex = tempIndex; + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex); + // reset the timeout flag and timer + magTimeout = false; + lastHealthyMagTime_ms = imuSampleTime_ms; + // zero the learned magnetometer bias states + stateStruct.body_magfield.zero(); + // clear the measurement buffer + storedMag.reset(); + // clear the data waiting flag so that we do not use any data pending from the previous sensor + magDataToFuse = false; + // request a reset of the magnetic field states + magStateResetRequest = true; + // declare the field unlearned so that the reset request will be obeyed + magFieldLearned = false; + } + } + } + + // detect changes to magnetometer offset parameters and reset states + Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex); + bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets); + if (changeDetected) { + // zero the learned magnetometer bias states + stateStruct.body_magfield.zero(); + // clear the measurement buffer + storedMag.reset(); + } + lastMagOffsets = nowMagOffsets; + lastMagOffsetsValid = true; + + // store time of last measurement update + lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex); + + // estimate of time magnetometer measurement was taken, allowing for delays + magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms; + + // Correct for the average intersampling delay due to the filter updaterate + magDataNew.time_ms -= localFilterTimeStep_ms/2; + + // read compass data and scale to improve numerical conditioning + magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f; + + // check for consistent data between magnetometers + consistentMagData = _ahrs->get_compass()->consistent(); + + // save magnetometer measurement to buffer to be fused later + storedMag.push(magDataNew); + } +} + +/******************************************************** +* Inertial Measurements * +********************************************************/ + +/* + * Read IMU delta angle and delta velocity measurements and downsample to 100Hz + * for storage in the data buffers used by the EKF. If the IMU data arrives at + * lower rate than 100Hz, then no downsampling or upsampling will be performed. + * Downsampling is done using a method that does not introduce coning or sculling + * errors. + */ +void NavEKF3_core::readIMUData() +{ + const AP_InertialSensor &ins = _ahrs->get_ins(); + + // average IMU sampling rate + dtIMUavg = ins.get_loop_delta_t(); + + // the imu sample time is used as a common time reference throughout the filter + imuSampleTime_ms = AP_HAL::millis(); + + // use the nominated imu or primary if not available + if (ins.use_accel(imu_index)) { + readDeltaVelocity(imu_index, imuDataNew.delVel, imuDataNew.delVelDT); + accelPosOffset = ins.get_imu_pos_offset(imu_index); + } else { + readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT); + accelPosOffset = ins.get_imu_pos_offset(ins.get_primary_accel()); + } + + // Get delta angle data from primary gyro or primary if not available + if (ins.use_gyro(imu_index)) { + readDeltaAngle(imu_index, imuDataNew.delAng); + } else { + readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng); + } + imuDataNew.delAngDT = MAX(ins.get_delta_angle_dt(imu_index),1.0e-4f); + + // Get current time stamp + imuDataNew.time_ms = imuSampleTime_ms; + + // Accumulate the measurement time interval for the delta velocity and angle data + imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT; + imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT; + + // Rotate quaternon atitude from previous to new and normalise. + // Accumulation using quaternions prevents introduction of coning errors due to downsampling + imuQuatDownSampleNew.rotate(imuDataNew.delAng); + imuQuatDownSampleNew.normalize(); + + // Rotate the latest delta velocity into body frame at the start of accumulation + Matrix3f deltaRotMat; + imuQuatDownSampleNew.rotation_matrix(deltaRotMat); + + // Apply the delta velocity to the delta velocity accumulator + imuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel; + + // Keep track of the number of IMU frames since the last state prediction + framesSincePredict++; + + // If 10msec has elapsed, and the frontend has allowed us to start a new predict cycle, then store the accumulated IMU data + // to be used by the state prediction, ignoring the frontend permission if more than 20msec has lapsed + if ((dtIMUavg*(float)framesSincePredict >= EKF_TARGET_DT && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) { + + // convert the accumulated quaternion to an equivalent delta angle + imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng); + + // Time stamp the data + imuDataDownSampledNew.time_ms = imuSampleTime_ms; + + // Write data to the FIFO IMU buffer + storedIMU.push_youngest_element(imuDataDownSampledNew); + + // calculate the achieved average time step rate for the EKF + float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.0f,10.0f*EKF_TARGET_DT); + dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow; + + // zero the accumulated IMU data and quaternion + imuDataDownSampledNew.delAng.zero(); + imuDataDownSampledNew.delVel.zero(); + imuDataDownSampledNew.delAngDT = 0.0f; + imuDataDownSampledNew.delVelDT = 0.0f; + imuQuatDownSampleNew[0] = 1.0f; + imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f; + + // reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle + framesSincePredict = 0; + + // set the flag to let the filter know it has new IMU data nad needs to run + runUpdates = true; + + // extract the oldest available data from the FIFO buffer + imuDataDelayed = storedIMU.pop_oldest_element(); + + // protect against delta time going to zero + // TODO - check if calculations can tolerate 0 + float minDT = 0.1f*dtEkfAvg; + imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT); + imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT); + + // correct the extracted IMU data for sensor errors + delAngCorrected = imuDataDelayed.delAng; + delVelCorrected = imuDataDelayed.delVel; + correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT); + correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT); + + } else { + // we don't have new IMU data in the buffer so don't run filter updates on this time step + runUpdates = false; + } +} + +// read the delta velocity and corresponding time interval from the IMU +// return false if data is not available +bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) { + const AP_InertialSensor &ins = _ahrs->get_ins(); + + if (ins_index < ins.get_accel_count()) { + ins.get_delta_velocity(ins_index,dVel); + dVel_dt = MAX(ins.get_delta_velocity_dt(ins_index),1.0e-4f); + return true; + } + return false; +} + +/******************************************************** +* Global Position Measurement * +********************************************************/ + +// check for new valid GPS data and update stored measurement if available +void NavEKF3_core::readGpsData() +{ + // check for new GPS data + // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer + if (_ahrs->get_gps().last_message_time_ms() - lastTimeGpsReceived_ms > 70) { + if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) { + // report GPS fix status + gpsCheckStatus.bad_fix = false; + + // store fix time from previous read + secondLastGpsTime_ms = lastTimeGpsReceived_ms; + + // get current fix time + lastTimeGpsReceived_ms = _ahrs->get_gps().last_message_time_ms(); + + // estimate when the GPS fix was valid, allowing for GPS processing and other delays + // ideally we should be using a timing signal from the GPS receiver to set this time + gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend->_gpsDelay_ms; + + // Correct for the average intersampling delay due to the filter updaterate + gpsDataNew.time_ms -= localFilterTimeStep_ms/2; + + // Prevent time delay exceeding age of oldest IMU data in the buffer + gpsDataNew.time_ms = MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms); + + // Get which GPS we are using for position information + gpsDataNew.sensor_idx = _ahrs->get_gps().primary_sensor(); + + // read the NED velocity from the GPS + gpsDataNew.vel = _ahrs->get_gps().velocity(); + + // Use the speed and position accuracy from the GPS if available, otherwise set it to zero. + // Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data + float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f); + gpsSpdAccuracy *= (1.0f - alpha); + float gpsSpdAccRaw; + if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) { + gpsSpdAccuracy = 0.0f; + } else { + gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw); + gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f); + } + gpsPosAccuracy *= (1.0f - alpha); + float gpsPosAccRaw; + if (!_ahrs->get_gps().horizontal_accuracy(gpsPosAccRaw)) { + gpsPosAccuracy = 0.0f; + } else { + gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw); + gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f); + } + gpsHgtAccuracy *= (1.0f - alpha); + float gpsHgtAccRaw; + if (!_ahrs->get_gps().vertical_accuracy(gpsHgtAccRaw)) { + gpsHgtAccuracy = 0.0f; + } else { + gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw); + gpsHgtAccuracy = MIN(gpsHgtAccuracy,100.0f); + } + + // check if we have enough GPS satellites and increase the gps noise scaler if we don't + if (_ahrs->get_gps().num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) { + gpsNoiseScaler = 1.0f; + } else if (_ahrs->get_gps().num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) { + gpsNoiseScaler = 1.4f; + } else { // <= 4 satellites or in constant position mode + gpsNoiseScaler = 2.0f; + } + + // Check if GPS can output vertical velocity and set GPS fusion mode accordingly + if (_ahrs->get_gps().have_vertical_velocity() && frontend->_fusionModeGPS == 0) { + useGpsVertVel = true; + } else { + useGpsVertVel = false; + } + + // Monitor quality of the GPS velocity data before and after alignment using separate checks + if (PV_AidingMode != AID_ABSOLUTE) { + // Pre-alignment checks + gpsGoodToAlign = calcGpsGoodToAlign(); + } else { + gpsGoodToAlign = false; + } + + // Post-alignment checks + calcGpsGoodForFlight(); + + // Read the GPS locaton in WGS-84 lat,long,height coordinates + const struct Location &gpsloc = _ahrs->get_gps().location(); + + // Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed + if (gpsGoodToAlign && !validOrigin) { + setOrigin(); + + // set the NE earth magnetic field states using the published declination + // and set the corresponding variances and covariances + alignMagStateDeclination(); + + // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum' + EKF_origin.alt = gpsloc.alt - baroDataNew.hgt; + + // Set the uncertinty of the GPS origin height + ekfOriginHgtVar = sq(gpsHgtAccuracy); + + } + + // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin + if (validOrigin) { + gpsDataNew.pos = location_diff(EKF_origin, gpsloc); + gpsDataNew.hgt = 0.01f * (gpsloc.alt - EKF_origin.alt); + storedGPS.push(gpsDataNew); + // declare GPS available for use + gpsNotAvailable = false; + } + + frontend->logging.log_gps = true; + + } else { + // report GPS fix status + gpsCheckStatus.bad_fix = true; + } + } +} + +// read the delta angle and corresponding time interval from the IMU +// return false if data is not available +bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) { + const AP_InertialSensor &ins = _ahrs->get_ins(); + + if (ins_index < ins.get_gyro_count()) { + ins.get_delta_angle(ins_index,dAng); + frontend->logging.log_imu = true; + return true; + } + return false; +} + + +/******************************************************** +* Height Measurements * +********************************************************/ + +// check for new pressure altitude measurement data and update stored measurement if available +void NavEKF3_core::readBaroData() +{ + // check to see if baro measurement has changed so we know if a new measurement has arrived + // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer + if (frontend->_baro.get_last_update() - lastBaroReceived_ms > 70) { + frontend->logging.log_baro = true; + + baroDataNew.hgt = frontend->_baro.get_altitude(); + + // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff + // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent + if (getTakeoffExpected()) { + baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff); + } + + // time stamp used to check for new measurement + lastBaroReceived_ms = frontend->_baro.get_last_update(); + + // estimate of time height measurement was taken, allowing for delays + baroDataNew.time_ms = lastBaroReceived_ms - frontend->_hgtDelay_ms; + + // Correct for the average intersampling delay due to the filter updaterate + baroDataNew.time_ms -= localFilterTimeStep_ms/2; + + // Prevent time delay exceeding age of oldest IMU data in the buffer + baroDataNew.time_ms = MAX(baroDataNew.time_ms,imuDataDelayed.time_ms); + + // save baro measurement to buffer to be fused later + storedBaro.push(baroDataNew); + } +} + +// calculate filtered offset between baro height measurement and EKF height estimate +// offset should be subtracted from baro measurement to match filter estimate +// offset is used to enable reversion to baro from alternate height data source +void NavEKF3_core::calcFiltBaroOffset() +{ + // Apply a first order LPF with spike protection + baroHgtOffset += 0.1f * constrain_float(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f); +} + +// calculate filtered offset between GPS height measurement and EKF height estimate +// offset should be subtracted from GPS measurement to match filter estimate +// offset is used to switch reversion to GPS from alternate height data source +void NavEKF3_core::calcFiltGpsHgtOffset() +{ + // Estimate the WGS-84 height of the EKF's origin using a Bayes filter + + // calculate the variance of our a-priori estimate of the ekf origin height + float deltaTime = constrain_float(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0f, 1.0f); + if (activeHgtSource == HGT_SOURCE_BARO) { + // Use the baro drift rate + const float baroDriftRate = 0.05f; + ekfOriginHgtVar += sq(baroDriftRate * deltaTime); + } else if (activeHgtSource == HGT_SOURCE_RNG) { + // use the worse case expected terrain gradient and vehicle horizontal speed + const float maxTerrGrad = 0.25f; + ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime); + } else if (activeHgtSource == HGT_SOURCE_GPS) { + // by definition we are using GPS height as the EKF datum in this mode + // so cannot run this filter + return; + } + lastOriginHgtTime_ms = imuDataDelayed.time_ms; + + // calculate the observation variance assuming EKF error relative to datum is independant of GPS observation error + // when not using GPS as height source + float originHgtObsVar = sq(gpsHgtAccuracy) + P[8][8]; + + // calculate the correction gain + float gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar); + + // calculate the innovation + float innovation = - stateStruct.position.z - gpsDataDelayed.hgt; + + // check the innovation variance ratio + float ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar); + + // correct the EKF origin and variance estimate if the innovation variance ratio is < 5-sigma + if (ratio < 5.0f) { + EKF_origin.alt -= (int)(100.0f * gain * innovation); + ekfOriginHgtVar -= fmaxf(gain * ekfOriginHgtVar , 0.0f); + } +} + +/******************************************************** +* Air Speed Measurements * +********************************************************/ + +// check for new airspeed data and update stored measurements if available +void NavEKF3_core::readAirSpdData() +{ + // if airspeed reading is valid and is set by the user to be used and has been updated then + // we take a new reading, convert from EAS to TAS and set the flag letting other functions + // know a new measurement is available + const AP_Airspeed *aspeed = _ahrs->get_airspeed(); + if (aspeed && + aspeed->use() && + aspeed->last_update_ms() != timeTasReceived_ms) { + tasDataNew.tas = aspeed->get_airspeed() * aspeed->get_EAS2TAS(); + timeTasReceived_ms = aspeed->last_update_ms(); + tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms; + + // Correct for the average intersampling delay due to the filter update rate + tasDataNew.time_ms -= localFilterTimeStep_ms/2; + + // Save data into the buffer to be fused when the fusion time horizon catches up with it + storedTAS.push(tasDataNew); + } + // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused + tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms); +} + +/******************************************************** +* Range Beacon Measurements * +********************************************************/ + +// check for new airspeed data and update stored measurements if available +void NavEKF3_core::readRngBcnData() +{ + // get the location of the beacon data + const AP_Beacon *beacon = _ahrs->get_beacon(); + + // exit immediately if no beacon object + if (beacon == nullptr) { + return; + } + + // get the number of beacons in use + N_beacons = beacon->count(); + + // search through all the beacons for new data and if we find it stop searching and push the data into the observation buffer + bool newDataToPush = false; + uint8_t numRngBcnsChecked = 0; + // start the search one index up from where we left it last time + uint8_t index = lastRngBcnChecked; + while (!newDataToPush && numRngBcnsChecked < N_beacons) { + // track the number of beacons checked + numRngBcnsChecked++; + + // move to next beacon, wrap index if necessary + index++; + if (index >= N_beacons) { + index = 0; + } + + // check that the beacon is healthy and has new data + if (beacon->beacon_healthy(index) && + beacon->beacon_last_update_ms(index) != lastTimeRngBcn_ms[index]) + { + // set the timestamp, correcting for measurement delay and average intersampling delay due to the filter update rate + lastTimeRngBcn_ms[index] = beacon->beacon_last_update_ms(index); + rngBcnDataNew.time_ms = lastTimeRngBcn_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms/2; + + // set the range noise + // TODO the range library should provide the noise/accuracy estimate for each beacon + rngBcnDataNew.rngErr = frontend->_rngBcnNoise; + + // set the range measurement + rngBcnDataNew.rng = beacon->beacon_distance(index); + + // set the beacon position + rngBcnDataNew.beacon_posNED = beacon->beacon_position(index); + + // identify the beacon identifier + rngBcnDataNew.beacon_ID = index; + + // indicate we have new data to push to the buffer + newDataToPush = true; + + // update the last checked index + lastRngBcnChecked = index; + } + } + + // Check if the beacon system has returned a 3D fix + if (beacon->get_vehicle_position_ned(beaconVehiclePosNED, beaconVehiclePosErr)) { + rngBcnLast3DmeasTime_ms = imuSampleTime_ms; + } + + // Check if the range beacon data can be used to align the vehicle position + if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && beaconVehiclePosErr < 1.0f && rngBcnAlignmentCompleted) { + // check for consistency between the position reported by the beacon and the position from the 3-State alignment filter + float posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y); + float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1]; + if (posDiffSq < 9.0f*posDiffVar) { + rngBcnGoodToAlign = true; + // Set the EKF origin and magnetic field declination if not previously set + if (!validOrigin && PV_AidingMode != AID_ABSOLUTE) { + // get origin from beacon system + Location origin_loc; + if (beacon->get_origin(origin_loc)) { + setOriginLLH(origin_loc); + + // set the NE earth magnetic field states using the published declination + // and set the corresponding variances and covariances + alignMagStateDeclination(); + + // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum' + EKF_origin.alt = origin_loc.alt - baroDataNew.hgt; + + // Set the uncertainty of the origin height + ekfOriginHgtVar = sq(beaconVehiclePosErr); + } + } + } else { + rngBcnGoodToAlign = false; + } + } else { + rngBcnGoodToAlign = false; + } + + // Save data into the buffer to be fused when the fusion time horizon catches up with it + if (newDataToPush) { + storedRangeBeacon.push(rngBcnDataNew); + } + + // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused + rngBcnDataToFuse = storedRangeBeacon.recall(rngBcnDataDelayed,imuDataDelayed.time_ms); + +} + +#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp new file mode 100644 index 0000000000..c970d5580b --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -0,0 +1,717 @@ +#include + +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 + +#include "AP_NavEKF3.h" +#include "AP_NavEKF3_core.h" +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +/******************************************************** +* RESET FUNCTIONS * +********************************************************/ + +/******************************************************** +* FUSE MEASURED_DATA * +********************************************************/ + +// select fusion of optical flow measurements +void NavEKF3_core::SelectFlowFusion() +{ + // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz + // If so, don't fuse measurements on this time step to reduce frame over-runs + // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements + if (magFusePerformed && dtIMUavg < 0.005f && !optFlowFusionDelayed) { + optFlowFusionDelayed = true; + return; + } else { + optFlowFusionDelayed = false; + } + + // start performance timer + hal.util->perf_begin(_perf_FuseOptFlow); + // Perform Data Checks + // Check if the optical flow data is still valid + flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000); + // check is the terrain offset estimate is still valid + gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000); + // Perform tilt check + bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin); + // Constrain measurements to zero if takeoff is not detected and the height above ground + // is insuffient to achieve acceptable focus. This allows the vehicle to be picked up + // and carried to test optical flow operation + if (!takeOffDetected && ((terrainState - stateStruct.position.z) < 0.5f)) { + ofDataDelayed.flowRadXYcomp.zero(); + ofDataDelayed.flowRadXY.zero(); + flowDataValid = true; + } + + // if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height + // we don't do terrain height estimation in optical flow only mode as the ground becomes our zero height reference + if ((flowDataToFuse || rangeDataToFuse) && tiltOK) { + // fuse optical flow data into the terrain estimator if available and if there is no range data (range data is better) + fuseOptFlowData = (flowDataToFuse && !rangeDataToFuse); + // Estimate the terrain offset (runs a one state EKF) + EstimateTerrainOffset(); + } + + // Fuse optical flow data into the main filter + if (flowDataToFuse && tiltOK) + { + // Set the flow noise used by the fusion processes + R_LOS = sq(MAX(frontend->_flowNoise, 0.05f)); + // Fuse the optical flow X and Y axis data into the main filter sequentially + FuseOptFlow(); + // reset flag to indicate that no new flow data is available for fusion + flowDataToFuse = false; + } + + // stop the performance timer + hal.util->perf_end(_perf_FuseOptFlow); +} + +/* +Estimation of terrain offset using a single state EKF +The filter can fuse motion compensated optiocal flow rates and range finder measurements +*/ +void NavEKF3_core::EstimateTerrainOffset() +{ + // start performance timer + hal.util->perf_begin(_perf_TerrainOffset); + + // constrain height above ground to be above range measured on ground + float heightAboveGndEst = MAX((terrainState - stateStruct.position.z), rngOnGnd); + + // calculate a predicted LOS rate squared + float velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y); + float losRateSq = velHorizSq / sq(heightAboveGndEst); + + // don't update terrain offset state if there is no range finder + // don't update terrain state if not generating enough LOS rate, or without GPS, as it is poorly observable + // don't update terrain state if we are using it as a height reference in the main filter + bool cantFuseFlowData = (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f); + if ((!rangeDataToFuse && cantFuseFlowData) || (activeHgtSource == HGT_SOURCE_RNG)) { + // skip update + inhibitGndState = true; + } else { + inhibitGndState = false; + // record the time we last updated the terrain offset state + gndHgtValidTime_ms = imuSampleTime_ms; + + // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption + // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning + float distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE); + distanceTravelledSq = MIN(distanceTravelledSq, 100.0f); + prevPosN = stateStruct.position[0]; + prevPosE = stateStruct.position[1]; + + // in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copters vertical velocity + float timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f); + float Pincrement = (distanceTravelledSq * sq(0.01f*float(frontend->gndGradientSigma))) + sq(timeLapsed)*P[6][6]; + Popt += Pincrement; + timeAtLastAuxEKF_ms = imuSampleTime_ms; + + // fuse range finder data + if (rangeDataToFuse) { + // predict range + float predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z; + + // Copy required states to local variable names + float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time + float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time + float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time + float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time + + // Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors + float R_RNG = frontend->_rngNoise; + + // calculate Kalman gain + float SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3); + float K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))); + + // Calculate the innovation variance for data logging + varInnovRng = (R_RNG + Popt/sq(SK_RNG)); + + // constrain terrain height to be below the vehicle + terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd); + + // Calculate the measurement innovation + innovRng = predRngMeas - rangeDataDelayed.rng; + + // calculate the innovation consistency test ratio + auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (float)frontend->_rngInnovGate, 1.0f)) * varInnovRng); + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((sq(innovRng)*SK_RNG) < 25.0f) + { + // correct the state + terrainState -= K_RNG * innovRng; + + // constrain the state + terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd); + + // correct the covariance + Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + + // prevent the state variance from becoming negative + Popt = MAX(Popt,0.0f); + + } + } + + if (fuseOptFlowData && !cantFuseFlowData) { + + Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes + float losPred; // predicted optical flow angular rate measurement + float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time + float q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time + float q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time + float q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time + float K_OPT; + float H_OPT; + + // predict range to centre of image + float flowRngPred = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z; + + // constrain terrain height to be below the vehicle + terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd); + + // calculate relative velocity in sensor frame + relVelSensor = prevTnb*stateStruct.velocity; + + // divide velocity by range, subtract body rates and apply scale factor to + // get predicted sensed angular optical rates relative to X and Y sensor axes + losPred = relVelSensor.length()/flowRngPred; + + // calculate innovations + auxFlowObsInnov = losPred - sqrtf(sq(flowRadXYcomp[0]) + sq(flowRadXYcomp[1])); + + // calculate observation jacobian + float t3 = sq(q0); + float t4 = sq(q1); + float t5 = sq(q2); + float t6 = sq(q3); + float t10 = q0*q3*2.0f; + float t11 = q1*q2*2.0f; + float t14 = t3+t4-t5-t6; + float t15 = t14*stateStruct.velocity.x; + float t16 = t10+t11; + float t17 = t16*stateStruct.velocity.y; + float t18 = q0*q2*2.0f; + float t19 = q1*q3*2.0f; + float t20 = t18-t19; + float t21 = t20*stateStruct.velocity.z; + float t2 = t15+t17-t21; + float t7 = t3-t4-t5+t6; + float t8 = stateStruct.position[2]-terrainState; + float t9 = 1.0f/sq(t8); + float t24 = t3-t4+t5-t6; + float t25 = t24*stateStruct.velocity.y; + float t26 = t10-t11; + float t27 = t26*stateStruct.velocity.x; + float t28 = q0*q1*2.0f; + float t29 = q2*q3*2.0f; + float t30 = t28+t29; + float t31 = t30*stateStruct.velocity.z; + float t12 = t25-t27+t31; + float t13 = sq(t7); + float t22 = sq(t2); + float t23 = 1.0f/(t8*t8*t8); + float t32 = sq(t12); + H_OPT = 0.5f*(t13*t22*t23*2.0f+t13*t23*t32*2.0f)/sqrtf(t9*t13*t22+t9*t13*t32); + + // calculate innovation variances + auxFlowObsInnovVar = H_OPT*Popt*H_OPT + R_LOS; + + // calculate Kalman gain + K_OPT = Popt*H_OPT/auxFlowObsInnovVar; + + // calculate the innovation consistency test ratio + auxFlowTestRatio = sq(auxFlowObsInnov) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar); + + // don't fuse if optical flow data is outside valid range + if (MAX(flowRadXY[0],flowRadXY[1]) < frontend->_maxFlowRate) { + + // correct the state + terrainState -= K_OPT * auxFlowObsInnov; + + // constrain the state + terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd); + + // correct the covariance + Popt = Popt - K_OPT * H_OPT * Popt; + + // prevent the state variances from becoming negative + Popt = MAX(Popt,0.0f); + } + } + } + + // stop the performance timer + hal.util->perf_end(_perf_TerrainOffset); +} + +/* + * Fuse angular motion compensated optical flow rates using explicit algebraic equations generated with Matlab symbolic toolbox. + * The script file used to generate these and other equations in this filter can be found here: + * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m + * Requires a valid terrain height estimate. +*/ +void NavEKF3_core::FuseOptFlow() +{ + Vector24 H_LOS; + Vector3f relVelSensor; + Vector14 SH_LOS; + Vector2 losPred; + + // Copy required states to local variable names + float q0 = stateStruct.quat[0]; + float q1 = stateStruct.quat[1]; + float q2 = stateStruct.quat[2]; + float q3 = stateStruct.quat[3]; + float vn = stateStruct.velocity.x; + float ve = stateStruct.velocity.y; + float vd = stateStruct.velocity.z; + float pd = stateStruct.position.z; + + // constrain height above ground to be above range measured on ground + float heightAboveGndEst = MAX((terrainState - pd), rngOnGnd); + float ptd = pd + heightAboveGndEst; + + // Calculate common expressions for observation jacobians + SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2); + SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2); + SH_LOS[3] = 1/(pd - ptd); + SH_LOS[4] = vd*SH_LOS[0] - ve*(2*q0*q1 - 2*q2*q3) + vn*(2*q0*q2 + 2*q1*q3); + SH_LOS[5] = 2.0f*q0*q2 - 2.0f*q1*q3; + SH_LOS[6] = 2.0f*q0*q1 + 2.0f*q2*q3; + SH_LOS[7] = q0*q0; + SH_LOS[8] = q1*q1; + SH_LOS[9] = q2*q2; + SH_LOS[10] = q3*q3; + SH_LOS[11] = q0*q3*2.0f; + SH_LOS[12] = pd-ptd; + SH_LOS[13] = 1.0f/(SH_LOS[12]*SH_LOS[12]); + + // Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated + for (uint8_t obsIndex=0; obsIndex<=1; obsIndex++) { // fuse X axis data first + // calculate range from ground plain to centre of sensor fov assuming flat earth + float range = constrain_float((heightAboveGndEst/prevTnb.c.z),rngOnGnd,1000.0f); + + // correct range for flow sensor offset body frame position offset + // the corrected value is the predicted range from the sensor focal point to the + // centre of the image on the ground assuming flat terrain + Vector3f posOffsetBody = (*ofDataDelayed.body_offset) - accelPosOffset; + if (!posOffsetBody.is_zero()) { + Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); + range -= posOffsetEarth.z / prevTnb.c.z; + } + + // calculate relative velocity in sensor frame including the relative motion due to rotation + relVelSensor = (prevTnb * stateStruct.velocity) + (ofDataDelayed.bodyRadXYZ % posOffsetBody); + + // divide velocity by range to get predicted angular LOS rates relative to X and Y axes + losPred[0] = relVelSensor.y/range; + losPred[1] = -relVelSensor.x/range; + + // calculate observation jacobians and Kalman gains + memset(&H_LOS[0], 0, sizeof(H_LOS)); + if (obsIndex == 0) { + // calculate X axis observation Jacobian + float t2 = 1.0f / range; + H_LOS[0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f); + H_LOS[1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f); + H_LOS[2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f); + H_LOS[3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f); + H_LOS[4] = -t2*(q0*q3*2.0f-q1*q2*2.0f); + H_LOS[5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3); + H_LOS[6] = t2*(q0*q1*2.0f+q2*q3*2.0f); + + // calculate intermediate variables for the X observaton innovatoin variance and Kalman gains + float t3 = q1*vd*2.0f; + float t4 = q0*ve*2.0f; + float t11 = q3*vn*2.0f; + float t5 = t3+t4-t11; + float t6 = q0*q3*2.0f; + float t29 = q1*q2*2.0f; + float t7 = t6-t29; + float t8 = q0*q1*2.0f; + float t9 = q2*q3*2.0f; + float t10 = t8+t9; + float t12 = P[0][0]*t2*t5; + float t13 = q0*vd*2.0f; + float t14 = q2*vn*2.0f; + float t28 = q1*ve*2.0f; + float t15 = t13+t14-t28; + float t16 = q3*vd*2.0f; + float t17 = q2*ve*2.0f; + float t18 = q1*vn*2.0f; + float t19 = t16+t17+t18; + float t20 = q3*ve*2.0f; + float t21 = q0*vn*2.0f; + float t30 = q2*vd*2.0f; + float t22 = t20+t21-t30; + float t23 = q0*q0; + float t24 = q1*q1; + float t25 = q2*q2; + float t26 = q3*q3; + float t27 = t23-t24+t25-t26; + float t31 = P[1][1]*t2*t15; + float t32 = P[6][0]*t2*t10; + float t33 = P[1][0]*t2*t15; + float t34 = P[2][0]*t2*t19; + float t35 = P[5][0]*t2*t27; + float t79 = P[4][0]*t2*t7; + float t80 = P[3][0]*t2*t22; + float t36 = t12+t32+t33+t34+t35-t79-t80; + float t37 = t2*t5*t36; + float t38 = P[6][1]*t2*t10; + float t39 = P[0][1]*t2*t5; + float t40 = P[2][1]*t2*t19; + float t41 = P[5][1]*t2*t27; + float t81 = P[4][1]*t2*t7; + float t82 = P[3][1]*t2*t22; + float t42 = t31+t38+t39+t40+t41-t81-t82; + float t43 = t2*t15*t42; + float t44 = P[6][2]*t2*t10; + float t45 = P[0][2]*t2*t5; + float t46 = P[1][2]*t2*t15; + float t47 = P[2][2]*t2*t19; + float t48 = P[5][2]*t2*t27; + float t83 = P[4][2]*t2*t7; + float t84 = P[3][2]*t2*t22; + float t49 = t44+t45+t46+t47+t48-t83-t84; + float t50 = t2*t19*t49; + float t51 = P[6][3]*t2*t10; + float t52 = P[0][3]*t2*t5; + float t53 = P[1][3]*t2*t15; + float t54 = P[2][3]*t2*t19; + float t55 = P[5][3]*t2*t27; + float t85 = P[4][3]*t2*t7; + float t86 = P[3][3]*t2*t22; + float t56 = t51+t52+t53+t54+t55-t85-t86; + float t57 = P[6][5]*t2*t10; + float t58 = P[0][5]*t2*t5; + float t59 = P[1][5]*t2*t15; + float t60 = P[2][5]*t2*t19; + float t61 = P[5][5]*t2*t27; + float t88 = P[4][5]*t2*t7; + float t89 = P[3][5]*t2*t22; + float t62 = t57+t58+t59+t60+t61-t88-t89; + float t63 = t2*t27*t62; + float t64 = P[6][4]*t2*t10; + float t65 = P[0][4]*t2*t5; + float t66 = P[1][4]*t2*t15; + float t67 = P[2][4]*t2*t19; + float t68 = P[5][4]*t2*t27; + float t90 = P[4][4]*t2*t7; + float t91 = P[3][4]*t2*t22; + float t69 = t64+t65+t66+t67+t68-t90-t91; + float t70 = P[6][6]*t2*t10; + float t71 = P[0][6]*t2*t5; + float t72 = P[1][6]*t2*t15; + float t73 = P[2][6]*t2*t19; + float t74 = P[5][6]*t2*t27; + float t93 = P[4][6]*t2*t7; + float t94 = P[3][6]*t2*t22; + float t75 = t70+t71+t72+t73+t74-t93-t94; + float t76 = t2*t10*t75; + float t87 = t2*t22*t56; + float t92 = t2*t7*t69; + float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92; + float t78; + + // calculate innovation variance for X axis observation and protect against a badly conditioned calculation + if (t77 > R_LOS) { + t78 = 1.0f/t77; + faultStatus.bad_xflow = false; + } else { + t77 = R_LOS; + t78 = 1.0f/R_LOS; + faultStatus.bad_xflow = true; + return; + } + varInnovOptFlow[0] = t77; + + // calculate innovation for X axis observation + innovOptFlow[0] = losPred[0] - ofDataDelayed.flowRadXYcomp.x; + + // calculate Kalman gains for X-axis observation + Kfusion[0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27); + Kfusion[1] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27); + Kfusion[2] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27); + Kfusion[3] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27); + Kfusion[4] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27); + Kfusion[5] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22); + Kfusion[6] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27); + Kfusion[7] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27); + Kfusion[8] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27); + Kfusion[9] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27); + Kfusion[10] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27); + Kfusion[11] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27); + Kfusion[12] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27); + Kfusion[13] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27); + Kfusion[14] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27); + Kfusion[15] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27); + if (!inhibitWindStates) { + Kfusion[22] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27); + Kfusion[23] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27); + } else { + Kfusion[22] = 0.0f; + Kfusion[23] = 0.0f; + } + if (!inhibitMagStates) { + Kfusion[16] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27); + Kfusion[17] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27); + Kfusion[18] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27); + Kfusion[19] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27); + Kfusion[20] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27); + Kfusion[21] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27); + } else { + for (uint8_t i = 16; i <= 21; i++) { + Kfusion[i] = 0.0f; + } + } + + } else { + + // calculate Y axis observation Jacobian + float t2 = 1.0f / range; + H_LOS[0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f); + H_LOS[1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f); + H_LOS[2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f); + H_LOS[3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f); + H_LOS[4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3); + H_LOS[5] = -t2*(q0*q3*2.0f+q1*q2*2.0f); + H_LOS[6] = t2*(q0*q2*2.0f-q1*q3*2.0f); + + // calculate intermediate variables for the Y observaton innovatoin variance and Kalman gains + float t3 = q3*ve*2.0f; + float t4 = q0*vn*2.0f; + float t11 = q2*vd*2.0f; + float t5 = t3+t4-t11; + float t6 = q0*q3*2.0f; + float t7 = q1*q2*2.0f; + float t8 = t6+t7; + float t9 = q0*q2*2.0f; + float t28 = q1*q3*2.0f; + float t10 = t9-t28; + float t12 = P[0][0]*t2*t5; + float t13 = q3*vd*2.0f; + float t14 = q2*ve*2.0f; + float t15 = q1*vn*2.0f; + float t16 = t13+t14+t15; + float t17 = q0*vd*2.0f; + float t18 = q2*vn*2.0f; + float t29 = q1*ve*2.0f; + float t19 = t17+t18-t29; + float t20 = q1*vd*2.0f; + float t21 = q0*ve*2.0f; + float t30 = q3*vn*2.0f; + float t22 = t20+t21-t30; + float t23 = q0*q0; + float t24 = q1*q1; + float t25 = q2*q2; + float t26 = q3*q3; + float t27 = t23+t24-t25-t26; + float t31 = P[1][1]*t2*t16; + float t32 = P[5][0]*t2*t8; + float t33 = P[1][0]*t2*t16; + float t34 = P[3][0]*t2*t22; + float t35 = P[4][0]*t2*t27; + float t80 = P[6][0]*t2*t10; + float t81 = P[2][0]*t2*t19; + float t36 = t12+t32+t33+t34+t35-t80-t81; + float t37 = t2*t5*t36; + float t38 = P[5][1]*t2*t8; + float t39 = P[0][1]*t2*t5; + float t40 = P[3][1]*t2*t22; + float t41 = P[4][1]*t2*t27; + float t82 = P[6][1]*t2*t10; + float t83 = P[2][1]*t2*t19; + float t42 = t31+t38+t39+t40+t41-t82-t83; + float t43 = t2*t16*t42; + float t44 = P[5][2]*t2*t8; + float t45 = P[0][2]*t2*t5; + float t46 = P[1][2]*t2*t16; + float t47 = P[3][2]*t2*t22; + float t48 = P[4][2]*t2*t27; + float t79 = P[2][2]*t2*t19; + float t84 = P[6][2]*t2*t10; + float t49 = t44+t45+t46+t47+t48-t79-t84; + float t50 = P[5][3]*t2*t8; + float t51 = P[0][3]*t2*t5; + float t52 = P[1][3]*t2*t16; + float t53 = P[3][3]*t2*t22; + float t54 = P[4][3]*t2*t27; + float t86 = P[6][3]*t2*t10; + float t87 = P[2][3]*t2*t19; + float t55 = t50+t51+t52+t53+t54-t86-t87; + float t56 = t2*t22*t55; + float t57 = P[5][4]*t2*t8; + float t58 = P[0][4]*t2*t5; + float t59 = P[1][4]*t2*t16; + float t60 = P[3][4]*t2*t22; + float t61 = P[4][4]*t2*t27; + float t88 = P[6][4]*t2*t10; + float t89 = P[2][4]*t2*t19; + float t62 = t57+t58+t59+t60+t61-t88-t89; + float t63 = t2*t27*t62; + float t64 = P[5][5]*t2*t8; + float t65 = P[0][5]*t2*t5; + float t66 = P[1][5]*t2*t16; + float t67 = P[3][5]*t2*t22; + float t68 = P[4][5]*t2*t27; + float t90 = P[6][5]*t2*t10; + float t91 = P[2][5]*t2*t19; + float t69 = t64+t65+t66+t67+t68-t90-t91; + float t70 = t2*t8*t69; + float t71 = P[5][6]*t2*t8; + float t72 = P[0][6]*t2*t5; + float t73 = P[1][6]*t2*t16; + float t74 = P[3][6]*t2*t22; + float t75 = P[4][6]*t2*t27; + float t92 = P[6][6]*t2*t10; + float t93 = P[2][6]*t2*t19; + float t76 = t71+t72+t73+t74+t75-t92-t93; + float t85 = t2*t19*t49; + float t94 = t2*t10*t76; + float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94; + float t78; + + // calculate innovation variance for X axis observation and protect against a badly conditioned calculation + // calculate innovation variance for X axis observation and protect against a badly conditioned calculation + if (t77 > R_LOS) { + t78 = 1.0f/t77; + faultStatus.bad_yflow = false; + } else { + t77 = R_LOS; + t78 = 1.0f/R_LOS; + faultStatus.bad_yflow = true; + return; + } + varInnovOptFlow[1] = t77; + + // calculate innovation for Y observation + innovOptFlow[1] = losPred[1] - ofDataDelayed.flowRadXYcomp.y; + + // calculate Kalman gains for the Y-axis observation + Kfusion[0] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27); + Kfusion[1] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27); + Kfusion[2] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27); + Kfusion[3] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27); + Kfusion[4] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22); + Kfusion[5] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27); + Kfusion[6] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27); + Kfusion[7] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27); + Kfusion[8] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27); + Kfusion[9] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27); + Kfusion[10] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27); + Kfusion[11] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27); + Kfusion[12] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27); + Kfusion[13] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27); + Kfusion[14] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27); + Kfusion[15] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27); + if (!inhibitWindStates) { + Kfusion[22] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27); + Kfusion[23] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27); + } else { + Kfusion[22] = 0.0f; + Kfusion[23] = 0.0f; + } + if (!inhibitMagStates) { + Kfusion[16] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27); + Kfusion[17] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27); + Kfusion[18] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27); + Kfusion[19] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27); + Kfusion[20] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27); + Kfusion[21] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27); + } else { + for (uint8_t i = 16; i <= 21; i++) { + Kfusion[i] = 0.0f; + } + } + } + + // calculate the innovation consistency test ratio + flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (float)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]); + + // Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable + if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) { + // record the last time observations were accepted for fusion + prevFlowFuseTime_ms = imuSampleTime_ms; + + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (unsigned i = 0; i<=stateIndexLim; i++) { + for (unsigned j = 0; j<=6; j++) { + KH[i][j] = Kfusion[i] * H_LOS[j]; + } + for (unsigned j = 7; j<=stateIndexLim; j++) { + KH[i][j] = 0.0f; + } + } + for (unsigned j = 0; j<=stateIndexLim; j++) { + for (unsigned i = 0; i<=stateIndexLim; i++) { + ftype res = 0; + res += KH[i][0] * P[0][j]; + res += KH[i][1] * P[1][j]; + res += KH[i][2] * P[2][j]; + res += KH[i][3] * P[3][j]; + res += KH[i][4] * P[4][j]; + res += KH[i][5] * P[5][j]; + res += KH[i][6] * P[6][j]; + KHP[i][j] = res; + } + } + + // Check that we are not going to drive any variances negative and skip the update if so + bool healthyFusion = true; + for (uint8_t i= 0; i<=stateIndexLim; i++) { + if (KHP[i][i] > P[i][i]) { + healthyFusion = false; + } + } + + if (healthyFusion) { + // update the covariance matrix + for (uint8_t i= 0; i<=stateIndexLim; i++) { + for (uint8_t j= 0; j<=stateIndexLim; j++) { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + ForceSymmetry(); + ConstrainVariances(); + + // correct the state vector + for (uint8_t j= 0; j<=stateIndexLim; j++) { + statesArray[j] = statesArray[j] - Kfusion[j] * innovOptFlow[obsIndex]; + } + stateStruct.quat.normalize(); + + } else { + // record bad axis + if (obsIndex == 0) { + faultStatus.bad_xflow = true; + } else if (obsIndex == 1) { + faultStatus.bad_yflow = true; + } + + } + } + } +} + +/******************************************************** +* MISC FUNCTIONS * +********************************************************/ + +#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp new file mode 100644 index 0000000000..9a7a3dc94b --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -0,0 +1,588 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 + +#include "AP_NavEKF3.h" +#include "AP_NavEKF3_core.h" +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + + +// Check basic filter health metrics and return a consolidated health status +bool NavEKF3_core::healthy(void) const +{ + uint16_t faultInt; + getFilterFaults(faultInt); + if (faultInt > 0) { + return false; + } + if (velTestRatio > 1 && posTestRatio > 1 && hgtTestRatio > 1) { + // all three metrics being above 1 means the filter is + // extremely unhealthy. + return false; + } + // Give the filter a second to settle before use + if ((imuSampleTime_ms - ekfStartTime_ms) < 1000 ) { + return false; + } + // position and height innovations must be within limits when on-ground and in a static mode of operation + float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]); + if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsf(hgtInnovFiltState) > 1.0f))) { + return false; + } + + // all OK + return true; +} + +// Return a consolidated error score where higher numbers represent larger errors +// Intended to be used by the front-end to determine which is the primary EKF +float NavEKF3_core::errorScore() const +{ + float score = 0.0f; + if (tiltAlignComplete && yawAlignComplete) { + // Check GPS fusion performance + score = MAX(score, 0.5f * (velTestRatio + posTestRatio)); + // Check altimeter fusion performance + score = MAX(score, hgtTestRatio); + } + return score; +} + +// return data for debugging optical flow fusion +void NavEKF3_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const +{ + varFlow = MAX(flowTestRatio[0],flowTestRatio[1]); + gndOffset = terrainState; + flowInnovX = innovOptFlow[0]; + flowInnovY = innovOptFlow[1]; + auxInnov = auxFlowObsInnov; + HAGL = terrainState - stateStruct.position.z; + rngInnov = innovRng; + range = rangeDataDelayed.rng; + gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset() +} + +// return data for debugging range beacon fusion one beacon at a time, incrementing the beacon index after each call +bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow) +{ + // if the states have not been initialised or we have not received any beacon updates then return zeros + if (!statesInitialised || N_beacons == 0) { + return false; + } + + // Ensure that beacons are not skipped due to calling this function at a rate lower than the updates + if (rngBcnFuseDataReportIndex >= N_beacons) { + rngBcnFuseDataReportIndex = 0; + } + + // Output the fusion status data for the specified beacon + ID = rngBcnFuseDataReportIndex; // beacon identifier + rng = rngBcnFusionReport[rngBcnFuseDataReportIndex].rng; // measured range to beacon (m) + innov = rngBcnFusionReport[rngBcnFuseDataReportIndex].innov; // range innovation (m) + innovVar = rngBcnFusionReport[rngBcnFuseDataReportIndex].innovVar; // innovation variance (m^2) + testRatio = rngBcnFusionReport[rngBcnFuseDataReportIndex].testRatio; // innovation consistency test ratio + beaconPosNED = rngBcnFusionReport[rngBcnFuseDataReportIndex].beaconPosNED; // beacon NED position + offsetHigh = bcnPosDownOffsetMax; // beacon system vertical pos offset upper estimate + offsetLow = bcnPosDownOffsetMin; // beacon system vertical pos offset lower estimate + rngBcnFuseDataReportIndex++; + return true; +} + +// provides the height limit to be observed by the control loops +// returns false if no height limiting is required +// this is needed to ensure the vehicle does not fly too high when using optical flow navigation +bool NavEKF3_core::getHeightControlLimit(float &height) const +{ + // only ask for limiting if we are doing optical flow navigation + if (frontend->_fusionModeGPS == 3) { + // If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors + height = MAX(float(frontend->_rng.max_distance_cm()) * 0.007f - 1.0f, 1.0f); + // If we are are not using the range finder as the height reference, then compensate for the difference between terrain and EKF origin + if (frontend->_altSource != 1) { + height -= terrainState; + } + return true; + } else { + return false; + } +} + + +// return the Euler roll, pitch and yaw angle in radians +void NavEKF3_core::getEulerAngles(Vector3f &euler) const +{ + outputDataNew.quat.to_euler(euler.x, euler.y, euler.z); + euler = euler - _ahrs->get_trim(); +} + +// return body axis gyro bias estimates in rad/sec +void NavEKF3_core::getGyroBias(Vector3f &gyroBias) const +{ + if (dtEkfAvg < 1e-6f) { + gyroBias.zero(); + return; + } + gyroBias = stateStruct.gyro_bias / dtEkfAvg; +} + +// return accelerometer bias in m/s/s +void NavEKF3_core::getAccelBias(Vector3f &accelBias) const +{ + if (!statesInitialised) { + accelBias.zero(); + return; + } + accelBias = stateStruct.accel_bias / dtEkfAvg; +} + +// return tilt error convergence metric +void NavEKF3_core::getTiltError(float &ang) const +{ + ang = stateStruct.quat.length(); +} + +// return the transformation matrix from XYZ (body) to NED axes +void NavEKF3_core::getRotationBodyToNED(Matrix3f &mat) const +{ + outputDataNew.quat.rotation_matrix(mat); + mat = mat * _ahrs->get_rotation_vehicle_body_to_autopilot_body(); +} + +// return the quaternions defining the rotation from NED to XYZ (body) axes +void NavEKF3_core::getQuaternion(Quaternion& ret) const +{ + ret = outputDataNew.quat; +} + +// return the amount of yaw angle change due to the last yaw angle reset in radians +// returns the time of the last yaw angle reset or 0 if no reset has ever occurred +uint32_t NavEKF3_core::getLastYawResetAngle(float &yawAng) const +{ + yawAng = yawResetAngle; + return lastYawReset_ms; +} + +// return the amount of NE position change due to the last position reset in metres +// returns the time of the last reset or 0 if no reset has ever occurred +uint32_t NavEKF3_core::getLastPosNorthEastReset(Vector2f &pos) const +{ + pos = posResetNE; + return lastPosReset_ms; +} + +// return the amount of vertical position change due to the last vertical position reset in metres +// returns the time of the last reset or 0 if no reset has ever occurred +uint32_t NavEKF3_core::getLastPosDownReset(float &posD) const +{ + posD = posResetD; + return lastPosResetD_ms; +} + +// return the amount of NE velocity change due to the last velocity reset in metres/sec +// returns the time of the last reset or 0 if no reset has ever occurred +uint32_t NavEKF3_core::getLastVelNorthEastReset(Vector2f &vel) const +{ + vel = velResetNE; + return lastVelReset_ms; +} + +// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) +void NavEKF3_core::getWind(Vector3f &wind) const +{ + wind.x = stateStruct.wind_vel.x; + wind.y = stateStruct.wind_vel.y; + wind.z = 0.0f; // currently don't estimate this +} + + +// return the NED velocity of the body frame origin in m/s +// +void NavEKF3_core::getVelNED(Vector3f &vel) const +{ + // correct for the IMU position offset (EKF calculations are at the IMU) + vel = outputDataNew.velocity + velOffsetNED; +} + +// Return the rate of change of vertical position in the down diection (dPosD/dt) of the body frame origin in m/s +float NavEKF3_core::getPosDownDerivative(void) const +{ + // return the value calculated from a complementary filter applied to the EKF height and vertical acceleration + // correct for the IMU offset (EKF calculations are at the IMU) + return posDownDerivative + velOffsetNED.z; +} + +// This returns the specific forces in the NED frame +void NavEKF3_core::getAccelNED(Vector3f &accelNED) const { + accelNED = velDotNED; + accelNED.z -= GRAVITY_MSS; +} + +// Write the last estimated NE position of the body frame origin relative to the reference point (m). +// Return true if the estimate is valid +bool NavEKF3_core::getPosNE(Vector2f &posNE) const +{ + // There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available) + if (PV_AidingMode != AID_NONE) { + // This is the normal mode of operation where we can use the EKF position states + // correct for the IMU offset (EKF calculations are at the IMU) + posNE.x = outputDataNew.position.x + posOffsetNED.x; + posNE.y = outputDataNew.position.y + posOffsetNED.y; + return true; + + } else { + // In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate + if(validOrigin) { + if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_2D)) { + // If the origin has been set and we have GPS, then return the GPS position relative to the origin + const struct Location &gpsloc = _ahrs->get_gps().location(); + Vector2f tempPosNE = location_diff(EKF_origin, gpsloc); + posNE.x = tempPosNE.x; + posNE.y = tempPosNE.y; + return false; + } else if (rngBcnAlignmentStarted) { + // If we are attempting alignment using range beacon data, then report the position + posNE.x = receiverPos.x; + posNE.y = receiverPos.y; + return false; + } else { + // If no GPS fix is available, all we can do is provide the last known position + posNE.x = outputDataNew.position.x; + posNE.y = outputDataNew.position.y; + return false; + } + } else { + // If the origin has not been set, then we have no means of providing a relative position + posNE.x = 0.0f; + posNE.y = 0.0f; + return false; + } + } + return false; +} + +// Write the last calculated D position of the body frame origin relative to the reference point (m). +// Return true if the estimate is valid +bool NavEKF3_core::getPosD(float &posD) const +{ + // The EKF always has a height estimate regardless of mode of operation + // correct for the IMU offset (EKF calculations are at the IMU) + posD = outputDataNew.position.z + posOffsetNED.z; + + // Return the current height solution status + return filterStatus.flags.vert_pos; + +} + +// return the estimated height of body frame origin above ground level +bool NavEKF3_core::getHAGL(float &HAGL) const +{ + HAGL = terrainState - outputDataNew.position.z - posOffsetNED.z; + // If we know the terrain offset and altitude, then we have a valid height above ground estimate + return !hgtTimeout && gndOffsetValid && healthy(); +} + +// Return the last calculated latitude, longitude and height in WGS-84 +// If a calculated location isn't available, return a raw GPS measurement +// The status will return true if a calculation or raw measurement is available +// The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control +bool NavEKF3_core::getLLH(struct Location &loc) const +{ + if(validOrigin) { + // Altitude returned is an absolute altitude relative to the WGS-84 spherioid + loc.alt = EKF_origin.alt - outputDataNew.position.z*100; + loc.flags.relative_alt = 0; + loc.flags.terrain_alt = 0; + + // there are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no aiding) + if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) { + loc.lat = EKF_origin.lat; + loc.lng = EKF_origin.lng; + location_offset(loc, outputDataNew.position.x, outputDataNew.position.y); + return true; + } else { + // we could be in constant position mode because the vehicle has taken off without GPS, or has lost GPS + // in this mode we cannot use the EKF states to estimate position so will return the best available data + if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_2D)) { + // we have a GPS position fix to return + const struct Location &gpsloc = _ahrs->get_gps().location(); + loc.lat = gpsloc.lat; + loc.lng = gpsloc.lng; + return true; + } else { + // if no GPS fix, provide last known position before entering the mode + location_offset(loc, lastKnownPositionNE.x, lastKnownPositionNE.y); + return false; + } + } + } else { + // If no origin has been defined for the EKF, then we cannot use its position states so return a raw + // GPS reading if available and return false + if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D)) { + const struct Location &gpsloc = _ahrs->get_gps().location(); + loc = gpsloc; + loc.flags.relative_alt = 0; + loc.flags.terrain_alt = 0; + } + return false; + } +} + + +// return the horizontal speed limit in m/s set by optical flow sensor limits +// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow +void NavEKF3_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const +{ + if (PV_AidingMode == AID_RELATIVE) { + // allow 1.0 rad/sec margin for angular motion + ekfGndSpdLimit = MAX((frontend->_maxFlowRate - 1.0f), 0.0f) * MAX((terrainState - stateStruct.position[2]), rngOnGnd); + // use standard gains up to 5.0 metres height and reduce above that + ekfNavVelGainScaler = 4.0f / MAX((terrainState - stateStruct.position[2]),4.0f); + } else { + ekfGndSpdLimit = 400.0f; //return 80% of max filter speed + ekfNavVelGainScaler = 1.0f; + } +} + + +// return the LLH location of the filters NED origin +bool NavEKF3_core::getOriginLLH(struct Location &loc) const +{ + if (validOrigin) { + loc = EKF_origin; + } + return validOrigin; +} + +// return earth magnetic field estimates in measurement units / 1000 +void NavEKF3_core::getMagNED(Vector3f &magNED) const +{ + magNED = stateStruct.earth_magfield * 1000.0f; +} + +// return body magnetic field estimates in measurement units / 1000 +void NavEKF3_core::getMagXYZ(Vector3f &magXYZ) const +{ + magXYZ = stateStruct.body_magfield*1000.0f; +} + +// return magnetometer offsets +// return true if offsets are valid +bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const +{ + // compass offsets are valid if we have finalised magnetic field initialisation, magnetic field learning is not prohibited, + // primary compass is valid and state variances have converged + const float maxMagVar = 5E-6f; + bool variancesConverged = (P[19][19] < maxMagVar) && (P[20][20] < maxMagVar) && (P[21][21] < maxMagVar); + if ((mag_idx == magSelectIndex) && + finalInflightMagInit && + !inhibitMagStates && + _ahrs->get_compass()->healthy(magSelectIndex) && + variancesConverged) { + magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f; + return true; + } else { + magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex); + return false; + } +} + +// return the index for the active magnetometer +uint8_t NavEKF3_core::getActiveMag() const +{ + return (uint8_t)magSelectIndex; +} + +// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements +void NavEKF3_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const +{ + velInnov.x = innovVelPos[0]; + velInnov.y = innovVelPos[1]; + velInnov.z = innovVelPos[2]; + posInnov.x = innovVelPos[3]; + posInnov.y = innovVelPos[4]; + posInnov.z = innovVelPos[5]; + magInnov.x = 1e3f*innovMag[0]; // Convert back to sensor units + magInnov.y = 1e3f*innovMag[1]; // Convert back to sensor units + magInnov.z = 1e3f*innovMag[2]; // Convert back to sensor units + tasInnov = innovVtas; + yawInnov = innovYaw; +} + +// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements +// this indicates the amount of margin available when tuning the various error traps +// also return the delta in position due to the last position reset +void NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const +{ + velVar = sqrtf(velTestRatio); + posVar = sqrtf(posTestRatio); + hgtVar = sqrtf(hgtTestRatio); + // If we are using simple compass yaw fusion, populate all three components with the yaw test ratio to provide an equivalent output + magVar.x = sqrtf(MAX(magTestRatio.x,yawTestRatio)); + magVar.y = sqrtf(MAX(magTestRatio.y,yawTestRatio)); + magVar.z = sqrtf(MAX(magTestRatio.z,yawTestRatio)); + tasVar = sqrtf(tasTestRatio); + offset = posResetNE; +} + + +/* +return the filter fault status as a bitmasked integer + 0 = quaternions are NaN + 1 = velocities are NaN + 2 = badly conditioned X magnetometer fusion + 3 = badly conditioned Y magnetometer fusion + 5 = badly conditioned Z magnetometer fusion + 6 = badly conditioned airspeed fusion + 7 = badly conditioned synthetic sideslip fusion + 7 = filter is not initialised +*/ +void NavEKF3_core::getFilterFaults(uint16_t &faults) const +{ + faults = (stateStruct.quat.is_nan()<<0 | + stateStruct.velocity.is_nan()<<1 | + faultStatus.bad_xmag<<2 | + faultStatus.bad_ymag<<3 | + faultStatus.bad_zmag<<4 | + faultStatus.bad_airspeed<<5 | + faultStatus.bad_sideslip<<6 | + !statesInitialised<<7); +} + +/* +return filter timeout status as a bitmasked integer + 0 = position measurement timeout + 1 = velocity measurement timeout + 2 = height measurement timeout + 3 = magnetometer measurement timeout + 4 = true airspeed measurement timeout + 5 = unassigned + 6 = unassigned + 7 = unassigned +*/ +void NavEKF3_core::getFilterTimeouts(uint8_t &timeouts) const +{ + timeouts = (posTimeout<<0 | + velTimeout<<1 | + hgtTimeout<<2 | + magTimeout<<3 | + tasTimeout<<4); +} + +// Return the navigation filter status message +void NavEKF3_core::getFilterStatus(nav_filter_status &status) const +{ + status = filterStatus; +} + +/* +return filter gps quality check status +*/ +void NavEKF3_core::getFilterGpsStatus(nav_gps_status &faults) const +{ + // init return value + faults.value = 0; + + // set individual flags + faults.flags.bad_sAcc = gpsCheckStatus.bad_sAcc; // reported speed accuracy is insufficient + faults.flags.bad_hAcc = gpsCheckStatus.bad_hAcc; // reported horizontal position accuracy is insufficient + faults.flags.bad_yaw = gpsCheckStatus.bad_yaw; // EKF heading accuracy is too large for GPS use + faults.flags.bad_sats = gpsCheckStatus.bad_sats; // reported number of satellites is insufficient + faults.flags.bad_horiz_drift = gpsCheckStatus.bad_horiz_drift; // GPS horizontal drift is too large to start using GPS (check assumes vehicle is static) + faults.flags.bad_hdop = gpsCheckStatus.bad_hdop; // reported HDoP is too large to start using GPS + faults.flags.bad_vert_vel = gpsCheckStatus.bad_vert_vel; // GPS vertical speed is too large to start using GPS (check assumes vehicle is static) + faults.flags.bad_fix = gpsCheckStatus.bad_fix; // The GPS cannot provide the 3D fix required + faults.flags.bad_horiz_vel = gpsCheckStatus.bad_horiz_vel; // The GPS horizontal speed is excessive (check assumes the vehicle is static) +} + +// send an EKF_STATUS message to GCS +void NavEKF3_core::send_status_report(mavlink_channel_t chan) +{ + // prepare flags + uint16_t flags = 0; + if (filterStatus.flags.attitude) { + flags |= EKF_ATTITUDE; + } + if (filterStatus.flags.horiz_vel) { + flags |= EKF_VELOCITY_HORIZ; + } + if (filterStatus.flags.vert_vel) { + flags |= EKF_VELOCITY_VERT; + } + if (filterStatus.flags.horiz_pos_rel) { + flags |= EKF_POS_HORIZ_REL; + } + if (filterStatus.flags.horiz_pos_abs) { + flags |= EKF_POS_HORIZ_ABS; + } + if (filterStatus.flags.vert_pos) { + flags |= EKF_POS_VERT_ABS; + } + if (filterStatus.flags.terrain_alt) { + flags |= EKF_POS_VERT_AGL; + } + if (filterStatus.flags.const_pos_mode) { + flags |= EKF_CONST_POS_MODE; + } + if (filterStatus.flags.pred_horiz_pos_rel) { + flags |= EKF_PRED_POS_HORIZ_REL; + } + if (filterStatus.flags.pred_horiz_pos_abs) { + flags |= EKF_PRED_POS_HORIZ_ABS; + } + if (filterStatus.flags.gps_glitching) { + flags |= (1<<15); + } + + // get variances + float velVar, posVar, hgtVar, tasVar; + Vector3f magVar; + Vector2f offset; + getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); + + // Only report range finder normalised innovation levels if the EKF needs the data for primary + // height estimation or optical flow operation. This prevents false alarms at the GCS if a + // range finder is fitted for other applications + float temp; + if ((frontend->_useRngSwHgt > 0) || PV_AidingMode == AID_RELATIVE || flowDataValid) { + temp = sqrtf(auxRngTestRatio); + } else { + temp = 0.0f; + } + + // send message + mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), temp); + +} + +// report the reason for why the backend is refusing to initialise +const char *NavEKF3_core::prearm_failure_reason(void) const +{ + if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) { + // we are not failing + return nullptr; + } + return prearm_fail_string; +} + + +// report the number of frames lapsed since the last state prediction +// this is used by other instances to level load +uint8_t NavEKF3_core::getFramesSincePredict(void) const +{ + return framesSincePredict; +} + +// publish output observer angular, velocity and position tracking error +void NavEKF3_core::getOutputTrackingError(Vector3f &error) const +{ + error = outputTrackError; +} + +#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp new file mode 100644 index 0000000000..f65ef23d28 --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -0,0 +1,819 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 + +#include "AP_NavEKF3.h" +#include "AP_NavEKF3_core.h" +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +/******************************************************** +* RESET FUNCTIONS * +********************************************************/ + +// Reset velocity states to last GPS measurement if available or to zero if in constant position mode or if PV aiding is not absolute +// Do not reset vertical velocity using GPS as there is baro alt available to constrain drift +void NavEKF3_core::ResetVelocity(void) +{ + // Store the position before the reset so that we can record the reset delta + velResetNE.x = stateStruct.velocity.x; + velResetNE.y = stateStruct.velocity.y; + + // reset the corresponding covariances + zeroRows(P,4,5); + zeroCols(P,4,5); + + if (PV_AidingMode != AID_ABSOLUTE) { + stateStruct.velocity.zero(); + // set the variances using the measurement noise parameter + P[5][5] = P[4][4] = sq(frontend->_gpsHorizVelNoise); + } else { + // reset horizontal velocity states to the GPS velocity if available + if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) { + stateStruct.velocity.x = gpsDataNew.vel.x; + stateStruct.velocity.y = gpsDataNew.vel.y; + // set the variances using the reported GPS speed accuracy + P[5][5] = P[4][4] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy)); + // clear the timeout flags and counters + velTimeout = false; + lastVelPassTime_ms = imuSampleTime_ms; + } else { + stateStruct.velocity.x = 0.0f; + stateStruct.velocity.y = 0.0f; + // set the variances using the likely speed range + P[5][5] = P[4][4] = sq(25.0f); + // clear the timeout flags and counters + velTimeout = false; + lastVelPassTime_ms = imuSampleTime_ms; + } + } + for (uint8_t i=0; i_gpsHorizPosNoise); + } else { + // Use GPS data as first preference if fresh data is available + if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) { + // write to state vector and compensate for offset between last GPS measurement and the EKF time horizon + stateStruct.position.x = gpsDataNew.pos.x + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms)); + stateStruct.position.y = gpsDataNew.pos.y + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms)); + // set the variances using the position measurement noise parameter + P[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise)); + // clear the timeout flags and counters + posTimeout = false; + lastPosPassTime_ms = imuSampleTime_ms; + } else if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250) { + // use the range beacon data as a second preference + stateStruct.position.x = receiverPos.x; + stateStruct.position.y = receiverPos.y; + // set the variances from the beacon alignment filter + P[7][7] = receiverPosCov[0][0]; + P[8][8] = receiverPosCov[1][1]; + // clear the timeout flags and counters + rngBcnTimeout = false; + lastRngBcnPassTime_ms = imuSampleTime_ms; + } + } + for (uint8_t i=0; i_fusionModeGPS == 0) { + stateStruct.velocity.z = gpsDataNew.vel.z; + } else if (onGround) { + stateStruct.velocity.z = 0.0f; + } + for (uint8_t i=0; i_gpsVertVelNoise); + +} + +// Zero the EKF height datum +// Return true if the height datum reset has been performed +bool NavEKF3_core::resetHeightDatum(void) +{ + if (activeHgtSource == HGT_SOURCE_RNG) { + // by definition the height dataum is at ground level so cannot perform the reset + return false; + } + // record the old height estimate + float oldHgt = -stateStruct.position.z; + // reset the barometer so that it reads zero at the current height + frontend->_baro.update_calibration(); + // reset the height state + stateStruct.position.z = 0.0f; + // adjust the height of the EKF origin so that the origin plus baro height before and afer the reset is the same + if (validOrigin) { + EKF_origin.alt += oldHgt*100; + } + // adjust the terrain state + terrainState += oldHgt; + return true; +} + +/******************************************************** +* FUSE MEASURED_DATA * +********************************************************/ +// select fusion of velocity, position and height measurements +void NavEKF3_core::SelectVelPosFusion() +{ + // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz + // If so, don't fuse measurements on this time step to reduce frame over-runs + // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements + if (magFusePerformed && dtIMUavg < 0.005f && !posVelFusionDelayed) { + posVelFusionDelayed = true; + return; + } else { + posVelFusionDelayed = false; + } + + // read GPS data from the sensor and check for new data in the buffer + readGpsData(); + gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms); + // Determine if we need to fuse position and velocity data on this time step + if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) { + // Don't fuse velocity data if GPS doesn't support it + if (frontend->_fusionModeGPS <= 1) { + fuseVelData = true; + } else { + fuseVelData = false; + } + fusePosData = true; + + // correct GPS data for position offset of antenna phase centre relative to the IMU + Vector3f posOffsetBody = _ahrs->get_gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset; + if (!posOffsetBody.is_zero()) { + if (fuseVelData) { + // TODO use a filtered angular rate with a group delay that matches the GPS delay + Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT); + Vector3f velOffsetBody = angRate % posOffsetBody; + Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody); + gpsDataDelayed.vel -= velOffsetEarth; + } + Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); + gpsDataDelayed.pos.x -= posOffsetEarth.x; + gpsDataDelayed.pos.y -= posOffsetEarth.y; + gpsDataDelayed.hgt += posOffsetEarth.z; + } + } else { + fuseVelData = false; + fusePosData = false; + } + + // we have GPS data to fuse and a request to align the yaw using the GPS course + if (gpsYawResetRequest) { + realignYawGPS(); + } + + // Select height data to be fused from the available baro, range finder and GPS sources + selectHeightForFusion(); + + // If we are operating without any aiding, fuse in the last known position + // to constrain tilt drift. This assumes a non-manoeuvring vehicle + // Do this to coincide with the height fusion + if (fuseHgtData && PV_AidingMode == AID_NONE) { + gpsDataDelayed.vel.zero(); + gpsDataDelayed.pos.x = lastKnownPositionNE.x; + gpsDataDelayed.pos.y = lastKnownPositionNE.y; + + fusePosData = true; + fuseVelData = false; + } + + // perform fusion + if (fuseVelData || fusePosData || fuseHgtData) { + FuseVelPosNED(); + // clear the flags to prevent repeated fusion of the same data + fuseVelData = false; + fuseHgtData = false; + fusePosData = false; + } +} + +// fuse selected position, velocity and height measurements +void NavEKF3_core::FuseVelPosNED() +{ + // start performance timer + hal.util->perf_begin(_perf_FuseVelPosNED); + + // health is set bad until test passed + velHealth = false; + posHealth = false; + hgtHealth = false; + + // declare variables used to check measurement errors + Vector3f velInnov; + + // declare variables used to control access to arrays + bool fuseData[6] = {false,false,false,false,false,false}; + uint8_t stateIndex; + uint8_t obsIndex; + + // declare variables used by state and covariance update calculations + Vector6 R_OBS; // Measurement variances used for fusion + Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only + Vector6 observation; + float SK; + + // perform sequential fusion of GPS measurements. This assumes that the + // errors in the different velocity and position components are + // uncorrelated which is not true, however in the absence of covariance + // data from the GPS receiver it is the only assumption we can make + // so we might as well take advantage of the computational efficiencies + // associated with sequential fusion + if (fuseVelData || fusePosData || fuseHgtData) { + // form the observation vector + observation[0] = gpsDataDelayed.vel.x; + observation[1] = gpsDataDelayed.vel.y; + observation[2] = gpsDataDelayed.vel.z; + observation[3] = gpsDataDelayed.pos.x; + observation[4] = gpsDataDelayed.pos.y; + observation[5] = -hgtMea; + + // calculate additional error in GPS position caused by manoeuvring + float posErr = frontend->gpsPosVarAccScale * accNavMag; + + // estimate the GPS Velocity, GPS horiz position and height measurement variances. + // Use different errors if operating without external aiding using an assumed position or velocity of zero + if (PV_AidingMode == AID_NONE) { + if (tiltAlignComplete && motorsArmed) { + // This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate + R_OBS[0] = sq(constrain_float(frontend->_noaidHorizNoise, 0.5f, 50.0f)); + } else { + // Use a smaller value to give faster initial alignment + R_OBS[0] = sq(0.5f); + } + R_OBS[1] = R_OBS[0]; + R_OBS[2] = R_OBS[0]; + R_OBS[3] = R_OBS[0]; + R_OBS[4] = R_OBS[0]; + for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i]; + } else { + if (gpsSpdAccuracy > 0.0f) { + // use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter + R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f)); + R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f)); + } else { + // calculate additional error in GPS velocity caused by manoeuvring + R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); + R_OBS[2] = sq(constrain_float(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag); + } + R_OBS[1] = R_OBS[0]; + // Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter + if (gpsPosAccuracy > 0.0f) { + R_OBS[3] = sq(constrain_float(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f)); + } else { + R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); + } + R_OBS[4] = R_OBS[3]; + // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity + // For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance + // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early + for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); + } + R_OBS[5] = posDownObsNoise; + for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i]; + + // if vertical GPS velocity data and an independent height source is being used, check to see if the GPS vertical velocity and altimeter + // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting + // the accelerometers and we should disable the GPS and barometer innovation consistency checks. + if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) { + // calculate innovations for height and vertical GPS vel measurements + float hgtErr = stateStruct.position.z - observation[5]; + float velDErr = stateStruct.velocity.z - observation[2]; + // check if they are the same sign and both more than 3-sigma out of bounds + if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[9][9] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[6][6] + R_OBS_DATA_CHECKS[2]))) { + badIMUdata = true; + } else { + badIMUdata = false; + } + } + + // calculate innovations and check GPS data validity using an innovation consistency check + // test position measurements + if (fusePosData) { + // test horizontal position measurements + innovVelPos[3] = stateStruct.position.x - observation[3]; + innovVelPos[4] = stateStruct.position.y - observation[4]; + varInnovVelPos[3] = P[7][7] + R_OBS_DATA_CHECKS[3]; + varInnovVelPos[4] = P[8][8] + R_OBS_DATA_CHECKS[4]; + // apply an innovation consistency threshold test, but don't fail if bad IMU data + float maxPosInnov2 = sq(MAX(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]); + posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2; + posHealth = ((posTestRatio < 1.0f) || badIMUdata); + // use position data if healthy or timed out + if (PV_AidingMode == AID_NONE) { + posHealth = true; + lastPosPassTime_ms = imuSampleTime_ms; + } else if (posHealth || posTimeout) { + posHealth = true; + lastPosPassTime_ms = imuSampleTime_ms; + // if timed out or outside the specified uncertainty radius, reset to the GPS + if (posTimeout || ((P[8][8] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) { + // reset the position to the current GPS position + ResetPosition(); + // reset the velocity to the GPS velocity + ResetVelocity(); + // don't fuse GPS data on this time step + fusePosData = false; + fuseVelData = false; + // Reset the position variances and corresponding covariances to a value that will pass the checks + zeroRows(P,7,8); + zeroCols(P,7,8); + P[7][7] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax)); + P[8][8] = P[7][7]; + // Reset the normalised innovation to avoid failing the bad fusion tests + posTestRatio = 0.0f; + velTestRatio = 0.0f; + } + } else { + posHealth = false; + } + } + + // test velocity measurements + if (fuseVelData) { + // test velocity measurements + uint8_t imax = 2; + // Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data + if (frontend->_fusionModeGPS >= 1 || PV_AidingMode != AID_ABSOLUTE) { + imax = 1; + } + float innovVelSumSq = 0; // sum of squares of velocity innovations + float varVelSum = 0; // sum of velocity innovation variances + for (uint8_t i = 0; i<=imax; i++) { + // velocity states start at index 4 + stateIndex = i + 4; + // calculate innovations using blended and single IMU predicted states + velInnov[i] = stateStruct.velocity[i] - observation[i]; // blended + // calculate innovation variance + varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i]; + // sum the innovation and innovation variances + innovVelSumSq += sq(velInnov[i]); + varVelSum += varInnovVelPos[i]; + } + // apply an innovation consistency threshold test, but don't fail if bad IMU data + // calculate the test ratio + velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f))); + // fail if the ratio is greater than 1 + velHealth = ((velTestRatio < 1.0f) || badIMUdata); + // use velocity data if healthy, timed out, or in constant position mode + if (velHealth || velTimeout) { + velHealth = true; + // restart the timeout count + lastVelPassTime_ms = imuSampleTime_ms; + // If we are doing full aiding and velocity fusion times out, reset to the GPS velocity + if (PV_AidingMode == AID_ABSOLUTE && velTimeout) { + // reset the velocity to the GPS velocity + ResetVelocity(); + // don't fuse GPS velocity data on this time step + fuseVelData = false; + // Reset the normalised innovation to avoid failing the bad fusion tests + velTestRatio = 0.0f; + } + } else { + velHealth = false; + } + } + + // test height measurements + if (fuseHgtData) { + // calculate height innovations + innovVelPos[5] = stateStruct.position.z - observation[5]; + varInnovVelPos[5] = P[9][9] + R_OBS_DATA_CHECKS[5]; + // calculate the innovation consistency test ratio + hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]); + // fail if the ratio is > 1, but don't fail if bad IMU data + hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata); + // Fuse height data if healthy or timed out or in constant position mode + if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE && onGround)) { + // Calculate a filtered value to be used by pre-flight health checks + // We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution + if (onGround) { + float dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f; + const float hgtInnovFiltTC = 2.0f; + float alpha = constrain_float(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f); + hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha; + } else { + hgtInnovFiltState = 0.0f; + } + + // if timed out, reset the height + if (hgtTimeout) { + ResetHeight(); + } + + // If we have got this far then declare the height data as healthy and reset the timeout counter + hgtHealth = true; + lastHgtPassTime_ms = imuSampleTime_ms; + } + } + + // set range for sequential fusion of velocity and position measurements depending on which data is available and its health + if (fuseVelData && velHealth) { + fuseData[0] = true; + fuseData[1] = true; + if (useGpsVertVel) { + fuseData[2] = true; + } + } + if (fusePosData && posHealth) { + fuseData[3] = true; + fuseData[4] = true; + } + if (fuseHgtData && hgtHealth) { + fuseData[5] = true; + } + + // fuse measurements sequentially + for (obsIndex=0; obsIndex<=5; obsIndex++) { + if (fuseData[obsIndex]) { + stateIndex = 4 + obsIndex; + // calculate the measurement innovation, using states from a different time coordinate if fusing height data + // adjust scaling on GPS measurement noise variances if not enough satellites + if (obsIndex <= 2) + { + innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - observation[obsIndex]; + R_OBS[obsIndex] *= sq(gpsNoiseScaler); + } + else if (obsIndex == 3 || obsIndex == 4) { + innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex]; + R_OBS[obsIndex] *= sq(gpsNoiseScaler); + } else if (obsIndex == 5) { + innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex]; + const float gndMaxBaroErr = 4.0f; + const float gndBaroInnovFloor = -0.5f; + + if(getTouchdownExpected()) { + // when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor + // constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr + // this function looks like this: + // |/ + //---------|--------- + // ____/| + // / | + // / | + innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr); + } + } + + // calculate the Kalman gain and calculate innovation variances + varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; + SK = 1.0f/varInnovVelPos[obsIndex]; + for (uint8_t i= 0; i<=15; i++) { + Kfusion[i] = P[i][stateIndex]*SK; + } + + // inhibit magnetic field state estimation by setting Kalman gains to zero + if (!inhibitMagStates) { + for (uint8_t i = 16; i<=21; i++) { + Kfusion[i] = P[i][stateIndex]*SK; + } + } else { + for (uint8_t i = 16; i<=21; i++) { + Kfusion[i] = 0.0f; + } + } + + // inhibit wind state estimation by setting Kalman gains to zero + if (!inhibitWindStates) { + Kfusion[22] = P[22][stateIndex]*SK; + Kfusion[23] = P[23][stateIndex]*SK; + } else { + Kfusion[22] = 0.0f; + Kfusion[23] = 0.0f; + } + + // update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations + // this is a numerically optimised implementation of standard equation P = (I - K*H)*P; + for (uint8_t i= 0; i<=stateIndexLim; i++) { + for (uint8_t j= 0; j<=stateIndexLim; j++) + { + KHP[i][j] = Kfusion[i] * P[stateIndex][j]; + } + } + // Check that we are not going to drive any variances negative and skip the update if so + bool healthyFusion = true; + for (uint8_t i= 0; i<=stateIndexLim; i++) { + if (KHP[i][i] > P[i][i]) { + healthyFusion = false; + } + } + if (healthyFusion) { + // update the covariance matrix + for (uint8_t i= 0; i<=stateIndexLim; i++) { + for (uint8_t j= 0; j<=stateIndexLim; j++) { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + ForceSymmetry(); + ConstrainVariances(); + + // update states and renormalise the quaternions + for (uint8_t i = 0; i<=stateIndexLim; i++) { + statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex]; + } + stateStruct.quat.normalize(); + + // record good fusion status + if (obsIndex == 0) { + faultStatus.bad_nvel = false; + } else if (obsIndex == 1) { + faultStatus.bad_evel = false; + } else if (obsIndex == 2) { + faultStatus.bad_dvel = false; + } else if (obsIndex == 3) { + faultStatus.bad_npos = false; + } else if (obsIndex == 4) { + faultStatus.bad_epos = false; + } else if (obsIndex == 5) { + faultStatus.bad_dpos = false; + } + } else { + // record bad fusion status + if (obsIndex == 0) { + faultStatus.bad_nvel = true; + } else if (obsIndex == 1) { + faultStatus.bad_evel = true; + } else if (obsIndex == 2) { + faultStatus.bad_dvel = true; + } else if (obsIndex == 3) { + faultStatus.bad_npos = true; + } else if (obsIndex == 4) { + faultStatus.bad_epos = true; + } else if (obsIndex == 5) { + faultStatus.bad_dpos = true; + } + } + } + } + } + + // stop performance timer + hal.util->perf_end(_perf_FuseVelPosNED); +} + +/******************************************************** +* MISC FUNCTIONS * +********************************************************/ + +// select the height measurement to be fused from the available baro, range finder and GPS sources +void NavEKF3_core::selectHeightForFusion() +{ + // Read range finder data and check for new data in the buffer + // This data is used by both height and optical flow fusion processing + readRangeFinder(); + rangeDataToFuse = storedRange.recall(rangeDataDelayed,imuDataDelayed.time_ms); + + // correct range data for the body frame position offset relative to the IMU + // the corrected reading is the reading that would have been taken if the sensor was + // co-located with the IMU + if (rangeDataToFuse) { + Vector3f posOffsetBody = frontend->_rng.get_pos_offset(rangeDataDelayed.sensor_idx) - accelPosOffset; + if (!posOffsetBody.is_zero()) { + Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); + rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z; + } + } + + // read baro height data from the sensor and check for new data in the buffer + readBaroData(); + baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms); + + // select height source + if (((frontend->_useRngSwHgt > 0) || (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) { + if (frontend->_altSource == 1) { + // always use range finder + activeHgtSource = HGT_SOURCE_RNG; + } else { + // determine if we are above or below the height switch region + float rangeMaxUse = 1e-4f * (float)frontend->_rng.max_distance_cm() * (float)frontend->_useRngSwHgt; + bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; + bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse; + + // If the terrain height is consistent and we are moving slowly, then it can be + // used as a height reference in combination with a range finder + float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y); + bool dontTrustTerrain = (horizSpeed > 2.0f) || !terrainHgtStable; + bool trustTerrain = (horizSpeed < 1.0f) && terrainHgtStable; + + /* + * Switch between range finder and primary height source using height above ground and speed thresholds with + * hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height + * which cannot be assumed if the vehicle is moving horizontally. + */ + if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG)) { + // cannot trust terrain or range finder so stop using range finder height + if (frontend->_altSource == 0) { + activeHgtSource = HGT_SOURCE_BARO; + } else if (frontend->_altSource == 2) { + activeHgtSource = HGT_SOURCE_GPS; + } + } else if (belowLowerSwHgt && trustTerrain && (activeHgtSource != HGT_SOURCE_RNG)) { + // reliable terrain and range finder so start using range finder height + activeHgtSource = HGT_SOURCE_RNG; + } + } + } else if ((frontend->_altSource == 2) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) { + activeHgtSource = HGT_SOURCE_GPS; + } else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) { + activeHgtSource = HGT_SOURCE_BCN; + } else { + activeHgtSource = HGT_SOURCE_BARO; + } + + // Use Baro alt as a fallback if we lose range finder or GPS + bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && ((imuSampleTime_ms - rngValidMeaTime_ms) > 500)); + bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000)); + if (lostRngHgt || lostGpsHgt) { + activeHgtSource = HGT_SOURCE_BARO; + } + + // if there is new baro data to fuse, calculate filtered baro data required by other processes + if (baroDataToFuse) { + // calculate offset to baro data that enables us to switch to Baro height use during operation + if (activeHgtSource != HGT_SOURCE_BARO) { + calcFiltBaroOffset(); + } + // filtered baro data used to provide a reference for takeoff + // it is is reset to last height measurement on disarming in performArmingChecks() + if (!getTakeoffExpected()) { + const float gndHgtFiltTC = 0.5f; + const float dtBaro = frontend->hgtAvg_ms*1.0e-3f; + float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f); + meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha; + } + } + + // calculate offset to GPS height data that enables us to switch to GPS height during operation + if (gpsDataToFuse && (activeHgtSource != HGT_SOURCE_GPS)) { + calcFiltGpsHgtOffset(); + } + + // Select the height measurement source + if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) { + // using range finder data + // correct for tilt using a flat earth model + if (prevTnb.c.z >= 0.7) { + // calculate height above ground + hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd); + // correct for terrain position relative to datum + hgtMea -= terrainState; + // enable fusion + fuseHgtData = true; + // set the observation noise + posDownObsNoise = sq(constrain_float(frontend->_rngNoise, 0.1f, 10.0f)); + // add uncertainty created by terrain gradient and vehicle tilt + posDownObsNoise += sq(rangeDataDelayed.rng * frontend->_terrGradMax) * MAX(0.0f , (1.0f - sq(prevTnb.c.z))); + } else { + // disable fusion if tilted too far + fuseHgtData = false; + } + } else if (gpsDataToFuse && (activeHgtSource == HGT_SOURCE_GPS)) { + // using GPS data + hgtMea = gpsDataDelayed.hgt; + // enable fusion + fuseHgtData = true; + // set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio + if (gpsHgtAccuracy > 0.0f) { + posDownObsNoise = sq(constrain_float(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise, 100.0f)); + } else { + posDownObsNoise = sq(constrain_float(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f)); + } + } else if (baroDataToFuse && (activeHgtSource == HGT_SOURCE_BARO)) { + // using Baro data + hgtMea = baroDataDelayed.hgt - baroHgtOffset; + // enable fusion + fuseHgtData = true; + // set the observation noise + posDownObsNoise = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f)); + // reduce weighting (increase observation noise) on baro if we are likely to be in ground effect + if (getTakeoffExpected() || getTouchdownExpected()) { + posDownObsNoise *= frontend->gndEffectBaroScaler; + } + // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff + // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent + if (motorsArmed && getTakeoffExpected()) { + hgtMea = MAX(hgtMea, meaHgtAtTakeOff); + } + } else { + fuseHgtData = false; + } + + // If we haven't fused height data for a while, then declare the height data as being timed out + // set timeout period based on whether we have vertical GPS velocity available to constrain drift + hgtRetryTime_ms = (useGpsVertVel && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms; + if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms) { + hgtTimeout = true; + } else { + hgtTimeout = false; + } +} + +#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp new file mode 100644 index 0000000000..e696d4b9d8 --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -0,0 +1,583 @@ +#include + +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 + +#include "AP_NavEKF3.h" +#include "AP_NavEKF3_core.h" +#include +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +/******************************************************** +* FUSE MEASURED_DATA * +********************************************************/ + +// select fusion of range beacon measurements +void NavEKF3_core::SelectRngBcnFusion() +{ + // read range data from the sensor and check for new data in the buffer + readRngBcnData(); + + // Determine if we need to fuse range beacon data on this time step + if (rngBcnDataToFuse) { + if (PV_AidingMode == AID_ABSOLUTE) { + // Normal operating mode is to fuse the range data into the main filter + FuseRngBcn(); + } else { + // If we aren't able to use the data in the main filter, use a simple 3-state filter to estimte position only + FuseRngBcnStatic(); + } + } +} + +void NavEKF3_core::FuseRngBcn() +{ + // declarations + float pn; + float pe; + float pd; + float bcn_pn; + float bcn_pe; + float bcn_pd; + const float R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f)); + float rngPred; + + // health is set bad until test passed + rngBcnHealth = false; + + if (activeHgtSource != HGT_SOURCE_BCN) { + // calculate the vertical offset from EKF datum to beacon datum + CalcRangeBeaconPosDownOffset(R_BCN, stateStruct.position, false); + } else { + bcnPosOffset = 0.0f; + } + + // copy required states to local variable names + pn = stateStruct.position.x; + pe = stateStruct.position.y; + pd = stateStruct.position.z; + bcn_pn = rngBcnDataDelayed.beacon_posNED.x; + bcn_pe = rngBcnDataDelayed.beacon_posNED.y; + bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffset; + + // predicted range + Vector3f deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED; + rngPred = deltaPosNED.length(); + + // calculate measurement innovation + innovRngBcn = rngPred - rngBcnDataDelayed.rng; + + // perform fusion of range measurement + if (rngPred > 0.1f) + { + // calculate observation jacobians + float H_BCN[24]; + memset(H_BCN, 0, sizeof(H_BCN)); + float t2 = bcn_pd-pd; + float t3 = bcn_pe-pe; + float t4 = bcn_pn-pn; + float t5 = t2*t2; + float t6 = t3*t3; + float t7 = t4*t4; + float t8 = t5+t6+t7; + float t9 = 1.0f/sqrtf(t8); + H_BCN[7] = -t4*t9; + H_BCN[8] = -t3*t9; + // If we are not using the beacons as a height reference, we pretend that the beacons + // are a the same height as the flight vehicle when calculating the observation derivatives + // and Kalman gains + // TODO - less hacky way of achieving this, preferably using an alternative derivation + if (activeHgtSource != HGT_SOURCE_BCN) { + t2 = 0.0f; + } + H_BCN[9] = -t2*t9; + + // calculate Kalman gains + float t10 = P[9][9]*t2*t9; + float t11 = P[8][9]*t3*t9; + float t12 = P[7][9]*t4*t9; + float t13 = t10+t11+t12; + float t14 = t2*t9*t13; + float t15 = P[9][8]*t2*t9; + float t16 = P[8][8]*t3*t9; + float t17 = P[7][8]*t4*t9; + float t18 = t15+t16+t17; + float t19 = t3*t9*t18; + float t20 = P[9][7]*t2*t9; + float t21 = P[8][7]*t3*t9; + float t22 = P[7][7]*t4*t9; + float t23 = t20+t21+t22; + float t24 = t4*t9*t23; + varInnovRngBcn = R_BCN+t14+t19+t24; + float t26; + if (varInnovRngBcn >= R_BCN) { + t26 = 1.0f/varInnovRngBcn; + faultStatus.bad_rngbcn = false; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we reset the covariance matrix and try again next measurement + CovarianceInit(); + faultStatus.bad_rngbcn = true; + return; + } + + Kfusion[0] = -t26*(P[0][7]*t4*t9+P[0][8]*t3*t9+P[0][9]*t2*t9); + Kfusion[1] = -t26*(P[1][7]*t4*t9+P[1][8]*t3*t9+P[1][9]*t2*t9); + Kfusion[2] = -t26*(P[2][7]*t4*t9+P[2][8]*t3*t9+P[2][9]*t2*t9); + Kfusion[3] = -t26*(P[3][7]*t4*t9+P[3][8]*t3*t9+P[3][9]*t2*t9); + Kfusion[4] = -t26*(P[4][7]*t4*t9+P[4][8]*t3*t9+P[4][9]*t2*t9); + Kfusion[5] = -t26*(P[5][7]*t4*t9+P[5][8]*t3*t9+P[5][9]*t2*t9); + Kfusion[7] = -t26*(t22+P[7][8]*t3*t9+P[7][9]*t2*t9); + Kfusion[8] = -t26*(t16+P[8][7]*t4*t9+P[8][9]*t2*t9); + Kfusion[10] = -t26*(P[10][7]*t4*t9+P[10][8]*t3*t9+P[10][9]*t2*t9); + Kfusion[11] = -t26*(P[11][7]*t4*t9+P[11][8]*t3*t9+P[11][9]*t2*t9); + Kfusion[12] = -t26*(P[12][7]*t4*t9+P[12][8]*t3*t9+P[12][9]*t2*t9); + Kfusion[13] = -t26*(P[13][7]*t4*t9+P[13][8]*t3*t9+P[13][9]*t2*t9); + Kfusion[14] = -t26*(P[14][7]*t4*t9+P[14][8]*t3*t9+P[14][9]*t2*t9); + Kfusion[15] = -t26*(P[15][7]*t4*t9+P[15][8]*t3*t9+P[15][9]*t2*t9); + // only allow the range observations to modify the vertical states if we are using it as a height reference + if (activeHgtSource == HGT_SOURCE_BCN) { + Kfusion[6] = -t26*(P[6][7]*t4*t9+P[6][8]*t3*t9+P[6][9]*t2*t9); + Kfusion[9] = -t26*(t10+P[9][7]*t4*t9+P[9][8]*t3*t9); + } else { + Kfusion[6] = 0.0f; + Kfusion[9] = 0.0f; + } + + if (!inhibitMagStates) { + Kfusion[16] = -t26*(P[16][7]*t4*t9+P[16][8]*t3*t9+P[16][9]*t2*t9); + Kfusion[17] = -t26*(P[17][7]*t4*t9+P[17][8]*t3*t9+P[17][9]*t2*t9); + Kfusion[18] = -t26*(P[18][7]*t4*t9+P[18][8]*t3*t9+P[18][9]*t2*t9); + Kfusion[19] = -t26*(P[19][7]*t4*t9+P[19][8]*t3*t9+P[19][9]*t2*t9); + Kfusion[20] = -t26*(P[20][7]*t4*t9+P[20][8]*t3*t9+P[20][9]*t2*t9); + Kfusion[21] = -t26*(P[21][7]*t4*t9+P[21][8]*t3*t9+P[21][9]*t2*t9); + } else { + // zero indexes 16 to 21 = 6*4 bytes + memset(&Kfusion[16], 0, 24); + } + Kfusion[22] = -t26*(P[22][7]*t4*t9+P[22][8]*t3*t9+P[22][9]*t2*t9); + Kfusion[23] = -t26*(P[23][7]*t4*t9+P[23][8]*t3*t9+P[23][9]*t2*t9); + + // Calculate innovation using the selected offset value + Vector3f delta = stateStruct.position - rngBcnDataDelayed.beacon_posNED; + innovRngBcn = delta.length() - rngBcnDataDelayed.rng; + + // calculate the innovation consistency test ratio + rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (float)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn); + + // fail if the ratio is > 1, but don't fail if bad IMU data + rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata); + + // test the ratio before fusing data + if (rngBcnHealth) { + + // restart the counter + lastRngBcnPassTime_ms = imuSampleTime_ms; + + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (unsigned i = 0; i<=stateIndexLim; i++) { + for (unsigned j = 0; j<=6; j++) { + KH[i][j] = 0.0f; + } + for (unsigned j = 7; j<=9; j++) { + KH[i][j] = Kfusion[i] * H_BCN[j]; + } + for (unsigned j = 10; j<=23; j++) { + KH[i][j] = 0.0f; + } + } + for (unsigned j = 0; j<=stateIndexLim; j++) { + for (unsigned i = 0; i<=stateIndexLim; i++) { + ftype res = 0; + res += KH[i][7] * P[7][j]; + res += KH[i][8] * P[8][j]; + res += KH[i][9] * P[9][j]; + KHP[i][j] = res; + } + } + // Check that we are not going to drive any variances negative and skip the update if so + bool healthyFusion = true; + for (uint8_t i= 0; i<=stateIndexLim; i++) { + if (KHP[i][i] > P[i][i]) { + healthyFusion = false; + } + } + if (healthyFusion) { + // update the covariance matrix + for (uint8_t i= 0; i<=stateIndexLim; i++) { + for (uint8_t j= 0; j<=stateIndexLim; j++) { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. + ForceSymmetry(); + ConstrainVariances(); + + // correct the state vector + for (uint8_t j= 0; j<=stateIndexLim; j++) { + statesArray[j] = statesArray[j] - Kfusion[j] * innovRngBcn; + } + + // record healthy fusion + faultStatus.bad_rngbcn = false; + + } else { + // record bad fusion + faultStatus.bad_rngbcn = true; + + } + } + + // Update the fusion report + rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED; + rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn; + rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn; + rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng; + rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio; + } +} + +/* +Use range beaon measurements to calculate a static position using a 3-state EKF algorithm. +Algorihtm based on the following: +https://github.com/priseborough/InertialNav/blob/master/derivations/range_beacon.m +*/ +void NavEKF3_core::FuseRngBcnStatic() +{ + // get the estimated range measurement variance + const float R_RNG = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f)); + + /* + The first thing to do is to check if we have started the alignment and if not, initialise the + states and covariance to a first guess. To do this iterate through the avilable beacons and then + initialise the initial position to the mean beacon position. The initial position uncertainty + is set to the mean range measurement. + */ + if (!rngBcnAlignmentStarted) { + if (rngBcnDataDelayed.beacon_ID != lastBeaconIndex) { + rngBcnPosSum += rngBcnDataDelayed.beacon_posNED; + lastBeaconIndex = rngBcnDataDelayed.beacon_ID; + rngSum += rngBcnDataDelayed.rng; + numBcnMeas++; + + // capture the beacon vertical spread + if (rngBcnDataDelayed.beacon_posNED.z > maxBcnPosD) { + maxBcnPosD = rngBcnDataDelayed.beacon_posNED.z; + } else if(rngBcnDataDelayed.beacon_posNED.z < minBcnPosD) { + minBcnPosD = rngBcnDataDelayed.beacon_posNED.z; + } + } + if (numBcnMeas >= 100) { + rngBcnAlignmentStarted = true; + float tempVar = 1.0f / (float)numBcnMeas; + // initialise the receiver position to the centre of the beacons and at zero height + receiverPos.x = rngBcnPosSum.x * tempVar; + receiverPos.y = rngBcnPosSum.y * tempVar; + receiverPos.z = 0.0f; + receiverPosCov[2][2] = receiverPosCov[1][1] = receiverPosCov[0][0] = rngSum * tempVar; + lastBeaconIndex = 0; + numBcnMeas = 0; + rngBcnPosSum.zero(); + rngSum = 0.0f; + } + } + + if (rngBcnAlignmentStarted) { + numBcnMeas++; + + if (numBcnMeas >= 100) { + // 100 observations is enough for a stable estimate under most conditions + // TODO monitor stability of the position estimate + rngBcnAlignmentCompleted = true; + + } + + if (rngBcnAlignmentCompleted) { + if (activeHgtSource != HGT_SOURCE_BCN) { + // We are using a different height reference for the main EKF so need to estimate a vertical + // position offset to be applied to the beacon system that minimises the range innovations + // The position estimate should be stable after 100 iterations so we use a simple dual + // hypothesis 1-state EKF to estiate the offset + Vector3f refPosNED; + refPosNED.x = receiverPos.x; + refPosNED.y = receiverPos.y; + refPosNED.z = stateStruct.position.z; + CalcRangeBeaconPosDownOffset(R_RNG, refPosNED, true); + + } else { + // we are using the beacons as the primary height source, so don't modify their vertical position + bcnPosOffset = 0.0f; + + } + } else { + if (activeHgtSource != HGT_SOURCE_BCN) { + // The position estimate is not yet stable so we cannot run the 1-state EKF to estimate + // beacon system vertical position offset. Instead we initialise the dual hypothesis offset states + // using the beacon vertical position, vertical position estimate relative to the beacon origin + // and the main EKF vertical position + + // Calculate the mid vertical position of all beacons + float bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD); + + // calculate the delta to the estimated receiver position + float delta = receiverPos.z - bcnMidPosD; + + // calcuate the two hypothesis for our vertical position + float receverPosDownMax; + float receverPosDownMin; + if (delta >= 0.0f) { + receverPosDownMax = receiverPos.z; + receverPosDownMin = receiverPos.z - 2.0f * delta; + } else { + receverPosDownMax = receiverPos.z - 2.0f * delta; + receverPosDownMin = receiverPos.z; + } + + bcnPosDownOffsetMax = stateStruct.position.z - receverPosDownMin; + bcnPosDownOffsetMin = stateStruct.position.z - receverPosDownMax; + } else { + // We are using the beacons as the primary height reference, so don't modify their vertical position + bcnPosOffset = 0.0f; + } + } + + // Add some process noise to the states at each time step + for (uint8_t i= 0; i<=2; i++) { + receiverPosCov[i][i] += 0.1f; + } + + // calculate the observation jacobian + float t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffset; + float t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y; + float t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x; + float t5 = t2*t2; + float t6 = t3*t3; + float t7 = t4*t4; + float t8 = t5+t6+t7; + if (t8 < 0.1f) { + // calculation will be badly conditioned + return; + } + float t9 = 1.0f/sqrtf(t8); + float t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f; + float t15 = receiverPos.x*2.0f; + float t11 = t10-t15; + float t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f; + float t14 = receiverPos.y*2.0f; + float t13 = t12-t14; + float t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f; + float t18 = receiverPos.z*2.0f; + float t17 = t16-t18; + float H_RNG[3]; + H_RNG[0] = -t9*t11*0.5f; + H_RNG[1] = -t9*t13*0.5f; + H_RNG[2] = -t9*t17*0.5f; + + // calculate the Kalman gains + float t19 = receiverPosCov[0][0]*t9*t11*0.5f; + float t20 = receiverPosCov[1][1]*t9*t13*0.5f; + float t21 = receiverPosCov[0][1]*t9*t11*0.5f; + float t22 = receiverPosCov[2][1]*t9*t17*0.5f; + float t23 = t20+t21+t22; + float t24 = t9*t13*t23*0.5f; + float t25 = receiverPosCov[1][2]*t9*t13*0.5f; + float t26 = receiverPosCov[0][2]*t9*t11*0.5f; + float t27 = receiverPosCov[2][2]*t9*t17*0.5f; + float t28 = t25+t26+t27; + float t29 = t9*t17*t28*0.5f; + float t30 = receiverPosCov[1][0]*t9*t13*0.5f; + float t31 = receiverPosCov[2][0]*t9*t17*0.5f; + float t32 = t19+t30+t31; + float t33 = t9*t11*t32*0.5f; + varInnovRngBcn = R_RNG+t24+t29+t33; + float t35 = 1.0f/varInnovRngBcn; + float K_RNG[3]; + K_RNG[0] = -t35*(t19+receiverPosCov[0][1]*t9*t13*0.5f+receiverPosCov[0][2]*t9*t17*0.5f); + K_RNG[1] = -t35*(t20+receiverPosCov[1][0]*t9*t11*0.5f+receiverPosCov[1][2]*t9*t17*0.5f); + K_RNG[2] = -t35*(t27+receiverPosCov[2][0]*t9*t11*0.5f+receiverPosCov[2][1]*t9*t13*0.5f); + + // calculate range measurement innovation + Vector3f deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED; + deltaPosNED.z -= bcnPosOffset; + innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng; + + // update the state + receiverPos.x -= K_RNG[0] * innovRngBcn; + receiverPos.y -= K_RNG[1] * innovRngBcn; + receiverPos.z -= K_RNG[2] * innovRngBcn; + receiverPos.z = MAX(receiverPos.z, minBcnPosD + 1.2f); + + // calculate the covariance correction + for (unsigned i = 0; i<=2; i++) { + for (unsigned j = 0; j<=2; j++) { + KH[i][j] = K_RNG[i] * H_RNG[j]; + } + } + for (unsigned j = 0; j<=2; j++) { + for (unsigned i = 0; i<=2; i++) { + ftype res = 0; + res += KH[i][0] * receiverPosCov[0][j]; + res += KH[i][1] * receiverPosCov[1][j]; + res += KH[i][2] * receiverPosCov[2][j]; + KHP[i][j] = res; + } + } + // prevent negative variances + for (uint8_t i= 0; i<=2; i++) { + if (receiverPosCov[i][i] < 0.0f) { + receiverPosCov[i][i] = 0.0f; + KHP[i][i] = 0.0f; + } else if (KHP[i][i] > receiverPosCov[i][i]) { + KHP[i][i] = receiverPosCov[i][i]; + } + } + // apply the covariance correction + for (uint8_t i= 0; i<=2; i++) { + for (uint8_t j= 0; j<=2; j++) { + receiverPosCov[i][j] -= KHP[i][j]; + } + } + // ensure the covariance matrix is symmetric + for (uint8_t i=1; i<=2; i++) { + for (uint8_t j=0; j<=i-1; j++) { + float temp = 0.5f*(receiverPosCov[i][j] + receiverPosCov[j][i]); + receiverPosCov[i][j] = temp; + receiverPosCov[j][i] = temp; + } + } + + if (numBcnMeas >= 100) { + // 100 observations is enough for a stable estimate under most conditions + // TODO monitor stability of the position estimate + rngBcnAlignmentCompleted = true; + } + } +} + +/* +Run a single state Kalman filter to estimate the vertical position offset of the range beacon constellation +Calculate using a high and low hypothesis and select the hypothesis with the lowest innovation sequence +*/ +void NavEKF3_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning) +{ + // Handle height offsets between the primary height source and the range beacons by estimating + // the beacon systems global vertical position offset using a single state Kalman filter + // The estimated offset is used to correct the beacon height when calculating innovations + // A high and low estimate is calculated to handle the ambiguity in height associated with beacon positions that are co-planar + // The main filter then uses the offset with the smaller innovations + + float innov; // range measurement innovation (m) + float innovVar; // range measurement innovation variance (m^2) + float gain; // Kalman gain + float obsDeriv; // derivative of observation relative to state + + const float stateNoiseVar = 0.1f; // State process noise variance + const float filtAlpha = 0.01f; // LPF constant + const float innovGateWidth = 5.0f; // width of innovation consistency check gate in std + + // estimate upper value for offset + + // calculate observation derivative + float t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMax; + float t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y; + float t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x; + float t5 = t2*t2; + float t6 = t3*t3; + float t7 = t4*t4; + float t8 = t5+t6+t7; + float t9; + if (t8 > 0.1f) { + t9 = 1.0f/sqrtf(t8); + obsDeriv = t2*t9; + + // Calculate innovation + innov = sqrtf(t8) - rngBcnDataDelayed.rng; + + // calculate a filtered innovation magnitude to be used to select between the high or low offset + OffsetMaxInnovFilt = (1.0f - filtAlpha) * bcnPosOffsetMaxVar + filtAlpha * fabsf(innov); + + // covariance prediction + bcnPosOffsetMaxVar += stateNoiseVar; + + // calculate the innovation variance + innovVar = obsDeriv * bcnPosOffsetMaxVar * obsDeriv + obsVar; + innovVar = MAX(innovVar, obsVar); + + // Reject range innovation spikes using a 5-sigma threshold unless aligning + if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) { + // calculate the Kalman gain + gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar; + + // state update + bcnPosDownOffsetMax -= innov * gain; + + // covariance update + bcnPosOffsetMaxVar -= gain * obsDeriv * bcnPosOffsetMaxVar; + bcnPosOffsetMaxVar = MAX(bcnPosOffsetMaxVar, 0.0f); + } + } + + // estimate lower value for offset + + // calculate observation derivative + t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosDownOffsetMin; + t5 = t2*t2; + t8 = t5+t6+t7; + if (t8 > 0.1f) { + t9 = 1.0f/sqrtf(t8); + obsDeriv = t2*t9; + + // Calculate innovation + innov = sqrtf(t8) - rngBcnDataDelayed.rng; + + // calculate a filtered innovation magnitude to be used to select between the high or low offset + OffsetMinInnovFilt = (1.0f - filtAlpha) * OffsetMinInnovFilt + filtAlpha * fabsf(innov); + + // covariance prediction + bcnPosOffsetMinVar += stateNoiseVar; + + // calculate the innovation variance + innovVar = obsDeriv * bcnPosOffsetMinVar * obsDeriv + obsVar; + innovVar = MAX(innovVar, obsVar); + + // Reject range innovation spikes using a 5-sigma threshold unless aligning + if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) { + // calculate the Kalman gain + gain = (bcnPosOffsetMinVar * obsDeriv) / innovVar; + + // state update + bcnPosDownOffsetMin -= innov * gain; + + // covariance update + bcnPosOffsetMinVar -= gain * obsDeriv * bcnPosOffsetMinVar; + bcnPosOffsetMinVar = MAX(bcnPosOffsetMinVar, 0.0f); + } + } + + // calculate the mid vertical position of all beacons + float bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD); + + // ensure the two beacon vertical offset hypothesis place the mid point of the beacons below and above the flight vehicle + bcnPosDownOffsetMax = MAX(bcnPosDownOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f); + bcnPosDownOffsetMin = MIN(bcnPosDownOffsetMin, vehiclePosNED.z - bcnMidPosD - 0.5f); + + // calculate the innovation for the main filter using the offset with the smallest innovation history + // apply hysteresis to prevent rapid switching + if (!usingMinHypothesis && OffsetMinInnovFilt < 0.8f * OffsetMaxInnovFilt) { + bcnPosOffset = bcnPosDownOffsetMin; + usingMinHypothesis = true; + } else if (usingMinHypothesis && OffsetMaxInnovFilt < 0.8f * OffsetMinInnovFilt) { + bcnPosOffset = bcnPosDownOffsetMax; + usingMinHypothesis = false; + } + +} + +#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp new file mode 100644 index 0000000000..5766218181 --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -0,0 +1,449 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 + +#include "AP_NavEKF3.h" +#include "AP_NavEKF3_core.h" +#include +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + + +/* Monitor GPS data to see if quality is good enough to initialise the EKF + Monitor magnetometer innovations to to see if the heading is good enough to use GPS + Return true if all criteria pass for 10 seconds + + We also record the failure reason so that prearm_failure_reason() + can give a good report to the user on why arming is failing +*/ +bool NavEKF3_core::calcGpsGoodToAlign(void) +{ + if (inFlight && assume_zero_sideslip() && !use_compass()) { + // this is a special case where a plane has launched without magnetometer + // is now in the air and needs to align yaw to the GPS and start navigating as soon as possible + return true; + } + + // User defined multiplier to be applied to check thresholds + float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler; + + // If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states + // This enables us to handle large changes to the external magnetic field environment that occur before arming + if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f && yawTestRatio <= 1.0f) || !consistentMagData) { + magYawResetTimer_ms = imuSampleTime_ms; + } + if ((imuSampleTime_ms - magYawResetTimer_ms > 5000) && !motorsArmed) { + // request reset of heading and magnetic field states + magYawResetRequest = true; + // reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds + magYawResetTimer_ms = imuSampleTime_ms; + } + + // Check for significant change in GPS position if disarmed which indicates bad GPS + // This check can only be used when the vehicle is stationary + const struct Location &gpsloc = _ahrs->get_gps().location(); // Current location + const float posFiltTimeConst = 10.0f; // time constant used to decay position drift + // calculate time lapsesd since last update and limit to prevent numerical errors + float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst); + lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms; + // Sum distance moved + gpsDriftNE += location_diff(gpsloc_prev, gpsloc).length(); + gpsloc_prev = gpsloc; + // Decay distance moved exponentially to zero + gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst); + // Clamp the fiter state to prevent excessive persistence of large transients + gpsDriftNE = MIN(gpsDriftNE,10.0f); + // Fail if more than 3 metres drift after filtering whilst on-ground + // This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m + bool gpsDriftFail = (gpsDriftNE > 3.0f*checkScaler) && onGround && (frontend->_gpsCheck & MASK_GPS_POS_DRIFT); + + // Report check result as a text string and bitmask + if (gpsDriftFail) { + hal.util->snprintf(prearm_fail_string, + sizeof(prearm_fail_string), + "GPS drift %.1fm (needs %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler)); + gpsCheckStatus.bad_horiz_drift = true; + } else { + gpsCheckStatus.bad_horiz_drift = false; + } + + // Check that the vertical GPS vertical velocity is reasonable after noise filtering + bool gpsVertVelFail; + if (_ahrs->get_gps().have_vertical_velocity() && onGround) { + // check that the average vertical GPS velocity is close to zero + gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt; + gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f); + gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD); + } else if ((frontend->_fusionModeGPS == 0) && !_ahrs->get_gps().have_vertical_velocity()) { + // If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail + gpsVertVelFail = true; + // if we have a 3D fix with no vertical velocity and + // EK2_GPS_TYPE=0 then change it to 1. It means the GPS is not + // capable of giving a vertical velocity + if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) { + frontend->_fusionModeGPS.set(1); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EK2: Changed EK2_GPS_TYPE to 1"); + } + } else { + gpsVertVelFail = false; + } + + // Report check result as a text string and bitmask + if (gpsVertVelFail) { + hal.util->snprintf(prearm_fail_string, + sizeof(prearm_fail_string), + "GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler)); + gpsCheckStatus.bad_vert_vel = true; + } else { + gpsCheckStatus.bad_vert_vel = false; + } + + // Check that the horizontal GPS vertical velocity is reasonable after noise filtering + // This check can only be used if the vehicle is stationary + bool gpsHorizVelFail; + if (onGround) { + gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt; + gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f); + gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD); + } else { + gpsHorizVelFail = false; + } + + // Report check result as a text string and bitmask + if (gpsHorizVelFail) { + hal.util->snprintf(prearm_fail_string, + sizeof(prearm_fail_string), + "GPS horizontal speed %.2fm/s (needs %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler)); + gpsCheckStatus.bad_horiz_vel = true; + } else { + gpsCheckStatus.bad_horiz_vel = false; + } + + // fail if horiziontal position accuracy not sufficient + float hAcc = 0.0f; + bool hAccFail; + if (_ahrs->get_gps().horizontal_accuracy(hAcc)) { + hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR); + } else { + hAccFail = false; + } + + // Report check result as a text string and bitmask + if (hAccFail) { + hal.util->snprintf(prearm_fail_string, + sizeof(prearm_fail_string), + "GPS horiz error %.1fm (needs %.1f)", (double)hAcc, (double)(5.0f*checkScaler)); + gpsCheckStatus.bad_hAcc = true; + } else { + gpsCheckStatus.bad_hAcc = false; + } + + // fail if reported speed accuracy greater than threshold + bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_SPD_ERR); + + // Report check result as a text string and bitmask + if (gpsSpdAccFail) { + hal.util->snprintf(prearm_fail_string, + sizeof(prearm_fail_string), + "GPS speed error %.1f (needs %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler)); + gpsCheckStatus.bad_sAcc = true; + } else { + gpsCheckStatus.bad_sAcc = false; + } + + // fail if satellite geometry is poor + bool hdopFail = (_ahrs->get_gps().get_hdop() > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP); + + // Report check result as a text string and bitmask + if (hdopFail) { + hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), + "GPS HDOP %.1f (needs 2.5)", (double)(0.01f * _ahrs->get_gps().get_hdop())); + gpsCheckStatus.bad_hdop = true; + } else { + gpsCheckStatus.bad_hdop = false; + } + + // fail if not enough sats + bool numSatsFail = (_ahrs->get_gps().num_sats() < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS); + + // Report check result as a text string and bitmask + if (numSatsFail) { + hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), + "GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats()); + gpsCheckStatus.bad_sats = true; + } else { + gpsCheckStatus.bad_sats = false; + } + + // fail if magnetometer innovations are outside limits indicating bad yaw + // with bad yaw we are unable to use GPS + bool yawFail; + if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f || yawTestRatio > 1.0f) && (frontend->_gpsCheck & MASK_GPS_YAW_ERR)) { + yawFail = true; + } else { + yawFail = false; + } + + // Report check result as a text string and bitmask + if (yawFail) { + hal.util->snprintf(prearm_fail_string, + sizeof(prearm_fail_string), + "Mag yaw error x=%.1f y=%.1f", + (double)magTestRatio.x, + (double)magTestRatio.y); + gpsCheckStatus.bad_yaw = true; + } else { + gpsCheckStatus.bad_yaw = false; + } + + // assume failed first time through and notify user checks have started + if (lastGpsVelFail_ms == 0) { + hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF starting GPS checks"); + lastGpsVelFail_ms = imuSampleTime_ms; + } + + // record time of fail + if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail) { + lastGpsVelFail_ms = imuSampleTime_ms; + } + + // continuous period without fail required to return a healthy status + if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) { + return true; + } + return false; +} + +// update inflight calculaton that determines if GPS data is good enough for reliable navigation +void NavEKF3_core::calcGpsGoodForFlight(void) +{ + // use a simple criteria based on the GPS receivers claimed speed accuracy and the EKF innovation consistency checks + + // set up varaibles and constants used by filter that is applied to GPS speed accuracy + const float alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data + const float tau = 10.0f; // time constant (sec) of peak hold decay + if (lastGpsCheckTime_ms == 0) { + lastGpsCheckTime_ms = imuSampleTime_ms; + } + float dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f; + lastGpsCheckTime_ms = imuSampleTime_ms; + float alpha2 = constrain_float(dtLPF/tau,0.0f,1.0f); + + // get the receivers reported speed accuracy + float gpsSpdAccRaw; + if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) { + gpsSpdAccRaw = 0.0f; + } + + // filter the raw speed accuracy using a LPF + sAccFilterState1 = constrain_float((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f); + + // apply a peak hold filter to the LPF output + sAccFilterState2 = MAX(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2)); + + // Apply a threshold test with hysteresis to the filtered GPS speed accuracy data + if (sAccFilterState2 > 1.5f ) { + gpsSpdAccPass = false; + } else if(sAccFilterState2 < 1.0f) { + gpsSpdAccPass = true; + } + + // Apply a threshold test with hysteresis to the normalised position and velocity innovations + // Require a fail for one second and a pass for 10 seconds to transition + if (lastInnovFailTime_ms == 0) { + lastInnovFailTime_ms = imuSampleTime_ms; + lastInnovPassTime_ms = imuSampleTime_ms; + } + if (velTestRatio < 1.0f && posTestRatio < 1.0f) { + lastInnovPassTime_ms = imuSampleTime_ms; + } else if (velTestRatio > 0.7f || posTestRatio > 0.7f) { + lastInnovFailTime_ms = imuSampleTime_ms; + } + if ((imuSampleTime_ms - lastInnovPassTime_ms) > 1000) { + ekfInnovationsPass = false; + } else if ((imuSampleTime_ms - lastInnovFailTime_ms) > 10000) { + ekfInnovationsPass = true; + } + + // both GPS speed accuracy and EKF innovations must pass + gpsAccuracyGood = gpsSpdAccPass && ekfInnovationsPass; +} + +// Detect if we are in flight or on ground +void NavEKF3_core::detectFlight() +{ + /* + If we are a fly forward type vehicle (eg plane), then in-air status can be determined through a combination of speed and height criteria. + Because of the differing certainty requirements of algorithms that need the in-flight / on-ground status we use two booleans where + onGround indicates a high certainty we are not flying and inFlight indicates a high certainty we are flying. It is possible for + both onGround and inFlight to be false if the status is uncertain, but they cannot both be true. + + If we are a plane as indicated by the assume_zero_sideslip() status, then different logic is used + + TODO - this logic should be moved out of the EKF and into the flight vehicle code. + */ + + if (assume_zero_sideslip()) { + // To be confident we are in the air we use a criteria which combines arm status, ground speed, airspeed and height change + float gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y); + bool highGndSpd = false; + bool highAirSpd = false; + bool largeHgtChange = false; + + // trigger at 8 m/s airspeed + if (_ahrs->airspeed_sensor_enabled()) { + const AP_Airspeed *airspeed = _ahrs->get_airspeed(); + if (airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 10.0f) { + highAirSpd = true; + } + } + + // trigger at 10 m/s GPS velocity, but not if GPS is reporting bad velocity errors + if (gndSpdSq > 100.0f && gpsSpdAccuracy < 1.0f) { + highGndSpd = true; + } + + // trigger if more than 10m away from initial height + if (fabsf(hgtMea) > 10.0f) { + largeHgtChange = true; + } + + // Determine to a high certainty we are flying + if (motorsArmed && highGndSpd && (highAirSpd || largeHgtChange)) { + onGround = false; + inFlight = true; + } + + // if is possible we are in flight, set the time this condition was last detected + if (motorsArmed && (highGndSpd || highAirSpd || largeHgtChange)) { + airborneDetectTime_ms = imuSampleTime_ms; + onGround = false; + } + + // Determine to a high certainty we are not flying + // after 5 seconds of not detecting a possible flight condition or we are disarmed, we transition to on-ground mode + if(!motorsArmed || ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) { + onGround = true; + inFlight = false; + } + } else { + // Non fly forward vehicle, so can only use height and motor arm status + + // If the motors are armed then we could be flying and if they are not armed then we are definitely not flying + if (motorsArmed) { + onGround = false; + } else { + inFlight = false; + onGround = true; + } + + // If height has increased since exiting on-ground, then we definitely are flying + if (!onGround && ((stateStruct.position.z - posDownAtTakeoff) < -1.5f)) { + inFlight = true; + } + + // If rangefinder has increased since exiting on-ground, then we definitely are flying + if (!onGround && ((rangeDataNew.rng - rngAtStartOfFlight) > 0.5f)) { + inFlight = true; + } + + } + + // store current on-ground and in-air status for next time + prevOnGround = onGround; + prevInFlight = inFlight; + + // Store vehicle height and range prior to takeoff for use in post takeoff checks + if (onGround) { + // store vertical position at start of flight to use as a reference for ground relative checks + posDownAtTakeoff = stateStruct.position.z; + // store the range finder measurement which will be used as a reference to detect when we have taken off + rngAtStartOfFlight = rangeDataNew.rng; + // if the magnetic field states have been set, then continue to update the vertical position + // quaternion and yaw innovation snapshots to use as a reference when we start to fly. + if (magStateInitComplete) { + posDownAtLastMagReset = stateStruct.position.z; + quatAtLastMagReset = stateStruct.quat; + yawInnovAtLastMagReset = innovYaw; + } + } + +} + + +// determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect +bool NavEKF3_core::getTakeoffExpected() +{ + if (expectGndEffectTakeoff && imuSampleTime_ms - takeoffExpectedSet_ms > frontend->gndEffectTimeout_ms) { + expectGndEffectTakeoff = false; + } + + return expectGndEffectTakeoff; +} + +// called by vehicle code to specify that a takeoff is happening +// causes the EKF to compensate for expected barometer errors due to ground effect +void NavEKF3_core::setTakeoffExpected(bool val) +{ + takeoffExpectedSet_ms = imuSampleTime_ms; + expectGndEffectTakeoff = val; +} + + +// determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect +bool NavEKF3_core::getTouchdownExpected() +{ + if (expectGndEffectTouchdown && imuSampleTime_ms - touchdownExpectedSet_ms > frontend->gndEffectTimeout_ms) { + expectGndEffectTouchdown = false; + } + + return expectGndEffectTouchdown; +} + +// called by vehicle code to specify that a touchdown is expected to happen +// causes the EKF to compensate for expected barometer errors due to ground effect +void NavEKF3_core::setTouchdownExpected(bool val) +{ + touchdownExpectedSet_ms = imuSampleTime_ms; + expectGndEffectTouchdown = val; +} + +// Set to true if the terrain underneath is stable enough to be used as a height reference +// in combination with a range finder. Set to false if the terrain underneath the vehicle +// cannot be used as a height reference +void NavEKF3_core::setTerrainHgtStable(bool val) +{ + terrainHgtStableSet_ms = imuSampleTime_ms; + terrainHgtStable = val; +} + +// Detect takeoff for optical flow navigation +void NavEKF3_core::detectOptFlowTakeoff(void) +{ + if (!onGround && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) { + // we are no longer confidently on the ground so check the range finder and gyro for signs of takeoff + const AP_InertialSensor &ins = _ahrs->get_ins(); + Vector3f angRateVec; + Vector3f gyroBias; + getGyroBias(gyroBias); + bool dual_ins = ins.get_gyro_health(0) && ins.get_gyro_health(1); + if (dual_ins) { + angRateVec = (ins.get_gyro(0) + ins.get_gyro(1)) * 0.5f - gyroBias; + } else { + angRateVec = ins.get_gyro() - gyroBias; + } + + takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rangeDataNew.rng > (rngAtStartOfFlight + 0.1f))); + } else if (onGround) { + // we are confidently on the ground so set the takeoff detected status to false + takeOffDetected = false; + } +} + +#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp new file mode 100644 index 0000000000..daf5c8d941 --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -0,0 +1,1649 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150 + +#include "AP_NavEKF3.h" +#include "AP_NavEKF3_core.h" +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +// constructor +NavEKF3_core::NavEKF3_core(void) : + stateStruct(*reinterpret_cast(&statesArray)), + + _perf_UpdateFilter(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_UpdateFilter")), + _perf_CovariancePrediction(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_CovariancePrediction")), + _perf_FuseVelPosNED(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseVelPosNED")), + _perf_FuseMagnetometer(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseMagnetometer")), + _perf_FuseAirspeed(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseAirspeed")), + _perf_FuseSideslip(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseSideslip")), + _perf_TerrainOffset(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_TerrainOffset")), + _perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_FuseOptFlow")) +{ + _perf_test[0] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test0"); + _perf_test[1] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test1"); + _perf_test[2] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test2"); + _perf_test[3] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test3"); + _perf_test[4] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test4"); + _perf_test[5] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test5"); + _perf_test[6] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test6"); + _perf_test[7] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test7"); + _perf_test[8] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test8"); + _perf_test[9] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test9"); +} + +// setup this core backend +bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _core_index) +{ + frontend = _frontend; + imu_index = _imu_index; + core_index = _core_index; + _ahrs = frontend->_ahrs; + + /* + the imu_buffer_length needs to cope with a 260ms delay at a + maximum fusion rate of 100Hz. Non-imu data coming in at faster + than 100Hz is downsampled. For 50Hz main loop rate we need a + shorter buffer. + */ + if (_ahrs->get_ins().get_sample_rate() < 100) { + imu_buffer_length = 13; + } else { + // maximum 260 msec delay at 100 Hz fusion rate + imu_buffer_length = 26; + } + if(!storedGPS.init(OBS_BUFFER_LENGTH)) { + return false; + } + if(!storedMag.init(OBS_BUFFER_LENGTH)) { + return false; + } + if(!storedBaro.init(OBS_BUFFER_LENGTH)) { + return false; + } + if(!storedTAS.init(OBS_BUFFER_LENGTH)) { + return false; + } + if(!storedOF.init(OBS_BUFFER_LENGTH)) { + return false; + } + // Note: the use of dual range finders potentially doubles the amount of to be stored + if(!storedRange.init(2*OBS_BUFFER_LENGTH)) { + return false; + } + // Note: range beacon data is read one beacon at a time and can arrive at a high rate + if(!storedRangeBeacon.init(imu_buffer_length)) { + return false; + } + if(!storedIMU.init(imu_buffer_length)) { + return false; + } + if(!storedOutput.init(imu_buffer_length)) { + return false; + } + + return true; +} + + +/******************************************************** +* INIT FUNCTIONS * +********************************************************/ + +// Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary. +void NavEKF3_core::InitialiseVariables() +{ + // calculate the nominal filter update rate + const AP_InertialSensor &ins = _ahrs->get_ins(); + localFilterTimeStep_ms = (uint8_t)(1000*ins.get_loop_delta_t()); + localFilterTimeStep_ms = MAX(localFilterTimeStep_ms,10); + + // initialise time stamps + imuSampleTime_ms = frontend->imuSampleTime_us / 1000; + lastHealthyMagTime_ms = imuSampleTime_ms; + prevTasStep_ms = imuSampleTime_ms; + prevBetaStep_ms = imuSampleTime_ms; + lastMagUpdate_us = 0; + lastBaroReceived_ms = imuSampleTime_ms; + lastVelPassTime_ms = 0; + lastPosPassTime_ms = 0; + lastHgtPassTime_ms = 0; + lastTasPassTime_ms = 0; + lastSynthYawTime_ms = imuSampleTime_ms; + lastTimeGpsReceived_ms = 0; + secondLastGpsTime_ms = 0; + lastDecayTime_ms = imuSampleTime_ms; + timeAtLastAuxEKF_ms = imuSampleTime_ms; + flowValidMeaTime_ms = imuSampleTime_ms; + rngValidMeaTime_ms = imuSampleTime_ms; + flowMeaTime_ms = 0; + prevFlowFuseTime_ms = 0; + gndHgtValidTime_ms = 0; + ekfStartTime_ms = imuSampleTime_ms; + lastGpsVelFail_ms = 0; + lastGpsAidBadTime_ms = 0; + timeTasReceived_ms = 0; + magYawResetTimer_ms = imuSampleTime_ms; + lastPreAlignGpsCheckTime_ms = imuSampleTime_ms; + lastPosReset_ms = 0; + lastVelReset_ms = 0; + lastPosResetD_ms = 0; + lastRngMeasTime_ms = 0; + terrainHgtStableSet_ms = 0; + + // initialise other variables + gpsNoiseScaler = 1.0f; + hgtTimeout = true; + magTimeout = false; + allMagSensorsFailed = false; + tasTimeout = true; + badMagYaw = false; + badIMUdata = false; + finalInflightYawInit = false; + finalInflightMagInit = false; + dtIMUavg = 0.0025f; + dtEkfAvg = EKF_TARGET_DT; + dt = 0; + velDotNEDfilt.zero(); + lastKnownPositionNE.zero(); + prevTnb.zero(); + memset(&P[0][0], 0, sizeof(P)); + memset(&nextP[0][0], 0, sizeof(nextP)); + memset(&processNoise[0], 0, sizeof(processNoise)); + flowDataValid = false; + rangeDataToFuse = false; + fuseOptFlowData = false; + Popt = 0.0f; + terrainState = 0.0f; + prevPosN = stateStruct.position.x; + prevPosE = stateStruct.position.y; + inhibitGndState = false; + flowGyroBias.x = 0; + flowGyroBias.y = 0; + heldVelNE.zero(); + PV_AidingMode = AID_NONE; + PV_AidingModePrev = AID_NONE; + posTimeout = true; + velTimeout = true; + memset(&faultStatus, 0, sizeof(faultStatus)); + hgtRate = 0.0f; + mag_state.q0 = 1; + mag_state.DCM.identity(); + onGround = true; + prevOnGround = true; + inFlight = false; + prevInFlight = false; + manoeuvring = false; + inhibitWindStates = true; + inhibitMagStates = true; + gndOffsetValid = false; + validOrigin = false; + takeoffExpectedSet_ms = 0; + expectGndEffectTakeoff = false; + touchdownExpectedSet_ms = 0; + expectGndEffectTouchdown = false; + gpsSpdAccuracy = 0.0f; + gpsPosAccuracy = 0.0f; + gpsHgtAccuracy = 0.0f; + baroHgtOffset = 0.0f; + yawResetAngle = 0.0f; + lastYawReset_ms = 0; + tiltAlignComplete = false; + yawAlignComplete = false; + stateIndexLim = 23; + baroStoreIndex = 0; + rangeStoreIndex = 0; + magStoreIndex = 0; + gpsStoreIndex = 0; + tasStoreIndex = 0; + ofStoreIndex = 0; + delAngCorrection.zero(); + velErrintegral.zero(); + posErrintegral.zero(); + gpsGoodToAlign = false; + gpsNotAvailable = true; + motorsArmed = false; + prevMotorsArmed = false; + innovationIncrement = 0; + lastInnovation = 0; + memset(&gpsCheckStatus, 0, sizeof(gpsCheckStatus)); + gpsSpdAccPass = false; + ekfInnovationsPass = false; + sAccFilterState1 = 0.0f; + sAccFilterState2 = 0.0f; + lastGpsCheckTime_ms = 0; + lastInnovPassTime_ms = 0; + lastInnovFailTime_ms = 0; + gpsAccuracyGood = false; + memset(&gpsloc_prev, 0, sizeof(gpsloc_prev)); + gpsDriftNE = 0.0f; + gpsVertVelFilt = 0.0f; + gpsHorizVelFilt = 0.0f; + memset(&statesArray, 0, sizeof(statesArray)); + posDownDerivative = 0.0f; + posDown = 0.0f; + posVelFusionDelayed = false; + optFlowFusionDelayed = false; + airSpdFusionDelayed = false; + sideSlipFusionDelayed = false; + posResetNE.zero(); + velResetNE.zero(); + posResetD = 0.0f; + hgtInnovFiltState = 0.0f; + if (_ahrs->get_compass()) { + magSelectIndex = _ahrs->get_compass()->get_primary(); + } + imuDataDownSampledNew.delAng.zero(); + imuDataDownSampledNew.delVel.zero(); + imuDataDownSampledNew.delAngDT = 0.0f; + imuDataDownSampledNew.delVelDT = 0.0f; + runUpdates = false; + framesSincePredict = 0; + lastMagOffsetsValid = false; + magStateResetRequest = false; + magStateInitComplete = false; + magYawResetRequest = false; + gpsYawResetRequest = false; + posDownAtLastMagReset = stateStruct.position.z; + yawInnovAtLastMagReset = 0.0f; + quatAtLastMagReset = stateStruct.quat; + magFieldLearned = false; + delAngBiasLearned = false; + memset(&filterStatus, 0, sizeof(filterStatus)); + gpsInhibit = false; + activeHgtSource = 0; + memset(&rngMeasIndex, 0, sizeof(rngMeasIndex)); + memset(&storedRngMeasTime_ms, 0, sizeof(storedRngMeasTime_ms)); + memset(&storedRngMeas, 0, sizeof(storedRngMeas)); + terrainHgtStable = true; + ekfOriginHgtVar = 0.0f; + velOffsetNED.zero(); + posOffsetNED.zero(); + + // range beacon fusion variables + memset(&rngBcnDataNew, 0, sizeof(rngBcnDataNew)); + memset(&rngBcnDataDelayed, 0, sizeof(rngBcnDataDelayed)); + rngBcnStoreIndex = 0; + lastRngBcnPassTime_ms = 0; + rngBcnTestRatio = 0.0f; + rngBcnHealth = false; + rngBcnTimeout = true; + varInnovRngBcn = 0.0f; + innovRngBcn = 0.0f; + memset(&lastTimeRngBcn_ms, 0, sizeof(lastTimeRngBcn_ms)); + rngBcnDataToFuse = false; + beaconVehiclePosNED.zero(); + beaconVehiclePosErr = 1.0f; + rngBcnLast3DmeasTime_ms = 0; + rngBcnGoodToAlign = false; + lastRngBcnChecked = 0; + receiverPos.zero(); + memset(&receiverPosCov, 0, sizeof(receiverPosCov)); + rngBcnAlignmentStarted = false; + rngBcnAlignmentCompleted = false; + lastBeaconIndex = 0; + rngBcnPosSum.zero(); + numBcnMeas = 0; + rngSum = 0.0f; + N_beacons = 0; + maxBcnPosD = 0.0f; + minBcnPosD = 0.0f; + bcnPosOffset = 0.0f; + bcnPosDownOffsetMax = 0.0f; + bcnPosOffsetMaxVar = 0.0f; + OffsetMaxInnovFilt = 0.0f; + bcnPosDownOffsetMin = 0.0f; + bcnPosOffsetMinVar = 0.0f; + OffsetMinInnovFilt = 0.0f; + rngBcnFuseDataReportIndex = 0; + memset(&rngBcnFusionReport, 0, sizeof(rngBcnFusionReport)); + + // zero data buffers + storedIMU.reset(); + storedGPS.reset(); + storedMag.reset(); + storedBaro.reset(); + storedTAS.reset(); + storedRange.reset(); + storedOutput.reset(); + storedRangeBeacon.reset(); +} + +// Initialise the states from accelerometer and magnetometer data (if present) +// This method can only be used when the vehicle is static +bool NavEKF3_core::InitialiseFilterBootstrap(void) +{ + // If we are a plane and don't have GPS lock then don't initialise + if (assume_zero_sideslip() && _ahrs->get_gps().status() < AP_GPS::GPS_OK_FIX_3D) { + statesInitialised = false; + return false; + } + + // set re-used variables to zero + InitialiseVariables(); + + // Initialise IMU data + dtIMUavg = _ahrs->get_ins().get_loop_delta_t(); + readIMUData(); + storedIMU.reset_history(imuDataNew); + imuDataDelayed = imuDataNew; + + // acceleration vector in XYZ body axes measured by the IMU (m/s^2) + Vector3f initAccVec; + + // TODO we should average accel readings over several cycles + initAccVec = _ahrs->get_ins().get_accel(imu_index); + + // read the magnetometer data + readMagData(); + + // normalise the acceleration vector + float pitch=0, roll=0; + if (initAccVec.length() > 0.001f) { + initAccVec.normalize(); + + // calculate initial pitch angle + pitch = asinf(initAccVec.x); + + // calculate initial roll angle + roll = atan2f(-initAccVec.y , -initAccVec.z); + } + + // calculate initial roll and pitch orientation + stateStruct.quat.from_euler(roll, pitch, 0.0f); + + // initialise dynamic states + stateStruct.velocity.zero(); + stateStruct.position.zero(); + + // initialise static process model states + stateStruct.gyro_bias.zero(); + stateStruct.accel_bias.zero(); + stateStruct.wind_vel.zero(); + stateStruct.earth_magfield.zero(); + stateStruct.body_magfield.zero(); + + // read the GPS and set the position and velocity states + readGpsData(); + ResetVelocity(); + ResetPosition(); + + // read the barometer and set the height state + readBaroData(); + ResetHeight(); + + // define Earth rotation vector in the NED navigation frame + calcEarthRateNED(earthRateNED, _ahrs->get_home().lat); + + // initialise the covariance matrix + CovarianceInit(); + + // reset output states + StoreOutputReset(); + + // set to true now that states have be initialised + statesInitialised = true; + + return true; +} + +// initialise the covariance matrix +void NavEKF3_core::CovarianceInit() +{ + // zero the matrix + for (uint8_t i=1; i<=stateIndexLim; i++) + { + for (uint8_t j=0; j<=stateIndexLim; j++) + { + P[i][j] = 0.0f; + } + } + + // define the initial angle uncertainty as variances for a rotation vector + Vector3f rot_vec_var; + rot_vec_var.x = rot_vec_var.y = rot_vec_var.z = sq(0.1f); + + // update the quaternion state covariances + initialiseQuatCovariances(rot_vec_var); + + // velocities + P[4][4] = sq(frontend->_gpsHorizVelNoise); + P[5][5] = P[4][4]; + P[6][6] = sq(frontend->_gpsVertVelNoise); + // positions + P[7][7] = sq(frontend->_gpsHorizPosNoise); + P[8][8] = P[7][7]; + P[9][9] = sq(frontend->_baroAltNoise); + // gyro delta angle biases + P[10][10] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg)); + P[11][11] = P[10][10]; + P[12][12] = P[10][10]; + // delta velocity biases + P[13][13] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtEkfAvg); + P[14][14] = P[13][13]; + P[15][15] = P[13][13]; + // earth magnetic field + P[16][16] = 0.0f; + P[17][17] = P[16][16]; + P[18][18] = P[16][16]; + // body magnetic field + P[19][19] = 0.0f; + P[20][20] = P[19][19]; + P[21][21] = P[19][19]; + // wind velocities + P[22][22] = 0.0f; + P[23][23] = P[22][22]; + + + // optical flow ground height covariance + Popt = 0.25f; + +} + +/******************************************************** +* UPDATE FUNCTIONS * +********************************************************/ +// Update Filter States - this should be called whenever new IMU data is available +void NavEKF3_core::UpdateFilter(bool predict) +{ + // Set the flag to indicate to the filter that the front-end has given permission for a new state prediction cycle to be started + startPredictEnabled = predict; + + // don't run filter updates if states have not been initialised + if (!statesInitialised) { + return; + } + + // start the timer used for load measurement +#if EK2_DISABLE_INTERRUPTS + irqstate_t istate = irqsave(); +#endif + hal.util->perf_begin(_perf_UpdateFilter); + + // TODO - in-flight restart method + + //get starting time for update step + imuSampleTime_ms = frontend->imuSampleTime_us / 1000; + + // Check arm status and perform required checks and mode changes + controlFilterModes(); + + // read IMU data as delta angles and velocities + readIMUData(); + + // Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the buffer + if (runUpdates) { + // Predict states using IMU data from the delayed time horizon + UpdateStrapdownEquationsNED(); + + // Predict the covariance growth + CovariancePrediction(); + + // Update states using magnetometer data + SelectMagFusion(); + + // Update states using GPS and altimeter data + SelectVelPosFusion(); + + // Update states using range beacon data + SelectRngBcnFusion(); + + // Update states using optical flow data + SelectFlowFusion(); + + // Update states using airspeed data + SelectTasFusion(); + + // Update states using sideslip constraint assumption for fly-forward vehicles + SelectBetaFusion(); + + // Update the filter status + updateFilterStatus(); + } + + // Wind output forward from the fusion to output time horizon + calcOutputStates(); + + // stop the timer used for load measurement + hal.util->perf_end(_perf_UpdateFilter); +#if EK2_DISABLE_INTERRUPTS + irqrestore(istate); +#endif +} + +void NavEKF3_core::correctDeltaAngle(Vector3f &delAng, float delAngDT) +{ + delAng -= stateStruct.gyro_bias * (delAngDT / dtEkfAvg); +} + +void NavEKF3_core::correctDeltaVelocity(Vector3f &delVel, float delVelDT) +{ + delVel -= stateStruct.accel_bias * (delVelDT / dtEkfAvg); +} + +/* + * Update the quaternion, velocity and position states using delayed IMU measurements + * because the EKF is running on a delayed time horizon. Note that the quaternion is + * not used by the EKF equations, which instead estimate the error in the attitude of + * the vehicle when each observtion is fused. This attitude error is then used to correct + * the quaternion. +*/ +void NavEKF3_core::UpdateStrapdownEquationsNED() +{ + // update the quaternion states by rotating from the previous attitude through + // the delta angle rotation quaternion and normalise + // apply correction for earth's rotation rate + // % * - and + operators have been overloaded + stateStruct.quat.rotate(delAngCorrected - prevTnb * earthRateNED*imuDataDelayed.delAngDT); + stateStruct.quat.normalize(); + + // transform body delta velocities to delta velocities in the nav frame + // use the nav frame from previous time step as the delta velocities + // have been rotated into that frame + // * and + operators have been overloaded + Vector3f delVelNav; // delta velocity vector in earth axes + delVelNav = prevTnb.mul_transpose(delVelCorrected); + delVelNav.z += GRAVITY_MSS*imuDataDelayed.delVelDT; + + // calculate the body to nav cosine matrix + stateStruct.quat.inverse().rotation_matrix(prevTnb); + + // calculate the rate of change of velocity (used for launch detect and other functions) + velDotNED = delVelNav / imuDataDelayed.delVelDT; + + // apply a first order lowpass filter + velDotNEDfilt = velDotNED * 0.05f + velDotNEDfilt * 0.95f; + + // calculate a magnitude of the filtered nav acceleration (required for GPS + // variance estimation) + accNavMag = velDotNEDfilt.length(); + accNavMagHoriz = norm(velDotNEDfilt.x , velDotNEDfilt.y); + + // if we are not aiding, then limit the horizontal magnitude of acceleration + // to prevent large manoeuvre transients disturbing the attitude + if ((PV_AidingMode == AID_NONE) && (accNavMagHoriz > 5.0f)) { + float gain = 5.0f/accNavMagHoriz; + delVelNav.x *= gain; + delVelNav.y *= gain; + } + + // save velocity for use in trapezoidal integration for position calcuation + Vector3f lastVelocity = stateStruct.velocity; + + // sum delta velocities to get velocity + stateStruct.velocity += delVelNav; + + // apply a trapezoidal integration to velocities to calculate position + stateStruct.position += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f); + + // accumulate the bias delta angle and time since last reset by an OF measurement arrival + delAngBodyOF += delAngCorrected; + delTimeOF += imuDataDelayed.delAngDT; + + // limit states to protect against divergence + ConstrainStates(); +} + +/* + * Propagate PVA solution forward from the fusion time horizon to the current time horizon + * using simple observer which performs two functions: + * 1) Corrects for the delayed time horizon used by the EKF. + * 2) Applies a LPF to state corrections to prevent 'stepping' in states due to measurement + * fusion introducing unwanted noise into the control loops. + * The inspiration for using a complementary filter to correct for time delays in the EKF + * is based on the work by A Khosravian. + * + * “Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements” + * A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University +*/ +void NavEKF3_core::calcOutputStates() +{ + // apply corrections to the IMU data + Vector3f delAngNewCorrected = imuDataNew.delAng; + Vector3f delVelNewCorrected = imuDataNew.delVel; + correctDeltaAngle(delAngNewCorrected, imuDataNew.delAngDT); + correctDeltaVelocity(delVelNewCorrected, imuDataNew.delVelDT); + + // apply corections to track EKF solution + Vector3f delAng = delAngNewCorrected + delAngCorrection; + + // convert the rotation vector to its equivalent quaternion + Quaternion deltaQuat; + deltaQuat.from_axis_angle(delAng); + + // update the quaternion states by rotating from the previous attitude through + // the delta angle rotation quaternion and normalise + outputDataNew.quat *= deltaQuat; + outputDataNew.quat.normalize(); + + // calculate the body to nav cosine matrix + Matrix3f Tbn_temp; + outputDataNew.quat.rotation_matrix(Tbn_temp); + + // transform body delta velocities to delta velocities in the nav frame + Vector3f delVelNav = Tbn_temp*delVelNewCorrected; + delVelNav.z += GRAVITY_MSS*imuDataNew.delVelDT; + + // save velocity for use in trapezoidal integration for position calcuation + Vector3f lastVelocity = outputDataNew.velocity; + + // sum delta velocities to get velocity + outputDataNew.velocity += delVelNav; + + // apply a trapezoidal integration to velocities to calculate position + outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f); + + // If the IMU accelerometer is offset from the body frame origin, then calculate corrections + // that can be added to the EKF velocity and position outputs so that they represent the velocity + // and position of the body frame origin. + // Note the * operator has been overloaded to operate as a dot product + if (!accelPosOffset.is_zero()) { + // calculate the average angular rate across the last IMU update + // note delAngDT is prevented from being zero in readIMUData() + Vector3f angRate = imuDataNew.delAng * (1.0f/imuDataNew.delAngDT); + + // Calculate the velocity of the body frame origin relative to the IMU in body frame + // and rotate into earth frame. Note % operator has been overloaded to perform a cross product + Vector3f velBodyRelIMU = angRate % (- accelPosOffset); + velOffsetNED = Tbn_temp * velBodyRelIMU; + + // calculate the earth frame position of the body frame origin relative to the IMU + posOffsetNED = Tbn_temp * (- accelPosOffset); + } else { + velOffsetNED.zero(); + posOffsetNED.zero(); + } + + // store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer + if (runUpdates) { + // store the states at the output time horizon + storedOutput[storedIMU.get_youngest_index()] = outputDataNew; + + // recall the states from the fusion time horizon + outputDataDelayed = storedOutput[storedIMU.get_oldest_index()]; + + // compare quaternion data with EKF quaternion at the fusion time horizon and calculate correction + + // divide the demanded quaternion by the estimated to get the error + Quaternion quatErr = stateStruct.quat / outputDataDelayed.quat; + + // Convert to a delta rotation using a small angle approximation + quatErr.normalize(); + Vector3f deltaAngErr; + float scaler; + if (quatErr[0] >= 0.0f) { + scaler = 2.0f; + } else { + scaler = -2.0f; + } + deltaAngErr.x = scaler * quatErr[1]; + deltaAngErr.y = scaler * quatErr[2]; + deltaAngErr.z = scaler * quatErr[3]; + + // calculate a gain that provides tight tracking of the estimator states and + // adjust for changes in time delay to maintain consistent damping ratio of ~0.7 + float timeDelay = 1e-3f * (float)(imuDataNew.time_ms - imuDataDelayed.time_ms); + timeDelay = fmaxf(timeDelay, dtIMUavg); + float errorGain = 0.5f / timeDelay; + + // calculate a corrrection to the delta angle + // that will cause the INS to track the EKF quaternions + delAngCorrection = deltaAngErr * errorGain * dtIMUavg; + + // calculate velocity and position tracking errors + Vector3f velErr = (stateStruct.velocity - outputDataDelayed.velocity); + Vector3f posErr = (stateStruct.position - outputDataDelayed.position); + + // collect magnitude tracking error for diagnostics + outputTrackError.x = deltaAngErr.length(); + outputTrackError.y = velErr.length(); + outputTrackError.z = posErr.length(); + + // convert user specified time constant from centi-seconds to seconds + float tauPosVel = constrain_float(0.01f*(float)frontend->_tauVelPosOutput, 0.1f, 0.5f); + + // calculate a gain to track the EKF position states with the specified time constant + float velPosGain = dtEkfAvg / constrain_float(tauPosVel, dtEkfAvg, 10.0f); + + // use a PI feedback to calculate a correction that will be applied to the output state history + posErrintegral += posErr; + velErrintegral += velErr; + Vector3f velCorrection = velErr * velPosGain + velErrintegral * sq(velPosGain) * 0.1f; + Vector3f posCorrection = posErr * velPosGain + posErrintegral * sq(velPosGain) * 0.1f; + + // loop through the output filter state history and apply the corrections to the velocity and position states + // this method is too expensive to use for the attitude states due to the quaternion operations required + // but does not introduce a time delay in the 'correction loop' and allows smaller tracking time constants + // to be used + output_elements outputStates; + for (unsigned index=0; index < imu_buffer_length; index++) { + outputStates = storedOutput[index]; + + // a constant velocity correction is applied + outputStates.velocity += velCorrection; + + // a constant position correction is applied + outputStates.position += posCorrection; + + // push the updated data to the buffer + storedOutput[index] = outputStates; + } + + // update output state to corrected values + outputDataNew = storedOutput[storedIMU.get_youngest_index()]; + + } +} + +/* + * Calculate the predicted state covariance matrix using algebraic equations generated with Matlab symbolic toolbox. + * The script file used to generate these and other equations in this filter can be found here: + * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m +*/ +void NavEKF3_core::CovariancePrediction() +{ + hal.util->perf_begin(_perf_CovariancePrediction); + float windVelSigma; // wind velocity 1-sigma process noise - m/s + float dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s + float dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s + float magEarthSigma;// earth magnetic field 1-sigma process noise + float magBodySigma; // body magnetic field 1-sigma process noise + float daxVar; // X axis delta angle noise variance rad^2 + float dayVar; // Y axis delta angle noise variance rad^2 + float dazVar; // Z axis delta angle noise variance rad^2 + float dvxVar; // X axis delta velocity variance noise (m/s)^2 + float dvyVar; // Y axis delta velocity variance noise (m/s)^2 + float dvzVar; // Z axis delta velocity variance noise (m/s)^2 + float dvx; // X axis delta velocity (m/s) + float dvy; // Y axis delta velocity (m/s) + float dvz; // Z axis delta velocity (m/s) + float dax; // X axis delta angle (rad) + float day; // Y axis delta angle (rad) + float daz; // Z axis delta angle (rad) + float q0; // attitude quaternion + float q1; // attitude quaternion + float q2; // attitude quaternion + float q3; // attitude quaternion + float dax_b; // X axis delta angle measurement bias (rad) + float day_b; // Y axis delta angle measurement bias (rad) + float daz_b; // Z axis delta angle measurement bias (rad) + float dvx_b; // X axis delta velocity measurement bias (rad) + float dvy_b; // Y axis delta velocity measurement bias (rad) + float dvz_b; // Z axis delta velocity measurement bias (rad) + + // calculate covariance prediction process noise + // use filtered height rate to increase wind process noise when climbing or descending + // this allows for wind gradient effects. + // filter height rate using a 10 second time constant filter + dt = imuDataDelayed.delAngDT; + float alpha = 0.1f * dt; + hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha; + + // use filtered height rate to increase wind process noise when climbing or descending + // this allows for wind gradient effects. + windVelSigma = dt * constrain_float(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_float(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate)); + dAngBiasSigma = sq(dt) * constrain_float(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f); + dVelBiasSigma = sq(dt) * constrain_float(frontend->_accelBiasProcessNoise, 0.0f, 1.0f); + magEarthSigma = dt * constrain_float(frontend->_magEarthProcessNoise, 0.0f, 1.0f); + magBodySigma = dt * constrain_float(frontend->_magBodyProcessNoise, 0.0f, 1.0f); + for (uint8_t i= 0; i<=9; i++) processNoise[i] = 0.0f; + for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma; + for (uint8_t i=13; i<=15; i++) processNoise[i] = dVelBiasSigma; + if (expectGndEffectTakeoff) { + processNoise[15] = 0.0f; + } else { + processNoise[15] = dVelBiasSigma; + } + for (uint8_t i=16; i<=18; i++) processNoise[i] = magEarthSigma; + for (uint8_t i=19; i<=21; i++) processNoise[i] = magBodySigma; + for (uint8_t i=22; i<=23; i++) processNoise[i] = windVelSigma; + + for (uint8_t i= 0; i<=stateIndexLim; i++) processNoise[i] = sq(processNoise[i]); + + // set variables used to calculate covariance growth + dvx = imuDataDelayed.delVel.x; + dvy = imuDataDelayed.delVel.y; + dvz = imuDataDelayed.delVel.z; + dax = imuDataDelayed.delAng.x; + day = imuDataDelayed.delAng.y; + daz = imuDataDelayed.delAng.z; + q0 = stateStruct.quat[0]; + q1 = stateStruct.quat[1]; + q2 = stateStruct.quat[2]; + q3 = stateStruct.quat[3]; + dax_b = stateStruct.gyro_bias.x; + day_b = stateStruct.gyro_bias.y; + daz_b = stateStruct.gyro_bias.z; + dvx_b = stateStruct.accel_bias.x; + dvy_b = stateStruct.accel_bias.y; + dvz_b = stateStruct.accel_bias.z; + float _gyrNoise = constrain_float(frontend->_gyrNoise, 0.0f, 1.0f); + daxVar = dayVar = dazVar = sq(dt*_gyrNoise); + float _accNoise = constrain_float(frontend->_accNoise, 0.0f, 10.0f); + dvxVar = dvyVar = dvzVar = sq(dt*_accNoise); + + // calculate the predicted covariance due to inertial sensor error propagation + // we calculate the upper diagonal and copy to take advantage of symmetry + SF[0] = dvz - dvz_b; + SF[1] = dvy - dvy_b; + SF[2] = dvx - dvx_b; + SF[3] = 2.0f*q1*SF[2] + 2.0f*q2*SF[1] + 2.0f*q3*SF[0]; + SF[4] = 2.0f*q0*SF[1] - 2.0f*q1*SF[0] + 2.0f*q3*SF[2]; + SF[5] = 2.0f*q0*SF[2] + 2.0f*q2*SF[0] - 2.0f*q3*SF[1]; + SF[6] = day*0.5f - day_b*0.5f; + SF[7] = daz*0.5f - daz_b*0.5f; + SF[8] = dax*0.5f - dax_b*0.5f; + SF[9] = dax_b*0.5f - dax*0.5f; + SF[10] = daz_b*0.5f - daz*0.5f; + SF[11] = day_b*0.5f - day*0.5f; + SF[12] = 2.0f*q1*SF[1]; + SF[13] = 2.0f*q0*SF[0]; + SF[14] = q1*0.5f; + SF[15] = q2*0.5f; + SF[16] = q3*0.5f; + SF[17] = sq(q3); + SF[18] = sq(q2); + SF[19] = sq(q1); + SF[20] = sq(q0); + + SG[0] = q0*0.5f; + SG[1] = sq(q3); + SG[2] = sq(q2); + SG[3] = sq(q1); + SG[4] = sq(q0); + SG[5] = 2.0f*q2*q3; + SG[6] = 2.0f*q1*q3; + SG[7] = 2.0f*q1*q2; + + SQ[0] = dvzVar*(SG[5] - 2.0f*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyVar*(SG[5] + 2.0f*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxVar*(SG[6] - 2.0f*q0*q2)*(SG[7] + 2.0f*q0*q3); + SQ[1] = dvzVar*(SG[6] + 2.0f*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxVar*(SG[6] - 2.0f*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyVar*(SG[5] + 2.0f*q0*q1)*(SG[7] - 2.0f*q0*q3); + SQ[2] = dvzVar*(SG[5] - 2.0f*q0*q1)*(SG[6] + 2.0f*q0*q2) - dvyVar*(SG[7] - 2.0f*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxVar*(SG[7] + 2.0f*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); + SQ[3] = (dayVar*q1*SG[0])*0.5f - (dazVar*q1*SG[0])*0.5f - (daxVar*q2*q3)*0.25f; + SQ[4] = (dazVar*q2*SG[0])*0.5f - (daxVar*q2*SG[0])*0.5f - (dayVar*q1*q3)*0.25f; + SQ[5] = (daxVar*q3*SG[0])*0.5f - (dayVar*q3*SG[0])*0.5f - (dazVar*q1*q2)*0.25f; + SQ[6] = (daxVar*q1*q2)*0.25f - (dazVar*q3*SG[0])*0.5f - (dayVar*q1*q2)*0.25f; + SQ[7] = (dazVar*q1*q3)*0.25f - (daxVar*q1*q3)*0.25f - (dayVar*q2*SG[0])*0.5f; + SQ[8] = (dayVar*q2*q3)*0.25f - (daxVar*q1*SG[0])*0.5f - (dazVar*q2*q3)*0.25f; + SQ[9] = sq(SG[0]); + SQ[10] = sq(q1); + + SPP[0] = SF[12] + SF[13] - 2.0f*q2*SF[2]; + SPP[1] = SF[17] - SF[18] - SF[19] + SF[20]; + SPP[2] = SF[17] - SF[18] + SF[19] - SF[20]; + SPP[3] = SF[17] + SF[18] - SF[19] - SF[20]; + SPP[4] = 2.0f*q0*q2 - 2.0f*q1*q3; + SPP[5] = 2.0f*q0*q1 - 2.0f*q2*q3; + SPP[6] = 2.0f*q0*q3 - 2.0f*q1*q2; + SPP[7] = 2.0f*q0*q1 + 2.0f*q2*q3; + SPP[8] = 2.0f*q0*q3 + 2.0f*q1*q2; + SPP[9] = 2.0f*q0*q2 + 2.0f*q1*q3; + SPP[10] = SF[16]; + + if (inhibitDelAngBiasStates) { + zeroRows(P,10,12); + zeroCols(P,10,12); + } + + if (inhibitDelVelBiasStates) { + zeroRows(P,13,15); + zeroCols(P,13,15); + } + + if (inhibitMagStates) { + zeroRows(P,16,21); + zeroCols(P,16,21); + } + + if (inhibitWindStates) { + zeroRows(P,22,23); + zeroCols(P,22,23); + } + + nextP[0][0] = P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10] + (daxVar*SQ[10])*0.25f + SF[9]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[11]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[10]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) + SF[15]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) + SPP[10]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + (dayVar*sq(q2))*0.25f + (dazVar*sq(q3))*0.25f; + nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10] + SF[8]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[7]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[11]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SF[15]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + SPP[10]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]))*0.5f; + nextP[1][1] = P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] + daxVar*SQ[9] - (P[10][1]*q0)*0.5f + SF[8]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[11]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SF[15]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) + SPP[10]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) + (dayVar*sq(q3))*0.25f + (dazVar*sq(q2))*0.25f - (q0*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2))*0.5f; + nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10] + SF[6]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[10]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[8]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) - SPP[10]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - (q0*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]))*0.5f; + nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)*0.5f + SF[6]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[10]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SF[14]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) - SPP[10]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2))*0.5f; + nextP[2][2] = P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] + dayVar*SQ[9] + (dazVar*SQ[10])*0.25f - (P[11][2]*q0)*0.5f + SF[6]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[10]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SF[14]*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2) - SPP[10]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) + (daxVar*sq(q3))*0.25f - (q0*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2))*0.5f; + nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10] + SF[7]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[6]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[9]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[15]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - SF[14]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]))*0.5f; + nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)*0.5f + SF[7]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[6]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[15]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - SF[14]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2))*0.5f; + nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)*0.5f + SF[7]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[6]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[15]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) - SF[14]*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2))*0.5f; + nextP[3][3] = P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] + (dayVar*SQ[10])*0.25f + dazVar*SQ[9] - (P[12][3]*q0)*0.5f + SF[7]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[6]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[15]*(P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)/2) - SF[14]*(P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)/2) + (daxVar*sq(q2))*0.25f - (q0*(P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)/2))*0.5f; + nextP[0][4] = P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10] + SF[5]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[4]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SPP[3]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[6]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[9]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); + nextP[1][4] = P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)*0.5f + SF[5]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[4]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[6]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[9]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); + nextP[2][4] = P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)*0.5f + SF[5]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[4]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[6]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[9]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); + nextP[3][4] = P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)*0.5f + SF[5]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[4]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[6]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[9]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); + nextP[4][4] = P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9] + dvyVar*sq(SG[7] - 2.0f*q0*q3) + dvzVar*sq(SG[6] + 2.0f*q0*q2) + SF[5]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[4]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SPP[3]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[6]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[9]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]) + dvxVar*sq(SG[1] + SG[2] - SG[3] - SG[4]); + nextP[0][5] = P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10] + SF[4]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[5]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SPP[0]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SPP[8]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[2]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) + SPP[5]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); + nextP[1][5] = P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)*0.5f + SF[4]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SPP[8]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[2]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) + SPP[5]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); + nextP[2][5] = P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)*0.5f + SF[4]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SPP[8]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[2]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) + SPP[5]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); + nextP[3][5] = P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)*0.5f + SF[4]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SPP[8]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[2]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) + SPP[5]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); + nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9] + SF[4]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[5]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) - SPP[0]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SPP[8]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[2]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) + SPP[5]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]); + nextP[5][5] = P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5] + dvxVar*sq(SG[7] + 2.0f*q0*q3) + dvzVar*sq(SG[5] - 2.0f*q0*q1) + SF[4]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SF[3]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[5]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) - SPP[0]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SPP[8]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) + SPP[2]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) + SPP[5]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]) + dvyVar*sq(SG[1] - SG[2] + SG[3] - SG[4]); + nextP[0][6] = P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10] + SF[4]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[5]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[3]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SPP[4]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) - SPP[7]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[1]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]); + nextP[1][6] = P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)*0.5f + SF[4]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[5]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SPP[4]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) - SPP[7]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[1]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2); + nextP[2][6] = P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)*0.5f + SF[4]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[5]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SPP[4]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) - SPP[7]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[1]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2); + nextP[3][6] = P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)*0.5f + SF[4]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[5]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SPP[4]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) - SPP[7]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[1]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2); + nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9] + SF[4]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[5]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[3]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SPP[4]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) - SPP[7]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[1]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]); + nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5] + SF[4]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SF[5]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[3]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) + SPP[0]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SPP[4]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) - SPP[7]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) - SPP[1]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]); + nextP[6][6] = P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1] + dvxVar*sq(SG[6] - 2.0f*q0*q2) + dvyVar*sq(SG[5] + 2.0f*q0*q1) + SF[4]*(P[6][1] + P[1][1]*SF[4] - P[2][1]*SF[5] + P[3][1]*SF[3] + P[0][1]*SPP[0] + P[13][1]*SPP[4] - P[14][1]*SPP[7] - P[15][1]*SPP[1]) - SF[5]*(P[6][2] + P[1][2]*SF[4] - P[2][2]*SF[5] + P[3][2]*SF[3] + P[0][2]*SPP[0] + P[13][2]*SPP[4] - P[14][2]*SPP[7] - P[15][2]*SPP[1]) + SF[3]*(P[6][3] + P[1][3]*SF[4] - P[2][3]*SF[5] + P[3][3]*SF[3] + P[0][3]*SPP[0] + P[13][3]*SPP[4] - P[14][3]*SPP[7] - P[15][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[4] - P[2][0]*SF[5] + P[3][0]*SF[3] + P[0][0]*SPP[0] + P[13][0]*SPP[4] - P[14][0]*SPP[7] - P[15][0]*SPP[1]) + SPP[4]*(P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]) - SPP[7]*(P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]) - SPP[1]*(P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]) + dvzVar*sq(SG[1] - SG[2] - SG[3] + SG[4]); + nextP[0][7] = P[0][7] + P[1][7]*SF[9] + P[2][7]*SF[11] + P[3][7]*SF[10] + P[10][7]*SF[14] + P[11][7]*SF[15] + P[12][7]*SPP[10] + dt*(P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10]); + nextP[1][7] = P[1][7] + P[0][7]*SF[8] + P[2][7]*SF[7] + P[3][7]*SF[11] - P[12][7]*SF[15] + P[11][7]*SPP[10] - (P[10][7]*q0)*0.5f + dt*(P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)/2); + nextP[2][7] = P[2][7] + P[0][7]*SF[6] + P[1][7]*SF[10] + P[3][7]*SF[8] + P[12][7]*SF[14] - P[10][7]*SPP[10] - (P[11][7]*q0)*0.5f + dt*(P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)/2); + nextP[3][7] = P[3][7] + P[0][7]*SF[7] + P[1][7]*SF[6] + P[2][7]*SF[9] + P[10][7]*SF[15] - P[11][7]*SF[14] - (P[12][7]*q0)*0.5f + dt*(P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)/2); + nextP[4][7] = P[4][7] + P[0][7]*SF[5] + P[1][7]*SF[3] - P[3][7]*SF[4] + P[2][7]*SPP[0] + P[13][7]*SPP[3] + P[14][7]*SPP[6] - P[15][7]*SPP[9] + dt*(P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9]); + nextP[5][7] = P[5][7] + P[0][7]*SF[4] + P[2][7]*SF[3] + P[3][7]*SF[5] - P[1][7]*SPP[0] - P[13][7]*SPP[8] + P[14][7]*SPP[2] + P[15][7]*SPP[5] + dt*(P[5][4] + P[0][4]*SF[4] + P[2][4]*SF[3] + P[3][4]*SF[5] - P[1][4]*SPP[0] - P[13][4]*SPP[8] + P[14][4]*SPP[2] + P[15][4]*SPP[5]); + nextP[6][7] = P[6][7] + P[1][7]*SF[4] - P[2][7]*SF[5] + P[3][7]*SF[3] + P[0][7]*SPP[0] + P[13][7]*SPP[4] - P[14][7]*SPP[7] - P[15][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[4] - P[2][4]*SF[5] + P[3][4]*SF[3] + P[0][4]*SPP[0] + P[13][4]*SPP[4] - P[14][4]*SPP[7] - P[15][4]*SPP[1]); + nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); + nextP[0][8] = P[0][8] + P[1][8]*SF[9] + P[2][8]*SF[11] + P[3][8]*SF[10] + P[10][8]*SF[14] + P[11][8]*SF[15] + P[12][8]*SPP[10] + dt*(P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10]); + nextP[1][8] = P[1][8] + P[0][8]*SF[8] + P[2][8]*SF[7] + P[3][8]*SF[11] - P[12][8]*SF[15] + P[11][8]*SPP[10] - (P[10][8]*q0)*0.5f + dt*(P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)/2); + nextP[2][8] = P[2][8] + P[0][8]*SF[6] + P[1][8]*SF[10] + P[3][8]*SF[8] + P[12][8]*SF[14] - P[10][8]*SPP[10] - (P[11][8]*q0)*0.5f + dt*(P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)/2); + nextP[3][8] = P[3][8] + P[0][8]*SF[7] + P[1][8]*SF[6] + P[2][8]*SF[9] + P[10][8]*SF[15] - P[11][8]*SF[14] - (P[12][8]*q0)*0.5f + dt*(P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)/2); + nextP[4][8] = P[4][8] + P[0][8]*SF[5] + P[1][8]*SF[3] - P[3][8]*SF[4] + P[2][8]*SPP[0] + P[13][8]*SPP[3] + P[14][8]*SPP[6] - P[15][8]*SPP[9] + dt*(P[4][5] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9]); + nextP[5][8] = P[5][8] + P[0][8]*SF[4] + P[2][8]*SF[3] + P[3][8]*SF[5] - P[1][8]*SPP[0] - P[13][8]*SPP[8] + P[14][8]*SPP[2] + P[15][8]*SPP[5] + dt*(P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5]); + nextP[6][8] = P[6][8] + P[1][8]*SF[4] - P[2][8]*SF[5] + P[3][8]*SF[3] + P[0][8]*SPP[0] + P[13][8]*SPP[4] - P[14][8]*SPP[7] - P[15][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[4] - P[2][5]*SF[5] + P[3][5]*SF[3] + P[0][5]*SPP[0] + P[13][5]*SPP[4] - P[14][5]*SPP[7] - P[15][5]*SPP[1]); + nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); + nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); + nextP[0][9] = P[0][9] + P[1][9]*SF[9] + P[2][9]*SF[11] + P[3][9]*SF[10] + P[10][9]*SF[14] + P[11][9]*SF[15] + P[12][9]*SPP[10] + dt*(P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10]); + nextP[1][9] = P[1][9] + P[0][9]*SF[8] + P[2][9]*SF[7] + P[3][9]*SF[11] - P[12][9]*SF[15] + P[11][9]*SPP[10] - (P[10][9]*q0)*0.5f + dt*(P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)/2); + nextP[2][9] = P[2][9] + P[0][9]*SF[6] + P[1][9]*SF[10] + P[3][9]*SF[8] + P[12][9]*SF[14] - P[10][9]*SPP[10] - (P[11][9]*q0)*0.5f + dt*(P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)/2); + nextP[3][9] = P[3][9] + P[0][9]*SF[7] + P[1][9]*SF[6] + P[2][9]*SF[9] + P[10][9]*SF[15] - P[11][9]*SF[14] - (P[12][9]*q0)*0.5f + dt*(P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)/2); + nextP[4][9] = P[4][9] + P[0][9]*SF[5] + P[1][9]*SF[3] - P[3][9]*SF[4] + P[2][9]*SPP[0] + P[13][9]*SPP[3] + P[14][9]*SPP[6] - P[15][9]*SPP[9] + dt*(P[4][6] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9]); + nextP[5][9] = P[5][9] + P[0][9]*SF[4] + P[2][9]*SF[3] + P[3][9]*SF[5] - P[1][9]*SPP[0] - P[13][9]*SPP[8] + P[14][9]*SPP[2] + P[15][9]*SPP[5] + dt*(P[5][6] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5]); + nextP[6][9] = P[6][9] + P[1][9]*SF[4] - P[2][9]*SF[5] + P[3][9]*SF[3] + P[0][9]*SPP[0] + P[13][9]*SPP[4] - P[14][9]*SPP[7] - P[15][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1]); + nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); + nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); + nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); + nextP[0][10] = P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]; + nextP[1][10] = P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)*0.5f; + nextP[2][10] = P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)*0.5f; + nextP[3][10] = P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)*0.5f; + nextP[4][10] = P[4][10] + P[0][10]*SF[5] + P[1][10]*SF[3] - P[3][10]*SF[4] + P[2][10]*SPP[0] + P[13][10]*SPP[3] + P[14][10]*SPP[6] - P[15][10]*SPP[9]; + nextP[5][10] = P[5][10] + P[0][10]*SF[4] + P[2][10]*SF[3] + P[3][10]*SF[5] - P[1][10]*SPP[0] - P[13][10]*SPP[8] + P[14][10]*SPP[2] + P[15][10]*SPP[5]; + nextP[6][10] = P[6][10] + P[1][10]*SF[4] - P[2][10]*SF[5] + P[3][10]*SF[3] + P[0][10]*SPP[0] + P[13][10]*SPP[4] - P[14][10]*SPP[7] - P[15][10]*SPP[1]; + nextP[7][10] = P[7][10] + P[4][10]*dt; + nextP[8][10] = P[8][10] + P[5][10]*dt; + nextP[9][10] = P[9][10] + P[6][10]*dt; + nextP[10][10] = P[10][10]; + nextP[0][11] = P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]; + nextP[1][11] = P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)*0.5f; + nextP[2][11] = P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)*0.5f; + nextP[3][11] = P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)*0.5f; + nextP[4][11] = P[4][11] + P[0][11]*SF[5] + P[1][11]*SF[3] - P[3][11]*SF[4] + P[2][11]*SPP[0] + P[13][11]*SPP[3] + P[14][11]*SPP[6] - P[15][11]*SPP[9]; + nextP[5][11] = P[5][11] + P[0][11]*SF[4] + P[2][11]*SF[3] + P[3][11]*SF[5] - P[1][11]*SPP[0] - P[13][11]*SPP[8] + P[14][11]*SPP[2] + P[15][11]*SPP[5]; + nextP[6][11] = P[6][11] + P[1][11]*SF[4] - P[2][11]*SF[5] + P[3][11]*SF[3] + P[0][11]*SPP[0] + P[13][11]*SPP[4] - P[14][11]*SPP[7] - P[15][11]*SPP[1]; + nextP[7][11] = P[7][11] + P[4][11]*dt; + nextP[8][11] = P[8][11] + P[5][11]*dt; + nextP[9][11] = P[9][11] + P[6][11]*dt; + nextP[10][11] = P[10][11]; + nextP[11][11] = P[11][11]; + nextP[0][12] = P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]; + nextP[1][12] = P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)*0.5f; + nextP[2][12] = P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)*0.5f; + nextP[3][12] = P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)*0.5f; + nextP[4][12] = P[4][12] + P[0][12]*SF[5] + P[1][12]*SF[3] - P[3][12]*SF[4] + P[2][12]*SPP[0] + P[13][12]*SPP[3] + P[14][12]*SPP[6] - P[15][12]*SPP[9]; + nextP[5][12] = P[5][12] + P[0][12]*SF[4] + P[2][12]*SF[3] + P[3][12]*SF[5] - P[1][12]*SPP[0] - P[13][12]*SPP[8] + P[14][12]*SPP[2] + P[15][12]*SPP[5]; + nextP[6][12] = P[6][12] + P[1][12]*SF[4] - P[2][12]*SF[5] + P[3][12]*SF[3] + P[0][12]*SPP[0] + P[13][12]*SPP[4] - P[14][12]*SPP[7] - P[15][12]*SPP[1]; + nextP[7][12] = P[7][12] + P[4][12]*dt; + nextP[8][12] = P[8][12] + P[5][12]*dt; + nextP[9][12] = P[9][12] + P[6][12]*dt; + nextP[10][12] = P[10][12]; + nextP[11][12] = P[11][12]; + nextP[12][12] = P[12][12]; + nextP[0][13] = P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]; + nextP[1][13] = P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)*0.5f; + nextP[2][13] = P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)*0.5f; + nextP[3][13] = P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)*0.5f; + nextP[4][13] = P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]; + nextP[5][13] = P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]; + nextP[6][13] = P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]; + nextP[7][13] = P[7][13] + P[4][13]*dt; + nextP[8][13] = P[8][13] + P[5][13]*dt; + nextP[9][13] = P[9][13] + P[6][13]*dt; + nextP[10][13] = P[10][13]; + nextP[11][13] = P[11][13]; + nextP[12][13] = P[12][13]; + nextP[13][13] = P[13][13]; + nextP[0][14] = P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]; + nextP[1][14] = P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)*0.5f; + nextP[2][14] = P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)*0.5f; + nextP[3][14] = P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)*0.5f; + nextP[4][14] = P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]; + nextP[5][14] = P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]; + nextP[6][14] = P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]; + nextP[7][14] = P[7][14] + P[4][14]*dt; + nextP[8][14] = P[8][14] + P[5][14]*dt; + nextP[9][14] = P[9][14] + P[6][14]*dt; + nextP[10][14] = P[10][14]; + nextP[11][14] = P[11][14]; + nextP[12][14] = P[12][14]; + nextP[13][14] = P[13][14]; + nextP[14][14] = P[14][14]; + nextP[0][15] = P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]; + nextP[1][15] = P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)*0.5f; + nextP[2][15] = P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)*0.5f; + nextP[3][15] = P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)*0.5f; + nextP[4][15] = P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]; + nextP[5][15] = P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]; + nextP[6][15] = P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]; + nextP[7][15] = P[7][15] + P[4][15]*dt; + nextP[8][15] = P[8][15] + P[5][15]*dt; + nextP[9][15] = P[9][15] + P[6][15]*dt; + nextP[10][15] = P[10][15]; + nextP[11][15] = P[11][15]; + nextP[12][15] = P[12][15]; + nextP[13][15] = P[13][15]; + nextP[14][15] = P[14][15]; + nextP[15][15] = P[15][15]; + + if (stateIndexLim > 15) { + nextP[0][16] = P[0][16] + P[1][16]*SF[9] + P[2][16]*SF[11] + P[3][16]*SF[10] + P[10][16]*SF[14] + P[11][16]*SF[15] + P[12][16]*SPP[10]; + nextP[1][16] = P[1][16] + P[0][16]*SF[8] + P[2][16]*SF[7] + P[3][16]*SF[11] - P[12][16]*SF[15] + P[11][16]*SPP[10] - (P[10][16]*q0)*0.5f; + nextP[2][16] = P[2][16] + P[0][16]*SF[6] + P[1][16]*SF[10] + P[3][16]*SF[8] + P[12][16]*SF[14] - P[10][16]*SPP[10] - (P[11][16]*q0)*0.5f; + nextP[3][16] = P[3][16] + P[0][16]*SF[7] + P[1][16]*SF[6] + P[2][16]*SF[9] + P[10][16]*SF[15] - P[11][16]*SF[14] - (P[12][16]*q0)*0.5f; + nextP[4][16] = P[4][16] + P[0][16]*SF[5] + P[1][16]*SF[3] - P[3][16]*SF[4] + P[2][16]*SPP[0] + P[13][16]*SPP[3] + P[14][16]*SPP[6] - P[15][16]*SPP[9]; + nextP[5][16] = P[5][16] + P[0][16]*SF[4] + P[2][16]*SF[3] + P[3][16]*SF[5] - P[1][16]*SPP[0] - P[13][16]*SPP[8] + P[14][16]*SPP[2] + P[15][16]*SPP[5]; + nextP[6][16] = P[6][16] + P[1][16]*SF[4] - P[2][16]*SF[5] + P[3][16]*SF[3] + P[0][16]*SPP[0] + P[13][16]*SPP[4] - P[14][16]*SPP[7] - P[15][16]*SPP[1]; + nextP[7][16] = P[7][16] + P[4][16]*dt; + nextP[8][16] = P[8][16] + P[5][16]*dt; + nextP[9][16] = P[9][16] + P[6][16]*dt; + nextP[10][16] = P[10][16]; + nextP[11][16] = P[11][16]; + nextP[12][16] = P[12][16]; + nextP[13][16] = P[13][16]; + nextP[14][16] = P[14][16]; + nextP[15][16] = P[15][16]; + nextP[16][16] = P[16][16]; + nextP[0][17] = P[0][17] + P[1][17]*SF[9] + P[2][17]*SF[11] + P[3][17]*SF[10] + P[10][17]*SF[14] + P[11][17]*SF[15] + P[12][17]*SPP[10]; + nextP[1][17] = P[1][17] + P[0][17]*SF[8] + P[2][17]*SF[7] + P[3][17]*SF[11] - P[12][17]*SF[15] + P[11][17]*SPP[10] - (P[10][17]*q0)*0.5f; + nextP[2][17] = P[2][17] + P[0][17]*SF[6] + P[1][17]*SF[10] + P[3][17]*SF[8] + P[12][17]*SF[14] - P[10][17]*SPP[10] - (P[11][17]*q0)*0.5f; + nextP[3][17] = P[3][17] + P[0][17]*SF[7] + P[1][17]*SF[6] + P[2][17]*SF[9] + P[10][17]*SF[15] - P[11][17]*SF[14] - (P[12][17]*q0)*0.5f; + nextP[4][17] = P[4][17] + P[0][17]*SF[5] + P[1][17]*SF[3] - P[3][17]*SF[4] + P[2][17]*SPP[0] + P[13][17]*SPP[3] + P[14][17]*SPP[6] - P[15][17]*SPP[9]; + nextP[5][17] = P[5][17] + P[0][17]*SF[4] + P[2][17]*SF[3] + P[3][17]*SF[5] - P[1][17]*SPP[0] - P[13][17]*SPP[8] + P[14][17]*SPP[2] + P[15][17]*SPP[5]; + nextP[6][17] = P[6][17] + P[1][17]*SF[4] - P[2][17]*SF[5] + P[3][17]*SF[3] + P[0][17]*SPP[0] + P[13][17]*SPP[4] - P[14][17]*SPP[7] - P[15][17]*SPP[1]; + nextP[7][17] = P[7][17] + P[4][17]*dt; + nextP[8][17] = P[8][17] + P[5][17]*dt; + nextP[9][17] = P[9][17] + P[6][17]*dt; + nextP[10][17] = P[10][17]; + nextP[11][17] = P[11][17]; + nextP[12][17] = P[12][17]; + nextP[13][17] = P[13][17]; + nextP[14][17] = P[14][17]; + nextP[15][17] = P[15][17]; + nextP[16][17] = P[16][17]; + nextP[17][17] = P[17][17]; + nextP[0][18] = P[0][18] + P[1][18]*SF[9] + P[2][18]*SF[11] + P[3][18]*SF[10] + P[10][18]*SF[14] + P[11][18]*SF[15] + P[12][18]*SPP[10]; + nextP[1][18] = P[1][18] + P[0][18]*SF[8] + P[2][18]*SF[7] + P[3][18]*SF[11] - P[12][18]*SF[15] + P[11][18]*SPP[10] - (P[10][18]*q0)*0.5f; + nextP[2][18] = P[2][18] + P[0][18]*SF[6] + P[1][18]*SF[10] + P[3][18]*SF[8] + P[12][18]*SF[14] - P[10][18]*SPP[10] - (P[11][18]*q0)*0.5f; + nextP[3][18] = P[3][18] + P[0][18]*SF[7] + P[1][18]*SF[6] + P[2][18]*SF[9] + P[10][18]*SF[15] - P[11][18]*SF[14] - (P[12][18]*q0)*0.5f; + nextP[4][18] = P[4][18] + P[0][18]*SF[5] + P[1][18]*SF[3] - P[3][18]*SF[4] + P[2][18]*SPP[0] + P[13][18]*SPP[3] + P[14][18]*SPP[6] - P[15][18]*SPP[9]; + nextP[5][18] = P[5][18] + P[0][18]*SF[4] + P[2][18]*SF[3] + P[3][18]*SF[5] - P[1][18]*SPP[0] - P[13][18]*SPP[8] + P[14][18]*SPP[2] + P[15][18]*SPP[5]; + nextP[6][18] = P[6][18] + P[1][18]*SF[4] - P[2][18]*SF[5] + P[3][18]*SF[3] + P[0][18]*SPP[0] + P[13][18]*SPP[4] - P[14][18]*SPP[7] - P[15][18]*SPP[1]; + nextP[7][18] = P[7][18] + P[4][18]*dt; + nextP[8][18] = P[8][18] + P[5][18]*dt; + nextP[9][18] = P[9][18] + P[6][18]*dt; + nextP[10][18] = P[10][18]; + nextP[11][18] = P[11][18]; + nextP[12][18] = P[12][18]; + nextP[13][18] = P[13][18]; + nextP[14][18] = P[14][18]; + nextP[15][18] = P[15][18]; + nextP[16][18] = P[16][18]; + nextP[17][18] = P[17][18]; + nextP[18][18] = P[18][18]; + nextP[0][19] = P[0][19] + P[1][19]*SF[9] + P[2][19]*SF[11] + P[3][19]*SF[10] + P[10][19]*SF[14] + P[11][19]*SF[15] + P[12][19]*SPP[10]; + nextP[1][19] = P[1][19] + P[0][19]*SF[8] + P[2][19]*SF[7] + P[3][19]*SF[11] - P[12][19]*SF[15] + P[11][19]*SPP[10] - (P[10][19]*q0)*0.5f; + nextP[2][19] = P[2][19] + P[0][19]*SF[6] + P[1][19]*SF[10] + P[3][19]*SF[8] + P[12][19]*SF[14] - P[10][19]*SPP[10] - (P[11][19]*q0)*0.5f; + nextP[3][19] = P[3][19] + P[0][19]*SF[7] + P[1][19]*SF[6] + P[2][19]*SF[9] + P[10][19]*SF[15] - P[11][19]*SF[14] - (P[12][19]*q0)*0.5f; + nextP[4][19] = P[4][19] + P[0][19]*SF[5] + P[1][19]*SF[3] - P[3][19]*SF[4] + P[2][19]*SPP[0] + P[13][19]*SPP[3] + P[14][19]*SPP[6] - P[15][19]*SPP[9]; + nextP[5][19] = P[5][19] + P[0][19]*SF[4] + P[2][19]*SF[3] + P[3][19]*SF[5] - P[1][19]*SPP[0] - P[13][19]*SPP[8] + P[14][19]*SPP[2] + P[15][19]*SPP[5]; + nextP[6][19] = P[6][19] + P[1][19]*SF[4] - P[2][19]*SF[5] + P[3][19]*SF[3] + P[0][19]*SPP[0] + P[13][19]*SPP[4] - P[14][19]*SPP[7] - P[15][19]*SPP[1]; + nextP[7][19] = P[7][19] + P[4][19]*dt; + nextP[8][19] = P[8][19] + P[5][19]*dt; + nextP[9][19] = P[9][19] + P[6][19]*dt; + nextP[10][19] = P[10][19]; + nextP[11][19] = P[11][19]; + nextP[12][19] = P[12][19]; + nextP[13][19] = P[13][19]; + nextP[14][19] = P[14][19]; + nextP[15][19] = P[15][19]; + nextP[16][19] = P[16][19]; + nextP[17][19] = P[17][19]; + nextP[18][19] = P[18][19]; + nextP[19][19] = P[19][19]; + nextP[0][20] = P[0][20] + P[1][20]*SF[9] + P[2][20]*SF[11] + P[3][20]*SF[10] + P[10][20]*SF[14] + P[11][20]*SF[15] + P[12][20]*SPP[10]; + nextP[1][20] = P[1][20] + P[0][20]*SF[8] + P[2][20]*SF[7] + P[3][20]*SF[11] - P[12][20]*SF[15] + P[11][20]*SPP[10] - (P[10][20]*q0)*0.5f; + nextP[2][20] = P[2][20] + P[0][20]*SF[6] + P[1][20]*SF[10] + P[3][20]*SF[8] + P[12][20]*SF[14] - P[10][20]*SPP[10] - (P[11][20]*q0)*0.5f; + nextP[3][20] = P[3][20] + P[0][20]*SF[7] + P[1][20]*SF[6] + P[2][20]*SF[9] + P[10][20]*SF[15] - P[11][20]*SF[14] - (P[12][20]*q0)*0.5f; + nextP[4][20] = P[4][20] + P[0][20]*SF[5] + P[1][20]*SF[3] - P[3][20]*SF[4] + P[2][20]*SPP[0] + P[13][20]*SPP[3] + P[14][20]*SPP[6] - P[15][20]*SPP[9]; + nextP[5][20] = P[5][20] + P[0][20]*SF[4] + P[2][20]*SF[3] + P[3][20]*SF[5] - P[1][20]*SPP[0] - P[13][20]*SPP[8] + P[14][20]*SPP[2] + P[15][20]*SPP[5]; + nextP[6][20] = P[6][20] + P[1][20]*SF[4] - P[2][20]*SF[5] + P[3][20]*SF[3] + P[0][20]*SPP[0] + P[13][20]*SPP[4] - P[14][20]*SPP[7] - P[15][20]*SPP[1]; + nextP[7][20] = P[7][20] + P[4][20]*dt; + nextP[8][20] = P[8][20] + P[5][20]*dt; + nextP[9][20] = P[9][20] + P[6][20]*dt; + nextP[10][20] = P[10][20]; + nextP[11][20] = P[11][20]; + nextP[12][20] = P[12][20]; + nextP[13][20] = P[13][20]; + nextP[14][20] = P[14][20]; + nextP[15][20] = P[15][20]; + nextP[16][20] = P[16][20]; + nextP[17][20] = P[17][20]; + nextP[18][20] = P[18][20]; + nextP[19][20] = P[19][20]; + nextP[20][20] = P[20][20]; + nextP[0][21] = P[0][21] + P[1][21]*SF[9] + P[2][21]*SF[11] + P[3][21]*SF[10] + P[10][21]*SF[14] + P[11][21]*SF[15] + P[12][21]*SPP[10]; + nextP[1][21] = P[1][21] + P[0][21]*SF[8] + P[2][21]*SF[7] + P[3][21]*SF[11] - P[12][21]*SF[15] + P[11][21]*SPP[10] - (P[10][21]*q0)*0.5f; + nextP[2][21] = P[2][21] + P[0][21]*SF[6] + P[1][21]*SF[10] + P[3][21]*SF[8] + P[12][21]*SF[14] - P[10][21]*SPP[10] - (P[11][21]*q0)*0.5f; + nextP[3][21] = P[3][21] + P[0][21]*SF[7] + P[1][21]*SF[6] + P[2][21]*SF[9] + P[10][21]*SF[15] - P[11][21]*SF[14] - (P[12][21]*q0)*0.5f; + nextP[4][21] = P[4][21] + P[0][21]*SF[5] + P[1][21]*SF[3] - P[3][21]*SF[4] + P[2][21]*SPP[0] + P[13][21]*SPP[3] + P[14][21]*SPP[6] - P[15][21]*SPP[9]; + nextP[5][21] = P[5][21] + P[0][21]*SF[4] + P[2][21]*SF[3] + P[3][21]*SF[5] - P[1][21]*SPP[0] - P[13][21]*SPP[8] + P[14][21]*SPP[2] + P[15][21]*SPP[5]; + nextP[6][21] = P[6][21] + P[1][21]*SF[4] - P[2][21]*SF[5] + P[3][21]*SF[3] + P[0][21]*SPP[0] + P[13][21]*SPP[4] - P[14][21]*SPP[7] - P[15][21]*SPP[1]; + nextP[7][21] = P[7][21] + P[4][21]*dt; + nextP[8][21] = P[8][21] + P[5][21]*dt; + nextP[9][21] = P[9][21] + P[6][21]*dt; + nextP[10][21] = P[10][21]; + nextP[11][21] = P[11][21]; + nextP[12][21] = P[12][21]; + nextP[13][21] = P[13][21]; + nextP[14][21] = P[14][21]; + nextP[15][21] = P[15][21]; + nextP[16][21] = P[16][21]; + nextP[17][21] = P[17][21]; + nextP[18][21] = P[18][21]; + nextP[19][21] = P[19][21]; + nextP[20][21] = P[20][21]; + nextP[21][21] = P[21][21]; + + if (stateIndexLim > 21) { + nextP[0][22] = P[0][22] + P[1][22]*SF[9] + P[2][22]*SF[11] + P[3][22]*SF[10] + P[10][22]*SF[14] + P[11][22]*SF[15] + P[12][22]*SPP[10]; + nextP[1][22] = P[1][22] + P[0][22]*SF[8] + P[2][22]*SF[7] + P[3][22]*SF[11] - P[12][22]*SF[15] + P[11][22]*SPP[10] - (P[10][22]*q0)*0.5f; + nextP[2][22] = P[2][22] + P[0][22]*SF[6] + P[1][22]*SF[10] + P[3][22]*SF[8] + P[12][22]*SF[14] - P[10][22]*SPP[10] - (P[11][22]*q0)*0.5f; + nextP[3][22] = P[3][22] + P[0][22]*SF[7] + P[1][22]*SF[6] + P[2][22]*SF[9] + P[10][22]*SF[15] - P[11][22]*SF[14] - (P[12][22]*q0)*0.5f; + nextP[4][22] = P[4][22] + P[0][22]*SF[5] + P[1][22]*SF[3] - P[3][22]*SF[4] + P[2][22]*SPP[0] + P[13][22]*SPP[3] + P[14][22]*SPP[6] - P[15][22]*SPP[9]; + nextP[5][22] = P[5][22] + P[0][22]*SF[4] + P[2][22]*SF[3] + P[3][22]*SF[5] - P[1][22]*SPP[0] - P[13][22]*SPP[8] + P[14][22]*SPP[2] + P[15][22]*SPP[5]; + nextP[6][22] = P[6][22] + P[1][22]*SF[4] - P[2][22]*SF[5] + P[3][22]*SF[3] + P[0][22]*SPP[0] + P[13][22]*SPP[4] - P[14][22]*SPP[7] - P[15][22]*SPP[1]; + nextP[7][22] = P[7][22] + P[4][22]*dt; + nextP[8][22] = P[8][22] + P[5][22]*dt; + nextP[9][22] = P[9][22] + P[6][22]*dt; + nextP[10][22] = P[10][22]; + nextP[11][22] = P[11][22]; + nextP[12][22] = P[12][22]; + nextP[13][22] = P[13][22]; + nextP[14][22] = P[14][22]; + nextP[15][22] = P[15][22]; + nextP[16][22] = P[16][22]; + nextP[17][22] = P[17][22]; + nextP[18][22] = P[18][22]; + nextP[19][22] = P[19][22]; + nextP[20][22] = P[20][22]; + nextP[21][22] = P[21][22]; + nextP[22][22] = P[22][22]; + nextP[0][23] = P[0][23] + P[1][23]*SF[9] + P[2][23]*SF[11] + P[3][23]*SF[10] + P[10][23]*SF[14] + P[11][23]*SF[15] + P[12][23]*SPP[10]; + nextP[1][23] = P[1][23] + P[0][23]*SF[8] + P[2][23]*SF[7] + P[3][23]*SF[11] - P[12][23]*SF[15] + P[11][23]*SPP[10] - (P[10][23]*q0)*0.5f; + nextP[2][23] = P[2][23] + P[0][23]*SF[6] + P[1][23]*SF[10] + P[3][23]*SF[8] + P[12][23]*SF[14] - P[10][23]*SPP[10] - (P[11][23]*q0)*0.5f; + nextP[3][23] = P[3][23] + P[0][23]*SF[7] + P[1][23]*SF[6] + P[2][23]*SF[9] + P[10][23]*SF[15] - P[11][23]*SF[14] - (P[12][23]*q0)*0.5f; + nextP[4][23] = P[4][23] + P[0][23]*SF[5] + P[1][23]*SF[3] - P[3][23]*SF[4] + P[2][23]*SPP[0] + P[13][23]*SPP[3] + P[14][23]*SPP[6] - P[15][23]*SPP[9]; + nextP[5][23] = P[5][23] + P[0][23]*SF[4] + P[2][23]*SF[3] + P[3][23]*SF[5] - P[1][23]*SPP[0] - P[13][23]*SPP[8] + P[14][23]*SPP[2] + P[15][23]*SPP[5]; + nextP[6][23] = P[6][23] + P[1][23]*SF[4] - P[2][23]*SF[5] + P[3][23]*SF[3] + P[0][23]*SPP[0] + P[13][23]*SPP[4] - P[14][23]*SPP[7] - P[15][23]*SPP[1]; + nextP[7][23] = P[7][23] + P[4][23]*dt; + nextP[8][23] = P[8][23] + P[5][23]*dt; + nextP[9][23] = P[9][23] + P[6][23]*dt; + nextP[10][23] = P[10][23]; + nextP[11][23] = P[11][23]; + nextP[12][23] = P[12][23]; + nextP[13][23] = P[13][23]; + nextP[14][23] = P[14][23]; + nextP[15][23] = P[15][23]; + nextP[16][23] = P[16][23]; + nextP[17][23] = P[17][23]; + nextP[18][23] = P[18][23]; + nextP[19][23] = P[19][23]; + nextP[20][23] = P[20][23]; + nextP[21][23] = P[21][23]; + nextP[22][23] = P[22][23]; + nextP[23][23] = P[23][23]; + } + } + + // Copy upper diagonal to lower diagonal taking advantage of symmetry + for (uint8_t colIndex=0; colIndex<=stateIndexLim; colIndex++) + { + for (uint8_t rowIndex=0; rowIndex 1e4f) + { + for (uint8_t i=7; i<=8; i++) + { + for (uint8_t j=0; j<=stateIndexLim; j++) + { + nextP[i][j] = P[i][j]; + nextP[j][i] = P[j][i]; + } + } + } + + // copy covariances to output + CopyCovariances(); + + // constrain diagonals to prevent ill-conditioning + ConstrainVariances(); + + hal.util->perf_end(_perf_CovariancePrediction); +} + +// zero specified range of rows in the state covariance matrix +void NavEKF3_core::zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last) +{ + uint8_t row; + for (row=first; row<=last; row++) + { + memset(&covMat[row][0], 0, sizeof(covMat[0][0])*24); + } +} + +// zero specified range of columns in the state covariance matrix +void NavEKF3_core::zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last) +{ + uint8_t row; + for (row=0; row<=23; row++) + { + memset(&covMat[row][first], 0, sizeof(covMat[0][0])*(1+last-first)); + } +} + +// reset the output data to the current EKF state +void NavEKF3_core::StoreOutputReset() +{ + outputDataNew.quat = stateStruct.quat; + outputDataNew.velocity = stateStruct.velocity; + outputDataNew.position = stateStruct.position; + // write current measurement to entire table + for (uint8_t i=0; iget_compass()->get_declination() : 0; + + // calculate yaw angle rel to true north + yaw = magDecAng - magHeading; + + // calculate initial filter quaternion states using yaw from magnetometer + // store the yaw change so that it can be retrieved externally for use by the control loops to prevent yaw disturbances following a reset + Vector3f tempEuler; + stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); + // this check ensures we accumulate the resets that occur within a single iteration of the EKF + if (imuSampleTime_ms != lastYawReset_ms) { + yawResetAngle = 0.0f; + } + yawResetAngle += wrap_PI(yaw - tempEuler.z); + lastYawReset_ms = imuSampleTime_ms; + // calculate an initial quaternion using the new yaw value + initQuat.from_euler(roll, pitch, yaw); + // zero the attitude covariances becasue the corelations will now be invalid + zeroAttCovOnly(); + + // calculate initial Tbn matrix and rotate Mag measurements into NED + // to set initial NED magnetic field states + // don't do this if the earth field has already been learned + if (!magFieldLearned) { + initQuat.rotation_matrix(Tbn); + stateStruct.earth_magfield = Tbn * magDataDelayed.mag; + + // set the NE earth magnetic field states using the published declination + // and set the corresponding variances and covariances + alignMagStateDeclination(); + + // set the remaining variances and covariances + zeroRows(P,18,21); + zeroCols(P,18,21); + P[18][18] = sq(frontend->_magNoise); + P[19][19] = P[18][18]; + P[20][20] = P[18][18]; + P[21][21] = P[18][18]; + + } + + // record the fact we have initialised the magnetic field states + recordMagReset(); + + // clear mag state reset request + magStateResetRequest = false; + + } else { + // this function should not be called if there is no compass data but if is is, return the + // current attitude + initQuat = stateStruct.quat; + } + + // return attitude quaternion + return initQuat; +} + +// zero the attitude covariances, but preserve the variances +void NavEKF3_core::zeroAttCovOnly() +{ + float varTemp[4]; + for (uint8_t index=0; index<=3; index++) { + varTemp[index] = P[index][index]; + } + zeroCols(P,0,3); + zeroRows(P,0,3); + for (uint8_t index=0; index<=3; index++) { + P[index][index] = varTemp[index]; + } +} + +// calculate the variances for the rotation vector equivalent +Vector3f NavEKF3_core::calcRotVecVariances() +{ + Vector3f rotVarVec; + float q0 = stateStruct.quat[0]; + float q1 = stateStruct.quat[1]; + float q2 = stateStruct.quat[2]; + float q3 = stateStruct.quat[3]; + if (q0 < 0) { + q0 = -q0; + q1 = -q1; + q2 = -q2; + q3 = -q3; + } + float t2 = q0*q0; + float t3 = acos(q0); + float t4 = -t2+1.0f; + float t5 = t2-1.0f; + if ((t4 > 1e-9f) && (t5 < -1e-9f)) { + float t6 = 1.0f/t5; + float t7 = q1*t6*2.0f; + float t8 = 1.0f/powf(t4,1.5f); + float t9 = q0*q1*t3*t8*2.0f; + float t10 = t7+t9; + float t11 = 1.0f/sqrtf(t4); + float t12 = q2*t6*2.0f; + float t13 = q0*q2*t3*t8*2.0f; + float t14 = t12+t13; + float t15 = q3*t6*2.0f; + float t16 = q0*q3*t3*t8*2.0f; + float t17 = t15+t16; + rotVarVec.x = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f; + rotVarVec.y = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f; + rotVarVec.z = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f; + } else { + rotVarVec.x = 4.0f * P[1][1]; + rotVarVec.y = 4.0f * P[2][2]; + rotVarVec.z = 4.0f * P[3][3]; + } + + return rotVarVec; +} + +// initialise the quaternion covariances using rotation vector variances +void NavEKF3_core::initialiseQuatCovariances(Vector3f &rotVarVec) +{ + // calculate an equivalent rotation vector from the quaternion + float q0 = stateStruct.quat[0]; + float q1 = stateStruct.quat[1]; + float q2 = stateStruct.quat[2]; + float q3 = stateStruct.quat[3]; + if (q0 < 0) { + q0 = -q0; + q1 = -q1; + q2 = -q2; + q3 = -q3; + } + float delta = 2.0f*acosf(q0); + float scaler; + if (fabsf(delta) > 1e-6f) { + scaler = (delta/sinf(delta*0.5f)); + } else { + scaler = 2.0f; + } + float rotX = scaler*q1; + float rotY = scaler*q2; + float rotZ = scaler*q3; + + // autocode generated using matlab symbolic toolbox + float t2 = rotX*rotX; + float t4 = rotY*rotY; + float t5 = rotZ*rotZ; + float t6 = t2+t4+t5; + if (t6 > 1e-9f) { + float t7 = sqrtf(t6); + float t8 = t7*0.5f; + float t3 = sinf(t8); + float t9 = t3*t3; + float t10 = 1.0f/t6; + float t11 = 1.0f/sqrtf(t6); + float t12 = cosf(t8); + float t13 = 1.0f/powf(t6,1.5f); + float t14 = t3*t11; + float t15 = rotX*rotY*t3*t13; + float t16 = rotX*rotZ*t3*t13; + float t17 = rotY*rotZ*t3*t13; + float t18 = t2*t10*t12*0.5f; + float t27 = t2*t3*t13; + float t19 = t14+t18-t27; + float t23 = rotX*rotY*t10*t12*0.5f; + float t28 = t15-t23; + float t20 = rotY*rotVarVec.y*t3*t11*t28*0.5f; + float t25 = rotX*rotZ*t10*t12*0.5f; + float t31 = t16-t25; + float t21 = rotZ*rotVarVec.z*t3*t11*t31*0.5f; + float t22 = t20+t21-rotX*rotVarVec.x*t3*t11*t19*0.5f; + float t24 = t15-t23; + float t26 = t16-t25; + float t29 = t4*t10*t12*0.5f; + float t34 = t3*t4*t13; + float t30 = t14+t29-t34; + float t32 = t5*t10*t12*0.5f; + float t40 = t3*t5*t13; + float t33 = t14+t32-t40; + float t36 = rotY*rotZ*t10*t12*0.5f; + float t39 = t17-t36; + float t35 = rotZ*rotVarVec.z*t3*t11*t39*0.5f; + float t37 = t15-t23; + float t38 = t17-t36; + float t41 = rotVarVec.x*(t15-t23)*(t16-t25); + float t42 = t41-rotVarVec.y*t30*t39-rotVarVec.z*t33*t39; + float t43 = t16-t25; + float t44 = t17-t36; + + // zero all the quaternion covariances + zeroRows(P,0,3); + zeroCols(P,0,3); + + // Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox + P[0][0] = rotVarVec.x*t2*t9*t10*0.25f+rotVarVec.y*t4*t9*t10*0.25f+rotVarVec.z*t5*t9*t10*0.25f; + P[0][1] = t22; + P[0][2] = t35+rotX*rotVarVec.x*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rotVarVec.y*t3*t11*t30*0.5f; + P[0][3] = rotX*rotVarVec.x*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rotVarVec.y*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rotVarVec.z*t3*t11*t33*0.5f; + P[1][0] = t22; + P[1][1] = rotVarVec.x*(t19*t19)+rotVarVec.y*(t24*t24)+rotVarVec.z*(t26*t26); + P[1][2] = rotVarVec.z*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarVec.x*t19*t28-rotVarVec.y*t28*t30; + P[1][3] = rotVarVec.y*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarVec.x*t19*t31-rotVarVec.z*t31*t33; + P[2][0] = t35-rotY*rotVarVec.y*t3*t11*t30*0.5f+rotX*rotVarVec.x*t3*t11*(t15-t23)*0.5f; + P[2][1] = rotVarVec.z*(t16-t25)*(t17-t36)-rotVarVec.x*t19*t28-rotVarVec.y*t28*t30; + P[2][2] = rotVarVec.y*(t30*t30)+rotVarVec.x*(t37*t37)+rotVarVec.z*(t38*t38); + P[2][3] = t42; + P[3][0] = rotZ*rotVarVec.z*t3*t11*t33*(-0.5f)+rotX*rotVarVec.x*t3*t11*(t16-t25)*0.5f+rotY*rotVarVec.y*t3*t11*(t17-t36)*0.5f; + P[3][1] = rotVarVec.y*(t15-t23)*(t17-t36)-rotVarVec.x*t19*t31-rotVarVec.z*t31*t33; + P[3][2] = t42; + P[3][3] = rotVarVec.z*(t33*t33)+rotVarVec.x*(t43*t43)+rotVarVec.y*(t44*t44); + + } else { + // the equations are badly conditioned so use a small angle approximation + P[0][0] = 0.0f; + P[0][1] = 0.0f; + P[0][2] = 0.0f; + P[0][3] = 0.0f; + P[1][0] = 0.0f; + P[1][1] = 0.25f*rotVarVec.x; + P[1][2] = 0.0f; + P[1][3] = 0.0f; + P[2][0] = 0.0f; + P[2][1] = 0.0f; + P[2][2] = 0.25f*rotVarVec.y; + P[2][3] = 0.0f; + P[3][0] = 0.0f; + P[3][1] = 0.0f; + P[3][2] = 0.0f; + P[3][3] = 0.25f*rotVarVec.z; + + } +} + +#endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h new file mode 100644 index 0000000000..c5ef3aa701 --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -0,0 +1,1135 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 24 state EKF based on https://github.com/priseborough/InertialNav + + Converted from Matlab to C++ by Paul Riseborough + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#pragma GCC optimize("O3") + +#define EK2_DISABLE_INTERRUPTS 0 + + +#include +#include "AP_NavEKF3.h" +#include +#include +#include + +// GPS pre-flight check bit locations +#define MASK_GPS_NSATS (1<<0) +#define MASK_GPS_HDOP (1<<1) +#define MASK_GPS_SPD_ERR (1<<2) +#define MASK_GPS_POS_ERR (1<<3) +#define MASK_GPS_YAW_ERR (1<<4) +#define MASK_GPS_POS_DRIFT (1<<5) +#define MASK_GPS_VERT_SPD (1<<6) +#define MASK_GPS_HORIZ_SPD (1<<7) + +// active height source +#define HGT_SOURCE_BARO 0 +#define HGT_SOURCE_RNG 1 +#define HGT_SOURCE_GPS 2 +#define HGT_SOURCE_BCN 3 + +#define earthRate 0.000072921f // earth rotation rate (rad/sec) + +// when the wind estimation first starts with no airspeed sensor, +// assume 3m/s to start +#define STARTUP_WIND_SPEED 3.0f + +// initial imu bias error (m/s/s) +#define INIT_ACCEL_BIAS_UNCERTAINTY 0.2f + +// maximum allowed gyro bias (rad/sec) +#define GYRO_BIAS_LIMIT 0.5f + +class AP_AHRS; + +class NavEKF3_core +{ +public: + // Constructor + NavEKF3_core(void); + + // setup this core backend + bool setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _core_index); + + // Initialise the states from accelerometer and magnetometer data (if present) + // This method can only be used when the vehicle is static + bool InitialiseFilterBootstrap(void); + + // Update Filter States - this should be called whenever new IMU data is available + // The predict flag is set true when a new prediction cycle can be started + void UpdateFilter(bool predict); + + // Check basic filter health metrics and return a consolidated health status + bool healthy(void) const; + + // Return a consolidated error score where higher numbers are less healthy + // Intended to be used by the front-end to determine which is the primary EKF + float errorScore(void) const; + + // Write the last calculated NE position relative to the reference point (m). + // If a calculated solution is not available, use the best available data and return false + // If false returned, do not use for flight control + bool getPosNE(Vector2f &posNE) const; + + // Write the last calculated D position relative to the reference point (m). + // If a calculated solution is not available, use the best available data and return false + // If false returned, do not use for flight control + bool getPosD(float &posD) const; + + // return NED velocity in m/s + void getVelNED(Vector3f &vel) const; + + // Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s + // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF + // but will always be kinematically consistent with the z component of the EKF position state + float getPosDownDerivative(void) const; + + // This returns the specific forces in the NED frame + void getAccelNED(Vector3f &accelNED) const; + + // return body axis gyro bias estimates in rad/sec + void getGyroBias(Vector3f &gyroBias) const; + + // return accelerometer bias in m/s/s + void getAccelBias(Vector3f &accelBias) const; + + // return tilt error convergence metric + void getTiltError(float &ang) const; + + // reset body axis gyro bias estimates + void resetGyroBias(void); + + // Resets the baro so that it reads zero at the current height + // Resets the EKF height to zero + // Adjusts the EKf origin height so that the EKF height + origin height is the same as before + // Returns true if the height datum reset has been performed + // If using a range finder for height no reset is performed and it returns false + bool resetHeightDatum(void); + + // Commands the EKF to not use GPS. + // This command must be sent prior to arming as it will only be actioned when the filter is in static mode + // This command is forgotten by the EKF each time it goes back into static mode (eg the vehicle disarms) + // Returns 0 if command rejected + // Returns 1 if attitude, vertical velocity and vertical position will be provided + // Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided + uint8_t setInhibitGPS(void); + + // return the horizontal speed limit in m/s set by optical flow sensor limits + // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow + void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const; + + // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) + void getWind(Vector3f &wind) const; + + // return earth magnetic field estimates in measurement units / 1000 + void getMagNED(Vector3f &magNED) const; + + // return body magnetic field estimates in measurement units / 1000 + void getMagXYZ(Vector3f &magXYZ) const; + + // return the index for the active magnetometer + uint8_t getActiveMag() const; + + // Return estimated magnetometer offsets + // Return true if magnetometer offsets are valid + bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const; + + // Return the last calculated latitude, longitude and height in WGS-84 + // If a calculated location isn't available, return a raw GPS measurement + // The status will return true if a calculation or raw measurement is available + // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control + bool getLLH(struct Location &loc) const; + + // return the latitude and longitude and height used to set the NED origin + // All NED positions calculated by the filter are relative to this location + // Returns false if the origin has not been set + bool getOriginLLH(struct Location &loc) const; + + // set the latitude and longitude and height used to set the NED origin + // All NED positions calcualted by the filter will be relative to this location + // The origin cannot be set if the filter is in a flight mode (eg vehicle armed) + // Returns false if the filter has rejected the attempt to set the origin + bool setOriginLLH(struct Location &loc); + + // return estimated height above ground level + // return false if ground height is not being estimated. + bool getHAGL(float &HAGL) const; + + // return the Euler roll, pitch and yaw angle in radians + void getEulerAngles(Vector3f &eulers) const; + + // return the transformation matrix from XYZ (body) to NED axes + void getRotationBodyToNED(Matrix3f &mat) const; + + // return the quaternions defining the rotation from NED to XYZ (body) axes + void getQuaternion(Quaternion &quat) const; + + // return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements + void getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const; + + // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements + void getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const; + + // should we use the compass? This is public so it can be used for + // reporting via ahrs.use_compass() + bool use_compass(void) const; + + // write the raw optical flow measurements + // rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality + // rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes. + // rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro + // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate + // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor. + // posOffset is the XYZ flow sensor position in the body frame in m + void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset); + + // return data for debugging optical flow fusion + void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const; + + /* + Returns the following data for debugging range beacon fusion + ID : beacon identifier + rng : measured range to beacon (m) + innov : range innovation (m) + innovVar : innovation variance (m^2) + testRatio : innovation consistency test ratio + beaconPosNED : beacon NED position (m) + */ + bool getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED, float &offsetHigh, float &offsetLow); + + // called by vehicle code to specify that a takeoff is happening + // causes the EKF to compensate for expected barometer errors due to ground effect + void setTakeoffExpected(bool val); + + // called by vehicle code to specify that a touchdown is expected to happen + // causes the EKF to compensate for expected barometer errors due to ground effect + void setTouchdownExpected(bool val); + + // Set to true if the terrain underneath is stable enough to be used as a height reference + // in combination with a range finder. Set to false if the terrain underneath the vehicle + // cannot be used as a height reference + void setTerrainHgtStable(bool val); + + /* + return the filter fault status as a bitmasked integer + 0 = quaternions are NaN + 1 = velocities are NaN + 2 = badly conditioned X magnetometer fusion + 3 = badly conditioned Y magnetometer fusion + 5 = badly conditioned Z magnetometer fusion + 6 = badly conditioned airspeed fusion + 7 = badly conditioned synthetic sideslip fusion + 7 = filter is not initialised + */ + void getFilterFaults(uint16_t &faults) const; + + /* + return filter timeout status as a bitmasked integer + 0 = position measurement timeout + 1 = velocity measurement timeout + 2 = height measurement timeout + 3 = magnetometer measurement timeout + 5 = unassigned + 6 = unassigned + 7 = unassigned + 7 = unassigned + */ + void getFilterTimeouts(uint8_t &timeouts) const; + + /* + return filter gps quality check status + */ + void getFilterGpsStatus(nav_gps_status &status) const; + + /* + Return a filter function status that indicates: + Which outputs are valid + If the filter has detected takeoff + If the filter has activated the mode that mitigates against ground effect static pressure errors + If GPS data is being used + */ + void getFilterStatus(nav_filter_status &status) const; + + // send an EKF_STATUS_REPORT message to GCS + void send_status_report(mavlink_channel_t chan); + + // provides the height limit to be observed by the control loops + // returns false if no height limiting is required + // this is needed to ensure the vehicle does not fly too high when using optical flow navigation + bool getHeightControlLimit(float &height) const; + + // return the amount of yaw angle change due to the last yaw angle reset in radians + // returns the time of the last yaw angle reset or 0 if no reset has ever occurred + uint32_t getLastYawResetAngle(float &yawAng) const; + + // return the amount of NE position change due to the last position reset in metres + // returns the time of the last reset or 0 if no reset has ever occurred + uint32_t getLastPosNorthEastReset(Vector2f &pos) const; + + // return the amount of D position change due to the last position reset in metres + // returns the time of the last reset or 0 if no reset has ever occurred + uint32_t getLastPosDownReset(float &posD) const; + + // return the amount of NE velocity change due to the last velocity reset in metres/sec + // returns the time of the last reset or 0 if no reset has ever occurred + uint32_t getLastVelNorthEastReset(Vector2f &vel) const; + + // report any reason for why the backend is refusing to initialise + const char *prearm_failure_reason(void) const; + + // report the number of frames lapsed since the last state prediction + // this is used by other instances to level load + uint8_t getFramesSincePredict(void) const; + + // publish output observer angular, velocity and position tracking error + void getOutputTrackingError(Vector3f &error) const; + + // get the IMU index + uint8_t getIMUIndex(void) const { return imu_index; } + +private: + // Reference to the global EKF frontend for parameters + NavEKF3 *frontend; + uint8_t imu_index; + uint8_t core_index; + uint8_t imu_buffer_length; + + typedef float ftype; +#if MATH_CHECK_INDEXES + typedef VectorN Vector2; + typedef VectorN Vector3; + typedef VectorN Vector4; + typedef VectorN Vector5; + typedef VectorN Vector6; + typedef VectorN Vector7; + typedef VectorN Vector8; + typedef VectorN Vector9; + typedef VectorN Vector10; + typedef VectorN Vector11; + typedef VectorN Vector13; + typedef VectorN Vector14; + typedef VectorN Vector15; + typedef VectorN Vector21; + typedef VectorN Vector22; + typedef VectorN Vector23; + typedef VectorN Vector24; + typedef VectorN Vector25; + typedef VectorN Vector31; + typedef VectorN Vector28; + typedef VectorN,3> Matrix3; + typedef VectorN,24> Matrix24; + typedef VectorN,50> Matrix34_50; + typedef VectorN Vector_u32_50; +#else + typedef ftype Vector2[2]; + typedef ftype Vector3[3]; + typedef ftype Vector4[4]; + typedef ftype Vector5[5]; + typedef ftype Vector6[6]; + typedef ftype Vector7[7]; + typedef ftype Vector8[8]; + typedef ftype Vector9[9]; + typedef ftype Vector10[10]; + typedef ftype Vector11[11]; + typedef ftype Vector13[13]; + typedef ftype Vector14[14]; + typedef ftype Vector15[15]; + typedef ftype Vector21[21]; + typedef ftype Vector22[22]; + typedef ftype Vector23[23]; + typedef ftype Vector24[24]; + typedef ftype Vector25[25]; + typedef ftype Vector28[28]; + typedef ftype Matrix3[3][3]; + typedef ftype Matrix24[24][24]; + typedef ftype Matrix34_50[34][50]; + typedef uint32_t Vector_u32_50[50]; +#endif + + const AP_AHRS *_ahrs; + + // the states are available in two forms, either as a Vector24, or + // broken down as individual elements. Both are equivalent (same + // memory) + Vector24 statesArray; + struct state_elements { + Quaternion quat; // 0..3 + Vector3f velocity; // 4..6 + Vector3f position; // 7..9 + Vector3f gyro_bias; // 10..12 + Vector3f accel_bias; // 13..15 + Vector3f earth_magfield; // 16..18 + Vector3f body_magfield; // 19..21 + Vector2f wind_vel; // 22..23 + } &stateStruct; + + struct output_elements { + Quaternion quat; // 0..3 + Vector3f velocity; // 4..6 + Vector3f position; // 7..9 + }; + + struct imu_elements { + Vector3f delAng; // 0..2 + Vector3f delVel; // 3..5 + float delAngDT; // 6 + float delVelDT; // 7 + uint32_t time_ms; // 8 + }; + + struct gps_elements { + Vector2f pos; // 0..1 + float hgt; // 2 + Vector3f vel; // 3..5 + uint32_t time_ms; // 6 + uint8_t sensor_idx; // 7 + }; + + struct mag_elements { + Vector3f mag; // 0..2 + uint32_t time_ms; // 3 + }; + + struct baro_elements { + float hgt; // 0 + uint32_t time_ms; // 1 + }; + + struct range_elements { + float rng; // 0 + uint32_t time_ms; // 1 + uint8_t sensor_idx; // 2 + }; + + struct rng_bcn_elements { + float rng; // range measurement to each beacon (m) + Vector3f beacon_posNED; // NED position of the beacon (m) + float rngErr; // range measurement error 1-std (m) + uint8_t beacon_ID; // beacon identification number + uint32_t time_ms; // measurement timestamp (msec) + }; + + struct tas_elements { + float tas; // 0 + uint32_t time_ms; // 1 + }; + + struct of_elements { + Vector2f flowRadXY; // 0..1 + Vector2f flowRadXYcomp; // 2..3 + uint32_t time_ms; // 4 + Vector3f bodyRadXYZ; //8..10 + const Vector3f *body_offset;// 5..7 + }; + + // update the navigation filter status + void updateFilterStatus(void); + + // update the quaternion, velocity and position states using IMU measurements + void UpdateStrapdownEquationsNED(); + + // calculate the predicted state covariance matrix + void CovariancePrediction(); + + // force symmetry on the state covariance matrix + void ForceSymmetry(); + + // copy covariances across from covariance prediction calculation and fix numerical errors + void CopyCovariances(); + + // constrain variances (diagonal terms) in the state covariance matrix + void ConstrainVariances(); + + // constrain states + void ConstrainStates(); + + // fuse selected position, velocity and height measurements + void FuseVelPosNED(); + + // fuse range beacon measurements + void FuseRngBcn(); + + // use range beaon measurements to calculate a static position + void FuseRngBcnStatic(); + + // calculate the offset from EKF vetical position datum to the range beacon system datum + void CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehiclePosNED, bool aligning); + + // fuse magnetometer measurements + void FuseMagnetometer(); + + // fuse true airspeed measurements + void FuseAirspeed(); + + // fuse sythetic sideslip measurement of zero + void FuseSideslip(); + + // zero specified range of rows in the state covariance matrix + void zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last); + + // zero specified range of columns in the state covariance matrix + void zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last); + + // Reset the stored output history to current data + void StoreOutputReset(void); + + // Reset the stored output quaternion history to current EKF state + void StoreQuatReset(void); + + // Rotate the stored output quaternion history through a quaternion rotation + void StoreQuatRotate(Quaternion deltaQuat); + + // store altimeter data + void StoreBaro(); + + // recall altimeter data at the fusion time horizon + // return true if data found + bool RecallBaro(); + + // store range finder data + void StoreRange(); + + // recall range finder data at the fusion time horizon + // return true if data found + bool RecallRange(); + + // store magnetometer data + void StoreMag(); + + // recall magetometer data at the fusion time horizon + // return true if data found + bool RecallMag(); + + // store true airspeed data + void StoreTAS(); + + // recall true airspeed data at the fusion time horizon + // return true if data found + bool RecallTAS(); + + // store optical flow data + void StoreOF(); + + // recall optical flow data at the fusion time horizon + // return true if data found + bool RecallOF(); + + // calculate nav to body quaternions from body to nav rotation matrix + void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const; + + // calculate the NED earth spin vector in rad/sec + void calcEarthRateNED(Vector3f &omega, int32_t latitude) const; + + // initialise the covariance matrix + void CovarianceInit(); + + // helper functions for readIMUData + bool readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt); + bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng); + + // helper functions for correcting IMU data + void correctDeltaAngle(Vector3f &delAng, float delAngDT); + void correctDeltaVelocity(Vector3f &delVel, float delVelDT); + + // update IMU delta angle and delta velocity measurements + void readIMUData(); + + // check for new valid GPS data and update stored measurement if available + void readGpsData(); + + // check for new altitude measurement data and update stored measurement if available + void readBaroData(); + + // check for new magnetometer data and update store measurements if available + void readMagData(); + + // check for new airspeed data and update stored measurements if available + void readAirSpdData(); + + // check for new range beacon data and update stored measurements if available + void readRngBcnData(); + + // determine when to perform fusion of GPS position and velocity measurements + void SelectVelPosFusion(); + + // determine when to perform fusion of range measurements take realtive to a beacon at a known NED position + void SelectRngBcnFusion(); + + // determine when to perform fusion of magnetometer measurements + void SelectMagFusion(); + + // determine when to perform fusion of true airspeed measurements + void SelectTasFusion(); + + // determine when to perform fusion of synthetic sideslp measurements + void SelectBetaFusion(); + + // force alignment of the yaw angle using GPS velocity data + void realignYawGPS(); + + // initialise the earth magnetic field states using declination and current attitude and magnetometer meaasurements + // and return attitude quaternion + Quaternion calcQuatAndFieldStates(float roll, float pitch); + + // zero stored variables + void InitialiseVariables(); + + // reset the horizontal position states uing the last GPS measurement + void ResetPosition(void); + + // reset velocity states using the last GPS measurement + void ResetVelocity(void); + + // reset the vertical position state using the last height measurement + void ResetHeight(void); + + // return true if we should use the airspeed sensor + bool useAirspeed(void) const; + + // return true if the vehicle code has requested the filter to be ready for flight + bool readyToUseGPS(void) const; + + // return true if the filter to be ready to use the beacon range measurements + bool readyToUseRangeBeacon(void) const; + + // Check for filter divergence + void checkDivergence(void); + + // Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2 + void calcIMU_Weighting(float K1, float K2); + + // return true if optical flow data is available + bool optFlowDataPresent(void) const; + + // return true if we should use the range finder sensor + bool useRngFinder(void) const; + + // determine when to perform fusion of optical flow measurements + void SelectFlowFusion(); + + // Estimate terrain offset using a single state EKF + void EstimateTerrainOffset(); + + // fuse optical flow measurements into the main filter + void FuseOptFlow(); + + // Control filter mode changes + void controlFilterModes(); + + // Determine if we are flying or on the ground + void detectFlight(); + + // Set inertial navigaton aiding mode + void setAidingMode(); + + // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to + // avoid unnecessary operations + void setWindMagStateLearningMode(); + + // Check the alignmnent status of the tilt attitude + // Used during initial bootstrap alignment of the filter + void checkAttitudeAlignmentStatus(); + + // Control reset of yaw and magnetic field states + void controlMagYawReset(); + + // Set the NED origin to be used until the next filter reset + void setOrigin(); + + // determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect + bool getTakeoffExpected(); + + // determine if a touchdown is expected so that we can compensate for expected barometer errors due to ground effect + bool getTouchdownExpected(); + + // Assess GPS data quality and return true if good enough to align the EKF + bool calcGpsGoodToAlign(void); + + // return true and set the class variable true if the delta angle bias has been learned + bool checkGyroCalStatus(void); + + // update inflight calculaton that determines if GPS data is good enough for reliable navigation + void calcGpsGoodForFlight(void); + + // Read the range finder and take new measurements if available + // Apply a median filter to range finder data + void readRangeFinder(); + + // check if the vehicle has taken off during optical flow navigation by looking at inertial and range finder data + void detectOptFlowTakeoff(void); + + // align the NE earth magnetic field states with the published declination + void alignMagStateDeclination(); + + // Fuse compass measurements using a simple declination observation (doesn't require magnetic field states) + void fuseEulerYaw(); + + // Fuse declination angle to keep earth field declination from changing when we don't have earth relative observations. + // Input is 1-sigma uncertainty in published declination + void FuseDeclination(float declErr); + + // Propagate PVA solution forward from the fusion time horizon to the current time horizon + // using a simple observer + void calcOutputStates(); + + // calculate a filtered offset between baro height measurement and EKF height estimate + void calcFiltBaroOffset(); + + // calculate a filtered offset between GPS height measurement and EKF height estimate + void calcFiltGpsHgtOffset(); + + // Select height data to be fused from the available baro, range finder and GPS sources + void selectHeightForFusion(); + + // zero attitude state covariances, but preserve variances + void zeroAttCovOnly(); + + // record a yaw reset event + void recordYawReset(); + + // record a magnetic field state reset event + void recordMagReset(); + + // effective value of MAG_CAL + uint8_t effective_magCal(void) const; + + // calculate the variances for the rotation vector equivalent + Vector3f calcRotVecVariances(void); + + // initialise the quaternion covariances using rotation vector variances + void initialiseQuatCovariances(Vector3f &rotVarVec); + + // Length of FIFO buffers used for non-IMU sensor data. + // Must be larger than the time period defined by IMU_BUFFER_LENGTH + static const uint32_t OBS_BUFFER_LENGTH = 5; + + // Variables + bool statesInitialised; // boolean true when filter states have been initialised + bool velHealth; // boolean true if velocity measurements have passed innovation consistency check + bool posHealth; // boolean true if position measurements have passed innovation consistency check + bool hgtHealth; // boolean true if height measurements have passed innovation consistency check + bool magHealth; // boolean true if magnetometer has passed innovation consistency check + bool tasHealth; // boolean true if true airspeed has passed innovation consistency check + bool velTimeout; // boolean true if velocity measurements have failed innovation consistency check and timed out + bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out + bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out + bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out + bool tasTimeout; // boolean true if true airspeed measurements have failed for too long and have timed out + bool badMagYaw; // boolean true if the magnetometer is declared to be producing bad data + bool badIMUdata; // boolean true if the bad IMU data is detected + + const float EKF_TARGET_DT = 0.01f; // target EKF update time step + + float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts + Vector28 Kfusion; // Kalman gain vector + Matrix24 KH; // intermediate result used for covariance updates + Matrix24 KHP; // intermediate result used for covariance updates + Matrix24 P; // covariance matrix + imu_ring_buffer_t storedIMU; // IMU data buffer + obs_ring_buffer_t storedGPS; // GPS data buffer + obs_ring_buffer_t storedMag; // Magnetometer data buffer + obs_ring_buffer_t storedBaro; // Baro data buffer + obs_ring_buffer_t storedTAS; // TAS data buffer + obs_ring_buffer_t storedRange; // Range finder data buffer + imu_ring_buffer_t storedOutput;// output state buffer + Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation + ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2) + ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2) + Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) + ftype dtIMUavg; // expected time between IMU measurements (sec) + ftype dtEkfAvg; // expected time between EKF updates (sec) + ftype dt; // time lapsed since the last covariance prediction (sec) + ftype hgtRate; // state for rate of change of height filter + bool onGround; // true when the flight vehicle is definitely on the ground + bool prevOnGround; // value of onGround from previous frame - used to detect transition + bool inFlight; // true when the vehicle is definitely flying + bool prevInFlight; // value inFlight from previous frame - used to detect transition + bool manoeuvring; // boolean true when the flight vehicle is performing horizontal changes in velocity + uint32_t airborneDetectTime_ms; // last time flight movement was detected + Vector6 innovVelPos; // innovation output for a group of measurements + Vector6 varInnovVelPos; // innovation variance output for a group of measurements + bool fuseVelData; // this boolean causes the velNED measurements to be fused + bool fusePosData; // this boolean causes the posNE measurements to be fused + bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused + Vector3f innovMag; // innovation output from fusion of X,Y,Z compass measurements + Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements + ftype innovVtas; // innovation output from fusion of airspeed measurements + ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements + bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step + bool magFuseRequired; // boolean set to true when magnetometer fusion will be perfomred in the next time step + uint32_t prevTasStep_ms; // time stamp of last TAS fusion step + uint32_t prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step + uint32_t lastMagUpdate_us; // last time compass was updated in usec + Vector3f velDotNED; // rate of change of velocity in NED frame + Vector3f velDotNEDfilt; // low pass filtered velDotNED + uint32_t imuSampleTime_ms; // time that the last IMU value was taken + bool tasDataToFuse; // true when new airspeed data is waiting to be fused + uint32_t lastBaroReceived_ms; // time last time we received baro height data + uint16_t hgtRetryTime_ms; // time allowed without use of height measurements before a height timeout is declared + uint32_t lastVelPassTime_ms; // time stamp when GPS velocity measurement last passed innovation consistency check (msec) + uint32_t lastPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec) + uint32_t lastHgtPassTime_ms; // time stamp when height measurement last passed innovation consistency check (msec) + uint32_t lastTasPassTime_ms; // time stamp when airspeed measurement last passed innovation consistency check (msec) + uint32_t lastTimeGpsReceived_ms;// last time we received GPS data + uint32_t timeAtLastAuxEKF_ms; // last time the auxiliary filter was run to fuse range or optical flow measurements + uint32_t secondLastGpsTime_ms; // time of second last GPS fix used to determine how long since last update + uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy + bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data + uint32_t lastSynthYawTime_ms; // time stamp when synthetic yaw measurement was last fused to maintain covariance health (msec) + uint32_t ekfStartTime_ms; // time the EKF was started (msec) + Matrix24 nextP; // Predicted covariance matrix before addition of process noise to diagonals + Vector24 processNoise; // process noise added to diagonals of predicted covariance matrix + Vector21 SF; // intermediate variables used to calculate predicted covariance matrix + Vector8 SG; // intermediate variables used to calculate predicted covariance matrix + Vector11 SQ; // intermediate variables used to calculate predicted covariance matrix + Vector11 SPP; // intermediate variables used to calculate predicted covariance matrix + Vector2f lastKnownPositionNE; // last known position + uint32_t lastDecayTime_ms; // time of last decay of GPS position offset + float velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold + float posTestRatio; // sum of squares of GPS position innovation divided by fail threshold + float hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold + Vector3f magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold + float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold + bool inhibitWindStates; // true when wind states and covariances are to remain constant + bool inhibitMagStates; // true when magnetic field states are inactive + bool inhibitDelVelBiasStates; // true when delta velocity bias states are inactive + bool inhibitDelAngBiasStates; + bool gpsNotAvailable; // bool true when valid GPS data is not available + struct Location EKF_origin; // LLH origin of the NED axis system - do not change unless filter is reset + bool validOrigin; // true when the EKF origin is valid + float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver + float gpsPosAccuracy; // estimated position accuracy in m returned by the GPS receiver + float gpsHgtAccuracy; // estimated height accuracy in m returned by the GPS receiver + uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail + uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad + float posDownAtTakeoff; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m) + bool useGpsVertVel; // true if GPS vertical velocity should be used + float yawResetAngle; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased. + uint32_t lastYawReset_ms; // System time at which the last yaw reset occurred. Returned by getLastYawResetAngle + bool tiltAlignComplete; // true when tilt alignment is complete + bool yawAlignComplete; // true when yaw alignment is complete + bool magStateInitComplete; // true when the magnetic field sttes have been initialised + uint8_t stateIndexLim; // Max state index used during matrix and array operations + imu_elements imuDataDelayed; // IMU data at the fusion time horizon + imu_elements imuDataNew; // IMU data at the current time horizon + imu_elements imuDataDownSampledNew; // IMU data at the current time horizon that has been downsampled to a 100Hz rate + Quaternion imuQuatDownSampleNew; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame + uint8_t fifoIndexNow; // Global index for inertial and output solution at current time horizon + uint8_t fifoIndexDelayed; // Global index for inertial and output solution at delayed/fusion time horizon + baro_elements baroDataNew; // Baro data at the current time horizon + baro_elements baroDataDelayed; // Baro data at the fusion time horizon + uint8_t baroStoreIndex; // Baro data storage index + range_elements rangeDataNew; // Range finder data at the current time horizon + range_elements rangeDataDelayed;// Range finder data at the fusion time horizon + uint8_t rangeStoreIndex; // Range finder data storage index + tas_elements tasDataNew; // TAS data at the current time horizon + tas_elements tasDataDelayed; // TAS data at the fusion time horizon + uint8_t tasStoreIndex; // TAS data storage index + mag_elements magDataNew; // Magnetometer data at the current time horizon + mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon + uint8_t magStoreIndex; // Magnetometer data storage index + gps_elements gpsDataNew; // GPS data at the current time horizon + gps_elements gpsDataDelayed; // GPS data at the fusion time horizon + uint8_t gpsStoreIndex; // GPS data storage index + output_elements outputDataNew; // output state data at the current time step + output_elements outputDataDelayed; // output state data at the current time step + Vector3f delAngCorrection; // correction applied to delta angles used by output observer to track the EKF + Vector3f velErrintegral; // integral of output predictor NED velocity tracking error (m) + Vector3f posErrintegral; // integral of output predictor NED position tracking error (m.sec) + float innovYaw; // compass yaw angle innovation (rad) + uint32_t timeTasReceived_ms; // time last TAS data was received (msec) + bool gpsGoodToAlign; // true when the GPS quality can be used to initialise the navigation system + uint32_t magYawResetTimer_ms; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks + bool consistentMagData; // true when the magnetometers are passing consistency checks + bool motorsArmed; // true when the motors have been armed + bool prevMotorsArmed; // value of motorsArmed from previous frame + bool posVelFusionDelayed; // true when the position and velocity fusion has been delayed + bool optFlowFusionDelayed; // true when the optical flow fusion has been delayed + bool airSpdFusionDelayed; // true when the air speed fusion has been delayed + bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed + Vector3f lastMagOffsets; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes. + bool lastMagOffsetsValid; // True when lastMagOffsets has been initialized + Vector2f posResetNE; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset + uint32_t lastPosReset_ms; // System time at which the last position reset occurred. Returned by getLastPosNorthEastReset + Vector2f velResetNE; // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset + uint32_t lastVelReset_ms; // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset + float posResetD; // Change in Down position due to last in-flight reset in metres. Returned by getLastPosDowntReset + uint32_t lastPosResetD_ms; // System time at which the last position reset occurred. Returned by getLastPosDownReset + float yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold + Quaternion prevQuatMagReset; // Quaternion from the last time the magnetic field state reset condition test was performed + uint8_t fusionHorizonOffset; // number of IMU samples that the fusion time horizon has been shifted to prevent multiple EKF instances fusing data at the same time + float hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks + uint8_t magSelectIndex; // Index of the magnetometer that is being used by the EKF + bool runUpdates; // boolean true when the EKF updates can be run + uint32_t framesSincePredict; // number of frames lapsed since EKF instance did a state prediction + bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycele + uint8_t localFilterTimeStep_ms; // average number of msec between filter updates + float posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2) + Vector3f delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad) + Vector3f delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s) + bool magFieldLearned; // true when the magnetic field has been learned + Vector3f earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2) + Vector3f bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2) + bool delAngBiasLearned; // true when the gyro bias has been learned + nav_filter_status filterStatus; // contains the status of various filter outputs + float ekfOriginHgtVar; // Variance of the the EKF WGS-84 origin height estimate (m^2) + uint32_t lastOriginHgtTime_ms; // last time the ekf's WGS-84 origin height was corrected + Vector3f outputTrackError; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer + Vector3f velOffsetNED; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s) + Vector3f posOffsetNED; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m) + + // variables used to calculate a vertical velocity that is kinematically consistent with the verical position + float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD. + float posDown; // Down position state used in calculation of posDownRate + + // variables used by the pre-initialisation GPS checks + struct Location gpsloc_prev; // LLH location of previous GPS measurement + uint32_t lastPreAlignGpsCheckTime_ms; // last time in msec the GPS quality was checked during pre alignment checks + float gpsDriftNE; // amount of drift detected in the GPS position during pre-flight GPs checks + float gpsVertVelFilt; // amount of filterred vertical GPS velocity detected durng pre-flight GPS checks + float gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks + + // variable used by the in-flight GPS quality check + bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks + bool ekfInnovationsPass; // true when GPS innovations pass in-flight checks + float sAccFilterState1; // state variable for LPF applid to reported GPS speed accuracy + float sAccFilterState2; // state variable for peak hold filter applied to reported GPS speed + uint32_t lastGpsCheckTime_ms; // last time in msec the GPS quality was checked + uint32_t lastInnovPassTime_ms; // last time in msec the GPS innovations passed + uint32_t lastInnovFailTime_ms; // last time in msec the GPS innovations failed + bool gpsAccuracyGood; // true when the GPS accuracy is considered to be good enough for safe flight. + + // States used for unwrapping of compass yaw error + float innovationIncrement; + float lastInnovation; + + // variables added for optical flow fusion + obs_ring_buffer_t storedOF; // OF data buffer + of_elements ofDataNew; // OF data at the current time horizon + of_elements ofDataDelayed; // OF data at the fusion time horizon + uint8_t ofStoreIndex; // OF data storage index + bool flowDataToFuse; // true when optical flow data has is ready for fusion + bool flowDataValid; // true while optical flow data is still fresh + bool fuseOptFlowData; // this boolean causes the last optical flow measurement to be fused + float auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator + float auxFlowObsInnovVar; // innovation variance for optical flow observations from 1-state terrain offset estimator + Vector2 flowRadXYcomp; // motion compensated optical flow angular rates(rad/sec) + Vector2 flowRadXY; // raw (non motion compensated) optical flow angular rates (rad/sec) + uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec) + uint32_t rngValidMeaTime_ms; // time stamp from latest valid range measurement (msec) + uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec) + uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (msec) + Matrix3f Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period + Vector2 varInnovOptFlow; // optical flow innovations variances (rad/sec)^2 + Vector2 innovOptFlow; // optical flow LOS innovations (rad/sec) + float Popt; // Optical flow terrain height state covariance (m^2) + float terrainState; // terrain position state (m) + float prevPosN; // north position at last measurement + float prevPosE; // east position at last measurement + float varInnovRng; // range finder observation innovation variance (m^2) + float innovRng; // range finder observation innovation (m) + float hgtMea; // height measurement derived from either baro, gps or range finder data (m) + bool inhibitGndState; // true when the terrain position state is to remain constant + uint32_t prevFlowFuseTime_ms; // time both flow measurement components passed their innovation consistency checks + Vector2 flowTestRatio; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail + float auxFlowTestRatio; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator + float R_LOS; // variance of optical flow rate measurements (rad/sec)^2 + float auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail + Vector2f flowGyroBias; // bias error of optical flow sensor gyro output + bool rangeDataToFuse; // true when valid range finder height data has arrived at the fusion time horizon. + bool baroDataToFuse; // true when valid baro height finder data has arrived at the fusion time horizon. + bool gpsDataToFuse; // true when valid GPS data has arrived at the fusion time horizon. + bool magDataToFuse; // true when valid magnetometer data has arrived at the fusion time horizon + Vector2f heldVelNE; // velocity held when no aiding is available + enum AidingMode {AID_ABSOLUTE=0, // GPS or some other form of absolute position reference aiding is being used (optical flow may also be used in parallel) so position estimates are absolute. + AID_NONE=1, // no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift. + AID_RELATIVE=2 // only optical flow aiding is being used so position estimates will be relative + }; + AidingMode PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS + AidingMode PV_AidingModePrev; // Value of PV_AidingMode from the previous frame - used to detect transitions + bool gpsInhibit; // externally set flag informing the EKF not to use the GPS + bool gndOffsetValid; // true when the ground offset state can still be considered valid + Vector3f delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement + float delTimeOF; // time that delAngBodyOF is summed across + Vector3f accelPosOffset; // position of IMU accelerometer unit in body frame (m) + + + // Range finder + float baroHgtOffset; // offset applied when when switching to use of Baro height + float rngOnGnd; // Expected range finder reading in metres when vehicle is on ground + float storedRngMeas[2][3]; // Ringbuffer of stored range measurements for dual range sensors + uint32_t storedRngMeasTime_ms[2][3]; // Ringbuffers of stored range measurement times for dual range sensors + uint32_t lastRngMeasTime_ms; // Timestamp of last range measurement + uint8_t rngMeasIndex[2]; // Current range measurement ringbuffer index for dual range sensors + bool terrainHgtStable; // true when the terrain height is stable enough to be used as a height reference + uint32_t terrainHgtStableSet_ms; // system time at which terrainHgtStable was set + + // Range Beacon Sensor Fusion + obs_ring_buffer_t storedRangeBeacon; // Beacon range buffer + rng_bcn_elements rngBcnDataNew; // Range beacon data at the current time horizon + rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon + uint8_t rngBcnStoreIndex; // Range beacon data storage index + uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innvovation consistency checks (msec) + float rngBcnTestRatio; // Innovation test ratio for range beacon measurements + bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check + bool rngBcnTimeout; // boolean true if range beacon measurements have faled innovation consistency checks for too long + float varInnovRngBcn; // range beacon observation innovation variance (m^2) + float innovRngBcn; // range beacon observation innovation (m) + uint32_t lastTimeRngBcn_ms[10]; // last time we received a range beacon measurement (msec) + bool rngBcnDataToFuse; // true when there is new range beacon data to fuse + Vector3f beaconVehiclePosNED; // NED position estimate from the beacon system (NED) + float beaconVehiclePosErr; // estimated position error from the beacon system (m) + uint32_t rngBcnLast3DmeasTime_ms; // last time the beacon system returned a 3D fix (msec) + bool rngBcnGoodToAlign; // true when the range beacon systems 3D fix can be used to align the filter + uint8_t lastRngBcnChecked; // index of the last range beacon checked for data + Vector3f receiverPos; // receiver NED position (m) - alignment 3 state filter + float receiverPosCov[3][3]; // Receiver position covariance (m^2) - alignment 3 state filter ( + bool rngBcnAlignmentStarted; // True when the initial position alignment using range measurements has started + bool rngBcnAlignmentCompleted; // True when the initial position alignment using range measurements has finished + uint8_t lastBeaconIndex; // Range beacon index last read - used during initialisation of the 3-state filter + Vector3f rngBcnPosSum; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter + uint8_t numBcnMeas; // Number of beacon measurements - used during initialisation of the 3-state filter + float rngSum; // Sum of range measurements (m) - used during initialisation of the 3-state filter + uint8_t N_beacons; // Number of range beacons in use + float maxBcnPosD; // maximum position of all beacons in the down direction (m) + float minBcnPosD; // minimum position of all beacons in the down direction (m) + float bcnPosOffset; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) + bool usingMinHypothesis; // true when the min beacob constellatio offset hyopthesis is being used + + float bcnPosDownOffsetMax; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) + float bcnPosOffsetMaxVar; // Variance of the bcnPosDownOffsetMax state (m) + float OffsetMaxInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetHigh + + float bcnPosDownOffsetMin; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m) + float bcnPosOffsetMinVar; // Variance of the bcnPosDownOffsetMin state (m) + float OffsetMinInnovFilt; // Filtered magnitude of the range innovations using bcnPosOffsetLow + + // Range Beacon Fusion Debug Reporting + uint8_t rngBcnFuseDataReportIndex;// index of range beacon fusion data last reported + struct { + float rng; // measured range to beacon (m) + float innov; // range innovation (m) + float innovVar; // innovation variance (m^2) + float testRatio; // innovation consistency test ratio + Vector3f beaconPosNED; // beacon NED position + } rngBcnFusionReport[10]; + + // height source selection logic + uint8_t activeHgtSource; // integer defining active height source + + // Movement detector + bool takeOffDetected; // true when takeoff for optical flow navigation has been detected + float rngAtStartOfFlight; // range finder measurement at start of flight + uint32_t timeAtArming_ms; // time in msec that the vehicle armed + + // baro ground effect + bool expectGndEffectTakeoff; // external state from ArduCopter - takeoff expected + uint32_t takeoffExpectedSet_ms; // system time at which expectGndEffectTakeoff was set + bool expectGndEffectTouchdown; // external state from ArduCopter - touchdown expected + uint32_t touchdownExpectedSet_ms; // system time at which expectGndEffectTouchdown was set + float meaHgtAtTakeOff; // height measured at commencement of takeoff + + // control of post takeoff magentic field and heading resets + bool finalInflightYawInit; // true when the final post takeoff initialisation of yaw angle has been performed + bool finalInflightMagInit; // true when the final post takeoff initialisation of magnetic field states been performed + bool magStateResetRequest; // true if magnetic field states need to be reset using the magneteomter measurements + bool magYawResetRequest; // true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements + bool gpsYawResetRequest; // true if the vehicle yaw needs to be reset to the GPS course + float posDownAtLastMagReset; // vertical position last time the mag states were reset (m) + float yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad) + Quaternion quatAtLastMagReset; // quaternion states last time the mag states were reset + + // flags indicating severe numerical errors in innovation variance calculation for different fusion operations + struct { + bool bad_xmag:1; + bool bad_ymag:1; + bool bad_zmag:1; + bool bad_airspeed:1; + bool bad_sideslip:1; + bool bad_nvel:1; + bool bad_evel:1; + bool bad_dvel:1; + bool bad_npos:1; + bool bad_epos:1; + bool bad_dpos:1; + bool bad_yaw:1; + bool bad_decl:1; + bool bad_xflow:1; + bool bad_yflow:1; + bool bad_rngbcn:1; + } faultStatus; + + // flags indicating which GPS quality checks are failing + struct { + bool bad_sAcc:1; + bool bad_hAcc:1; + bool bad_yaw:1; + bool bad_sats:1; + bool bad_VZ:1; + bool bad_horiz_drift:1; + bool bad_hdop:1; + bool bad_vert_vel:1; + bool bad_fix:1; + bool bad_horiz_vel:1; + } gpsCheckStatus; + + // states held by magnetomter fusion across time steps + // magnetometer X,Y,Z measurements are fused across three time steps + // to level computational load as this is an expensive operation + struct { + ftype q0; + ftype q1; + ftype q2; + ftype q3; + ftype magN; + ftype magE; + ftype magD; + ftype magXbias; + ftype magYbias; + ftype magZbias; + uint8_t obsIndex; + Matrix3f DCM; + Vector3f MagPred; + ftype R_MAG; + Vector9 SH_MAG; + } mag_state; + + + // string representing last reason for prearm failure + char prearm_fail_string[40]; + + // performance counters + AP_HAL::Util::perf_counter_t _perf_UpdateFilter; + AP_HAL::Util::perf_counter_t _perf_CovariancePrediction; + AP_HAL::Util::perf_counter_t _perf_FuseVelPosNED; + AP_HAL::Util::perf_counter_t _perf_FuseMagnetometer; + AP_HAL::Util::perf_counter_t _perf_FuseAirspeed; + AP_HAL::Util::perf_counter_t _perf_FuseSideslip; + AP_HAL::Util::perf_counter_t _perf_TerrainOffset; + AP_HAL::Util::perf_counter_t _perf_FuseOptFlow; + AP_HAL::Util::perf_counter_t _perf_test[10]; + + // should we assume zero sideslip? + bool assume_zero_sideslip(void) const; + + // vehicle specific initial gyro bias uncertainty + float InitialGyroBiasUncertainty(void) const; +};