Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_NavEKF/AP_NavEKF.cpp

3571 lines
230 KiB
C++
Raw Normal View History

AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
AP_NavEKF: use vectorN to make indexes safe
2013-12-29 22:25:02 -04:00
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
#include <AP_HAL.h>
AP_NavEKF: use float arrays when not doing bounds checking g++ is doing a lousy job of inlining VectorN, so avoid it when we don't need bounds checking for production code
2013-12-30 06:27:50 -04:00
AP_NavEKF: only build on fast CPUs
2013-12-30 19:24:21 -04:00
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
AP_NavEKF: Removed accel bias states
2014-01-03 03:52:37 -04:00
// uncomment this to force the optimisation of this code, note that
AP_NavEKF: use float arrays when not doing bounds checking g++ is doing a lousy job of inlining VectorN, so avoid it when we don't need bounds checking for production code
2013-12-30 06:27:50 -04:00
// this makes debugging harder
AP_NavEKF: some debug code
2014-01-05 01:37:24 -04:00
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
AP_NavEKF: don't report altitude yet, and removed debug code
2014-01-01 22:45:59 -04:00
#pragma GCC optimize("O0")
#else
AP_NavEKF : Additional flash logging
2014-01-30 18:11:54 -04:00
#pragma GCC optimize("O3")
AP_NavEKF: don't report altitude yet, and removed debug code
2014-01-01 22:45:59 -04:00
#endif
AP_NavEKF: use float arrays when not doing bounds checking g++ is doing a lousy job of inlining VectorN, so avoid it when we don't need bounds checking for production code
2013-12-30 06:27:50 -04:00
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
#include "AP_NavEKF.h"
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
#include <AP_AHRS.h>
AP_NavEKF: fixup parameter handling
2014-01-20 01:47:56 -04:00
#include <AP_Param.h>
AP_NavEKF: added a mechanism for per-vehicle-type defaults in EKF
2014-03-25 22:30:17 -03:00
#include <AP_Vehicle.h>
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
AP_NavEKF: some debug code
2014-01-05 01:37:24 -04:00
#include <stdio.h>
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
AP_NavEKF: added a mechanism for per-vehicle-type defaults in EKF
2014-03-25 22:30:17 -03:00
/*
parameter defaults for different types of vehicle. The
APM_BUILD_DIRECTORY is taken from the main vehicle directory name
where the code is built. Note that this trick won't work for arduino
builds on APM2, but NavEKF doesn't run on APM2, so that's OK
*/
AP_NavEKF: use APM_BUILD_TYPE() macro
2014-03-25 22:42:11 -03:00
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
AP_NavEKF: added a mechanism for per-vehicle-type defaults in EKF
2014-03-25 22:30:17 -03:00
// copter defaults
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
#define VELNE_NOISE_DEFAULT 0.5f
#define VELD_NOISE_DEFAULT 0.7f
#define POSNE_NOISE_DEFAULT 0.5f
#define ALT_NOISE_DEFAULT 1.0f
#define MAG_NOISE_DEFAULT 0.05f
#define GYRO_PNOISE_DEFAULT 0.015f
#define ACC_PNOISE_DEFAULT 0.25f
AP_NavEKF: Increase gyro bias process noise This is required to stop the bias estimate from becoming too static towards the end of longer flights.
2014-05-16 07:59:48 -03:00
#define GBIAS_PNOISE_DEFAULT 1E-06f
AP_NavEKF : Reduce Z accel bias process noise to provide a more stable estimate
2014-04-07 08:01:08 -03:00
#define ABIAS_PNOISE_DEFAULT 0.0001f
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
#define MAGE_PNOISE_DEFAULT 0.0003f
#define MAGB_PNOISE_DEFAULT 0.0003f
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
#define VEL_GATE_DEFAULT 6
AP_NavEKF : Increased position gate default to reduce impact of accel errors Flight testing with windup turns has shown that the position gate threshold can be tripped with good GPS data causing position jerks. This increases the initial GPS glitch rejection threshold to effectively 5m when using the default POSNE_NOISE value of 0.5m.
2014-04-12 01:26:49 -03:00
#define POS_GATE_DEFAULT 10
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
#define HGT_GATE_DEFAULT 10
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
#define MAG_GATE_DEFAULT 3
#define MAG_CAL_DEFAULT 1
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
#define GLITCH_ACCEL_DEFAULT 150
#define GLITCH_RADIUS_DEFAULT 15
AP_NavEKF: added a mechanism for per-vehicle-type defaults in EKF
2014-03-25 22:30:17 -03:00
AP_NavEKF: use APM_BUILD_TYPE() macro
2014-03-25 22:42:11 -03:00
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
AP_NavEKF: added a mechanism for per-vehicle-type defaults in EKF
2014-03-25 22:30:17 -03:00
// rover defaults
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
#define VELNE_NOISE_DEFAULT 0.5f
#define VELD_NOISE_DEFAULT 0.7f
#define POSNE_NOISE_DEFAULT 0.5f
#define ALT_NOISE_DEFAULT 1.0f
#define MAG_NOISE_DEFAULT 0.05f
#define GYRO_PNOISE_DEFAULT 0.015f
#define ACC_PNOISE_DEFAULT 0.25f
AP_NavEKF: Increase gyro bias process noise This is required to stop the bias estimate from becoming too static towards the end of longer flights.
2014-05-16 07:59:48 -03:00
#define GBIAS_PNOISE_DEFAULT 1E-06f
AP_NavEKF: Default parameter adjustments Bring Plane glitch protection thresholds into alignment with copter and rover Slight increase in accelerometer bias process noise to prevent bias estimate divergence into limits (Rover and Plane only as Copter does not seem respond as well to this change) effective increase in threshold on divergence test to allow increased margin for bad GPS velocities
2014-05-17 03:55:22 -03:00
#define ABIAS_PNOISE_DEFAULT 0.0002f
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
#define MAGE_PNOISE_DEFAULT 0.0003f
#define MAGB_PNOISE_DEFAULT 0.0003f
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
#define VEL_GATE_DEFAULT 6
AP_NavEKF : Increased position gate default to reduce impact of accel errors Flight testing with windup turns has shown that the position gate threshold can be tripped with good GPS data causing position jerks. This increases the initial GPS glitch rejection threshold to effectively 5m when using the default POSNE_NOISE value of 0.5m.
2014-04-12 01:26:49 -03:00
#define POS_GATE_DEFAULT 10
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
#define HGT_GATE_DEFAULT 10
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
#define MAG_GATE_DEFAULT 3
#define MAG_CAL_DEFAULT 1
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
#define GLITCH_ACCEL_DEFAULT 150
#define GLITCH_RADIUS_DEFAULT 15
AP_NavEKF: added a mechanism for per-vehicle-type defaults in EKF
2014-03-25 22:30:17 -03:00
#else
// generic defaults (and for plane)
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
#define VELNE_NOISE_DEFAULT 0.3f
#define VELD_NOISE_DEFAULT 0.5f
#define POSNE_NOISE_DEFAULT 0.5f
#define ALT_NOISE_DEFAULT 0.5f
#define MAG_NOISE_DEFAULT 0.05f
#define GYRO_PNOISE_DEFAULT 0.015f
#define ACC_PNOISE_DEFAULT 0.25f
AP_NavEKF: Increase gyro bias process noise This is required to stop the bias estimate from becoming too static towards the end of longer flights.
2014-05-16 07:59:48 -03:00
#define GBIAS_PNOISE_DEFAULT 1E-06f
AP_NavEKF: Default parameter adjustments Bring Plane glitch protection thresholds into alignment with copter and rover Slight increase in accelerometer bias process noise to prevent bias estimate divergence into limits (Rover and Plane only as Copter does not seem respond as well to this change) effective increase in threshold on divergence test to allow increased margin for bad GPS velocities
2014-05-17 03:55:22 -03:00
#define ABIAS_PNOISE_DEFAULT 0.0002f
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
#define MAGE_PNOISE_DEFAULT 0.0003f
#define MAGB_PNOISE_DEFAULT 0.0003f
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
#define VEL_GATE_DEFAULT 6
AP_NavEKF : Increased position gate default to reduce impact of accel errors Flight testing with windup turns has shown that the position gate threshold can be tripped with good GPS data causing position jerks. This increases the initial GPS glitch rejection threshold to effectively 5m when using the default POSNE_NOISE value of 0.5m.
2014-04-12 01:26:49 -03:00
#define POS_GATE_DEFAULT 10
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
#define HGT_GATE_DEFAULT 20
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
#define MAG_GATE_DEFAULT 3
#define MAG_CAL_DEFAULT 0
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
#define GLITCH_ACCEL_DEFAULT 150
AP_NavEKF: Default parameter adjustments Bring Plane glitch protection thresholds into alignment with copter and rover Slight increase in accelerometer bias process noise to prevent bias estimate divergence into limits (Rover and Plane only as Copter does not seem respond as well to this change) effective increase in threshold on divergence test to allow increased margin for bad GPS velocities
2014-05-17 03:55:22 -03:00
#define GLITCH_RADIUS_DEFAULT 15
AP_NavEKF: added a mechanism for per-vehicle-type defaults in EKF
2014-03-25 22:30:17 -03:00
#endif // APM_BUILD_DIRECTORY
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
extern const AP_HAL::HAL& hal;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
#define earthRate 0.000072921f // earth rotation rate (rad/sec)
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
AP_NavEKF: start with a wind estimate of 3m/s, when no direct measurement this will cope better with users with low roll/pitch gains, to ensure they get enough control on takeoff Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-13 06:42:23 -03:00
// when the wind estimation first starts with no airspeed sensor,
// assume 3m/s to start
#define STARTUP_WIND_SPEED 3.0f
AP_NavEKF : add INIT_GYRO_BIAS_UNCERTAINTY definition
2014-10-29 05:32:57 -03:00
// initial gyro bias uncertainty (deg/sec)
#define INIT_GYRO_BIAS_UNCERTAINTY 0.1f
AP_NavEKF : Mavlink tunable parameter - first attempt
2014-01-30 18:16:58 -04:00
// Define tuning parameters
AP_NavEKF: fixup parameter handling
2014-01-20 01:47:56 -04:00
const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
AP_NavEKF : Mavlink tunable parameter - first attempt
2014-01-30 18:16:58 -04:00
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: VELNE_NOISE
// @DisplayName: GPS horizontal velocity measurement noise (m/s)
// @Description: This is the RMS value of noise in the North and East GPS velocity measurements. Increasing it reduces the weighting on these measurements.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0.05 5.0
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 0.05
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF: added a mechanism for per-vehicle-type defaults in EKF
2014-03-25 22:30:17 -03:00
AP_GROUPINFO("VELNE_NOISE", 0, NavEKF, _gpsHorizVelNoise, VELNE_NOISE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: VELD_NOISE
// @DisplayName: GPS vertical velocity measurement noise (m/s)
// @Description: This is the RMS value of noise in the vertical GPS velocity measurement. Increasing it reduces the weighting on this measurement.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0.05 5.0
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 0.05
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("VELD_NOISE", 1, NavEKF, _gpsVertVelNoise, VELD_NOISE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: POSNE_NOISE
// @DisplayName: GPS horizontal position measurement noise (m)
// @Description: This is the RMS value of noise in the GPS horizontal position measurements. Increasing it reduces the weighting on these measurements.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0.1 10.0
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 0.1
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("POSNE_NOISE", 2, NavEKF, _gpsHorizPosNoise, POSNE_NOISE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: ALT_NOISE
// @DisplayName: Altitude measurement noise (m)
// @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting on this measurement.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0.1 10.0
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 0.1
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("ALT_NOISE", 3, NavEKF, _baroAltNoise, ALT_NOISE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: MAG_NOISE
AP_NavEKF: Fix display names in parameter list
2014-04-29 17:32:42 -03:00
// @DisplayName: Magnetometer measurement noise (Gauss)
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Description: This is the RMS value of noise in magnetometer measurements. Increasing it reduces the weighting on these measurements.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0.01 0.5
AP_NavEKF : Increase the tuning range for magnetometer measurement noise
2014-02-27 01:45:03 -04:00
// @Increment: 0.01
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("MAG_NOISE", 4, NavEKF, _magNoise, MAG_NOISE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: EAS_NOISE
// @DisplayName: Equivalent airspeed measurement noise (m/s)
// @Description: This is the RMS value of noise in magnetometer measurements. Increasing it reduces the weighting on these measurements.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0.5 5.0
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 0.1
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF: Reduced number of Mavlink tuneable parameters
2014-01-29 04:03:07 -04:00
AP_GROUPINFO("EAS_NOISE", 5, NavEKF, _easNoise, 1.4f),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: WIND_PNOISE
AP_NavEKF: Fix display names in parameter list
2014-04-29 17:32:42 -03:00
// @DisplayName: Wind velocity process noise (m/s^2)
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Description: This noise controls the growth of wind state error estimates. Increasing it makes wind estimation faster and noisier.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0.01 1.0
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 0.1
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF: Reduced number of Mavlink tuneable parameters
2014-01-29 04:03:07 -04:00
AP_GROUPINFO("WIND_PNOISE", 6, NavEKF, _windVelProcessNoise, 0.1f),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: WIND_PSCALE
AP_NavEKF: Fix display names in parameter list
2014-04-29 17:32:42 -03:00
// @DisplayName: Height rate to wind procss noise scaler
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Description: Increasing this parameter increases how rapidly the wind states adapt when changing altitude, but does make wind speed estimation noiser.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0.0 1.0
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 0.1
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF: Reduced number of Mavlink tuneable parameters
2014-01-29 04:03:07 -04:00
AP_GROUPINFO("WIND_PSCALE", 7, NavEKF, _wndVarHgtRateScale, 0.5f),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: GYRO_PNOISE
// @DisplayName: Rate gyro noise (rad/s)
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
// @Description: This 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.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0.001 0.05
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 0.001
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("GYRO_PNOISE", 8, NavEKF, _gyrNoise, GYRO_PNOISE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: ACC_PNOISE
// @DisplayName: Accelerometer noise (m/s^2)
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
// @Description: This 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.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0.05 1.0
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 0.01
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("ACC_PNOISE", 9, NavEKF, _accNoise, ACC_PNOISE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: GBIAS_PNOISE
AP_NavEKF: Fix display names in parameter list
2014-04-29 17:32:42 -03:00
// @DisplayName: Rate gyro bias process noise (rad/s)
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Description: This noise controls the growth of gyro bias state error estimates. Increasing it makes rate gyro bias estimation faster and noisier.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0.0000001 0.00001
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("GBIAS_PNOISE", 10, NavEKF, _gyroBiasProcessNoise, GBIAS_PNOISE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: ABIAS_PNOISE
AP_NavEKF: Fix display names in parameter list
2014-04-29 17:32:42 -03:00
// @DisplayName: Accelerometer bias process noise (m/s^2)
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Description: This noise controls the growth of the vertical acelerometer bias state error estimate. Increasing it makes accelerometer bias estimation faster and noisier.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0.00001 0.001
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("ABIAS_PNOISE", 11, NavEKF, _accelBiasProcessNoise, ABIAS_PNOISE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: MAGE_PNOISE
AP_NavEKF: Fix display names in parameter list
2014-04-29 17:32:42 -03:00
// @DisplayName: Earth magnetic field process noise (gauss/s)
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Description: This noise controls the growth of earth magnetic field state error estimates. Increasing it makes earth magnetic field bias estimation faster and noisier.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0.0001 0.01
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("MAGE_PNOISE", 12, NavEKF, _magEarthProcessNoise, MAGE_PNOISE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: MAGB_PNOISE
AP_NavEKF: Fix display names in parameter list
2014-04-29 17:32:42 -03:00
// @DisplayName: Body magnetic field process noise (gauss/s)
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Description: This noise controls the growth of body magnetic field state error estimates. Increasing it makes compass offset estimation faster and noisier.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0.0001 0.01
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("MAGB_PNOISE", 13, NavEKF, _magBodyProcessNoise, MAGB_PNOISE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: VEL_DELAY
// @DisplayName: GPS velocity measurement delay (msec)
// @Description: This is the number of msec that the GPS velocity measurements lag behind the inertial measurements.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0 500
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 10
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF: Reduced number of Mavlink tuneable parameters
2014-01-29 04:03:07 -04:00
AP_GROUPINFO("VEL_DELAY", 14, NavEKF, _msecVelDelay, 220),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: POS_DELAY
// @DisplayName: GPS position measurement delay (msec)
// @Description: This is the number of msec that the GPS position measurements lag behind the inertial measurements.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0 500
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 10
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF: Reduced number of Mavlink tuneable parameters
2014-01-29 04:03:07 -04:00
AP_GROUPINFO("POS_DELAY", 15, NavEKF, _msecPosDelay, 220),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: GPS_TYPE
// @DisplayName: GPS velocity mode control
// @Description: This parameter controls use of GPS velocity measurements : 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 0 3
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 1
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF: Reduced number of Mavlink tuneable parameters
2014-01-29 04:03:07 -04:00
AP_GROUPINFO("GPS_TYPE", 16, NavEKF, _fusionModeGPS, 0),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: VEL_GATE
// @DisplayName: GPS velocity measurement gate size
// @Description: This parameter sets the 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.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 1 100
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 1
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("VEL_GATE", 17, NavEKF, _gpsVelInnovGate, VEL_GATE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: POS_GATE
// @DisplayName: GPS position measurement gate size
// @Description: This parameter sets the 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.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 1 100
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 1
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("POS_GATE", 18, NavEKF, _gpsPosInnovGate, POS_GATE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: HGT_GATE
// @DisplayName: Height measurement gate size
// @Description: This parameter sets the 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.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 1 100
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 1
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("HGT_GATE", 19, NavEKF, _hgtInnovGate, HGT_GATE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: MAG_GATE
// @DisplayName: Magnetometer measurement gate size
// @Description: This parameter sets the 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.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 1 100
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 1
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("MAG_GATE", 20, NavEKF, _magInnovGate, MAG_GATE_DEFAULT),
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Param: EAS_GATE
// @DisplayName: Airspeed measurement gate size
// @Description: This parameter sets the 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.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 1 100
AP_NavEKF: Made most tuning parameters Mavlink adustable
2014-01-20 06:27:50 -04:00
// @Increment: 1
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF: Reduced number of Mavlink tuneable parameters
2014-01-29 04:03:07 -04:00
AP_GROUPINFO("EAS_GATE", 21, NavEKF, _tasInnovGate, 10),
AP_NavEKF : Mavlink tunable parameter - first attempt
2014-01-30 18:16:58 -04:00
AP_NavEKF : Add low speed magnetometer calibration option
2014-03-06 02:13:22 -04:00
// @Param: MAG_CAL
AP_NavEKF: Fix display names in parameter list
2014-04-29 17:32:42 -03:00
// @DisplayName: Magnetometer calibration mode
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// @Description: EKF_MAG_CAL = 0 enables calibration based on flying speed and altitude and is the default setting for Plane users. EKF_MAG_CAL = 1 enables calibration based on manoeuvre level and is the default setting for Copter and Rover users. EKF_MAG_CAL = 2 prevents magnetometer calibration regardless of flight condition and is recommended if in-flight magnetometer calibration is unreliable.
// @Values: 0:Speed and Height,1:Acceleration,2:Never
AP_NavEKF : Add low speed magnetometer calibration option
2014-03-06 02:13:22 -04:00
// @Increment: 1
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF : Add initial parameter defaults for Copter, Rover and Plane
2014-03-28 03:12:38 -03:00
AP_GROUPINFO("MAG_CAL", 22, NavEKF, _magCal, MAG_CAL_DEFAULT),
AP_NavEKF : Add low speed magnetometer calibration option
2014-03-06 02:13:22 -04:00
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
// @Param: GLITCH_ACCEL
AP_NavEKF: Fix display names in parameter list
2014-04-29 17:32:42 -03:00
// @DisplayName: GPS glitch accel gate size (cm/s^2)
// @Description: This parameter controls the maximum amount of difference in horizontal acceleration between the value predicted by the filter and the value measured by the GPS before the GPS position data is rejected. If this value is set too low, then valid GPS data will be regularly discarded, and the position accuracy will degrade. If this parameter is set too high, then large GPS glitches will cause large rapid changes in position.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 100 500
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
// @Increment: 50
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
AP_GROUPINFO("GLITCH_ACCEL", 23, NavEKF, _gpsGlitchAccelMax, GLITCH_ACCEL_DEFAULT),
// @Param: GLITCH_RAD
AP_NavEKF: Fix display names in parameter list
2014-04-29 17:32:42 -03:00
// @DisplayName: GPS glitch radius gate size (m)
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
// @Description: This parameter controls the maximum amount of difference in horizontal position (in m) between the value predicted by the filter and the value measured by the GPS before the long term glitch protection logic is activated and an offset is applied to the GPS measurement to compensate. Position steps smaller than this value will be temporarily ignored, but will then be accepted and the filter will move to the new position. Position steps larger than this value will be ignored initially, but the filter will then apply an offset to the GPS position measurement.
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @Range: 10 50
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
// @Increment: 5
AP_NavEKF: fix parameter descriptions Errors pointed out by Richard (DK)
2014-10-27 10:20:03 -03:00
// @User: Advanced
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
AP_GROUPINFO("GLITCH_RAD", 24, NavEKF, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
AP_NavEKF: Add parameter that specifies EKF-to-DCM/INAV fallback strictness
2014-10-31 21:47:55 -03:00
// @Param: FALLBACK
// @DisplayName: Fallback strictness
// @Description: This parameter controls the conditions necessary to trigger a fallback to DCM and INAV. A value of 1 will cause fallbacks to occur on loss of GPS and other conditions. A value of 0 will trust the EKF more.
// @Values: 0:Trust EKF more, 1:Trust DCM more
// @User: Advanced
AP_GROUPINFO("FALLBACK", 31, NavEKF, _fallback, 1),
AP_NavEKF : Mavlink tunable parameter - first attempt
2014-01-30 18:16:58 -04:00
AP_GROUPEND
};
AP_NavEKF: allow states to be accessed using names as well as a Vector22
2014-02-19 22:21:09 -04:00
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
// constructor
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
_ahrs(ahrs),
_baro(baro),
AP_NavEKF: allow states to be accessed using names as well as a Vector22
2014-02-19 22:21:09 -04:00
state(*reinterpret_cast<struct state_elements *>(&states)),
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates
covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates
AP_NavEKF: Improved control of fusion update intervals
2014-01-22 05:42:39 -04:00
TASmsecMax(200), // maximum allowed interval between airspeed measurement updates
AP_NaVEKF : Enable operation without airspeed and compass
2014-03-10 00:18:40 -03:00
fuseMeNow(false), // forces airspeed and sythetic sideslip fusion to occur on the IMU frame that data arrives
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
staticMode(true), // staticMode forces position and velocity fusion with zero values
AP_NavEKF : improved on-ground, in-air check and GPS yaw alignment
2014-03-09 20:49:34 -03:00
prevStaticMode(true), // staticMode from previous filter update
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
yawAligned(false), // set true when heading or yaw angle has been aligned
inhibitWindStates(true), // inhibit wind state updates on startup
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
inhibitMagStates(true), // inhibit magnetometer state updates on startup
onGround(true), // initialise assuming we are on ground
manoeuvring(false) // initialise assuming we are not maneouvring
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF: added #defines for VRBRAIN board
2014-03-31 14:54:01 -03:00
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
AP_NavEKF: added perf counters on PX4
2013-12-30 06:41:28 -04:00
,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")),
_perf_CovariancePrediction(perf_alloc(PC_ELAPSED, "EKF_CovariancePrediction")),
_perf_FuseVelPosNED(perf_alloc(PC_ELAPSED, "EKF_FuseVelPosNED")),
_perf_FuseMagnetometer(perf_alloc(PC_ELAPSED, "EKF_FuseMagnetometer")),
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
_perf_FuseAirspeed(perf_alloc(PC_ELAPSED, "EKF_FuseAirspeed")),
_perf_FuseSideslip(perf_alloc(PC_ELAPSED, "EKF_FuseSideslip"))
AP_NavEKF: added perf counters on PX4
2013-12-30 06:41:28 -04:00
#endif
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF: fixup parameter handling
2014-01-20 01:47:56 -04:00
AP_Param::setup_object_defaults(this, var_info);
AP_NavEKF: brought some tuning parameters out to header
2014-01-01 08:15:37 -04:00
// Tuning parameters
AP_NavEKF: Improved magnetometer consistency checks A magnetometer axis that fails the innovation consistency check will cause all axes not to be used. If this condition continues for 10 seconds, a magnetometer timeout condition will be declared. When the timeout has occurred, if it is not a fly forward vehicle, then individual channels will be used again, but with a reduced weighting.
2014-04-21 03:15:17 -03:00
_gpsNEVelVarAccScale = 0.05f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
AP_NavEKF : fixed bug in velocity rate of change filter This bug meant the velocity rate of change used to scale the GPS measurement variances was noisy and too small
2014-02-23 03:21:42 -04:00
_gpsDVelVarAccScale = 0.07f; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
AP_NavEKF: Improved magnetometer consistency checks A magnetometer axis that fails the innovation consistency check will cause all axes not to be used. If this condition continues for 10 seconds, a magnetometer timeout condition will be declared. When the timeout has occurred, if it is not a fly forward vehicle, then individual channels will be used again, but with a reduced weighting.
2014-04-21 03:15:17 -03:00
_gpsPosVarAccScale = 0.05f; // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
AP_NavEKF: Reduced number of Mavlink tuneable parameters
2014-01-29 04:03:07 -04:00
_msecHgtDelay = 60; // Height measurement delay (msec)
_msecMagDelay = 40; // Magnetometer measurement delay (msec)
_msecTasDelay = 240; // Airspeed measurement delay (msec)
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
_gpsRetryTimeUseTAS = 20000; // GPS retry time with airspeed measurements (msec)
AP_NavEKF: Improved magnetometer consistency checks A magnetometer axis that fails the innovation consistency check will cause all axes not to be used. If this condition continues for 10 seconds, a magnetometer timeout condition will be declared. When the timeout has occurred, if it is not a fly forward vehicle, then individual channels will be used again, but with a reduced weighting.
2014-04-21 03:15:17 -03:00
_gpsRetryTimeNoTAS = 10000; // GPS retry time without airspeed measurements (msec)
AP_NavEKF: Reduced number of Mavlink tuneable parameters
2014-01-29 04:03:07 -04:00
_hgtRetryTimeMode0 = 10000; // Height retry time with vertical velocity measurement (msec)
_hgtRetryTimeMode12 = 5000; // Height retry time without vertical velocity measurement (msec)
AP_NavEKF: cleanup some build warnings Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-21 04:11:06 -03:00
_magFailTimeLimit_ms = 10000; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
_magVarRateScale = 0.05f; // scale factor applied to magnetometer variance due to angular rate
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
_gyroBiasNoiseScaler = 2.0f; // scale factor applied to gyro bias state process noise when on ground
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
_msecGpsAvg = 200; // average number of msec between GPS measurements
_msecHgtAvg = 100; // average number of msec between height measurements
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
_msecMagAvg = 100; // average number of msec between magnetometer measurements
AP_NaVEKF : Enable operation without airspeed and compass
2014-03-10 00:18:40 -03:00
_msecBetaAvg = 100; // average number of msec between synthetic sideslip measurements
AP_NavEKF: Improved control of fusion update intervals
2014-01-22 05:42:39 -04:00
dtVelPos = 0.02; // number of seconds between position and velocity corrections. This should be a multiple of the imu update interval.
AP_NavEKF: brought some tuning parameters out to header
2014-01-01 08:15:37 -04:00
// Misc initial conditions
AP_NavEKF: Added height rate adaptive wind speed process noise
2014-01-01 17:12:06 -04:00
hgtRate = 0.0f;
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
mag_state.q0 = 1;
mag_state.DCM.identity();
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
IMU1_weighting = 0.5f;
AP_NavEKF: Fix bug causing immediate clearing of diverged status on reset
2014-05-16 08:11:39 -03:00
memset(&faultStatus, 0, sizeof(faultStatus));
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// Check basic filter health metrics and return a consolidated health status
AP_NavEKF: make health() API const
2014-01-03 20:16:19 -04:00
bool NavEKF::healthy(void) const
AP_NavEKF: added healthy() API
2014-01-02 01:16:16 -04:00
{
if (!statesInitialised) {
return false;
}
AP_NavEKF: allow states to be accessed using names as well as a Vector22
2014-02-19 22:21:09 -04:00
if (state.quat.is_nan()) {
AP_NavEKF: added healthy() API
2014-01-02 01:16:16 -04:00
return false;
}
AP_NavEKF: allow states to be accessed using names as well as a Vector22
2014-02-19 22:21:09 -04:00
if (state.velocity.is_nan()) {
AP_NavEKF: added healthy() API
2014-01-02 01:16:16 -04:00
return false;
}
AP_NavEKF: add another health check in the EKF if SV, SP and SH are all off then the most likely cause is divergence of the EKF. This was done based on a flight log with bad gyro cal
2014-11-21 22:26:16 -04:00
if (_fallback && velTestRatio > 1 && posTestRatio > 1 && hgtTestRatio > 1) {
// all three metrics being above 1 means the filter is
// extremely unhealthy.
return false;
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
// If measurements have failed innovation consistency checks for long enough to time-out
// and force fusion then the nav solution can be conidered to be unhealthy
// This will only be set as a transient
AP_NavEKF: Add parameter that specifies EKF-to-DCM/INAV fallback strictness
2014-10-31 21:47:55 -03:00
if (_fallback && (posTimeout || velTimeout || hgtTimeout)) {
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
return false;
}
AP_NavEKF: added healthy() API
2014-01-02 01:16:16 -04:00
// all OK
return true;
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return true if filter is dead-reckoning height
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
bool NavEKF::HeightDrifting(void) const
{
// Set to true if height measurements are failing the innovation consistency check
return !hgtHealth;
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return true if filter is dead-reckoning position
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
bool NavEKF::PositionDrifting(void) const
{
// Set to true if position measurements are failing the innovation consistency check
return !posHealth;
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// resets position states to last GPS measurement or to zero if in static mode
AP_NavEKF : Added position and height reset public method
2014-01-30 18:46:10 -04:00
void NavEKF::ResetPosition(void)
{
AP_NavEKF : Position and Velocity reset bugfix
2014-02-16 05:42:55 -04:00
if (staticMode) {
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
state.position.x = 0;
state.position.y = 0;
AP_NavEKF: changes for new GPS API
2014-03-30 19:52:03 -03:00
} else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
AP_NavEKF : Position and Velocity reset bugfix
2014-02-16 05:42:55 -04:00
// read the GPS
readGpsData();
AP_NavEKF : Fix bug in reset of position, height and velocity states If the inertial solution velocity or position needs to be reset to the GPS or baro, the stored state history for the corresponding states should also be reset. Otherwise the next GPS or baro measurement will be compared to an invalid previous state and will be rejected. This is particularly a problem if IMU saturation or timeout has occurred because the previous states could be out by a large amount The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s.
2014-09-16 15:52:23 -03:00
// write to state vector and compensate for GPS latency
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
state.position.x = gpsPosNE.x + gpsPosGlitchOffsetNE.x + 0.001f*velNED.x*float(_msecPosDelay);
state.position.y = gpsPosNE.y + gpsPosGlitchOffsetNE.y + 0.001f*velNED.y*float(_msecPosDelay);
AP_NavEKF : Position and Velocity reset bugfix
2014-02-16 05:42:55 -04:00
}
AP_NavEKF : Fix bug in reset of position, height and velocity states If the inertial solution velocity or position needs to be reset to the GPS or baro, the stored state history for the corresponding states should also be reset. Otherwise the next GPS or baro measurement will be compared to an invalid previous state and will be rejected. This is particularly a problem if IMU saturation or timeout has occurred because the previous states could be out by a large amount The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s.
2014-09-16 15:52:23 -03:00
// stored horizontal position states to prevent subsequent GPS measurements from being rejected
for (uint8_t i=0; i<=49; i++){
storedStates[i].position[0] = state.position[0];
storedStates[i].position[1] = state.position[1];
}
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation. Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and the various calls to the hal.scheduler->millis() object have been consolidated. Unused variables have been removed
2014-08-23 23:02:29 -03:00
// reset the glitch ofset correction states
gpsPosGlitchOffsetNE.zero();
AP_NavEKF: Improved behaviour following loss and regaining of GPS
2014-01-20 15:41:41 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// resets velocity states to last GPS measurement or to zero if in static mode
AP_NavEKF: Improved behaviour following loss and regaining of GPS
2014-01-20 15:41:41 -04:00
void NavEKF::ResetVelocity(void)
{
AP_NavEKF : Position and Velocity reset bugfix
2014-02-16 05:42:55 -04:00
if (staticMode) {
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
state.velocity.zero();
state.vel1.zero();
state.vel2.zero();
AP_NavEKF: changes for new GPS API
2014-03-30 19:52:03 -03:00
} else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
AP_NavEKF : Position and Velocity reset bugfix
2014-02-16 05:42:55 -04:00
// read the GPS
readGpsData();
AP_NavEKF : Fix bug in reset of position and velocity states If the inertial solution velocity or position needs to be reset to the GPS, the stored state history for the corresponding states should also be reset. Otherwise the next GPS measurement will be compared to an invalid previous state and will be rejected. The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s
2014-09-06 20:04:19 -03:00
// reset horizontal velocity states
if (_fusionModeGPS <= 1) {
states[4] = velNED[0]; // north velocity from blended accel data
states[5] = velNED[1]; // east velocity from blended accel data
states[23] = velNED[0]; // north velocity from IMU1 accel data
states[24] = velNED[1]; // east velocity from IMU1 accel data
states[27] = velNED[0]; // north velocity from IMU2 accel data
states[28] = velNED[1]; // east velocity from IMU2 accel data
// over write stored horizontal velocity states to prevent subsequent GPS measurements from being rejected
for (uint8_t i=0; i<=49; i++){
storedStates[i].velocity[0] = velNED[0];
storedStates[i].velocity[1] = velNED[1];
}
AP_NavEKF : Position and Velocity reset bugfix
2014-02-16 05:42:55 -04:00
}
AP_NavEKF : Fix bug in reset of position, height and velocity states If the inertial solution velocity or position needs to be reset to the GPS or baro, the stored state history for the corresponding states should also be reset. Otherwise the next GPS or baro measurement will be compared to an invalid previous state and will be rejected. This is particularly a problem if IMU saturation or timeout has occurred because the previous states could be out by a large amount The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s.
2014-09-16 15:52:23 -03:00
// reset filter velocity states
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
state.velocity = velNED;
state.vel1 = velNED;
state.vel2 = velNED;
AP_NavEKF : Fix bug in reset of position, height and velocity states If the inertial solution velocity or position needs to be reset to the GPS or baro, the stored state history for the corresponding states should also be reset. Otherwise the next GPS or baro measurement will be compared to an invalid previous state and will be rejected. This is particularly a problem if IMU saturation or timeout has occurred because the previous states could be out by a large amount The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s.
2014-09-16 15:52:23 -03:00
// reset stored velocity states to prevent subsequent GPS measurements from being rejected
for (uint8_t i=0; i<=49; i++){
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
storedStates[i].velocity = velNED;
AP_NavEKF : Position and Velocity reset bugfix
2014-02-16 05:42:55 -04:00
}
AP_NavEKF: Improved behaviour following loss and regaining of GPS
2014-01-20 15:41:41 -04:00
}
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// reset the vertical position state using the last height measurement
AP_NavEKF: Improved behaviour following loss and regaining of GPS
2014-01-20 15:41:41 -04:00
void NavEKF::ResetHeight(void)
{
// read the altimeter
readHgtData();
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// write to the state vector
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
state.position.z = -hgtMea; // down position from blended accel data
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
state.posD1 = -hgtMea; // down position from IMU1 accel data
state.posD2 = -hgtMea; // down position from IMU2 accel data
AP_NavEKF : Fix bug in reset of position, height and velocity states If the inertial solution velocity or position needs to be reset to the GPS or baro, the stored state history for the corresponding states should also be reset. Otherwise the next GPS or baro measurement will be compared to an invalid previous state and will be rejected. This is particularly a problem if IMU saturation or timeout has occurred because the previous states could be out by a large amount The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s.
2014-09-16 15:52:23 -03:00
// reset stored vertical position states to prevent subsequent GPS measurements from being rejected
for (uint8_t i=0; i<=49; i++){
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
storedStates[i].position.z = -hgtMea;
AP_NavEKF : Fix bug in reset of position, height and velocity states If the inertial solution velocity or position needs to be reset to the GPS or baro, the stored state history for the corresponding states should also be reset. Otherwise the next GPS or baro measurement will be compared to an invalid previous state and will be rejected. This is particularly a problem if IMU saturation or timeout has occurred because the previous states could be out by a large amount The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s.
2014-09-16 15:52:23 -03:00
}
AP_NavEKF : Added position and height reset public method
2014-01-30 18:46:10 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// this function is used to initialise the filter whilst moving, using the AHRS DCM solution
// it should NOT be used to re-initialise after a timeout as DCM will also be corrupted
AP_NavEKF: Improved in-air reset behaviour
2014-01-20 15:52:27 -04:00
void NavEKF::InitialiseFilterDynamic(void)
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF: force healthy false when initialising this prevents us feeding off our own values when booting Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-02-14 18:25:03 -04:00
// this forces healthy() to be false so that when we ask for ahrs
// attitude we get the DCM attitude regardless of the state of AHRS_EKF_USE
statesInitialised = false;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
// Set re-used variables to zero
ZeroVariables();
AP_NavEKF: zero more variables on filter re-init
2014-01-02 17:31:33 -04:00
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
// get initial time deltat between IMU measurements (sec)
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f);
// set number of updates over which gps and baro measurements are applied to the velocity and position states
gpsUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecGpsAvg);
gpsUpdateCountMax = uint8_t(1.0f/gpsUpdateCountMaxInv);
hgtUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecHgtAvg);
hgtUpdateCountMax = uint8_t(1.0f/hgtUpdateCountMaxInv);
magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecMagAvg);
magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv);
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
AP_NavEKF : Clean up angle and mag field initialisation This path reduces duplicated code, eliminates unused variables and causes the earth magnetic field states to be reset when exiting static mode which will occur every time copter is armed. This enables copters to be powered on and initialised inside vehicles or houses, without bad earth field values affecting flight.
2014-03-24 04:34:05 -03:00
// calculate initial orientation and earth magnetic field states
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// write to state vector
AP_NavEKF: allow states to be accessed using names as well as a Vector22
2014-02-19 22:21:09 -04:00
state.gyro_bias.zero();
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
state.accel_zbias1 = 0;
state.accel_zbias2 = 0;
AP_NavEKF: allow states to be accessed using names as well as a Vector22
2014-02-19 22:21:09 -04:00
state.wind_vel.zero();
AP_NavEKF: Improved in-air reset behaviour
2014-01-20 15:52:27 -04:00
ResetVelocity();
ResetPosition();
ResetHeight();
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
state.body_magfield.zero();
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// set to true now that states have be initialised
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
statesInitialised = true;
// initialise the covariance matrix
AP_NavEKF : Clean up angle and mag field initialisation This path reduces duplicated code, eliminates unused variables and causes the earth magnetic field states to be reset when exiting static mode which will occur every time copter is armed. This enables copters to be powered on and initialised inside vehicles or houses, without bad earth field values affecting flight.
2014-03-24 04:34:05 -03:00
CovarianceInit();
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// define Earth rotation vector in the NED navigation frame
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// initialise IMU pre-processing states
AP_NavEKF: debug updates
2013-12-29 21:12:09 -04:00
readIMUData();
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
}
AP_NavEKF: some debug code
2014-01-05 01:37:24 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// Initialise the states from accelerometer and magnetometer data (if present)
// This method can only be used when the vehicle is static
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
void NavEKF::InitialiseFilterBootstrap(void)
{
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// set re-used variables to zero
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
ZeroVariables();
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
// get initial time deltat between IMU measurements (sec)
dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f);
// set number of updates over which gps and baro measurements are applied to the velocity and position states
gpsUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecGpsAvg);
gpsUpdateCountMax = uint8_t(1.0f/gpsUpdateCountMaxInv);
hgtUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecHgtAvg);
hgtUpdateCountMax = uint8_t(1.0f/hgtUpdateCountMaxInv);
magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecMagAvg);
magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv);
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
// acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f initAccVec;
AP_NavEKF : Fixes zero compass offsets on initialisation
2014-02-28 03:54:07 -04:00
// TODO we should average accel readings over several cycles
AP_NavEKF: only use the active accel from DCM if fly_forward is set
2014-02-27 02:40:13 -04:00
initAccVec = _ahrs->get_ins().get_accel();
AP_NavEKF : Fixes zero compass offsets on initialisation
2014-02-28 03:54:07 -04:00
// read the magnetometer data
readMagData();
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// normalise the acceleration vector
AP_NavEKF: prevent a possible numerical error on startup fixes issue #1294
2014-08-08 02:55:20 -03:00
float pitch=0, roll=0;
if (initAccVec.length() > 0.001f) {
initAccVec.normalize();
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF: prevent a possible numerical error on startup fixes issue #1294
2014-08-08 02:55:20 -03:00
// calculate initial pitch angle
pitch = asinf(initAccVec.x);
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF: prevent a possible numerical error on startup fixes issue #1294
2014-08-08 02:55:20 -03:00
// calculate initial roll angle
roll = -asinf(initAccVec.y / cosf(pitch));
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : Clean up angle and mag field initialisation This path reduces duplicated code, eliminates unused variables and causes the earth magnetic field states to be reset when exiting static mode which will occur every time copter is armed. This enables copters to be powered on and initialised inside vehicles or houses, without bad earth field values affecting flight.
2014-03-24 04:34:05 -03:00
// calculate initial orientation and earth magnetic field states
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
Quaternion initQuat;
AP_NavEKF : Clean up angle and mag field initialisation This path reduces duplicated code, eliminates unused variables and causes the earth magnetic field states to be reset when exiting static mode which will occur every time copter is armed. This enables copters to be powered on and initialised inside vehicles or houses, without bad earth field values affecting flight.
2014-03-24 04:34:05 -03:00
initQuat = calcQuatAndFieldStates(roll, pitch);
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
// read the GPS
readGpsData();
// read the barometer
readHgtData();
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// check on ground status
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
SetFlightAndFusionModes();
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
// write to state vector
AP_NavEKF: allow states to be accessed using names as well as a Vector22
2014-02-19 22:21:09 -04:00
state.quat = initQuat;
state.gyro_bias.zero();
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
state.accel_zbias1 = 0;
state.accel_zbias2 = 0;
AP_NavEKF: allow states to be accessed using names as well as a Vector22
2014-02-19 22:21:09 -04:00
state.wind_vel.zero();
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
state.body_magfield.zero();
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// set to true now we have intialised the states
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
statesInitialised = true;
// initialise the covariance matrix
AP_NavEKF : Clean up angle and mag field initialisation This path reduces duplicated code, eliminates unused variables and causes the earth magnetic field states to be reset when exiting static mode which will occur every time copter is armed. This enables copters to be powered on and initialised inside vehicles or houses, without bad earth field values affecting flight.
2014-03-24 04:34:05 -03:00
CovarianceInit();
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// define Earth rotation vector in the NED navigation frame
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// initialise IMU pre-processing states
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
readIMUData();
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// Update Filter States - this should be called whenever new IMU data is available
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
void NavEKF::UpdateFilter()
{
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// don't run filter updates if states have not been initialised
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
if (!statesInitialised) {
return;
}
AP_NavEKF: Removed accel bias states
2014-01-03 03:52:37 -04:00
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// start the timer used for load measurement
AP_NavEKF: added perf counters on PX4
2013-12-30 06:41:28 -04:00
perf_begin(_perf_UpdateFilter);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// read IMU data and convert to delta angles and velocities
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
readIMUData();
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// detect if the filter update has been delayed for too long
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
if (dtIMU > 0.2f) {
AP_NavEKF : Changed timout behaviour to only reset PosVel states
2014-02-16 01:54:11 -04:00
// we have stalled for too long - reset states
ResetVelocity();
ResetPosition();
ResetHeight();
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
StoreStatesReset();
AP_NavEKF : Changed timout behaviour to only reset PosVel states
2014-02-16 01:54:11 -04:00
//Initialise IMU pre-processing states
readIMUData();
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// stop the timer used for load measurement
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
perf_end(_perf_UpdateFilter);
return;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// check if on ground
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
SetFlightAndFusionModes();
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// define rules used to set staticMode
// staticMode enables ground operation without GPS by fusing zeros for position and height measurements
AP_NavEKF : improved static and on-ground mode selection logic
2014-03-09 14:39:59 -03:00
if (static_mode_demanded()) {
AP_NavEKF : staticMode bugfix and robustness improvements
2014-02-16 05:13:34 -04:00
staticMode = true;
AP_NavEKF: Fixed bug in pre GPS fix updates
2014-01-18 17:48:12 -04:00
} else {
AP_NavEKF : staticMode bugfix and robustness improvements
2014-02-16 05:13:34 -04:00
staticMode = false;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
}
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
// check to see if static mode has changed and reset states if it has
if (prevStaticMode != staticMode) {
ResetVelocity();
ResetPosition();
ResetHeight();
StoreStatesReset();
AP_NavEKF: Reduce vulnerability to ground fixed magnetic interference Re-initialisation of the magnetic field states and yaw angle is now only performed a maximum of two times after start-up. Once when coming out of static modefor the first time (first arm event) Again (for copter only) when the altitude gain above the arming altitude exceeds 1.5m this prevents magnetic interference present at arming (eg arming on a metal roof)from corrupting the magnetic field states enough to cause bad heading errors and toilet bowling on copter
2014-12-02 04:32:59 -04:00
// only reset the magnetic field and heading on the first arm. This prevents in-flight learning being forgotten for vehicles that do multiple short flights and disarm in-between.
if (!staticMode && !firstArmComplete) {
firstArmComplete = true;
state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
firstArmPosD = state.position.z;
}
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
prevStaticMode = staticMode;
AP_NavEKF: Reduce vulnerability to ground fixed magnetic interference Re-initialisation of the magnetic field states and yaw angle is now only performed a maximum of two times after start-up. Once when coming out of static modefor the first time (first arm event) Again (for copter only) when the altitude gain above the arming altitude exceeds 1.5m this prevents magnetic interference present at arming (eg arming on a metal roof)from corrupting the magnetic field states enough to cause bad heading errors and toilet bowling on copter
2014-12-02 04:32:59 -04:00
} else if (!staticMode && !finalMagYawInit && firstArmPosD - state.position.z > 1.5f && !assume_zero_sideslip()) {
// Do a final yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
finalMagYawInit = true;
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
}
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// run the strapdown INS equations every IMU update
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
UpdateStrapdownEquationsNED();
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
// store the predicted states for subsequent use by measurement fusion
StoreStates();
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
// sum delta angles and time used by covariance prediction
summedDelAng = summedDelAng + correctedDelAng;
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
summedDelVel = summedDelVel + correctedDelVel1;
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
dt += dtIMU;
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
if (((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax))) {
CovariancePrediction();
covPredStep = true;
summedDelAng.zero();
summedDelVel.zero();
dt = 0.0;
} else {
covPredStep = false;
}
AP_NaVEKF : Enable operation without airspeed and compass
2014-03-10 00:18:40 -03:00
// Update states using GPS, altimeter, compass, airspeed and synthetic sideslip observations
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
SelectVelPosFusion();
SelectMagFusion();
SelectTasFusion();
AP_NaVEKF : Enable operation without airspeed and compass
2014-03-10 00:18:40 -03:00
SelectBetaFusion();
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// stop the timer used for load measurement
AP_NavEKF: added perf counters on PX4
2013-12-30 06:41:28 -04:00
perf_end(_perf_UpdateFilter);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// select fusion of velocity, position and height measurements
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
void NavEKF::SelectVelPosFusion()
{
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// check for new data, specify which measurements should be used and check data for freshness
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
if (!staticMode) {
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// check for and read new GPS data
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
readGpsData();
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// command fusion of GPS data and reset states as required
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
if (newDataGps) {
// reset data arrived flag
newDataGps = false;
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
// reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
memset(&gpsIncrStateDelta[0], 0, sizeof(gpsIncrStateDelta));
gpsUpdateCount = 0;
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
// enable fusion
fuseVelData = true;
fusePosData = true;
// If a long time since last GPS update, then reset position and velocity and reset stored state history
AP_NavEKF: fixed millisecond subtraction for rollover Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-21 00:16:20 -03:00
uint32_t gpsRetryTimeout = useAirspeed() ? _gpsRetryTimeUseTAS : _gpsRetryTimeNoTAS;
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
if (imuSampleTime_ms - secondLastFixTime_ms > gpsRetryTimeout) {
AP_NavEKF: fixed millisecond subtraction for rollover Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-21 00:16:20 -03:00
ResetPosition();
ResetVelocity();
StoreStatesReset();
AP_NavEKF : fixed bug in pos and vel reset when in static mode
2014-02-16 07:32:17 -04:00
}
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
} else {
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
fuseVelData = false;
fusePosData = false;
}
AP_NavEKF : staticMode bugfix and robustness improvements
2014-02-16 05:13:34 -04:00
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
} else {
AP_NavEKF: Use baro data during launch transients whilst in static mode Baro data can be used as it will still be valid during the launch and is not a synthesised measurement.
2014-04-28 23:59:19 -03:00
// in static mode use synthetic position measurements set to zero
// only fuse synthetic measurements when rate of change of velocity is less than 0.5g to reduce attitude errors due to launch acceleration
// do not use velocity fusion to reduce the effect of movement on attitude
AP_NavEKF : enable operation without a compass for planes
2014-03-09 05:44:13 -03:00
if (accNavMag < 4.9f) {
fusePosData = true;
} else {
fusePosData = false;
}
AP_NavEKF: Use baro data during launch transients whilst in static mode Baro data can be used as it will still be valid during the launch and is not a synthesised measurement.
2014-04-28 23:59:19 -03:00
fuseVelData = false;
AP_NavEKF: Track baro alt when pre-armed This will help prevent spurious alt disparity warning messages for copter
2014-10-12 08:17:38 -03:00
}
// check for and read new height data
readHgtData();
// command fusion of height data
if (newDataHgt)
{
// reset data arrived flag
newDataHgt = false;
// reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta));
hgtUpdateCount = 0;
// enable fusion
AP_NavEKF: Use baro data during launch transients whilst in static mode Baro data can be used as it will still be valid during the launch and is not a synthesised measurement.
2014-04-28 23:59:19 -03:00
fuseHgtData = true;
AP_NavEKF: Track baro alt when pre-armed This will help prevent spurious alt disparity warning messages for copter
2014-10-12 08:17:38 -03:00
} else {
fuseHgtData = false;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
}
AP_NavEKF : staticMode bugfix and robustness improvements
2014-02-16 05:13:34 -04:00
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
// perform fusion
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
if (fuseVelData || fusePosData || fuseHgtData) {
AP_NavEKF : staticMode bugfix and robustness improvements
2014-02-16 05:13:34 -04:00
FuseVelPosNED();
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF : staticMode bugfix and robustness improvements
2014-02-16 05:13:34 -04:00
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
// Fuse corrections to quaternion, position and velocity states across several time steps to reduce 5 and 10Hz pulsing in the output
if (gpsUpdateCount < gpsUpdateCountMax) {
gpsUpdateCount ++;
for (uint8_t i = 0; i <= 9; i++) {
states[i] += gpsIncrStateDelta[i];
}
}
if (hgtUpdateCount < hgtUpdateCountMax) {
hgtUpdateCount ++;
for (uint8_t i = 0; i <= 9; i++) {
states[i] += hgtIncrStateDelta[i];
}
}
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
// select fusion of magnetometer data
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
void NavEKF::SelectMagFusion()
{
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// check for and read new magnetometer measurements
readMagData();
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// 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) > _magFailTimeLimit_ms && use_compass()) {
magTimeout = true;
}
AP_NavEKF : update comments
2014-03-08 16:51:45 -04:00
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// determine if conditions are right to start a new fusion cycle
bool dataReady = statesInitialised && use_compass() && newDataMag;
if (dataReady)
{
fuseMagData = true;
// reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta));
magUpdateCount = 0;
}
else
{
fuseMagData = false;
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// call the function that performs fusion of magnetometer data
FuseMagnetometer();
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// Fuse corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output
if (magUpdateCount < magUpdateCountMax) {
magUpdateCount ++;
for (uint8_t i = 0; i <= 9; i++) {
states[i] += magIncrStateDelta[i];
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
}
AP_NavEKF : Modified compass fail logic If the vehicle can fly without a compass (a fly forward vehicle) then if the compass times out (large errors for more than 10 seconds, then it will be declared permanently failed and will not be used until the filter is reset
2014-07-31 06:16:38 -03:00
}
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NaVEKF : Enable operation without airspeed and compass
2014-03-10 00:18:40 -03:00
// select fusion of true airspeed measurements
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
void NavEKF::SelectTasFusion()
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// get true airspeed measurement
AP_NavEKF: debug updates
2013-12-29 21:12:09 -04:00
readAirSpdData();
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// if the filter is initialised, wind states are not inhibited and we have data to fuse, then queue TAS fusion
tasDataWaiting = (statesInitialised && !inhibitWindStates && (tasDataWaiting || newDataTas));
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// if we have waited too long, set a timeout flag which will force fusion to occur
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
bool timeout = ((imuSampleTime_ms - TASmsecPrev) >= TASmsecMax);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// we don't fuse airspeed measurements if magnetometer fusion has been performed in the same frame, unless timed out or the fuseMeNow option is selected
// this helps to spreasthe load associated with fusion of different measurements across multiple frames
// setting fuseMeNow to true disables this load spreading feature
AP_NavEKF: Improved control of fusion update intervals
2014-01-22 05:42:39 -04:00
if (tasDataWaiting && (!magFusePerformed || timeout || fuseMeNow))
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
FuseAirspeed();
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
TASmsecPrev = imuSampleTime_ms;
AP_NavEKF: Improved control of fusion update intervals
2014-01-22 05:42:39 -04:00
tasDataWaiting = false;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
}
AP_NaVEKF : Enable operation without airspeed and compass
2014-03-10 00:18:40 -03:00
// select fusion of synthetic sideslip measurements
AP_AHRS : Tidy up logic used to enable synthetic sideslip fusion
2014-10-06 05:53:44 -03:00
// 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)
AP_NaVEKF : Enable operation without airspeed and compass
2014-03-10 00:18:40 -03:00
void NavEKF::SelectBetaFusion()
{
AP_AHRS : Tidy up logic used to enable synthetic sideslip fusion
2014-10-06 05:53:44 -03:00
// set to true if fusion is locked out due to magnetometer fusion on the same time step (done for load levelling)
bool f_lockedOut = (magFusePerformed && !fuseMeNow);
// set true when the fusion time interval has triggered
bool f_timeTrigger = ((imuSampleTime_ms - BETAmsecPrev) >= _msecBetaAvg);
// 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() && posHealth);
// 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, enough time has lapsed since the last fusion and it is not locked out
if (f_feasible && f_required && f_timeTrigger && !f_lockedOut) {
AP_NaVEKF : Enable operation without airspeed and compass
2014-03-10 00:18:40 -03:00
FuseSideslip();
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
BETAmsecPrev = imuSampleTime_ms;
AP_NaVEKF : Enable operation without airspeed and compass
2014-03-10 00:18:40 -03:00
}
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// update the quaternion, velocity and position states using IMU measurements
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
void NavEKF::UpdateStrapdownEquationsNED()
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
Vector3f delVelNav; // delta velocity vector calculated using a blend of IMU1 and IMU2 data
Vector3f delVelNav1; // delta velocity vector calculated using IMU1 data
Vector3f delVelNav2; // delta velocity vector calculated using IMU2 data
float rotationMag; // magnitude of rotation vector from previous to current time step
float rotScaler; // scaling variable used to calculate delta quaternion from last to current time step
Quaternion qUpdated; // quaternion at current time step after application of delta quaternion
Quaternion deltaQuat; // quaternion from last to current time step
const Vector3f gravityNED(0, 0, GRAVITY_MSS); // NED gravity vector m/s^2
// remove sensor bias errors
AP_NavEKF: allow states to be accessed using names as well as a Vector22
2014-02-19 22:21:09 -04:00
correctedDelAng = dAngIMU - state.gyro_bias;
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
correctedDelVel1 = dVelIMU1;
correctedDelVel2 = dVelIMU2;
correctedDelVel1.z -= state.accel_zbias1;
correctedDelVel2.z -= state.accel_zbias2;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// use weighted average of both IMU units for delta velocities
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
correctedDelVel12 = correctedDelVel1 * IMU1_weighting + correctedDelVel2 * (1.0f - IMU1_weighting);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// apply corrections for earths rotation rate and coning errors
AP_NavEKF: Reduced heading drift on ground
2013-12-30 16:56:28 -04:00
// % * - and + operators have been overloaded
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
correctedDelAng = correctedDelAng - prevTnb * earthRateNED*dtIMU + (prevDelAng % correctedDelAng) * 8.333333e-2f;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF: Set prevDelAng after using it
2014-10-12 08:11:05 -03:00
// save current measurements
prevDelAng = correctedDelAng;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// convert the rotation vector to its equivalent quaternion
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
rotationMag = correctedDelAng.length();
if (rotationMag < 1e-12f)
{
deltaQuat[0] = 1;
deltaQuat[1] = 0;
deltaQuat[2] = 0;
deltaQuat[3] = 0;
}
else
{
deltaQuat[0] = cosf(0.5f * rotationMag);
rotScaler = (sinf(0.5f * rotationMag)) / rotationMag;
deltaQuat[1] = correctedDelAng.x * rotScaler;
deltaQuat[2] = correctedDelAng.y * rotScaler;
deltaQuat[3] = correctedDelAng.z * rotScaler;
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// update the quaternions by rotating from the previous attitude through
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// the delta angle rotation quaternion
qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3];
qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2];
qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3];
qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// normalise the quaternions and update the quaternion states
AP_NavEKF : increased useage of helper functions
2014-03-10 06:08:15 -03:00
qUpdated.normalize();
state.quat = qUpdated;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate the body to nav cosine matrix
AP_NavEKF: first working GPS fusion
2013-12-29 23:43:29 -04:00
Matrix3f Tbn_temp;
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
state.quat.rotation_matrix(Tbn_temp);
AP_NavEKF: first working GPS fusion
2013-12-29 23:43:29 -04:00
prevTnb = Tbn_temp.transposed();
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// transform body delta velocities to delta velocities in the nav frame
// * and + operators have been overloaded
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
// blended IMU calc
delVelNav = Tbn_temp*correctedDelVel12 + gravityNED*dtIMU;
// single IMU calcs
delVelNav1 = Tbn_temp*correctedDelVel1 + gravityNED*dtIMU;
delVelNav2 = Tbn_temp*correctedDelVel2 + gravityNED*dtIMU;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate the rate of change of velocity (used for launch detect and other functions)
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
velDotNED = delVelNav / dtIMU ;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// apply a first order lowpass filter
AP_NavEKF : fixed bug in velocity rate of change filter This bug meant the velocity rate of change used to scale the GPS measurement variances was noisy and too small
2014-02-23 03:21:42 -04:00
velDotNEDfilt = velDotNED * 0.05f + velDotNEDfilt * 0.95f;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
// calculate a magnitude of the filtered nav acceleration (required for GPS
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// variance estimation)
AP_NavEKF : fixed bug in velocity rate of change filter This bug meant the velocity rate of change used to scale the GPS measurement variances was noisy and too small
2014-02-23 03:21:42 -04:00
accNavMag = velDotNEDfilt.length();
AP_NavEKF : increased useage of helper functions
2014-03-10 06:08:15 -03:00
accNavMagHoriz = pythagorous2(velDotNEDfilt.x , velDotNEDfilt.y);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// save velocity for use in trapezoidal intergration for position calcuation
AP_NavEKF: allow states to be accessed using names as well as a Vector22
2014-02-19 22:21:09 -04:00
Vector3f lastVelocity = state.velocity;
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
Vector3f lastVel1 = state.vel1;
Vector3f lastVel2 = state.vel2;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// sum delta velocities to get velocity
AP_NavEKF: allow states to be accessed using names as well as a Vector22
2014-02-19 22:21:09 -04:00
state.velocity += delVelNav;
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
state.vel1 += delVelNav1;
state.vel2 += delVelNav2;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// apply a trapezoidal integration to velocities to calculate position
AP_NavEKF: allow states to be accessed using names as well as a Vector22
2014-02-19 22:21:09 -04:00
state.position += (state.velocity + lastVelocity) * (dtIMU*0.5f);
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
state.posD1 += (state.vel1.z + lastVel1.z) * (dtIMU*0.5f);
state.posD2 += (state.vel2.z + lastVel2.z) * (dtIMU*0.5f);
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// limit states to protect against divergence
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
ConstrainStates();
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate the predicted state covariance matrix
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
void NavEKF::CovariancePrediction()
{
AP_NavEKF: added perf counters on PX4
2013-12-30 06:41:28 -04:00
perf_begin(_perf_CovariancePrediction);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
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 daxCov; // X axis delta angle variance rad^2
float dayCov; // Y axis delta angle variance rad^2
float dazCov; // Z axis delta angle variance rad^2
float dvxCov; // X axis delta velocity variance (m/s)^2
float dvyCov; // Y axis delta velocity variance (m/s)^2
float dvzCov; // Z axis delta velocity variance (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 dvz_b; // Z axis delta velocity measurement bias (rad)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// calculate covariance prediction process noise
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
// use filtered height rate to increase wind process noise when climbing or descending
// this allows for wind gradient effects.
AP_NavEKF: Added height rate adaptive wind speed process noise
2014-01-01 17:12:06 -04:00
// filter height rate using a 10 second time constant filter
float alpha = 0.1f * dt;
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
hgtRate = hgtRate * (1.0f - alpha) - state.velocity.z * alpha;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF: Added height rate adaptive wind speed process noise
2014-01-01 17:12:06 -04:00
// use filtered height rate to increase wind process noise when climbing or descending
// this allows for wind gradient effects.
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (!inhibitWindStates) {
windVelSigma = dt * constrain_float(_windVelProcessNoise, 0.01f, 1.0f) * (1.0f + constrain_float(_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate));
} else {
windVelSigma = 0.0f;
}
AP_NavEKF: Added constraints to Mavlink tuneable parameters
2014-01-22 02:32:28 -04:00
dAngBiasSigma = dt * constrain_float(_gyroBiasProcessNoise, 1e-7f, 1e-5f);
AP_NavEKF: Add protection for accel bias estimation errors Don't do bias estimation if tilted by more than 60 degrees to prevent scale factor errors affecting result unnecessarily. Prevent Kalman gain from having the wrong sign due to numerical errors associated with small process noise values. Allow smaller Z accel bias process noise values to be set
2014-05-17 10:30:27 -03:00
dVelBiasSigma = dt * constrain_float(_accelBiasProcessNoise, 1e-5f, 1e-3f);
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (!inhibitMagStates) {
magEarthSigma = dt * constrain_float(_magEarthProcessNoise, 1e-4f, 1e-2f);
magBodySigma = dt * constrain_float(_magBodyProcessNoise, 1e-4f, 1e-2f);
} else {
magEarthSigma = 0.0f;
magBodySigma = 0.0f;
}
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f;
for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma;
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// scale gyro bias noise when in static mode to allow for faster bias estimation
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t i=10; i<=12; i++) {
processNoise[i] = dAngBiasSigma;
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (staticMode) {
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
processNoise[i] *= _gyroBiasNoiseScaler;
}
}
processNoise[13] = dVelBiasSigma;
for (uint8_t i=14; i<=15; i++) processNoise[i] = windVelSigma;
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= 0; i<=21; i++) processNoise[i] = sq(processNoise[i]);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// set variables used to calculate covariance growth
dvx = summedDelVel.x;
dvy = summedDelVel.y;
dvz = summedDelVel.z;
dax = summedDelAng.x;
day = summedDelAng.y;
daz = summedDelAng.z;
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
q0 = state.quat[0];
q1 = state.quat[1];
q2 = state.quat[2];
q3 = state.quat[3];
dax_b = state.gyro_bias.x;
day_b = state.gyro_bias.y;
daz_b = state.gyro_bias.z;
dvz_b = IMU1_weighting * state.accel_zbias1 + (1.0f - IMU1_weighting) * state.accel_zbias2;
AP_NavEKF: Added constraints to Mavlink tuneable parameters
2014-01-22 02:32:28 -04:00
_gyrNoise = constrain_float(_gyrNoise, 1e-3f, 5e-2f);
AP_NavEKF: Amended tuning parameter list, and changed from variance to noise definitions
2014-01-05 02:28:21 -04:00
daxCov = sq(dt*_gyrNoise);
dayCov = sq(dt*_gyrNoise);
dazCov = sq(dt*_gyrNoise);
AP_NavEKF: Added constraints to Mavlink tuneable parameters
2014-01-22 02:32:28 -04:00
_accNoise = constrain_float(_accNoise, 5e-2f, 1.0f);
AP_NavEKF: Amended tuning parameter list, and changed from variance to noise definitions
2014-01-05 02:28:21 -04:00
dvxCov = sq(dt*_accNoise);
dvyCov = sq(dt*_accNoise);
dvzCov = sq(dt*_accNoise);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate the predicted covariance due to inertial sensor error propagation
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
SF[0] = dvz - dvz_b;
SF[1] = 2*q3*SF[0] + 2*dvx*q1 + 2*dvy*q2;
SF[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0;
SF[3] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3;
SF[4] = day/2 - day_b/2;
SF[5] = daz/2 - daz_b/2;
SF[6] = dax/2 - dax_b/2;
SF[7] = dax_b/2 - dax/2;
SF[8] = daz_b/2 - daz/2;
SF[9] = day_b/2 - day/2;
SF[10] = 2*q0*SF[0];
SF[11] = q1/2;
SF[12] = q2/2;
SF[13] = q3/2;
SF[14] = 2*dvy*q1;
SG[0] = q0/2;
SG[1] = sq(q3);
SG[2] = sq(q2);
SG[3] = sq(q1);
SG[4] = sq(q0);
SG[5] = 2*q2*q3;
SG[6] = 2*q1*q3;
SG[7] = 2*q1*q2;
SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3);
SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3);
SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4;
SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4;
SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4;
SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4;
SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2;
SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4;
SQ[9] = sq(SG[0]);
SQ[10] = sq(q1);
SPP[0] = SF[10] + SF[14] - 2*dvx*q2;
SPP[1] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3;
SPP[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0;
SPP[3] = 2*q0*q1 - 2*q2*q3;
SPP[4] = 2*q0*q2 + 2*q1*q3;
SPP[5] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
SPP[6] = SF[13];
SPP[7] = SF[12];
nextP[0][0] = P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6] + (daxCov*SQ[10])/4 + SF[7]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[9]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[8]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) + SPP[7]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[6]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4;
nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6] + SF[6]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[5]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[9]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[6]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) - SPP[7]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - (q0*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]))/2;
nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6] + SF[4]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[8]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[6]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - SPP[6]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]))/2;
nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6] + SF[5]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[4]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[7]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SF[11]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[7]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]))/2;
nextP[0][4] = P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6] + SF[3]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[0]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SPP[2]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[4]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
nextP[0][5] = P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6] + SF[2]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[3]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[0]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[3]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
nextP[0][6] = P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6] + SF[2]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[1]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[0]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) - SPP[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
nextP[0][7] = P[0][7] + P[1][7]*SF[7] + P[2][7]*SF[9] + P[3][7]*SF[8] + P[10][7]*SF[11] + P[11][7]*SPP[7] + P[12][7]*SPP[6] + dt*(P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6]);
nextP[0][8] = P[0][8] + P[1][8]*SF[7] + P[2][8]*SF[9] + P[3][8]*SF[8] + P[10][8]*SF[11] + P[11][8]*SPP[7] + P[12][8]*SPP[6] + dt*(P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6]);
nextP[0][9] = P[0][9] + P[1][9]*SF[7] + P[2][9]*SF[9] + P[3][9]*SF[8] + P[10][9]*SF[11] + P[11][9]*SPP[7] + P[12][9]*SPP[6] + dt*(P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6]);
nextP[0][10] = P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6];
nextP[0][11] = P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6];
nextP[0][12] = P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6];
nextP[0][13] = P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6];
nextP[0][14] = P[0][14] + P[1][14]*SF[7] + P[2][14]*SF[9] + P[3][14]*SF[8] + P[10][14]*SF[11] + P[11][14]*SPP[7] + P[12][14]*SPP[6];
nextP[0][15] = P[0][15] + P[1][15]*SF[7] + P[2][15]*SF[9] + P[3][15]*SF[8] + P[10][15]*SF[11] + P[11][15]*SPP[7] + P[12][15]*SPP[6];
nextP[0][16] = P[0][16] + P[1][16]*SF[7] + P[2][16]*SF[9] + P[3][16]*SF[8] + P[10][16]*SF[11] + P[11][16]*SPP[7] + P[12][16]*SPP[6];
nextP[0][17] = P[0][17] + P[1][17]*SF[7] + P[2][17]*SF[9] + P[3][17]*SF[8] + P[10][17]*SF[11] + P[11][17]*SPP[7] + P[12][17]*SPP[6];
nextP[0][18] = P[0][18] + P[1][18]*SF[7] + P[2][18]*SF[9] + P[3][18]*SF[8] + P[10][18]*SF[11] + P[11][18]*SPP[7] + P[12][18]*SPP[6];
nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6];
nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6];
nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6];
nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2 + SF[7]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) + SPP[7]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[6]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2);
nextP[1][1] = P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[5]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[9]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[6]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) - SPP[7]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2))/2;
nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[8]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[6]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) - SPP[6]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2))/2;
nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[4]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SF[11]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[7]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2))/2;
nextP[1][4] = P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[4]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2);
nextP[1][5] = P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2);
nextP[1][6] = P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2 + SF[2]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[1]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2);
nextP[1][7] = P[1][7] + P[0][7]*SF[6] + P[2][7]*SF[5] + P[3][7]*SF[9] + P[11][7]*SPP[6] - P[12][7]*SPP[7] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2);
nextP[1][8] = P[1][8] + P[0][8]*SF[6] + P[2][8]*SF[5] + P[3][8]*SF[9] + P[11][8]*SPP[6] - P[12][8]*SPP[7] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2);
nextP[1][9] = P[1][9] + P[0][9]*SF[6] + P[2][9]*SF[5] + P[3][9]*SF[9] + P[11][9]*SPP[6] - P[12][9]*SPP[7] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2);
nextP[1][10] = P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2;
nextP[1][11] = P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2;
nextP[1][12] = P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2;
nextP[1][13] = P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2;
nextP[1][14] = P[1][14] + P[0][14]*SF[6] + P[2][14]*SF[5] + P[3][14]*SF[9] + P[11][14]*SPP[6] - P[12][14]*SPP[7] - (P[10][14]*q0)/2;
nextP[1][15] = P[1][15] + P[0][15]*SF[6] + P[2][15]*SF[5] + P[3][15]*SF[9] + P[11][15]*SPP[6] - P[12][15]*SPP[7] - (P[10][15]*q0)/2;
nextP[1][16] = P[1][16] + P[0][16]*SF[6] + P[2][16]*SF[5] + P[3][16]*SF[9] + P[11][16]*SPP[6] - P[12][16]*SPP[7] - (P[10][16]*q0)/2;
nextP[1][17] = P[1][17] + P[0][17]*SF[6] + P[2][17]*SF[5] + P[3][17]*SF[9] + P[11][17]*SPP[6] - P[12][17]*SPP[7] - (P[10][17]*q0)/2;
nextP[1][18] = P[1][18] + P[0][18]*SF[6] + P[2][18]*SF[5] + P[3][18]*SF[9] + P[11][18]*SPP[6] - P[12][18]*SPP[7] - (P[10][18]*q0)/2;
nextP[1][19] = P[1][19] + P[0][19]*SF[6] + P[2][19]*SF[5] + P[3][19]*SF[9] + P[11][19]*SPP[6] - P[12][19]*SPP[7] - (P[10][19]*q0)/2;
nextP[1][20] = P[1][20] + P[0][20]*SF[6] + P[2][20]*SF[5] + P[3][20]*SF[9] + P[11][20]*SPP[6] - P[12][20]*SPP[7] - (P[10][20]*q0)/2;
nextP[1][21] = P[1][21] + P[0][21]*SF[6] + P[2][21]*SF[5] + P[3][21]*SF[9] + P[11][21]*SPP[6] - P[12][21]*SPP[7] - (P[10][21]*q0)/2;
nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2 + SF[7]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + SPP[7]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[6]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2);
nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[5]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[9]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[6]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) - SPP[7]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2))/2;
nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[8]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[6]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - SPP[6]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2))/2;
nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[4]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[7]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SF[11]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[7]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2))/2;
nextP[2][4] = P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[4]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2);
nextP[2][5] = P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2);
nextP[2][6] = P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2 + SF[2]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[1]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2);
nextP[2][7] = P[2][7] + P[0][7]*SF[4] + P[1][7]*SF[8] + P[3][7]*SF[6] + P[12][7]*SF[11] - P[10][7]*SPP[6] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2);
nextP[2][8] = P[2][8] + P[0][8]*SF[4] + P[1][8]*SF[8] + P[3][8]*SF[6] + P[12][8]*SF[11] - P[10][8]*SPP[6] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2);
nextP[2][9] = P[2][9] + P[0][9]*SF[4] + P[1][9]*SF[8] + P[3][9]*SF[6] + P[12][9]*SF[11] - P[10][9]*SPP[6] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2);
nextP[2][10] = P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2;
nextP[2][11] = P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2;
nextP[2][12] = P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2;
nextP[2][13] = P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2;
nextP[2][14] = P[2][14] + P[0][14]*SF[4] + P[1][14]*SF[8] + P[3][14]*SF[6] + P[12][14]*SF[11] - P[10][14]*SPP[6] - (P[11][14]*q0)/2;
nextP[2][15] = P[2][15] + P[0][15]*SF[4] + P[1][15]*SF[8] + P[3][15]*SF[6] + P[12][15]*SF[11] - P[10][15]*SPP[6] - (P[11][15]*q0)/2;
nextP[2][16] = P[2][16] + P[0][16]*SF[4] + P[1][16]*SF[8] + P[3][16]*SF[6] + P[12][16]*SF[11] - P[10][16]*SPP[6] - (P[11][16]*q0)/2;
nextP[2][17] = P[2][17] + P[0][17]*SF[4] + P[1][17]*SF[8] + P[3][17]*SF[6] + P[12][17]*SF[11] - P[10][17]*SPP[6] - (P[11][17]*q0)/2;
nextP[2][18] = P[2][18] + P[0][18]*SF[4] + P[1][18]*SF[8] + P[3][18]*SF[6] + P[12][18]*SF[11] - P[10][18]*SPP[6] - (P[11][18]*q0)/2;
nextP[2][19] = P[2][19] + P[0][19]*SF[4] + P[1][19]*SF[8] + P[3][19]*SF[6] + P[12][19]*SF[11] - P[10][19]*SPP[6] - (P[11][19]*q0)/2;
nextP[2][20] = P[2][20] + P[0][20]*SF[4] + P[1][20]*SF[8] + P[3][20]*SF[6] + P[12][20]*SF[11] - P[10][20]*SPP[6] - (P[11][20]*q0)/2;
nextP[2][21] = P[2][21] + P[0][21]*SF[4] + P[1][21]*SF[8] + P[3][21]*SF[6] + P[12][21]*SF[11] - P[10][21]*SPP[6] - (P[11][21]*q0)/2;
nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2 + SF[7]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[8]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + SPP[7]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[6]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2);
nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2 + SF[6]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[5]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[9]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[6]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) - SPP[7]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2))/2;
nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[8]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[6]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - SPP[6]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2))/2;
nextP[3][3] = P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[4]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[7]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SF[11]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[7]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2))/2;
nextP[3][4] = P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[4]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2);
nextP[3][5] = P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2);
nextP[3][6] = P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2 + SF[2]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[1]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2);
nextP[3][7] = P[3][7] + P[0][7]*SF[5] + P[1][7]*SF[4] + P[2][7]*SF[7] - P[11][7]*SF[11] + P[10][7]*SPP[7] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2);
nextP[3][8] = P[3][8] + P[0][8]*SF[5] + P[1][8]*SF[4] + P[2][8]*SF[7] - P[11][8]*SF[11] + P[10][8]*SPP[7] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2);
nextP[3][9] = P[3][9] + P[0][9]*SF[5] + P[1][9]*SF[4] + P[2][9]*SF[7] - P[11][9]*SF[11] + P[10][9]*SPP[7] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2);
nextP[3][10] = P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2;
nextP[3][11] = P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2;
nextP[3][12] = P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2;
nextP[3][13] = P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2;
nextP[3][14] = P[3][14] + P[0][14]*SF[5] + P[1][14]*SF[4] + P[2][14]*SF[7] - P[11][14]*SF[11] + P[10][14]*SPP[7] - (P[12][14]*q0)/2;
nextP[3][15] = P[3][15] + P[0][15]*SF[5] + P[1][15]*SF[4] + P[2][15]*SF[7] - P[11][15]*SF[11] + P[10][15]*SPP[7] - (P[12][15]*q0)/2;
nextP[3][16] = P[3][16] + P[0][16]*SF[5] + P[1][16]*SF[4] + P[2][16]*SF[7] - P[11][16]*SF[11] + P[10][16]*SPP[7] - (P[12][16]*q0)/2;
nextP[3][17] = P[3][17] + P[0][17]*SF[5] + P[1][17]*SF[4] + P[2][17]*SF[7] - P[11][17]*SF[11] + P[10][17]*SPP[7] - (P[12][17]*q0)/2;
nextP[3][18] = P[3][18] + P[0][18]*SF[5] + P[1][18]*SF[4] + P[2][18]*SF[7] - P[11][18]*SF[11] + P[10][18]*SPP[7] - (P[12][18]*q0)/2;
nextP[3][19] = P[3][19] + P[0][19]*SF[5] + P[1][19]*SF[4] + P[2][19]*SF[7] - P[11][19]*SF[11] + P[10][19]*SPP[7] - (P[12][19]*q0)/2;
nextP[3][20] = P[3][20] + P[0][20]*SF[5] + P[1][20]*SF[4] + P[2][20]*SF[7] - P[11][20]*SF[11] + P[10][20]*SPP[7] - (P[12][20]*q0)/2;
nextP[3][21] = P[3][21] + P[0][21]*SF[5] + P[1][21]*SF[4] + P[2][21]*SF[7] - P[11][21]*SF[11] + P[10][21]*SPP[7] - (P[12][21]*q0)/2;
nextP[4][0] = P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4] + SF[7]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[9]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[8]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) + SPP[7]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[6]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]);
nextP[4][1] = P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4] + SF[6]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[5]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[9]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[6]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) - SPP[7]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - (q0*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]))/2;
nextP[4][2] = P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4] + SF[4]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[8]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[6]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - SPP[6]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]))/2;
nextP[4][3] = P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4] + SF[5]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[4]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[7]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SF[11]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[7]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]))/2;
nextP[4][4] = P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[3]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[0]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SPP[2]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[4]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]);
nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4] + SF[2]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[3]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[0]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[3]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]);
nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4] + SF[2]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[1]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[0]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) - SPP[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]);
nextP[4][7] = P[4][7] + P[0][7]*SF[3] + P[1][7]*SF[1] + P[2][7]*SPP[0] - P[3][7]*SPP[2] - P[13][7]*SPP[4] + dt*(P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4]);
nextP[4][8] = P[4][8] + P[0][8]*SF[3] + P[1][8]*SF[1] + P[2][8]*SPP[0] - P[3][8]*SPP[2] - P[13][8]*SPP[4] + dt*(P[4][5] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4]);
nextP[4][9] = P[4][9] + P[0][9]*SF[3] + P[1][9]*SF[1] + P[2][9]*SPP[0] - P[3][9]*SPP[2] - P[13][9]*SPP[4] + dt*(P[4][6] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4]);
nextP[4][10] = P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4];
nextP[4][11] = P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4];
nextP[4][12] = P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4];
nextP[4][13] = P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4];
nextP[4][14] = P[4][14] + P[0][14]*SF[3] + P[1][14]*SF[1] + P[2][14]*SPP[0] - P[3][14]*SPP[2] - P[13][14]*SPP[4];
nextP[4][15] = P[4][15] + P[0][15]*SF[3] + P[1][15]*SF[1] + P[2][15]*SPP[0] - P[3][15]*SPP[2] - P[13][15]*SPP[4];
nextP[4][16] = P[4][16] + P[0][16]*SF[3] + P[1][16]*SF[1] + P[2][16]*SPP[0] - P[3][16]*SPP[2] - P[13][16]*SPP[4];
nextP[4][17] = P[4][17] + P[0][17]*SF[3] + P[1][17]*SF[1] + P[2][17]*SPP[0] - P[3][17]*SPP[2] - P[13][17]*SPP[4];
nextP[4][18] = P[4][18] + P[0][18]*SF[3] + P[1][18]*SF[1] + P[2][18]*SPP[0] - P[3][18]*SPP[2] - P[13][18]*SPP[4];
nextP[4][19] = P[4][19] + P[0][19]*SF[3] + P[1][19]*SF[1] + P[2][19]*SPP[0] - P[3][19]*SPP[2] - P[13][19]*SPP[4];
nextP[4][20] = P[4][20] + P[0][20]*SF[3] + P[1][20]*SF[1] + P[2][20]*SPP[0] - P[3][20]*SPP[2] - P[13][20]*SPP[4];
nextP[4][21] = P[4][21] + P[0][21]*SF[3] + P[1][21]*SF[1] + P[2][21]*SPP[0] - P[3][21]*SPP[2] - P[13][21]*SPP[4];
nextP[5][0] = P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3] + SF[7]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[9]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[8]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) + SPP[7]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[6]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]);
nextP[5][1] = P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3] + SF[6]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[5]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[9]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[6]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) - SPP[7]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - (q0*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]))/2;
nextP[5][2] = P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3] + SF[4]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[8]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[6]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - SPP[6]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]))/2;
nextP[5][3] = P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3] + SF[5]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[4]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[7]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SF[11]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[7]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]))/2;
nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3] + SF[3]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[0]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SPP[2]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[4]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]);
nextP[5][5] = P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[2]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[3]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[0]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[3]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]);
nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3] + SF[2]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[1]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[0]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) - SPP[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]);
nextP[5][7] = P[5][7] + P[0][7]*SF[2] + P[2][7]*SF[1] + P[3][7]*SF[3] - P[1][7]*SPP[0] + P[13][7]*SPP[3] + dt*(P[5][4] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3]);
nextP[5][8] = P[5][8] + P[0][8]*SF[2] + P[2][8]*SF[1] + P[3][8]*SF[3] - P[1][8]*SPP[0] + P[13][8]*SPP[3] + dt*(P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3]);
nextP[5][9] = P[5][9] + P[0][9]*SF[2] + P[2][9]*SF[1] + P[3][9]*SF[3] - P[1][9]*SPP[0] + P[13][9]*SPP[3] + dt*(P[5][6] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3]);
nextP[5][10] = P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3];
nextP[5][11] = P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3];
nextP[5][12] = P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3];
nextP[5][13] = P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3];
nextP[5][14] = P[5][14] + P[0][14]*SF[2] + P[2][14]*SF[1] + P[3][14]*SF[3] - P[1][14]*SPP[0] + P[13][14]*SPP[3];
nextP[5][15] = P[5][15] + P[0][15]*SF[2] + P[2][15]*SF[1] + P[3][15]*SF[3] - P[1][15]*SPP[0] + P[13][15]*SPP[3];
nextP[5][16] = P[5][16] + P[0][16]*SF[2] + P[2][16]*SF[1] + P[3][16]*SF[3] - P[1][16]*SPP[0] + P[13][16]*SPP[3];
nextP[5][17] = P[5][17] + P[0][17]*SF[2] + P[2][17]*SF[1] + P[3][17]*SF[3] - P[1][17]*SPP[0] + P[13][17]*SPP[3];
nextP[5][18] = P[5][18] + P[0][18]*SF[2] + P[2][18]*SF[1] + P[3][18]*SF[3] - P[1][18]*SPP[0] + P[13][18]*SPP[3];
nextP[5][19] = P[5][19] + P[0][19]*SF[2] + P[2][19]*SF[1] + P[3][19]*SF[3] - P[1][19]*SPP[0] + P[13][19]*SPP[3];
nextP[5][20] = P[5][20] + P[0][20]*SF[2] + P[2][20]*SF[1] + P[3][20]*SF[3] - P[1][20]*SPP[0] + P[13][20]*SPP[3];
nextP[5][21] = P[5][21] + P[0][21]*SF[2] + P[2][21]*SF[1] + P[3][21]*SF[3] - P[1][21]*SPP[0] + P[13][21]*SPP[3];
nextP[6][0] = P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[7]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
nextP[6][1] = P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[6]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[5]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[7]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2;
nextP[6][2] = P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[4]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[6]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[6]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2;
nextP[6][3] = P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[5]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[4]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[7]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SF[11]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2;
nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[3]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[2]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[4]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[2]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[3]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[0]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[3]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
nextP[6][6] = P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) - SPP[5]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]) + SF[2]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]);
nextP[6][7] = P[6][7] + P[1][7]*SF[2] + P[3][7]*SF[1] + P[0][7]*SPP[0] - P[2][7]*SPP[1] - P[13][7]*SPP[5] + dt*(P[6][4] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*SPP[5]);
nextP[6][8] = P[6][8] + P[1][8]*SF[2] + P[3][8]*SF[1] + P[0][8]*SPP[0] - P[2][8]*SPP[1] - P[13][8]*SPP[5] + dt*(P[6][5] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*SPP[5]);
nextP[6][9] = P[6][9] + P[1][9]*SF[2] + P[3][9]*SF[1] + P[0][9]*SPP[0] - P[2][9]*SPP[1] - P[13][9]*SPP[5] + dt*(P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*SPP[5]);
nextP[6][10] = P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*SPP[5];
nextP[6][11] = P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*SPP[5];
nextP[6][12] = P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*SPP[5];
nextP[6][13] = P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5];
nextP[6][14] = P[6][14] + P[1][14]*SF[2] + P[3][14]*SF[1] + P[0][14]*SPP[0] - P[2][14]*SPP[1] - P[13][14]*SPP[5];
nextP[6][15] = P[6][15] + P[1][15]*SF[2] + P[3][15]*SF[1] + P[0][15]*SPP[0] - P[2][15]*SPP[1] - P[13][15]*SPP[5];
nextP[6][16] = P[6][16] + P[1][16]*SF[2] + P[3][16]*SF[1] + P[0][16]*SPP[0] - P[2][16]*SPP[1] - P[13][16]*SPP[5];
nextP[6][17] = P[6][17] + P[1][17]*SF[2] + P[3][17]*SF[1] + P[0][17]*SPP[0] - P[2][17]*SPP[1] - P[13][17]*SPP[5];
nextP[6][18] = P[6][18] + P[1][18]*SF[2] + P[3][18]*SF[1] + P[0][18]*SPP[0] - P[2][18]*SPP[1] - P[13][18]*SPP[5];
nextP[6][19] = P[6][19] + P[1][19]*SF[2] + P[3][19]*SF[1] + P[0][19]*SPP[0] - P[2][19]*SPP[1] - P[13][19]*SPP[5];
nextP[6][20] = P[6][20] + P[1][20]*SF[2] + P[3][20]*SF[1] + P[0][20]*SPP[0] - P[2][20]*SPP[1] - P[13][20]*SPP[5];
nextP[6][21] = P[6][21] + P[1][21]*SF[2] + P[3][21]*SF[1] + P[0][21]*SPP[0] - P[2][21]*SPP[1] - P[13][21]*SPP[5];
nextP[7][0] = P[7][0] + P[4][0]*dt + SF[7]*(P[7][1] + P[4][1]*dt) + SF[9]*(P[7][2] + P[4][2]*dt) + SF[8]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][10] + P[4][10]*dt) + SPP[7]*(P[7][11] + P[4][11]*dt) + SPP[6]*(P[7][12] + P[4][12]*dt);
nextP[7][1] = P[7][1] + P[4][1]*dt + SF[6]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][2] + P[4][2]*dt) + SF[9]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][11] + P[4][11]*dt) - SPP[7]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2;
nextP[7][2] = P[7][2] + P[4][2]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[8]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][12] + P[4][12]*dt) - SPP[6]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2;
nextP[7][3] = P[7][3] + P[4][3]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][1] + P[4][1]*dt) + SF[7]*(P[7][2] + P[4][2]*dt) - SF[11]*(P[7][11] + P[4][11]*dt) + SPP[7]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2;
nextP[7][4] = P[7][4] + P[4][4]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[3]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt) - SPP[4]*(P[7][13] + P[4][13]*dt);
nextP[7][5] = P[7][5] + P[4][5]*dt + SF[2]*(P[7][0] + P[4][0]*dt) + SF[1]*(P[7][2] + P[4][2]*dt) + SF[3]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt) + SPP[3]*(P[7][13] + P[4][13]*dt);
nextP[7][6] = P[7][6] + P[4][6]*dt + SF[2]*(P[7][1] + P[4][1]*dt) + SF[1]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt) - SPP[5]*(P[7][13] + P[4][13]*dt);
nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);
nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt);
nextP[7][10] = P[7][10] + P[4][10]*dt;
nextP[7][11] = P[7][11] + P[4][11]*dt;
nextP[7][12] = P[7][12] + P[4][12]*dt;
nextP[7][13] = P[7][13] + P[4][13]*dt;
nextP[7][14] = P[7][14] + P[4][14]*dt;
nextP[7][15] = P[7][15] + P[4][15]*dt;
nextP[7][16] = P[7][16] + P[4][16]*dt;
nextP[7][17] = P[7][17] + P[4][17]*dt;
nextP[7][18] = P[7][18] + P[4][18]*dt;
nextP[7][19] = P[7][19] + P[4][19]*dt;
nextP[7][20] = P[7][20] + P[4][20]*dt;
nextP[7][21] = P[7][21] + P[4][21]*dt;
nextP[8][0] = P[8][0] + P[5][0]*dt + SF[7]*(P[8][1] + P[5][1]*dt) + SF[9]*(P[8][2] + P[5][2]*dt) + SF[8]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][10] + P[5][10]*dt) + SPP[7]*(P[8][11] + P[5][11]*dt) + SPP[6]*(P[8][12] + P[5][12]*dt);
nextP[8][1] = P[8][1] + P[5][1]*dt + SF[6]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][2] + P[5][2]*dt) + SF[9]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][11] + P[5][11]*dt) - SPP[7]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2;
nextP[8][2] = P[8][2] + P[5][2]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[8]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][12] + P[5][12]*dt) - SPP[6]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2;
nextP[8][3] = P[8][3] + P[5][3]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][1] + P[5][1]*dt) + SF[7]*(P[8][2] + P[5][2]*dt) - SF[11]*(P[8][11] + P[5][11]*dt) + SPP[7]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2;
nextP[8][4] = P[8][4] + P[5][4]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[3]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt) - SPP[4]*(P[8][13] + P[5][13]*dt);
nextP[8][5] = P[8][5] + P[5][5]*dt + SF[2]*(P[8][0] + P[5][0]*dt) + SF[1]*(P[8][2] + P[5][2]*dt) + SF[3]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt) + SPP[3]*(P[8][13] + P[5][13]*dt);
nextP[8][6] = P[8][6] + P[5][6]*dt + SF[2]*(P[8][1] + P[5][1]*dt) + SF[1]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt) - SPP[5]*(P[8][13] + P[5][13]*dt);
nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt);
nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);
nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt);
nextP[8][10] = P[8][10] + P[5][10]*dt;
nextP[8][11] = P[8][11] + P[5][11]*dt;
nextP[8][12] = P[8][12] + P[5][12]*dt;
nextP[8][13] = P[8][13] + P[5][13]*dt;
nextP[8][14] = P[8][14] + P[5][14]*dt;
nextP[8][15] = P[8][15] + P[5][15]*dt;
nextP[8][16] = P[8][16] + P[5][16]*dt;
nextP[8][17] = P[8][17] + P[5][17]*dt;
nextP[8][18] = P[8][18] + P[5][18]*dt;
nextP[8][19] = P[8][19] + P[5][19]*dt;
nextP[8][20] = P[8][20] + P[5][20]*dt;
nextP[8][21] = P[8][21] + P[5][21]*dt;
nextP[9][0] = P[9][0] + P[6][0]*dt + SF[7]*(P[9][1] + P[6][1]*dt) + SF[9]*(P[9][2] + P[6][2]*dt) + SF[8]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][10] + P[6][10]*dt) + SPP[7]*(P[9][11] + P[6][11]*dt) + SPP[6]*(P[9][12] + P[6][12]*dt);
nextP[9][1] = P[9][1] + P[6][1]*dt + SF[6]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][2] + P[6][2]*dt) + SF[9]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][11] + P[6][11]*dt) - SPP[7]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2;
nextP[9][2] = P[9][2] + P[6][2]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[8]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][12] + P[6][12]*dt) - SPP[6]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2;
nextP[9][3] = P[9][3] + P[6][3]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][1] + P[6][1]*dt) + SF[7]*(P[9][2] + P[6][2]*dt) - SF[11]*(P[9][11] + P[6][11]*dt) + SPP[7]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2;
nextP[9][4] = P[9][4] + P[6][4]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[3]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt) - SPP[4]*(P[9][13] + P[6][13]*dt);
nextP[9][5] = P[9][5] + P[6][5]*dt + SF[2]*(P[9][0] + P[6][0]*dt) + SF[1]*(P[9][2] + P[6][2]*dt) + SF[3]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt) + SPP[3]*(P[9][13] + P[6][13]*dt);
nextP[9][6] = P[9][6] + P[6][6]*dt + SF[2]*(P[9][1] + P[6][1]*dt) + SF[1]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt) - SPP[5]*(P[9][13] + P[6][13]*dt);
nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt);
nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt);
nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt);
nextP[9][10] = P[9][10] + P[6][10]*dt;
nextP[9][11] = P[9][11] + P[6][11]*dt;
nextP[9][12] = P[9][12] + P[6][12]*dt;
nextP[9][13] = P[9][13] + P[6][13]*dt;
nextP[9][14] = P[9][14] + P[6][14]*dt;
nextP[9][15] = P[9][15] + P[6][15]*dt;
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
nextP[9][16] = P[9][16] + P[6][16]*dt;
nextP[9][17] = P[9][17] + P[6][17]*dt;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
nextP[9][18] = P[9][18] + P[6][18]*dt;
nextP[9][19] = P[9][19] + P[6][19]*dt;
nextP[9][20] = P[9][20] + P[6][20]*dt;
nextP[9][21] = P[9][21] + P[6][21]*dt;
nextP[10][0] = P[10][0] + P[10][1]*SF[7] + P[10][2]*SF[9] + P[10][3]*SF[8] + P[10][10]*SF[11] + P[10][11]*SPP[7] + P[10][12]*SPP[6];
nextP[10][1] = P[10][1] + P[10][0]*SF[6] + P[10][2]*SF[5] + P[10][3]*SF[9] + P[10][11]*SPP[6] - P[10][12]*SPP[7] - (P[10][10]*q0)/2;
nextP[10][2] = P[10][2] + P[10][0]*SF[4] + P[10][1]*SF[8] + P[10][3]*SF[6] + P[10][12]*SF[11] - P[10][10]*SPP[6] - (P[10][11]*q0)/2;
nextP[10][3] = P[10][3] + P[10][0]*SF[5] + P[10][1]*SF[4] + P[10][2]*SF[7] - P[10][11]*SF[11] + P[10][10]*SPP[7] - (P[10][12]*q0)/2;
nextP[10][4] = P[10][4] + P[10][1]*SF[1] + P[10][0]*SF[3] + P[10][2]*SPP[0] - P[10][3]*SPP[2] - P[10][13]*SPP[4];
nextP[10][5] = P[10][5] + P[10][0]*SF[2] + P[10][2]*SF[1] + P[10][3]*SF[3] - P[10][1]*SPP[0] + P[10][13]*SPP[3];
nextP[10][6] = P[10][6] + P[10][1]*SF[2] + P[10][3]*SF[1] + P[10][0]*SPP[0] - P[10][2]*SPP[1] - P[10][13]*SPP[5];
nextP[10][7] = P[10][7] + P[10][4]*dt;
nextP[10][8] = P[10][8] + P[10][5]*dt;
nextP[10][9] = P[10][9] + P[10][6]*dt;
nextP[10][10] = P[10][10];
nextP[10][11] = P[10][11];
nextP[10][12] = P[10][12];
nextP[10][13] = P[10][13];
nextP[10][14] = P[10][14];
nextP[10][15] = P[10][15];
nextP[10][16] = P[10][16];
nextP[10][17] = P[10][17];
nextP[10][18] = P[10][18];
nextP[10][19] = P[10][19];
nextP[10][20] = P[10][20];
nextP[10][21] = P[10][21];
nextP[11][0] = P[11][0] + P[11][1]*SF[7] + P[11][2]*SF[9] + P[11][3]*SF[8] + P[11][10]*SF[11] + P[11][11]*SPP[7] + P[11][12]*SPP[6];
nextP[11][1] = P[11][1] + P[11][0]*SF[6] + P[11][2]*SF[5] + P[11][3]*SF[9] + P[11][11]*SPP[6] - P[11][12]*SPP[7] - (P[11][10]*q0)/2;
nextP[11][2] = P[11][2] + P[11][0]*SF[4] + P[11][1]*SF[8] + P[11][3]*SF[6] + P[11][12]*SF[11] - P[11][10]*SPP[6] - (P[11][11]*q0)/2;
nextP[11][3] = P[11][3] + P[11][0]*SF[5] + P[11][1]*SF[4] + P[11][2]*SF[7] - P[11][11]*SF[11] + P[11][10]*SPP[7] - (P[11][12]*q0)/2;
nextP[11][4] = P[11][4] + P[11][1]*SF[1] + P[11][0]*SF[3] + P[11][2]*SPP[0] - P[11][3]*SPP[2] - P[11][13]*SPP[4];
nextP[11][5] = P[11][5] + P[11][0]*SF[2] + P[11][2]*SF[1] + P[11][3]*SF[3] - P[11][1]*SPP[0] + P[11][13]*SPP[3];
nextP[11][6] = P[11][6] + P[11][1]*SF[2] + P[11][3]*SF[1] + P[11][0]*SPP[0] - P[11][2]*SPP[1] - P[11][13]*SPP[5];
nextP[11][7] = P[11][7] + P[11][4]*dt;
nextP[11][8] = P[11][8] + P[11][5]*dt;
nextP[11][9] = P[11][9] + P[11][6]*dt;
nextP[11][10] = P[11][10];
nextP[11][11] = P[11][11];
nextP[11][12] = P[11][12];
nextP[11][13] = P[11][13];
nextP[11][14] = P[11][14];
nextP[11][15] = P[11][15];
nextP[11][16] = P[11][16];
nextP[11][17] = P[11][17];
nextP[11][18] = P[11][18];
nextP[11][19] = P[11][19];
nextP[11][20] = P[11][20];
nextP[11][21] = P[11][21];
nextP[12][0] = P[12][0] + P[12][1]*SF[7] + P[12][2]*SF[9] + P[12][3]*SF[8] + P[12][10]*SF[11] + P[12][11]*SPP[7] + P[12][12]*SPP[6];
nextP[12][1] = P[12][1] + P[12][0]*SF[6] + P[12][2]*SF[5] + P[12][3]*SF[9] + P[12][11]*SPP[6] - P[12][12]*SPP[7] - (P[12][10]*q0)/2;
nextP[12][2] = P[12][2] + P[12][0]*SF[4] + P[12][1]*SF[8] + P[12][3]*SF[6] + P[12][12]*SF[11] - P[12][10]*SPP[6] - (P[12][11]*q0)/2;
nextP[12][3] = P[12][3] + P[12][0]*SF[5] + P[12][1]*SF[4] + P[12][2]*SF[7] - P[12][11]*SF[11] + P[12][10]*SPP[7] - (P[12][12]*q0)/2;
nextP[12][4] = P[12][4] + P[12][1]*SF[1] + P[12][0]*SF[3] + P[12][2]*SPP[0] - P[12][3]*SPP[2] - P[12][13]*SPP[4];
nextP[12][5] = P[12][5] + P[12][0]*SF[2] + P[12][2]*SF[1] + P[12][3]*SF[3] - P[12][1]*SPP[0] + P[12][13]*SPP[3];
nextP[12][6] = P[12][6] + P[12][1]*SF[2] + P[12][3]*SF[1] + P[12][0]*SPP[0] - P[12][2]*SPP[1] - P[12][13]*SPP[5];
nextP[12][7] = P[12][7] + P[12][4]*dt;
nextP[12][8] = P[12][8] + P[12][5]*dt;
nextP[12][9] = P[12][9] + P[12][6]*dt;
nextP[12][10] = P[12][10];
nextP[12][11] = P[12][11];
nextP[12][12] = P[12][12];
nextP[12][13] = P[12][13];
nextP[12][14] = P[12][14];
nextP[12][15] = P[12][15];
nextP[12][16] = P[12][16];
nextP[12][17] = P[12][17];
nextP[12][18] = P[12][18];
nextP[12][19] = P[12][19];
nextP[12][20] = P[12][20];
nextP[12][21] = P[12][21];
nextP[13][0] = P[13][0] + P[13][1]*SF[7] + P[13][2]*SF[9] + P[13][3]*SF[8] + P[13][10]*SF[11] + P[13][11]*SPP[7] + P[13][12]*SPP[6];
nextP[13][1] = P[13][1] + P[13][0]*SF[6] + P[13][2]*SF[5] + P[13][3]*SF[9] + P[13][11]*SPP[6] - P[13][12]*SPP[7] - (P[13][10]*q0)/2;
nextP[13][2] = P[13][2] + P[13][0]*SF[4] + P[13][1]*SF[8] + P[13][3]*SF[6] + P[13][12]*SF[11] - P[13][10]*SPP[6] - (P[13][11]*q0)/2;
nextP[13][3] = P[13][3] + P[13][0]*SF[5] + P[13][1]*SF[4] + P[13][2]*SF[7] - P[13][11]*SF[11] + P[13][10]*SPP[7] - (P[13][12]*q0)/2;
nextP[13][4] = P[13][4] + P[13][1]*SF[1] + P[13][0]*SF[3] + P[13][2]*SPP[0] - P[13][3]*SPP[2] - P[13][13]*SPP[4];
nextP[13][5] = P[13][5] + P[13][0]*SF[2] + P[13][2]*SF[1] + P[13][3]*SF[3] - P[13][1]*SPP[0] + P[13][13]*SPP[3];
nextP[13][6] = P[13][6] + P[13][1]*SF[2] + P[13][3]*SF[1] + P[13][0]*SPP[0] - P[13][2]*SPP[1] - P[13][13]*SPP[5];
nextP[13][7] = P[13][7] + P[13][4]*dt;
nextP[13][8] = P[13][8] + P[13][5]*dt;
nextP[13][9] = P[13][9] + P[13][6]*dt;
nextP[13][10] = P[13][10];
nextP[13][11] = P[13][11];
nextP[13][12] = P[13][12];
nextP[13][13] = P[13][13];
nextP[13][14] = P[13][14];
nextP[13][15] = P[13][15];
nextP[13][16] = P[13][16];
nextP[13][17] = P[13][17];
nextP[13][18] = P[13][18];
nextP[13][19] = P[13][19];
nextP[13][20] = P[13][20];
nextP[13][21] = P[13][21];
nextP[14][0] = P[14][0] + P[14][1]*SF[7] + P[14][2]*SF[9] + P[14][3]*SF[8] + P[14][10]*SF[11] + P[14][11]*SPP[7] + P[14][12]*SPP[6];
nextP[14][1] = P[14][1] + P[14][0]*SF[6] + P[14][2]*SF[5] + P[14][3]*SF[9] + P[14][11]*SPP[6] - P[14][12]*SPP[7] - (P[14][10]*q0)/2;
nextP[14][2] = P[14][2] + P[14][0]*SF[4] + P[14][1]*SF[8] + P[14][3]*SF[6] + P[14][12]*SF[11] - P[14][10]*SPP[6] - (P[14][11]*q0)/2;
nextP[14][3] = P[14][3] + P[14][0]*SF[5] + P[14][1]*SF[4] + P[14][2]*SF[7] - P[14][11]*SF[11] + P[14][10]*SPP[7] - (P[14][12]*q0)/2;
nextP[14][4] = P[14][4] + P[14][1]*SF[1] + P[14][0]*SF[3] + P[14][2]*SPP[0] - P[14][3]*SPP[2] - P[14][13]*SPP[4];
nextP[14][5] = P[14][5] + P[14][0]*SF[2] + P[14][2]*SF[1] + P[14][3]*SF[3] - P[14][1]*SPP[0] + P[14][13]*SPP[3];
nextP[14][6] = P[14][6] + P[14][1]*SF[2] + P[14][3]*SF[1] + P[14][0]*SPP[0] - P[14][2]*SPP[1] - P[14][13]*SPP[5];
nextP[14][7] = P[14][7] + P[14][4]*dt;
nextP[14][8] = P[14][8] + P[14][5]*dt;
nextP[14][9] = P[14][9] + P[14][6]*dt;
nextP[14][10] = P[14][10];
nextP[14][11] = P[14][11];
nextP[14][12] = P[14][12];
nextP[14][13] = P[14][13];
nextP[14][14] = P[14][14];
nextP[14][15] = P[14][15];
nextP[14][16] = P[14][16];
nextP[14][17] = P[14][17];
nextP[14][18] = P[14][18];
nextP[14][19] = P[14][19];
nextP[14][20] = P[14][20];
nextP[14][21] = P[14][21];
nextP[15][0] = P[15][0] + P[15][1]*SF[7] + P[15][2]*SF[9] + P[15][3]*SF[8] + P[15][10]*SF[11] + P[15][11]*SPP[7] + P[15][12]*SPP[6];
nextP[15][1] = P[15][1] + P[15][0]*SF[6] + P[15][2]*SF[5] + P[15][3]*SF[9] + P[15][11]*SPP[6] - P[15][12]*SPP[7] - (P[15][10]*q0)/2;
nextP[15][2] = P[15][2] + P[15][0]*SF[4] + P[15][1]*SF[8] + P[15][3]*SF[6] + P[15][12]*SF[11] - P[15][10]*SPP[6] - (P[15][11]*q0)/2;
nextP[15][3] = P[15][3] + P[15][0]*SF[5] + P[15][1]*SF[4] + P[15][2]*SF[7] - P[15][11]*SF[11] + P[15][10]*SPP[7] - (P[15][12]*q0)/2;
nextP[15][4] = P[15][4] + P[15][1]*SF[1] + P[15][0]*SF[3] + P[15][2]*SPP[0] - P[15][3]*SPP[2] - P[15][13]*SPP[4];
nextP[15][5] = P[15][5] + P[15][0]*SF[2] + P[15][2]*SF[1] + P[15][3]*SF[3] - P[15][1]*SPP[0] + P[15][13]*SPP[3];
nextP[15][6] = P[15][6] + P[15][1]*SF[2] + P[15][3]*SF[1] + P[15][0]*SPP[0] - P[15][2]*SPP[1] - P[15][13]*SPP[5];
nextP[15][7] = P[15][7] + P[15][4]*dt;
nextP[15][8] = P[15][8] + P[15][5]*dt;
nextP[15][9] = P[15][9] + P[15][6]*dt;
nextP[15][10] = P[15][10];
nextP[15][11] = P[15][11];
nextP[15][12] = P[15][12];
nextP[15][13] = P[15][13];
nextP[15][14] = P[15][14];
nextP[15][15] = P[15][15];
nextP[15][16] = P[15][16];
nextP[15][17] = P[15][17];
nextP[15][18] = P[15][18];
nextP[15][19] = P[15][19];
nextP[15][20] = P[15][20];
nextP[15][21] = P[15][21];
nextP[16][0] = P[16][0] + P[16][1]*SF[7] + P[16][2]*SF[9] + P[16][3]*SF[8] + P[16][10]*SF[11] + P[16][11]*SPP[7] + P[16][12]*SPP[6];
nextP[16][1] = P[16][1] + P[16][0]*SF[6] + P[16][2]*SF[5] + P[16][3]*SF[9] + P[16][11]*SPP[6] - P[16][12]*SPP[7] - (P[16][10]*q0)/2;
nextP[16][2] = P[16][2] + P[16][0]*SF[4] + P[16][1]*SF[8] + P[16][3]*SF[6] + P[16][12]*SF[11] - P[16][10]*SPP[6] - (P[16][11]*q0)/2;
nextP[16][3] = P[16][3] + P[16][0]*SF[5] + P[16][1]*SF[4] + P[16][2]*SF[7] - P[16][11]*SF[11] + P[16][10]*SPP[7] - (P[16][12]*q0)/2;
nextP[16][4] = P[16][4] + P[16][1]*SF[1] + P[16][0]*SF[3] + P[16][2]*SPP[0] - P[16][3]*SPP[2] - P[16][13]*SPP[4];
nextP[16][5] = P[16][5] + P[16][0]*SF[2] + P[16][2]*SF[1] + P[16][3]*SF[3] - P[16][1]*SPP[0] + P[16][13]*SPP[3];
nextP[16][6] = P[16][6] + P[16][1]*SF[2] + P[16][3]*SF[1] + P[16][0]*SPP[0] - P[16][2]*SPP[1] - P[16][13]*SPP[5];
nextP[16][7] = P[16][7] + P[16][4]*dt;
nextP[16][8] = P[16][8] + P[16][5]*dt;
nextP[16][9] = P[16][9] + P[16][6]*dt;
nextP[16][10] = P[16][10];
nextP[16][11] = P[16][11];
nextP[16][12] = P[16][12];
nextP[16][13] = P[16][13];
nextP[16][14] = P[16][14];
nextP[16][15] = P[16][15];
nextP[16][16] = P[16][16];
nextP[16][17] = P[16][17];
nextP[16][18] = P[16][18];
nextP[16][19] = P[16][19];
nextP[16][20] = P[16][20];
nextP[16][21] = P[16][21];
nextP[17][0] = P[17][0] + P[17][1]*SF[7] + P[17][2]*SF[9] + P[17][3]*SF[8] + P[17][10]*SF[11] + P[17][11]*SPP[7] + P[17][12]*SPP[6];
nextP[17][1] = P[17][1] + P[17][0]*SF[6] + P[17][2]*SF[5] + P[17][3]*SF[9] + P[17][11]*SPP[6] - P[17][12]*SPP[7] - (P[17][10]*q0)/2;
nextP[17][2] = P[17][2] + P[17][0]*SF[4] + P[17][1]*SF[8] + P[17][3]*SF[6] + P[17][12]*SF[11] - P[17][10]*SPP[6] - (P[17][11]*q0)/2;
nextP[17][3] = P[17][3] + P[17][0]*SF[5] + P[17][1]*SF[4] + P[17][2]*SF[7] - P[17][11]*SF[11] + P[17][10]*SPP[7] - (P[17][12]*q0)/2;
nextP[17][4] = P[17][4] + P[17][1]*SF[1] + P[17][0]*SF[3] + P[17][2]*SPP[0] - P[17][3]*SPP[2] - P[17][13]*SPP[4];
nextP[17][5] = P[17][5] + P[17][0]*SF[2] + P[17][2]*SF[1] + P[17][3]*SF[3] - P[17][1]*SPP[0] + P[17][13]*SPP[3];
nextP[17][6] = P[17][6] + P[17][1]*SF[2] + P[17][3]*SF[1] + P[17][0]*SPP[0] - P[17][2]*SPP[1] - P[17][13]*SPP[5];
nextP[17][7] = P[17][7] + P[17][4]*dt;
nextP[17][8] = P[17][8] + P[17][5]*dt;
nextP[17][9] = P[17][9] + P[17][6]*dt;
nextP[17][10] = P[17][10];
nextP[17][11] = P[17][11];
nextP[17][12] = P[17][12];
nextP[17][13] = P[17][13];
nextP[17][14] = P[17][14];
nextP[17][15] = P[17][15];
nextP[17][16] = P[17][16];
nextP[17][17] = P[17][17];
nextP[17][18] = P[17][18];
nextP[17][19] = P[17][19];
nextP[17][20] = P[17][20];
nextP[17][21] = P[17][21];
nextP[18][0] = P[18][0] + P[18][1]*SF[7] + P[18][2]*SF[9] + P[18][3]*SF[8] + P[18][10]*SF[11] + P[18][11]*SPP[7] + P[18][12]*SPP[6];
nextP[18][1] = P[18][1] + P[18][0]*SF[6] + P[18][2]*SF[5] + P[18][3]*SF[9] + P[18][11]*SPP[6] - P[18][12]*SPP[7] - (P[18][10]*q0)/2;
nextP[18][2] = P[18][2] + P[18][0]*SF[4] + P[18][1]*SF[8] + P[18][3]*SF[6] + P[18][12]*SF[11] - P[18][10]*SPP[6] - (P[18][11]*q0)/2;
nextP[18][3] = P[18][3] + P[18][0]*SF[5] + P[18][1]*SF[4] + P[18][2]*SF[7] - P[18][11]*SF[11] + P[18][10]*SPP[7] - (P[18][12]*q0)/2;
nextP[18][4] = P[18][4] + P[18][1]*SF[1] + P[18][0]*SF[3] + P[18][2]*SPP[0] - P[18][3]*SPP[2] - P[18][13]*SPP[4];
nextP[18][5] = P[18][5] + P[18][0]*SF[2] + P[18][2]*SF[1] + P[18][3]*SF[3] - P[18][1]*SPP[0] + P[18][13]*SPP[3];
nextP[18][6] = P[18][6] + P[18][1]*SF[2] + P[18][3]*SF[1] + P[18][0]*SPP[0] - P[18][2]*SPP[1] - P[18][13]*SPP[5];
nextP[18][7] = P[18][7] + P[18][4]*dt;
nextP[18][8] = P[18][8] + P[18][5]*dt;
nextP[18][9] = P[18][9] + P[18][6]*dt;
nextP[18][10] = P[18][10];
nextP[18][11] = P[18][11];
nextP[18][12] = P[18][12];
nextP[18][13] = P[18][13];
nextP[18][14] = P[18][14];
nextP[18][15] = P[18][15];
nextP[18][16] = P[18][16];
nextP[18][17] = P[18][17];
nextP[18][18] = P[18][18];
nextP[18][19] = P[18][19];
nextP[18][20] = P[18][20];
nextP[18][21] = P[18][21];
nextP[19][0] = P[19][0] + P[19][1]*SF[7] + P[19][2]*SF[9] + P[19][3]*SF[8] + P[19][10]*SF[11] + P[19][11]*SPP[7] + P[19][12]*SPP[6];
nextP[19][1] = P[19][1] + P[19][0]*SF[6] + P[19][2]*SF[5] + P[19][3]*SF[9] + P[19][11]*SPP[6] - P[19][12]*SPP[7] - (P[19][10]*q0)/2;
nextP[19][2] = P[19][2] + P[19][0]*SF[4] + P[19][1]*SF[8] + P[19][3]*SF[6] + P[19][12]*SF[11] - P[19][10]*SPP[6] - (P[19][11]*q0)/2;
nextP[19][3] = P[19][3] + P[19][0]*SF[5] + P[19][1]*SF[4] + P[19][2]*SF[7] - P[19][11]*SF[11] + P[19][10]*SPP[7] - (P[19][12]*q0)/2;
nextP[19][4] = P[19][4] + P[19][1]*SF[1] + P[19][0]*SF[3] + P[19][2]*SPP[0] - P[19][3]*SPP[2] - P[19][13]*SPP[4];
nextP[19][5] = P[19][5] + P[19][0]*SF[2] + P[19][2]*SF[1] + P[19][3]*SF[3] - P[19][1]*SPP[0] + P[19][13]*SPP[3];
nextP[19][6] = P[19][6] + P[19][1]*SF[2] + P[19][3]*SF[1] + P[19][0]*SPP[0] - P[19][2]*SPP[1] - P[19][13]*SPP[5];
nextP[19][7] = P[19][7] + P[19][4]*dt;
nextP[19][8] = P[19][8] + P[19][5]*dt;
nextP[19][9] = P[19][9] + P[19][6]*dt;
nextP[19][10] = P[19][10];
nextP[19][11] = P[19][11];
nextP[19][12] = P[19][12];
nextP[19][13] = P[19][13];
nextP[19][14] = P[19][14];
nextP[19][15] = P[19][15];
nextP[19][16] = P[19][16];
nextP[19][17] = P[19][17];
nextP[19][18] = P[19][18];
nextP[19][19] = P[19][19];
nextP[19][20] = P[19][20];
nextP[19][21] = P[19][21];
nextP[20][0] = P[20][0] + P[20][1]*SF[7] + P[20][2]*SF[9] + P[20][3]*SF[8] + P[20][10]*SF[11] + P[20][11]*SPP[7] + P[20][12]*SPP[6];
nextP[20][1] = P[20][1] + P[20][0]*SF[6] + P[20][2]*SF[5] + P[20][3]*SF[9] + P[20][11]*SPP[6] - P[20][12]*SPP[7] - (P[20][10]*q0)/2;
nextP[20][2] = P[20][2] + P[20][0]*SF[4] + P[20][1]*SF[8] + P[20][3]*SF[6] + P[20][12]*SF[11] - P[20][10]*SPP[6] - (P[20][11]*q0)/2;
nextP[20][3] = P[20][3] + P[20][0]*SF[5] + P[20][1]*SF[4] + P[20][2]*SF[7] - P[20][11]*SF[11] + P[20][10]*SPP[7] - (P[20][12]*q0)/2;
nextP[20][4] = P[20][4] + P[20][1]*SF[1] + P[20][0]*SF[3] + P[20][2]*SPP[0] - P[20][3]*SPP[2] - P[20][13]*SPP[4];
nextP[20][5] = P[20][5] + P[20][0]*SF[2] + P[20][2]*SF[1] + P[20][3]*SF[3] - P[20][1]*SPP[0] + P[20][13]*SPP[3];
nextP[20][6] = P[20][6] + P[20][1]*SF[2] + P[20][3]*SF[1] + P[20][0]*SPP[0] - P[20][2]*SPP[1] - P[20][13]*SPP[5];
nextP[20][7] = P[20][7] + P[20][4]*dt;
nextP[20][8] = P[20][8] + P[20][5]*dt;
nextP[20][9] = P[20][9] + P[20][6]*dt;
nextP[20][10] = P[20][10];
nextP[20][11] = P[20][11];
nextP[20][12] = P[20][12];
nextP[20][13] = P[20][13];
nextP[20][14] = P[20][14];
nextP[20][15] = P[20][15];
nextP[20][16] = P[20][16];
nextP[20][17] = P[20][17];
nextP[20][18] = P[20][18];
nextP[20][19] = P[20][19];
nextP[20][20] = P[20][20];
nextP[20][21] = P[20][21];
nextP[21][0] = P[21][0] + P[21][1]*SF[7] + P[21][2]*SF[9] + P[21][3]*SF[8] + P[21][10]*SF[11] + P[21][11]*SPP[7] + P[21][12]*SPP[6];
nextP[21][1] = P[21][1] + P[21][0]*SF[6] + P[21][2]*SF[5] + P[21][3]*SF[9] + P[21][11]*SPP[6] - P[21][12]*SPP[7] - (P[21][10]*q0)/2;
nextP[21][2] = P[21][2] + P[21][0]*SF[4] + P[21][1]*SF[8] + P[21][3]*SF[6] + P[21][12]*SF[11] - P[21][10]*SPP[6] - (P[21][11]*q0)/2;
nextP[21][3] = P[21][3] + P[21][0]*SF[5] + P[21][1]*SF[4] + P[21][2]*SF[7] - P[21][11]*SF[11] + P[21][10]*SPP[7] - (P[21][12]*q0)/2;
nextP[21][4] = P[21][4] + P[21][1]*SF[1] + P[21][0]*SF[3] + P[21][2]*SPP[0] - P[21][3]*SPP[2] - P[21][13]*SPP[4];
nextP[21][5] = P[21][5] + P[21][0]*SF[2] + P[21][2]*SF[1] + P[21][3]*SF[3] - P[21][1]*SPP[0] + P[21][13]*SPP[3];
nextP[21][6] = P[21][6] + P[21][1]*SF[2] + P[21][3]*SF[1] + P[21][0]*SPP[0] - P[21][2]*SPP[1] - P[21][13]*SPP[5];
nextP[21][7] = P[21][7] + P[21][4]*dt;
nextP[21][8] = P[21][8] + P[21][5]*dt;
nextP[21][9] = P[21][9] + P[21][6]*dt;
nextP[21][10] = P[21][10];
nextP[21][11] = P[21][11];
nextP[21][12] = P[21][12];
nextP[21][13] = P[21][13];
nextP[21][14] = P[21][14];
nextP[21][15] = P[21][15];
nextP[21][16] = P[21][16];
nextP[21][17] = P[21][17];
nextP[21][18] = P[21][18];
nextP[21][19] = P[21][19];
nextP[21][20] = P[21][20];
nextP[21][21] = P[21][21];
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// add the general state process noise variances
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t i=0; i<= 21; i++)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
nextP[i][i] = nextP[i][i] + processNoise[i];
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// if the total position variance exceeds 1E6 (1000m), then stop covariance
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// growth by setting the predicted to the previous values
// This prevent an ill conditioned matrix from occurring for long periods
// without GPS
AP_NavEKF: use float arrays when not doing bounds checking g++ is doing a lousy job of inlining VectorN, so avoid it when we don't need bounds checking for production code
2013-12-30 06:27:50 -04:00
if ((P[7][7] + P[8][8]) > 1e6f)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
for (uint8_t i=7; i<=8; i++)
{
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t j=0; j<=21; j++)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
nextP[i][j] = P[i][j];
nextP[j][i] = P[j][i];
}
}
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// copy covariances to output and fix numerical errors
AP_NavEKF : covariance prediction cleanup
2014-02-18 04:27:23 -04:00
CopyAndFixCovariances();
AP_NavEKF: improved covariance matrix numerical stability protection
2014-01-03 15:59:47 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// constrain diagonals to prevent ill-conditioning
AP_NavEKF: improved covariance matrix numerical stability protection
2014-01-03 15:59:47 -04:00
ConstrainVariances();
AP_NavEKF: Added improved covariance matrix error fix
2014-01-03 04:00:15 -04:00
AP_NavEKF: added perf counters on PX4
2013-12-30 06:41:28 -04:00
perf_end(_perf_CovariancePrediction);
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// fuse selected position, velocity and height measurements, checking dat for consistency
// provide a static mode that allows maintenance of the attitude reference without GPS provided the vehicle is not accelerating
// check innovation consistency of velocity states calculated using IMU1 and IMU2 and calculate the optimal weighting of accel data
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
void NavEKF::FuseVelPosNED()
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// start performance timer
AP_NavEKF: added perf counters on PX4
2013-12-30 06:41:28 -04:00
perf_begin(_perf_FuseVelPosNED);
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
// health is set bad until test passed
velHealth = false;
posHealth = false;
hgtHealth = false;
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
AP_NavEKF: brought some tuning parameters out to header
2014-01-01 08:15:37 -04:00
// declare variables used to check measurement errors
AP_NavEKF: use vectorN to make indexes safe
2013-12-29 22:25:02 -04:00
Vector3f velInnov;
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
Vector3f velInnov1;
Vector3f velInnov2;
AP_NavEKF: use float arrays when not doing bounds checking g++ is doing a lousy job of inlining VectorN, so avoid it when we don't need bounds checking for production code
2013-12-30 06:27:50 -04:00
Vector2 posInnov;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
float hgtInnov = 0;
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
AP_NavEKF: brought some tuning parameters out to header
2014-01-01 08:15:37 -04:00
// declare variables used to control access to arrays
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
bool fuseData[6] = {false,false,false,false,false,false};
uint8_t stateIndex;
uint8_t obsIndex;
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
AP_NavEKF: brought some tuning parameters out to header
2014-01-01 08:15:37 -04:00
// declare variables used by state and covariance update calculations
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
float NEvelErr;
float DvelErr;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
float posErr;
AP_NavEKF: use vectorN to make indexes safe
2013-12-29 22:25:02 -04:00
Vector6 R_OBS;
Vector6 observation;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
float SK;
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// perform sequential fusion of GPS measurements. This assumes that the
AP_NavEKF: brought some tuning parameters out to header
2014-01-01 08:15:37 -04:00
// 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
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (fuseVelData || fusePosData || fuseHgtData) {
AP_NavEKF: Fixed bug in pre GPS fix updates
2014-01-18 17:48:12 -04:00
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
// if static mode is active use the current states to calculate the predicted
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// measurement rather than use states from a previous time. We need to do this
// because there may be no stored states due to lack of real measurements.
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
// in static mode, only position and height fusion is used
AP_NavEKF: Fixed bug in pre GPS fix updates
2014-01-18 17:48:12 -04:00
if (staticMode) {
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
statesAtPosTime = state;
statesAtHgtTime = state;
AP_NavEKF: Fixed bug in pre GPS fix updates
2014-01-18 17:48:12 -04:00
}
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// set the GPS data timeout depending on whether airspeed data is present
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
uint32_t gpsRetryTime;
AP_NavEKF: fixed detection of airspeed sensor the get_airspeed() AHRS call can change as the user enables/disables the airspeed sensor, plus it only gets enabled after the NavEKF constructor runs.
2014-02-18 18:19:46 -04:00
if (useAirspeed()) gpsRetryTime = _gpsRetryTimeUseTAS;
AP_NavEKF: Reduced number of Mavlink tuneable parameters
2014-01-29 04:03:07 -04:00
else gpsRetryTime = _gpsRetryTimeNoTAS;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Track baro height observations pre-arm This prevents copter from failing the pre-arm height discrepancy test
2014-03-23 01:04:20 -03:00
// form the observation vector and zero velocity and horizontal position observations if in static mode
AP_NavEKF: fixed some matlab ! -> ~ typos Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-20 08:44:37 -03:00
if (!staticMode) {
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i];
AP_NavEKF: Fix bug in position reset logic
2014-04-23 04:37:07 -03:00
observation[3] = gpsPosNE.x + gpsPosGlitchOffsetNE.x;
observation[4] = gpsPosNE.y + gpsPosGlitchOffsetNE.y;
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
} else {
AP_NavEKF : Track baro height observations pre-arm This prevents copter from failing the pre-arm height discrepancy test
2014-03-23 01:04:20 -03:00
for (uint8_t i=0; i<=4; i++) observation[i] = 0.0f;
AP_NavEKF: added static mode for pre-arm and bench testing
2014-01-04 00:20:14 -04:00
}
AP_NavEKF : Track baro height observations pre-arm This prevents copter from failing the pre-arm height discrepancy test
2014-03-23 01:04:20 -03:00
observation[5] = -hgtMea;
AP_NavEKF: brought some tuning parameters out to header
2014-01-01 08:15:37 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate additional error in GPS velocity caused by manoeuvring
AP_NavEKF: Reduced number of Mavlink tuneable parameters
2014-01-29 04:03:07 -04:00
NEvelErr = _gpsNEVelVarAccScale * accNavMag;
DvelErr = _gpsDVelVarAccScale * accNavMag;
AP_NavEKF: brought some tuning parameters out to header
2014-01-01 08:15:37 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate additional error in GPS position caused by manoeuvring
AP_NavEKF: Reduced number of Mavlink tuneable parameters
2014-01-29 04:03:07 -04:00
posErr = _gpsPosVarAccScale * accNavMag;
AP_NavEKF: Added height rate adaptive wind speed process noise
2014-01-01 17:12:06 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// estimate the GPS Velocity, GPS horiz position and height measurement variances.
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
R_OBS[0] = sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(NEvelErr);
AP_NavEKF: Added height rate adaptive wind speed process noise
2014-01-01 17:12:06 -04:00
R_OBS[1] = R_OBS[0];
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
R_OBS[2] = sq(constrain_float(_gpsVertVelNoise, 0.05f, 5.0f)) + sq(DvelErr);
R_OBS[3] = sq(constrain_float(_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
AP_NavEKF: Added height rate adaptive wind speed process noise
2014-01-01 17:12:06 -04:00
R_OBS[4] = R_OBS[3];
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
R_OBS[5] = sq(constrain_float(_baroAltNoise, 0.1f, 10.0f));
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer
AP_NavEKF : Prevents aliasing triggering innovation consistency check failures
2014-02-24 04:56:20 -04:00
// 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.
bool badIMUdata = false;
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
if (_fusionModeGPS == 0 && fuseVelData && (imuSampleTime_ms - lastHgtTime_ms) < (2 * _msecHgtAvg)) {
AP_NavEKF : Prevents aliasing triggering innovation consistency check failures
2014-02-24 04:56:20 -04:00
// calculate innovations for height and vertical GPS vel measurements
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
float hgtErr = statesAtHgtTime.position.z - observation[5];
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
float velDErr = statesAtVelTime.velocity.z - observation[2];
AP_NavEKF : Relax aliasing check limits This reduces the chance of noisy data inhibiting GPS glitch protection
2014-04-02 03:11:18 -03:00
// 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[5])) && (sq(velDErr) > 9.0f * (P[6][6] + R_OBS[2]))) {
AP_NavEKF : Prevents aliasing triggering innovation consistency check failures
2014-02-24 04:56:20 -04:00
badIMUdata = true;
} else {
badIMUdata = false;
}
}
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// calculate innovations and check GPS data validity using an innovation consistency check
AP_NavEKF : Do not reset on GPS velocity timeout if good position data
2014-04-02 14:40:34 -03:00
// test position measurements
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (fusePosData) {
AP_NavEKF : Do not reset on GPS velocity timeout if good position data
2014-04-02 14:40:34 -03:00
// test horizontal position measurements
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
posInnov[0] = statesAtPosTime.position.x - observation[3];
posInnov[1] = statesAtPosTime.position.y - observation[4];
AP_NavEKF : Do not reset on GPS velocity timeout if good position data
2014-04-02 14:40:34 -03:00
varInnovVelPos[3] = P[7][7] + R_OBS[3];
varInnovVelPos[4] = P[8][8] + R_OBS[4];
// apply an innovation consistency threshold test, but don't fail if bad IMU data
// calculate max valid position innovation squared based on a maximum horizontal inertial nav accel error and GPS noise parameter
// max inertial nav error is scaled with horizontal g to allow for increased errors when manoeuvring
AP_NavEKF : Fix bug in GPS innovation variance growth calculation The uncertainty in acceleration is currently only scaled using horizontal accelerations, however during vertical plane aerobatics and high g pullups, misalignment in angles can cause significant horizontal acceleration error. The scaling now uses the 3D acceleration vector length to better work during vertical plane high g maneouvres. This error was found during flight testing with 8g pullups
2014-09-06 20:14:13 -03:00
float accelScale = (1.0f + 0.1f * accNavMag);
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - posFailTime)));
AP_NavEKF : Do not reset on GPS velocity timeout if good position data
2014-04-02 14:40:34 -03:00
posTestRatio = (sq(posInnov[0]) + sq(posInnov[1])) / maxPosInnov2;
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
AP_NavEKF : Improved GPS position glitch handling When using GPS after previously rejecting it, the GPS position will always be offset if outside the specified glitch radius. This was the original intent of the design and makes handling of glitches smoother. It has been tested on replay using glitchy flight data
2014-04-02 15:15:59 -03:00
// declare a timeout condition if we have been too long without data
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
posTimeout = ((imuSampleTime_ms - posFailTime) > gpsRetryTime);
AP_NavEKF : Improved GPS position glitch handling When using GPS after previously rejecting it, the GPS position will always be offset if outside the specified glitch radius. This was the original intent of the design and makes handling of glitches smoother. It has been tested on replay using glitchy flight data
2014-04-02 15:15:59 -03:00
// use position data if healthy, timed out, or in static mode
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (posHealth || posTimeout || staticMode) {
AP_NavEKF : Do not reset on GPS velocity timeout if good position data
2014-04-02 14:40:34 -03:00
posHealth = true;
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
posFailTime = imuSampleTime_ms;
AP_NavEKF : Improved GPS position glitch handling When using GPS after previously rejecting it, the GPS position will always be offset if outside the specified glitch radius. This was the original intent of the design and makes handling of glitches smoother. It has been tested on replay using glitchy flight data
2014-04-02 15:15:59 -03:00
// if timed out or outside the specified glitch radius, increment the offset applied to GPS data to compensate for large GPS position jumps
if (posTimeout || (maxPosInnov2 > sq(float(_gpsGlitchRadiusMax)))) {
AP_NavEKF: Fix bug in position reset logic
2014-04-23 04:37:07 -03:00
gpsPosGlitchOffsetNE.x += posInnov[0];
gpsPosGlitchOffsetNE.y += posInnov[1];
// limit the radius of the offset to 100m and decay the offset to zero radially
decayGpsOffset();
// reset the position to the current GPS position which will include the glitch correction offset
ResetPosition();
AP_NavEKF : Do not reset on GPS velocity timeout if good position data
2014-04-02 14:40:34 -03:00
// don't fuse data on this time step
fusePosData = false;
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
} else {
AP_NavEKF : Do not reset on GPS velocity timeout if good position data
2014-04-02 14:40:34 -03:00
posHealth = false;
}
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
AP_NavEKF : Do not reset on GPS velocity timeout if good position data
2014-04-02 14:40:34 -03:00
// test velocity measurements
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (fuseVelData) {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// test velocity measurements
uint8_t imax = 2;
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
if (_fusionModeGPS == 1) {
imax = 1;
}
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
float K1 = 0; // innovation to error ratio for IMU1
float K2 = 0; // innovation to error ratio for IMU2
float innovVelSumSq = 0; // sum of squares of velocity innovations
float varVelSum = 0; // sum of velocity innovation variances
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t i = 0; i<=imax; i++) {
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
// velocity states start at index 4
stateIndex = i + 4;
// calculate innovations using blended and single IMU predicted states
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
velInnov[i] = statesAtVelTime.velocity[i] - observation[i]; // blended
velInnov1[i] = statesAtVelTime.vel1[i] - observation[i]; // IMU1
velInnov2[i] = statesAtVelTime.vel2[i] - observation[i]; // IMU2
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
// calculate innovation variance
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
AP_NavEKF : Reduce weighting on GPS when not enough satellites GPS measurement variance is doubled if only 5 satellites, and quadrupled if 4 or less. The GPS glitch rejection thresholds remain the same This will reduce the impact of GPS glitches on attitude.
2014-09-23 06:30:01 -03:00
// calculate error weightings for single IMU velocity states using
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
// observation error to normalise
float R_hgt;
if (i == 2) {
AP_NavEKF : Add div0 protection to the IMU1_weighting calc
2014-03-01 17:07:25 -04:00
R_hgt = sq(constrain_float(_gpsVertVelNoise, 0.05f, 5.0f));
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
} else {
AP_NavEKF : Add div0 protection to the IMU1_weighting calc
2014-03-01 17:07:25 -04:00
R_hgt = sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f));
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
}
K1 += R_hgt / (R_hgt + sq(velInnov1[i]));
K2 += R_hgt / (R_hgt + sq(velInnov2[i]));
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
// sum the innovation and innovation variances
innovVelSumSq += sq(velInnov[i]);
varVelSum += varInnovVelPos[i];
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate weighting used by fuseVelPosNED to do IMU accel data blending
// this is used to detect and compensate for aliasing errors with the accelerometers
// provide for a first order lowpass filter to reduce noise on the weighting if required
IMU1_weighting = 1.0f * (K1 / (K1 + K2)) + 0.0f * IMU1_weighting; // filter currently inactive
AP_NavEKF : Prevents aliasing triggering innovation consistency check failures
2014-02-24 04:56:20 -04:00
// apply an innovation consistency threshold test, but don't fail if bad IMU data
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
// calculate the test ratio
velTestRatio = innovVelSumSq / (varVelSum * sq(_gpsVelInnovGate));
// fail if the ratio is greater than 1
AP_NavEKF : clean up convoluted logic used in sensor health checks This doesn't change the behavior, but does make the code easier to understand
2014-04-02 02:54:07 -03:00
velHealth = ((velTestRatio < 1.0f) || badIMUdata);
AP_NavEKF : Do not reset on GPS velocity timeout if good position data
2014-04-02 14:40:34 -03:00
// declare a timeout if we have not fused velocity data for too long
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
velTimeout = (imuSampleTime_ms - velFailTime) > gpsRetryTime;
AP_NavEKF : Do not reset on GPS velocity timeout if good position data
2014-04-02 14:40:34 -03:00
// if data is healthy or in static mode we fuse it
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (velHealth || staticMode) {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
velHealth = true;
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
velFailTime = imuSampleTime_ms;
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
} else if (velTimeout && !posHealth) {
// if data is not healthy and timed out and position is unhealthy we reset the velocity, but do not fuse data on this time step
AP_NavEKF : Do not reset on GPS velocity timeout if good position data
2014-04-02 14:40:34 -03:00
ResetVelocity();
StoreStatesReset();
fuseVelData = false;
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
} else {
// if data is unhealthy and position is healthy, we do not fuse it
AP_NavEKF : Do not reset on GPS velocity timeout if good position data
2014-04-02 14:40:34 -03:00
velHealth = false;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// test height measurements
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (fuseHgtData) {
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
// set the height data timeout depending on whether vertical velocity data is being used
uint32_t hgtRetryTime;
AP_NavEKF: Reduced number of Mavlink tuneable parameters
2014-01-29 04:03:07 -04:00
if (_fusionModeGPS == 0) hgtRetryTime = _hgtRetryTimeMode0;
else hgtRetryTime = _hgtRetryTimeMode12;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
// calculate height innovations
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
hgtInnov = statesAtHgtTime.position.z - observation[5];
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
varInnovVelPos[5] = P[9][9] + R_OBS[5];
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
// calculate the innovation consistency test ratio
hgtTestRatio = sq(hgtInnov) / (sq(_hgtInnovGate) * varInnovVelPos[5]);
// fail if the ratio is > 1, but don't fail if bad IMU data
AP_NavEKF : clean up convoluted logic used in sensor health checks This doesn't change the behavior, but does make the code easier to understand
2014-04-02 02:54:07 -03:00
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
hgtTimeout = (imuSampleTime_ms - hgtFailTime) > hgtRetryTime;
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
// Fuse height data if healthy or timed out or in static mode
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (hgtHealth || hgtTimeout || staticMode) {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
hgtHealth = true;
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
hgtFailTime = imuSampleTime_ms;
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
// if timed out, reset the height, but do not fuse data on this time step
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (hgtTimeout) {
AP_NavEKF: Improved behaviour following loss and regaining of GPS
2014-01-20 15:41:41 -04:00
ResetHeight();
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
StoreStatesReset();
AP_NavEKF: Fixed bug in pos/vel/hgt reset on timout
2014-01-24 01:04:42 -04:00
fuseHgtData = false;
AP_NavEKF: allow initialisation before GPS lock to aid indoor testing
2014-01-17 20:57:59 -04:00
}
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
else {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
hgtHealth = false;
}
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// set range for sequential fusion of velocity and position measurements depending on which data is available and its health
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (fuseVelData && _fusionModeGPS == 0 && velHealth && !staticMode) {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
fuseData[0] = true;
fuseData[1] = true;
fuseData[2] = true;
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (fuseVelData && _fusionModeGPS == 1 && velHealth && !staticMode) {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
fuseData[0] = true;
fuseData[1] = true;
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if ((fusePosData && _fusionModeGPS <= 2 && posHealth) || staticMode) {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
fuseData[3] = true;
fuseData[4] = true;
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if ((fuseHgtData && hgtHealth) || staticMode) {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
fuseData[5] = true;
}
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// fuse measurements sequentially
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (obsIndex=0; obsIndex<=5; obsIndex++) {
if (fuseData[obsIndex]) {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
stateIndex = 4 + obsIndex;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate the measurement innovation, using states from a different time coordinate if fusing height data
AP_NavEKF : Reduce weighting on GPS when not enough satellites GPS measurement variance is doubled if only 5 satellites, and quadrupled if 4 or less. The GPS glitch rejection thresholds remain the same This will reduce the impact of GPS glitches on attitude.
2014-09-23 06:30:01 -03:00
// adjust scaling on GPS measurement noise variances if not enough satellites
AP_NavEKF: use float arrays when not doing bounds checking g++ is doing a lousy job of inlining VectorN, so avoid it when we don't need bounds checking for production code
2013-12-30 06:27:50 -04:00
if (obsIndex <= 2)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
innovVelPos[obsIndex] = statesAtVelTime.velocity[obsIndex] - observation[obsIndex];
AP_NavEKF : Reduce weighting on GPS when not enough satellites GPS measurement variance is doubled if only 5 satellites, and quadrupled if 4 or less. The GPS glitch rejection thresholds remain the same This will reduce the impact of GPS glitches on attitude.
2014-09-23 06:30:01 -03:00
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
else if (obsIndex == 3 || obsIndex == 4) {
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
innovVelPos[obsIndex] = statesAtPosTime.position[obsIndex-3] - observation[obsIndex];
AP_NavEKF : Reduce weighting on GPS when not enough satellites GPS measurement variance is doubled if only 5 satellites, and quadrupled if 4 or less. The GPS glitch rejection thresholds remain the same This will reduce the impact of GPS glitches on attitude.
2014-09-23 06:30:01 -03:00
R_OBS[obsIndex] *= sq(gpsNoiseScaler);
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
} else {
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
innovVelPos[obsIndex] = statesAtHgtTime.position[obsIndex-3] - observation[obsIndex];
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate the Kalman gain and calculate innovation variances
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
SK = 1.0f/varInnovVelPos[obsIndex];
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t i= 0; i<=12; i++) {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
Kfusion[i] = P[i][stateIndex]*SK;
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// Only height observations are used to update z accel bias estimate
AP_NavEKF: Add protection for accel bias estimation errors Don't do bias estimation if tilted by more than 60 degrees to prevent scale factor errors affecting result unnecessarily. Prevent Kalman gain from having the wrong sign due to numerical errors associated with small process noise values. Allow smaller Z accel bias process noise values to be set
2014-05-17 10:30:27 -03:00
// Protect Kalman gain from ill-conditioning
// Don't update Z accel bias if off-level by greater than 60 degrees to avoid scale factor error effects
if (obsIndex == 5 && prevTnb.c.z > 0.5f) {
Kfusion[13] = constrain_float(P[13][stateIndex]*SK,-1.0f,0.0f);
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
} else {
Kfusion[13] = 0.0f;
}
// inhibit wind state estimation by setting Kalman gains to zero
if (!inhibitWindStates) {
Kfusion[14] = P[14][stateIndex]*SK;
Kfusion[15] = P[15][stateIndex]*SK;
} else {
Kfusion[14] = 0.0f;
Kfusion[15] = 0.0f;
}
// 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;
}
}
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
// Set the Kalman gain values for the single IMU states
Kfusion[22] = Kfusion[13]; // IMU2 Z accel bias
Kfusion[26] = Kfusion[9]; // IMU1 posD
Kfusion[30] = Kfusion[9]; // IMU2 posD
for (uint8_t i = 0; i<=2; i++) {
Kfusion[i+23] = Kfusion[i+4]; // IMU1 velNED
Kfusion[i+27] = Kfusion[i+4]; // IMU2 velNED
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
// Correct states that have been predicted using single (not blended) IMU data
if (obsIndex == 5){
// Calculate height measurement innovations using single IMU states
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
float hgtInnov1 = statesAtHgtTime.posD1 - observation[obsIndex];
float hgtInnov2 = statesAtHgtTime.posD2 - observation[obsIndex];
AP_NavEKF : Limit rate of Z accel bias adaptation Aliasing can causes the bias estimate to fluctuate very rapidly as it tries to keep up, which degrades the benefit of switching between accelerometers to avoid aliasing. This patch give a much more stable bias estimate during aliasing, and allows the bias to adapt at a maximum rate of 1.0 m/s2 in 50 seconds
2014-04-02 03:52:13 -03:00
// Correct single IMU prediction states using height measurement, limiting rate of change of bias to 0.02 m/s3
float correctionLimit = 0.02f * dtIMU *dtVelPos;
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
state.accel_zbias1 -= constrain_float(Kfusion[13] * hgtInnov1, -correctionLimit, correctionLimit); // IMU1 Z accel bias
state.accel_zbias2 -= constrain_float(Kfusion[22] * hgtInnov2, -correctionLimit, correctionLimit); // IMU2 Z accel bias
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t i = 23; i<=26; i++) {
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
states[i] = states[i] - Kfusion[i] * hgtInnov1; // IMU1 velNED,posD
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t i = 27; i<=30; i++) {
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
states[i] = states[i] - Kfusion[i] * hgtInnov2; // IMU2 velNED,posD
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
} else if (obsIndex == 0 || obsIndex == 1 || obsIndex == 2) {
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
// Correct single IMU prediction states using velocity measurements
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t i = 23; i<=26; i++) {
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
states[i] = states[i] - Kfusion[i] * velInnov1[obsIndex]; // IMU1 velNED,posD
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t i = 27; i<=30; i++) {
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
states[i] = states[i] - Kfusion[i] * velInnov2[obsIndex]; // IMU2 velNED,posD
}
AP_NavEKF : Improved stability of Z accel bias
2014-02-21 00:24:09 -04:00
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
// calculate state corrections and re-normalise the quaternions for states predicted using the blended IMU data
// attitude, velocity and position corrections are spread across multiple prediction cycles between now
// and the anticipated time for the next measurement.
// Don't spread quaternion corrections if total angle change across predicted interval is going to exceed 0.1 rad
bool highRates = ((gpsUpdateCountMax * correctedDelAng.length()) > 0.1f);
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t i = 0; i<=21; i++) {
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
if ((i <= 3 && highRates) || i >= 10 || staticMode) {
states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
} else {
if (obsIndex == 5) {
hgtIncrStateDelta[i] -= Kfusion[i] * innovVelPos[obsIndex] * hgtUpdateCountMaxInv;
} else {
gpsIncrStateDelta[i] -= Kfusion[i] * innovVelPos[obsIndex] * gpsUpdateCountMaxInv;
}
}
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF: allow states to be accessed using names as well as a Vector22
2014-02-19 22:21:09 -04:00
state.quat.normalize();
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// 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;
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t i= 0; i<=21; i++) {
for (uint8_t j= 0; j<=21; j++)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
KHP[i][j] = Kfusion[i] * P[stateIndex][j];
}
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t i= 0; i<=21; i++) {
for (uint8_t j= 0; j<=21; j++) {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
P[i][j] = P[i][j] - KHP[i][j];
}
}
}
}
}
AP_NavEKF: improved covariance matrix numerical stability protection
2014-01-03 15:59:47 -04:00
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
AP_NavEKF: improved covariance matrix numerical stability protection
2014-01-03 15:59:47 -04:00
ForceSymmetry();
ConstrainVariances();
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// stop performance timer
AP_NavEKF: added perf counters on PX4
2013-12-30 06:41:28 -04:00
perf_end(_perf_FuseVelPosNED);
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// fuse magnetometer measurements and apply innovation consistency checks
// fuse each axis on consecutive time steps to spread computional load
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
void NavEKF::FuseMagnetometer()
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// start performance timer
AP_NavEKF: added perf counters on PX4
2013-12-30 06:41:28 -04:00
perf_begin(_perf_FuseMagnetometer);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// declarations
AP_NavEKF: allow filter to run as single or double precision useful for checking if there are numerical precision issues
2014-01-03 00:37:19 -04:00
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;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
uint8_t &obsIndex = mag_state.obsIndex;
Matrix3f &DCM = mag_state.DCM;
Vector3f &MagPred = mag_state.MagPred;
AP_NavEKF: allow filter to run as single or double precision useful for checking if there are numerical precision issues
2014-01-03 00:37:19 -04:00
ftype &R_MAG = mag_state.R_MAG;
ftype *SH_MAG = &mag_state.SH_MAG[0];
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
Vector22 H_MAG;
AP_NavEKF: use vectorN to make indexes safe
2013-12-29 22:25:02 -04:00
Vector6 SK_MX;
Vector6 SK_MY;
Vector6 SK_MZ;
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// perform sequential fusion of magnetometer measurements.
// this assumes that the errors in the different components are
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
// 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
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
if (fuseMagData || obsIndex == 1 || obsIndex == 2)
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate observation jacobians and Kalman gains
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
if (fuseMagData)
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// copy required states to local variable names
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
q0 = statesAtMagMeasTime.quat[0];
q1 = statesAtMagMeasTime.quat[1];
q2 = statesAtMagMeasTime.quat[2];
q3 = statesAtMagMeasTime.quat[3];
magN = statesAtMagMeasTime.earth_magfield[0];
magE = statesAtMagMeasTime.earth_magfield[1];
magD = statesAtMagMeasTime.earth_magfield[2];
magXbias = statesAtMagMeasTime.body_magfield[0];
magYbias = statesAtMagMeasTime.body_magfield[1];
magZbias = statesAtMagMeasTime.body_magfield[2];
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// 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*(q1*q2 + q0*q3);
DCM[0][2] = 2*(q1*q3-q0*q2);
DCM[1][0] = 2*(q1*q2 - q0*q3);
DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
DCM[1][2] = 2*(q2*q3 + q0*q1);
DCM[2][0] = 2*(q1*q3 + q0*q2);
DCM[2][1] = 2*(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;
// scale magnetometer observation error with total angular rate
AP_NavEKF : Increase the tuning range for magnetometer measurement noise
2014-02-27 01:45:03 -04:00
R_MAG = sq(constrain_float(_magNoise, 0.01f, 0.5f)) + sq(_magVarRateScale*dAngIMU.length() / dtIMU);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate observation jacobians
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*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*magN*q0;
SH_MAG[8] = 2*magE*q3;
for (uint8_t i=0; i<=21; i++) H_MAG[i] = 0;
H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
H_MAG[1] = SH_MAG[0];
H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2;
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*q0*q3 + 2*q1*q2;
H_MAG[18] = 2*q1*q3 - 2*q0*q2;
H_MAG[19] = 1;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate Kalman gain
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
float temp = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + 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*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + 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*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + 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*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + 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*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + 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*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + 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*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + 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*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
if (temp >= R_MAG) {
SK_MX[0] = 1.0f / temp;
AP_NavEKF: fixed up handling of bitfields in faultStatus Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 08:42:39 -03:00
faultStatus.bad_xmag = false;
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we increase the state variances and try again next time
P[19][19] += 0.1f*R_MAG;
obsIndex = 1;
AP_NavEKF: fixed up handling of bitfields in faultStatus Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 08:42:39 -03:00
faultStatus.bad_xmag = true;
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
return;
}
SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
SK_MX[4] = 2*q0*q2 - 2*q1*q3;
SK_MX[5] = 2*q0*q3 + 2*q1*q2;
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[5] - P[0][18]*SK_MX[4]);
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[5] - P[1][18]*SK_MX[4]);
Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[5] - P[2][18]*SK_MX[4]);
Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[5] - P[3][18]*SK_MX[4]);
Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[5] - P[4][18]*SK_MX[4]);
Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[5] - P[5][18]*SK_MX[4]);
Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[5] - P[6][18]*SK_MX[4]);
Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[5] - P[7][18]*SK_MX[4]);
Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[5] - P[8][18]*SK_MX[4]);
Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[5] - P[9][18]*SK_MX[4]);
Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[5] - P[10][18]*SK_MX[4]);
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[5] - P[11][18]*SK_MX[4]);
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// this term has been zeroed to improve stability of the Z accel bias
AP_NavEKF : Improved stability of Z accel bias
2014-02-21 00:24:09 -04:00
Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]);
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) {
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]);
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]);
} else {
Kfusion[14] = 0.0;
Kfusion[15] = 0.0;
}
// 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][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]);
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]);
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]);
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]);
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]);
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]);
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate the observation innovation variance
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
varInnovMag[0] = 1.0f/SK_MX[0];
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// reset the observation index to 0 (we start by fusing the X measurement)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
obsIndex = 0;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
// set flags to indicate to other processes that fusion has been performed and is required on the next frame
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
AP_NavEKF: Load levelling scheduler - first attempt
2013-12-30 04:58:24 -04:00
magFusePerformed = true;
magFuseRequired = true;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
else if (obsIndex == 1) // we are now fusing the Y measurement
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate observation jacobians
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t i=0; i<=21; i++) H_MAG[i] = 0;
H_MAG[0] = SH_MAG[2];
H_MAG[1] = SH_MAG[1];
H_MAG[2] = SH_MAG[0];
H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7];
H_MAG[16] = 2*q1*q2 - 2*q0*q3;
H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
H_MAG[18] = 2*q0*q1 + 2*q2*q3;
H_MAG[20] = 1;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate Kalman gain
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
float temp = (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*q0*q3 - 2*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*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*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*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*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*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*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*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*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*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*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*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*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*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
if (temp >= R_MAG) {
SK_MY[0] = 1.0f / temp;
AP_NavEKF: fixed up handling of bitfields in faultStatus Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 08:42:39 -03:00
faultStatus.bad_ymag = false;
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we increase the state variances and try again next time
P[20][20] += 0.1f*R_MAG;
obsIndex = 2;
AP_NavEKF: fixed up handling of bitfields in faultStatus Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 08:42:39 -03:00
faultStatus.bad_ymag = true;
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
return;
}
SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
SK_MY[3] = 2*q0*q3 - 2*q1*q2;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
SK_MY[4] = 2*q0*q1 + 2*q2*q3;
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
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]);
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
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]);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// this term has been zeroed to improve stability of the Z accel bias
AP_NavEKF : Improved stability of Z accel bias
2014-02-21 00:24:09 -04:00
Kfusion[13] = 0.0f;//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]);
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) {
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]);
} else {
Kfusion[14] = 0.0;
Kfusion[15] = 0.0;
}
// 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;
}
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate the observation innovation variance
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
varInnovMag[1] = 1.0f/SK_MY[0];
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// 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
AP_NavEKF: Load levelling scheduler - first attempt
2013-12-30 04:58:24 -04:00
magFusePerformed = true;
magFuseRequired = true;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
else if (obsIndex == 2) // we are now fusing the Z measurement
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate observation jacobians
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t i=0; i<=21; i++) H_MAG[i] = 0;
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
H_MAG[0] = SH_MAG[1];
H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1;
H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
H_MAG[3] = SH_MAG[0];
H_MAG[16] = 2*q0*q2 + 2*q1*q3;
H_MAG[17] = 2*q2*q3 - 2*q0*q1;
H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
H_MAG[21] = 1;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate Kalman gain
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
float temp = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + 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*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + 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*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + 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*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + 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*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + 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*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + 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*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + 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*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
if (temp >= R_MAG) {
SK_MZ[0] = 1.0f / temp;
AP_NavEKF: fixed up handling of bitfields in faultStatus Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 08:42:39 -03:00
faultStatus.bad_zmag = false;
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we increase the state variances and try again next time
P[21][21] += 0.1f*R_MAG;
obsIndex = 3;
AP_NavEKF: fixed up handling of bitfields in faultStatus Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 08:42:39 -03:00
faultStatus.bad_zmag = true;
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
return;
}
SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
SK_MZ[4] = 2*q0*q1 - 2*q2*q3;
SK_MZ[5] = 2*q0*q2 + 2*q1*q3;
Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[5] - P[0][17]*SK_MZ[4]);
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[5] - P[1][17]*SK_MZ[4]);
Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[5] - P[2][17]*SK_MZ[4]);
Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[5] - P[3][17]*SK_MZ[4]);
Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[5] - P[4][17]*SK_MZ[4]);
Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[5] - P[5][17]*SK_MZ[4]);
Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[5] - P[6][17]*SK_MZ[4]);
Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[5] - P[7][17]*SK_MZ[4]);
Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[5] - P[8][17]*SK_MZ[4]);
Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[5] - P[9][17]*SK_MZ[4]);
Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[5] - P[10][17]*SK_MZ[4]);
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[5] - P[11][17]*SK_MZ[4]);
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// this term has been zeroed to improve stability of the Z accel bias
AP_NavEKF : Improved stability of Z accel bias
2014-02-21 00:24:09 -04:00
Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]);
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) {
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]);
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]);
} else {
Kfusion[14] = 0.0;
Kfusion[15] = 0.0;
}
// 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][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]);
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]);
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]);
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]);
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]);
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]);
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate the observation innovation variance
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
varInnovMag[2] = 1.0f/SK_MZ[0];
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// 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
AP_NavEKF: Load levelling scheduler - first attempt
2013-12-30 04:58:24 -04:00
magFusePerformed = true;
magFuseRequired = false;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate the measurement innovation
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
innovMag[obsIndex] = MagPred[obsIndex] - magData[obsIndex];
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
// calculate the innovation test ratio
magTestRatio[obsIndex] = sq(innovMag[obsIndex]) / (sq(_magInnovGate) * varInnovMag[obsIndex]);
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// check the last values from all components and set magnetometer health accordingly
AP_NavEKF: Improved magnetometer consistency checks A magnetometer axis that fails the innovation consistency check will cause all axes not to be used. If this condition continues for 10 seconds, a magnetometer timeout condition will be declared. When the timeout has occurred, if it is not a fly forward vehicle, then individual channels will be used again, but with a reduced weighting.
2014-04-21 03:15:17 -03:00
magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
// Don't fuse unless all componenets pass. The exception is if the bad health has timed out and we are not a fly forward vehicle
// In this case we might as well try using the magnetometer, but with a reduced weighting
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !assume_zero_sideslip() && magTimeout)) {
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
// Attitude, velocity and position corrections are averaged across multiple prediction cycles between now and the anticipated time for the next measurement.
// Don't do averaging of quaternion state corrections if total angle change across predicted interval is going to exceed 0.1 rad
bool highRates = ((magUpdateCountMax * correctedDelAng.length()) > 0.1f);
// Calculate the number of averaging frames left to go. This is required becasue magnetometer fusion is applied across three consecutive prediction cycles
// There is no point averaging if the number of cycles left is less than 2
float minorFramesToGo = float(magUpdateCountMax) - float(magUpdateCount);
// correct the state vector or store corrections to be applied incrementally
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t j= 0; j<=21; j++) {
AP_NavEKF: Improved magnetometer consistency checks A magnetometer axis that fails the innovation consistency check will cause all axes not to be used. If this condition continues for 10 seconds, a magnetometer timeout condition will be declared. When the timeout has occurred, if it is not a fly forward vehicle, then individual channels will be used again, but with a reduced weighting.
2014-04-21 03:15:17 -03:00
// If we are forced to use a bad compass, we reduce the weighting by a factor of 4
if (!magHealth) {
Kfusion[j] *= 0.25f;
}
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
if ((j <= 3 && highRates) || j >= 10 || staticMode || minorFramesToGo < 1.5f ) {
states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
} else {
// scale the correction based on the number of averaging frames left to go
magIncrStateDelta[j] -= Kfusion[j] * innovMag[obsIndex] * (magUpdateCountMaxInv * float(magUpdateCountMax) / minorFramesToGo);
}
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
// normalise the quaternion states
AP_NavEKF : increased useage of helper functions
2014-03-10 06:08:15 -03:00
state.quat.normalize();
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the
// number of operations
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t i = 0; i<=21; i++) {
for (uint8_t j = 0; j<=3; j++) {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
KH[i][j] = Kfusion[i] * H_MAG[j];
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t j = 4; j<=15; j++) {
KH[i][j] = 0.0f;
}
if (!inhibitMagStates) {
for (uint8_t j = 16; j<=21; j++) {
AP_NavEKF: Cleaned up on-ground state and covariance update logic
2013-12-30 19:21:04 -04:00
KH[i][j] = Kfusion[i] * H_MAG[j];
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
} else {
for (uint8_t j = 16; j<=21; j++) {
AP_NavEKF: Cleaned up on-ground state and covariance update logic
2013-12-30 19:21:04 -04:00
KH[i][j] = 0.0f;
}
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t i = 0; i<=21; i++) {
for (uint8_t j = 0; j<=21; j++) {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
KHP[i][j] = 0;
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t k = 0; k<=3; k++) {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
if (!inhibitMagStates) {
for (uint8_t k = 16; k<=21; k++) {
AP_NavEKF: Cleaned up on-ground state and covariance update logic
2013-12-30 19:21:04 -04:00
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
}
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
}
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
for (uint8_t i = 0; i<=21; i++) {
for (uint8_t j = 0; j<=21; j++) {
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
P[i][j] = P[i][j] - KHP[i][j];
}
}
}
obsIndex = obsIndex + 1;
}
else
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// set flags to indicate to other processes that fusion has not been performed and is not required on the next time step
AP_NavEKF: Load levelling scheduler - first attempt
2013-12-30 04:58:24 -04:00
magFusePerformed = false;
magFuseRequired = false;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF: improved covariance matrix numerical stability protection
2014-01-03 15:59:47 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// force the covariance matrix to be symmetrical and limit the variances to prevent
AP_NavEKF : Enable staticMode to be set externally
2014-01-30 18:39:11 -04:00
// ill-condiioning.
AP_NavEKF: improved covariance matrix numerical stability protection
2014-01-03 15:59:47 -04:00
ForceSymmetry();
ConstrainVariances();
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// stop performance timer
AP_NavEKF: added perf counters on PX4
2013-12-30 06:41:28 -04:00
perf_end(_perf_FuseMagnetometer);
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// fuse true airspeed measurements
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
void NavEKF::FuseAirspeed()
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// start performance timer
AP_NavEKF: added perf counters on PX4
2013-12-30 06:41:28 -04:00
perf_begin(_perf_FuseAirspeed);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// declarations
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
float vn;
float ve;
float vd;
float vwn;
float vwe;
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
float EAS2TAS = _ahrs->get_EAS2TAS();
AP_NavEKF: Added constraints to Mavlink tuneable parameters
2014-01-22 02:32:28 -04:00
const float R_TAS = sq(constrain_float(_easNoise, 0.5f, 5.0f) * constrain_float(EAS2TAS, 0.9f, 10.0f));
AP_NavEKF: use vectorN to make indexes safe
2013-12-29 22:25:02 -04:00
Vector3f SH_TAS;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
float SK_TAS;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
Vector22 H_TAS;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
float VtasPred;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// copy required states to local variable names
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
vn = statesAtVtasMeasTime.velocity.x;
ve = statesAtVtasMeasTime.velocity.y;
vd = statesAtVtasMeasTime.velocity.z;
vwn = statesAtVtasMeasTime.wind_vel.x;
vwe = statesAtVtasMeasTime.wind_vel.y;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate the predicted airspeed
AP_NavEKF : increased useage of helper functions
2014-03-10 06:08:15 -03:00
VtasPred = pythagorous3((ve - vwe) , (vn - vwn) , vd);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// perform fusion of True Airspeed measurement
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
if (VtasPred > 1.0f)
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate observation jacobians
AP_NavEKF : increased useage of helper functions
2014-03-10 06:08:15 -03:00
SH_TAS[0] = 1.0f/VtasPred;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2;
SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2;
for (uint8_t i=0; i<=21; i++) H_TAS[i] = 0.0f;
H_TAS[4] = SH_TAS[2];
H_TAS[5] = SH_TAS[1];
H_TAS[6] = vd*SH_TAS[0];
H_TAS[14] = -SH_TAS[2];
H_TAS[15] = -SH_TAS[1];
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate Kalman gains
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
float temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][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[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
if (temp >= R_TAS) {
SK_TAS = 1.0f / temp;
AP_NavEKF: fixed up handling of bitfields in faultStatus Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 08:42:39 -03:00
faultStatus.bad_airspeed = false;
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we increase the wind state variances and try again next time
P[14][14] += 0.05f*R_TAS;
P[15][15] += 0.05f*R_TAS;
AP_NavEKF: fixed up handling of bitfields in faultStatus Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 08:42:39 -03:00
faultStatus.bad_airspeed = true;
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
return;
}
Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][14]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][15]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]);
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][14]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][15]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]);
Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][14]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][15]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]);
Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][14]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][15]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]);
Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][14]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][15]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]);
Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][14]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][15]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]);
Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][14]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][15]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]);
Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][14]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][15]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]);
Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][14]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][15]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]);
Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][14]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][15]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]);
Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][14]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][15]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]);
Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][14]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][15]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]);
Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// this term has been zeroed to improve stability of the Z accel bias
AP_NavEKF : Improved stability of Z accel bias
2014-02-21 00:24:09 -04:00
Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]);
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]);
Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]);
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]);
Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]);
Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]);
Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]);
Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]);
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate measurement innovation variance
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
varInnovVtas = 1.0f/SK_TAS;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate measurement innovation
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
innovVtas = VtasPred - VtasMeas;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
// calculate the innovation consistency test ratio
tasTestRatio = sq(innovVtas) / (sq(_tasInnovGate) * varInnovVtas);
// test the ratio before fusing data
if (tasTestRatio < 1.0f)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
// correct the state vector
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t j=0; j<=21; j++)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
states[j] = states[j] - Kfusion[j] * innovVtas;
}
AP_NavEKF: use quaternion normalize and cleanup sqrt calls
2013-12-30 02:32:09 -04:00
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
state.quat.normalize();
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// correct the covariance P = (I - K*H)*P
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// take advantage of the empty columns in H to reduce the number of operations
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t i = 0; i<=21; i++)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0;
for (uint8_t j = 4; j<=6; j++)
{
KH[i][j] = Kfusion[i] * H_TAS[j];
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t j = 7; j<=13; j++) KH[i][j] = 0.0;
for (uint8_t j = 14; j<=15; j++)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
KH[i][j] = Kfusion[i] * H_TAS[j];
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t j = 16; j<=21; j++) KH[i][j] = 0.0;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t i = 0; i<=21; i++)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t j = 0; j<=21; j++)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
KHP[i][j] = 0;
for (uint8_t k = 4; k<=6; k++)
{
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t k = 14; k<=15; k++)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
}
}
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t i = 0; i<=21; i++)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t j = 0; j<=21; j++)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
P[i][j] = P[i][j] - KHP[i][j];
}
}
}
}
AP_NavEKF: improved covariance matrix numerical stability protection
2014-01-03 15:59:47 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// force the covariance matrix to me symmetrical and limit the variances to prevent ill-condiioning.
AP_NavEKF: improved covariance matrix numerical stability protection
2014-01-03 15:59:47 -04:00
ForceSymmetry();
ConstrainVariances();
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// stop performance timer
AP_NavEKF: added perf counters on PX4
2013-12-30 06:41:28 -04:00
perf_end(_perf_FuseAirspeed);
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// fuse sythetic sideslip measurement of zero
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
void NavEKF::FuseSideslip()
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// start performance timer
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
perf_begin(_perf_FuseSideslip);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// declarations
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
float q0;
float q1;
float q2;
float q3;
float vn;
float ve;
float vd;
float vwn;
float vwe;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
const float R_BETA = 0.03f; // assume a sideslip angle RMS of ~10 deg
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
float SH_BETA[13];
float SK_BETA[8];
Vector3f vel_rel_wind;
Vector22 H_BETA;
float innovBeta;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// copy required states to local variable names
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
q0 = state.quat[0];
q1 = state.quat[1];
q2 = state.quat[2];
q3 = state.quat[3];
vn = state.velocity.x;
ve = state.velocity.y;
vd = state.velocity.z;
vwn = state.wind_vel.x;
vwe = state.wind_vel.y;
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate predicted wind relative velocity in NED
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
vel_rel_wind.x = vn - vwn;
vel_rel_wind.y = ve - vwe;
vel_rel_wind.z = vd;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// rotate into body axes
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
vel_rel_wind = prevTnb * vel_rel_wind;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// perform fusion of assumed sideslip = 0
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
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);
AP_NavEKF: Synthetic sideslip fusion numerical error protections
2014-04-26 21:08:52 -03:00
if (fabsf(SH_BETA[0]) <= 1e-9f) {
AP_NavEKF: fixed up handling of bitfields in faultStatus Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 08:42:39 -03:00
faultStatus.bad_sideslip = true;
AP_NavEKF: Synthetic sideslip fusion numerical error protections
2014-04-26 21:08:52 -03:00
return;
AP_NavEKF: Improved divergence detection and reset Divergence is now detected by looking for very large changes in the gyro bias. This will cause the filter to be reset and declared unhealthy for 10 seconds. Don't reset filterDiverged status immediately during reset Set filterDiverged true if covariance blows up Add fault status reporting
2014-05-12 04:35:22 -03:00
} else {
AP_NavEKF: fixed up handling of bitfields in faultStatus Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 08:42:39 -03:00
faultStatus.bad_sideslip = false;
AP_NavEKF: Synthetic sideslip fusion numerical error protections
2014-04-26 21:08:52 -03:00
}
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
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;
for (uint8_t i=0; i<=21; i++) {
H_BETA[i] = 0.0f;
}
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);
H_BETA[14] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7];
H_BETA[15] = SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2) - SH_BETA[6];
// Calculate Kalman gains
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
float temp = (R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[14][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[15][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[14][14]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][14]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][14]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[15][14]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][14]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][14]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][14]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][14]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][14]*(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[14][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[15][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[14][15]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][15]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][15]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[15][15]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][15]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][15]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][15]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][15]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][15]*(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[14][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[15][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[14][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[15][1]*(SH_BETA[6] - SH_BETA[1]*SH_B
if (temp >= R_BETA) {
SK_BETA[0] = 1.0f / temp;
AP_NavEKF: fixed up handling of bitfields in faultStatus Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 08:42:39 -03:00
faultStatus.bad_sideslip = false;
AP_NavEKF: Synthetic sideslip fusion numerical error protections
2014-04-26 21:08:52 -03:00
} else {
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
// the calculation is badly conditioned, so we cannot perform fusion on this step
AP_NavEKF: fixed up handling of bitfields in faultStatus Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 08:42:39 -03:00
faultStatus.bad_sideslip = true;
AP_NavEKF: Add protection for badly conditioned innovation variances
2014-05-05 08:06:30 -03:00
return;
AP_NavEKF: Synthetic sideslip fusion numerical error protections
2014-04-26 21:08:52 -03:00
}
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
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][14]*SK_BETA[1] - P[0][15]*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][14]*SK_BETA[1] - P[1][15]*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][14]*SK_BETA[1] - P[2][15]*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][14]*SK_BETA[1] - P[3][15]*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][14]*SK_BETA[1] - P[4][15]*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][14]*SK_BETA[1] - P[5][15]*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][14]*SK_BETA[1] - P[6][15]*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][14]*SK_BETA[1] - P[7][15]*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][14]*SK_BETA[1] - P[8][15]*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][14]*SK_BETA[1] - P[9][15]*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][14]*SK_BETA[1] - P[10][15]*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][14]*SK_BETA[1] - P[11][15]*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][14]*SK_BETA[1] - P[12][15]*SK_BETA[2]);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// this term has been zeroed to improve stability of the Z accel bias
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
Kfusion[13] = 0.0f;//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][14]*SK_BETA[1] - P[13][15]*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][14]*SK_BETA[1] - P[14][15]*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][14]*SK_BETA[1] - P[15][15]*SK_BETA[2]);
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// 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][14]*SK_BETA[1] - P[16][15]*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][14]*SK_BETA[1] - P[17][15]*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][14]*SK_BETA[1] - P[18][15]*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][14]*SK_BETA[1] - P[19][15]*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][14]*SK_BETA[1] - P[20][15]*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][14]*SK_BETA[1] - P[21][15]*SK_BETA[2]);
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
AP_NavEKF: Synthetic sideslip fusion numerical error protections
2014-04-26 21:08:52 -03:00
// calculate predicted sideslip angle and innovation using small angle approximation
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
innovBeta = vel_rel_wind.y / vel_rel_wind.x;
AP_NavEKF: Synthetic sideslip fusion numerical error protections
2014-04-26 21:08:52 -03:00
// reject measurement if greater than 3-sigma inconsistency
if (innovBeta > 0.5f) {
return;
}
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
// correct the state vector
for (uint8_t j=0; j<=21; j++)
{
states[j] = states[j] - Kfusion[j] * innovBeta;
}
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
state.quat.normalize();
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
// correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in H to reduce the
// number of operations
for (uint8_t i = 0; i<=21; i++)
{
for (uint8_t j = 0; j<=6; j++)
{
KH[i][j] = Kfusion[i] * H_BETA[j];
}
for (uint8_t j = 7; j<=13; j++) KH[i][j] = 0.0;
for (uint8_t j = 14; j<=15; j++)
{
KH[i][j] = Kfusion[i] * H_BETA[j];
}
for (uint8_t j = 16; j<=21; j++) KH[i][j] = 0.0;
}
for (uint8_t i = 0; i<=21; i++)
{
for (uint8_t j = 0; j<=21; j++)
{
KHP[i][j] = 0;
for (uint8_t k = 0; k<=6; k++)
{
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
}
for (uint8_t k = 14; k<=15; k++)
{
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
}
}
}
for (uint8_t i = 0; i<=21; i++)
{
for (uint8_t j = 0; j<=21; j++)
{
P[i][j] = P[i][j] - KHP[i][j];
}
}
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// force the covariance matrix to me symmetrical and limit the variances to prevent ill-condiioning.
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
ForceSymmetry();
ConstrainVariances();
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// stop the performance timer
AP_NavEKF : Add sideslip constraint to allow plane flight without magnetometer
2014-03-06 05:43:24 -04:00
perf_end(_perf_FuseSideslip);
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// zero specified range of rows in the state covariance matrix
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
void NavEKF::zeroRows(Matrix22 &covMat, uint8_t first, uint8_t last)
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
uint8_t row;
for (row=first; row<=last; row++)
{
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
memset(&covMat[row][0], 0, sizeof(covMat[0][0])*22);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// zero specified range of columns in the state covariance matrix
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
void NavEKF::zeroCols(Matrix22 &covMat, uint8_t first, uint8_t last)
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
uint8_t row;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (row=0; row<=21; row++)
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
AP_NavEKF: allow filter to run as single or double precision useful for checking if there are numerical precision issues
2014-01-03 00:37:19 -04:00
memset(&covMat[row][first], 0, sizeof(covMat[0][0])*(1+last-first));
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// store states in a history array along with time stamp
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
void NavEKF::StoreStates()
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF : Fix bug that limits copter sensor delay compensation to 125 msec
2014-03-21 06:56:32 -03:00
// Don't need to store states more often than every 10 msec
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
if (imuSampleTime_ms - lastStateStoreTime_ms >= 10) {
lastStateStoreTime_ms = imuSampleTime_ms;
AP_NavEKF : Fix bug that limits copter sensor delay compensation to 125 msec
2014-03-21 06:56:32 -03:00
if (storeIndex > 49) {
storeIndex = 0;
}
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
storedStates[storeIndex] = state;
AP_NavEKF : Fix bug that limits copter sensor delay compensation to 125 msec
2014-03-21 06:56:32 -03:00
statetimeStamp[storeIndex] = lastStateStoreTime_ms;
storeIndex = storeIndex + 1;
}
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// reset the stored state history and store the current state
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
void NavEKF::StoreStatesReset()
{
// clear stored state history
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
memset(&storedStates[0], 0, sizeof(storedStates));
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
// store current state vector in first column
storeIndex = 0;
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
storedStates[storeIndex] = state;
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
statetimeStamp[storeIndex] = imuSampleTime_ms;
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
storeIndex = storeIndex + 1;
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// recall state vector stored at closest time to the one specified by msec
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
void NavEKF::RecallStates(state_elements &statesForFusion, uint32_t msec)
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
uint32_t timeDelta;
uint32_t bestTimeDelta = 200;
uint8_t bestStoreIndex = 0;
for (uint8_t i=0; i<=49; i++)
{
timeDelta = msec - statetimeStamp[i];
if (timeDelta < bestTimeDelta)
{
bestStoreIndex = i;
bestTimeDelta = timeDelta;
}
}
if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
{
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
statesForFusion = storedStates[bestStoreIndex];
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
else // otherwise output current state
{
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
statesForFusion = state;
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate nav to body quaternions from body to nav rotation matrix
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
void NavEKF::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
// Calculate the body to nav cosine matrix
AP_NavEKF: first working GPS fusion
2013-12-29 23:43:29 -04:00
quat.rotation_matrix(Tbn);
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return the Euler roll, pitch and yaw angle in radians
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
void NavEKF::getEulerAngles(Vector3f &euler) const
AP_NavEKF: added euler angle functions
2013-12-29 08:20:53 -04:00
{
AP_NavEKF: Quaternion::to_euler now uses references instead of pointers
2014-10-13 00:41:59 -03:00
state.quat.to_euler(euler.x, euler.y, euler.z);
AP_NavEKF: Added pitch and roll inertial sensor to body trim adjustment
2014-01-17 08:08:14 -04:00
euler = euler - _ahrs->get_trim();
AP_NavEKF: added euler angle functions
2013-12-29 08:20:53 -04:00
}
AP_NavEKF: Added getAccelNED function
2014-10-31 19:06:04 -03:00
// This returns the specific forces in the NED frame
void NavEKF::getAccelNED(Vector3f &accelNED) const {
accelNED = velDotNED;
accelNED.z -= GRAVITY_MSS;
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return NED velocity in m/s
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
void NavEKF::getVelNED(Vector3f &vel) const
AP_NavEKF: Load levelling scheduler - first attempt
2013-12-30 04:58:24 -04:00
{
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
vel = state.velocity;
AP_NavEKF: Load levelling scheduler - first attempt
2013-12-30 04:58:24 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return the last calculated NED position relative to the reference point (m).
// return false if no position is available
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
bool NavEKF::getPosNED(Vector3f &pos) const
AP_NavEKF: Added public function to get NED position
2013-12-31 00:27:35 -04:00
{
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
pos = state.position;
AP_NavEKF: Added public function to get NED position
2013-12-31 00:27:35 -04:00
return true;
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return body axis gyro bias estimates in rad/sec
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
void NavEKF::getGyroBias(Vector3f &gyroBias) const
AP_NavEKF: bug fixes for height fusion and covariance initialisation
2013-12-31 17:54:56 -04:00
{
AP_NavEKF: catch covarience errors and reset filter this catches covariance values beyond a reasonable limit and resets the filter is they happen
2014-04-21 02:24:16 -03:00
if (dtIMU == 0) {
gyroBias.zero();
return;
}
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
gyroBias = state.gyro_bias / dtIMU;
AP_NavEKF: bug fixes for height fusion and covariance initialisation
2013-12-31 17:54:56 -04:00
}
AP_NavEKF : Add public method to reset gyro bias states
2014-10-28 22:21:42 -03:00
// reset the body axis gyro bias states to zero and re-initialise the corresponding covariances
void NavEKF::resetGyroBias(void)
{
state.gyro_bias.zero();
zeroRows(P,10,12);
zeroCols(P,10,12);
AP_NavEKF : add INIT_GYRO_BIAS_UNCERTAINTY definition
2014-10-29 05:32:57 -03:00
P[10][10] = sq(radians(INIT_GYRO_BIAS_UNCERTAINTY * dtIMU));
AP_NavEKF : Add public method to reset gyro bias states
2014-10-28 22:21:42 -03:00
P[11][11] = P[10][10];
P[12][12] = P[10][10];
}
AP_NavEKF: Split getAccelBias into getAccelZBias and getIMU1Weighting
2014-10-30 03:42:47 -03:00
// return weighting of first IMU in blending function
void NavEKF::getIMU1Weighting(float &ret) const
AP_NavEKF: bug fixes for height fusion and covariance initialisation
2013-12-31 17:54:56 -04:00
{
AP_NavEKF: Split getAccelBias into getAccelZBias and getIMU1Weighting
2014-10-30 03:42:47 -03:00
ret = IMU1_weighting;
}
// return the individual Z-accel bias estimates in m/s^2
void NavEKF::getAccelZBias(float &zbias1, float &zbias2) const {
AP_NavEKF: catch covarience errors and reset filter this catches covariance values beyond a reasonable limit and resets the filter is they happen
2014-04-21 02:24:16 -03:00
if (dtIMU == 0) {
AP_NavEKF: Split getAccelBias into getAccelZBias and getIMU1Weighting
2014-10-30 03:42:47 -03:00
zbias1 = 0;
zbias2 = 0;
AP_NavEKF: catch covarience errors and reset filter this catches covariance values beyond a reasonable limit and resets the filter is they happen
2014-04-21 02:24:16 -03:00
} else {
AP_NavEKF: Split getAccelBias into getAccelZBias and getIMU1Weighting
2014-10-30 03:42:47 -03:00
zbias1 = state.accel_zbias1 / dtIMU;
zbias2 = state.accel_zbias2 / dtIMU;
AP_NavEKF: catch covarience errors and reset filter this catches covariance values beyond a reasonable limit and resets the filter is they happen
2014-04-21 02:24:16 -03:00
}
AP_NavEKF: bug fixes for height fusion and covariance initialisation
2013-12-31 17:54:56 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
void NavEKF::getWind(Vector3f &wind) const
AP_NavEKF : Additional flash logging
2014-01-30 18:11:54 -04:00
{
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
wind.x = state.wind_vel.x;
wind.y = state.wind_vel.y;
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
wind.z = 0.0f; // currently don't estimate this
AP_NavEKF : Additional flash logging
2014-01-30 18:11:54 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return earth magnetic field estimates in measurement units / 1000
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
void NavEKF::getMagNED(Vector3f &magNED) const
AP_NavEKF: bug fixes for height fusion and covariance initialisation
2013-12-31 17:54:56 -04:00
{
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
magNED = state.earth_magfield * 1000.0f;
AP_NavEKF: bug fixes for height fusion and covariance initialisation
2013-12-31 17:54:56 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return body magnetic field estimates in measurement units / 1000
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
void NavEKF::getMagXYZ(Vector3f &magXYZ) const
AP_NavEKF: bug fixes for height fusion and covariance initialisation
2013-12-31 17:54:56 -04:00
{
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
magXYZ = state.body_magfield*1000.0f;
AP_NavEKF: bug fixes for height fusion and covariance initialisation
2013-12-31 17:54:56 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return the last calculated latitude, longitude and height
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
bool NavEKF::getLLH(struct Location &loc) const
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
loc.lat = _ahrs->get_home().lat;
loc.lng = _ahrs->get_home().lng;
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
loc.alt = _ahrs->get_home().alt - state.position.z*100;
AP_NavEKF: ensure get_position() fills in flags
2014-07-24 22:40:26 -03:00
loc.flags.relative_alt = 0;
loc.flags.terrain_alt = 0;
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
location_offset(loc, state.position.x, state.position.y);
AP_NavEKF: enable airspeed and fix getLLH()
2013-12-30 01:19:50 -04:00
return true;
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
void NavEKF::SetFlightAndFusionModes()
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// determine if the vehicle is manoevring
if (accNavMagHoriz > 0.5f){
manoeuvring = true;
AP_NavEKF : Add low speed magnetometer calibration option
2014-03-06 02:13:22 -04:00
} else {
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
manoeuvring = false;
}
// if we are a fly forward type vehicle, then in-air mode can be determined through a combination of speed and height criteria
if (assume_zero_sideslip()) {
// Evaluate a numerical score that defines the likelihood we are in the air
const AP_Airspeed *airspeed = _ahrs->get_airspeed();
float gndSpdSq = sq(velNED[0]) + sq(velNED[1]);
AP_NavEKF: Clean up flight detector logic
2014-10-31 23:23:43 -03:00
uint8_t inAirSum = 0;
bool highGndSpdStage2 = false;
// trigger at 8 m/s airspeed
if (airspeed && airspeed->use() && airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 8.0f) {
inAirSum++;
}
// this will trigger during change in baro height
if (fabsf(_baro.get_climb_rate()) > 0.5f) {
inAirSum++;
}
// trigger at 3 m/s GPS velocity
if (gndSpdSq > 9.0f) {
inAirSum++;
}
// trigger at 6 m/s GPS velocity
if (gndSpdSq > 36.0f) {
highGndSpdStage2 = true;
inAirSum++;
}
// trigger at 9 m/s GPS velocity
if (gndSpdSq > 81.0f) {
inAirSum++;
}
// trigger if more than 15m away from initial height
if (fabsf(hgtMea) > 15.0f) {
inAirSum++;
}
// this will trigger due to air turbulence during flight
if (accNavMag > 0.5f) {
inAirSum++;
}
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// if we on-ground then 4 or more out of 7 criteria are required to transition to the
// in-air mode and we also need enough GPS velocity to be able to calculate a reliable ground track heading
if (onGround && (inAirSum >= 4) && highGndSpdStage2) {
AP_NavEKF : improved on-ground, in-air check and GPS yaw alignment
2014-03-09 20:49:34 -03:00
onGround = false;
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
}
// if is possible we are in flight, set the time this condition was last detected
if (inAirSum >= 1) {
airborneDetectTime_ms = imuSampleTime_ms;
}
// after 5 seconds of not detecting a possible flight condition, we transition to on-ground mode
if(!onGround && ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) {
AP_NavEKF : improved on-ground, in-air check and GPS yaw alignment
2014-03-09 20:49:34 -03:00
onGround = true;
}
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// perform a yaw alignment check against GPS if exiting on-ground mode
// this is done to protect against unrecoverable heading alignment errors due to compass faults
if (!onGround && prevOnGround) {
AP_NavEKF : Clean up angle and mag field initialisation This path reduces duplicated code, eliminates unused variables and causes the earth magnetic field states to be reset when exiting static mode which will occur every time copter is armed. This enables copters to be powered on and initialised inside vehicles or houses, without bad earth field values affecting flight.
2014-03-24 04:34:05 -03:00
alignYawGPS();
AP_NavEKF : improved static and on-ground mode selection logic
2014-03-09 14:39:59 -03:00
}
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// If we aren't using an airspeed sensor we set the wind velocity to the reciprocal
// of the velocity vector and scale states so that the wind speed is equal to 3m/s. This helps prevent gains
// being too high at the start of flight if launching into a headwind until the first turn when the EKF can form
// a wind speed estimate and also corrects bad initial wind estimates due to heading errors
if (!onGround && prevOnGround && !useAirspeed()) {
AP_NavEKF : Enable calc of wind velocity when not using airspeed sensing This patch also cleans up the logic associated with use of the synthetic sideslip measurement so that it can never be used for a non fly-forward vehicle type This patch adds functionality that initialises the wind-speed vector to the reciprocal of the ground speed vector, and scaled to 6 m/s. On average this gives a better initial wind velocity estimate on launch by assuming: a) launch will be into wind b) wind speed is equal to global average It also helps prevent a headwind causing initial underestimation of airspeed causing high autopilot gains and limit cycles on climb-out, until first turn when the EKF is able to estimate the wind.
2014-04-10 05:05:49 -03:00
setWindVelStates();
}
AP_NavEKF : Add low speed magnetometer calibration option
2014-03-06 02:13:22 -04:00
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// store current on-ground status for next time
AP_NavEKF : improved static and on-ground mode selection logic
2014-03-09 14:39:59 -03:00
prevOnGround = onGround;
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// If we are on ground, or in static mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || staticMode);
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// request mag calibration for both in-air and manoeuvre threshold options
bool magCalRequested = ((_magCal == 0) && !onGround) || ((_magCal == 1) && manoeuvring);
// deny mag calibration request if we aren't using the compass, are in the pre-arm static mode or it has been inhibited by the user
bool magCalDenied = (!use_compass() || staticMode || (_magCal == 2));
// inhibit the magnetic field calibration if not requested or denied
inhibitMagStates = (!magCalRequested || magCalDenied);
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// initialise the covariance matrix
AP_NavEKF : Clean up angle and mag field initialisation This path reduces duplicated code, eliminates unused variables and causes the earth magnetic field states to be reset when exiting static mode which will occur every time copter is armed. This enables copters to be powered on and initialised inside vehicles or houses, without bad earth field values affecting flight.
2014-03-24 04:34:05 -03:00
void NavEKF::CovarianceInit()
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF: explicitly zeroed covariance matrix priro to setting initial values
2014-01-03 16:57:52 -04:00
// zero the matrix
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t i=1; i<=21; i++)
AP_NavEKF: explicitly zeroed covariance matrix priro to setting initial values
2014-01-03 16:57:52 -04:00
{
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t j=0; j<=21; j++)
AP_NavEKF: explicitly zeroed covariance matrix priro to setting initial values
2014-01-03 16:57:52 -04:00
{
P[i][j] = 0.0f;
}
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// quaternions - TODO better maths for initial quaternion covariances that uses roll, pitch and yaw
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
P[0][0] = 1.0e-9f;
AP_NavEKF: fixed bug in variance constraint code
2014-01-05 01:15:04 -04:00
P[1][1] = 0.25f*sq(radians(1.0f));
P[2][2] = 0.25f*sq(radians(1.0f));
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
P[3][3] = 0.25f*sq(radians(1.0f));
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// velocities
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
P[4][4] = sq(0.7f);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
P[5][5] = P[4][4];
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
P[6][6] = sq(0.7f);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// positions
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
P[7][7] = sq(15.0f);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
P[8][8] = P[7][7];
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
P[9][9] = sq(5.0f);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// delta angle biases
AP_NavEKF : add INIT_GYRO_BIAS_UNCERTAINTY definition
2014-10-29 05:32:57 -03:00
P[10][10] = sq(radians(INIT_GYRO_BIAS_UNCERTAINTY * dtIMU));
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
P[11][11] = P[10][10];
P[12][12] = P[10][10];
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// Z delta velocity bias
AP_NavEKF : modifed Zaccel bias noise parameter to prevent unstable bias estimate
2014-03-19 06:48:31 -03:00
P[13][13] = 0.0f;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// wind velocities
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
P[14][14] = 0.0f;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
P[15][15] = P[14][14];
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// earth magnetic field
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
P[16][16] = 0.0f;
AP_NavEKF : Fix bug in initial East mag field state variance
2014-03-05 03:52:42 -04:00
P[17][17] = P[16][16];
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
P[18][18] = P[16][16];
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// body magnetic field
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
P[19][19] = 0.0f;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
P[20][20] = P[19][19];
P[21][21] = P[19][19];
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// force symmetry on the covariance matrix to prevent ill-conditioning
AP_NavEKF: improved covariance matrix numerical stability protection
2014-01-03 15:59:47 -04:00
void NavEKF::ForceSymmetry()
AP_NavEKF: covariance protection (not enabled) and tuning changes for stability
2014-01-02 22:10:38 -04:00
{
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t i=1; i<=21; i++)
AP_NavEKF: covariance protection (not enabled) and tuning changes for stability
2014-01-02 22:10:38 -04:00
{
for (uint8_t j=0; j<=i-1; j++)
{
AP_NavEKF: improved covariance matrix numerical stability protection
2014-01-03 15:59:47 -04:00
float temp = 0.5f*(P[i][j] + P[j][i]);
AP_NavEKF: covariance protection (not enabled) and tuning changes for stability
2014-01-02 22:10:38 -04:00
P[i][j] = temp;
P[j][i] = temp;
}
}
AP_NavEKF: improved covariance matrix numerical stability protection
2014-01-03 15:59:47 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// copy covariances across from covariance prediction calculation and fix numerical errors
AP_NavEKF : covariance prediction cleanup
2014-02-18 04:27:23 -04:00
void NavEKF::CopyAndFixCovariances()
{
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// copy predicted variances
for (uint8_t i=0; i<=21; i++) {
P[i][i] = nextP[i][i];
AP_NavEKF : covariance prediction cleanup
2014-02-18 04:27:23 -04:00
}
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// copy predicted covariances and force symmetry
for (uint8_t i=1; i<=21; i++) {
for (uint8_t j=0; j<=i-1; j++)
AP_NavEKF : covariance prediction cleanup
2014-02-18 04:27:23 -04:00
{
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]);
P[j][i] = P[i][j];
AP_NavEKF : covariance prediction cleanup
2014-02-18 04:27:23 -04:00
}
}
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// constrain variances (diagonal terms) in the state covariance matrix to prevent ill-conditioning
AP_NavEKF: improved covariance matrix numerical stability protection
2014-01-03 15:59:47 -04:00
void NavEKF::ConstrainVariances()
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
for (uint8_t i=0; i<=3; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // quaternions
for (uint8_t i=4; i<=6; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // velocities
for (uint8_t i=7; i<=9; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f); // positions
for (uint8_t i=10; i<=12; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtIMU)); // delta angle biases
P[13][13] = constrain_float(P[13][13],0.0f,sq(10.0f * dtIMU)); // delta velocity bias
for (uint8_t i=14; i<=15; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // earth magnetic field
for (uint8_t i=16; i<=21; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // body magnetic field
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// constrain states to prevent ill-conditioning
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
void NavEKF::ConstrainStates()
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// quaternions are limited between +-1
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t i=0; i<=3; i++) states[i] = constrain_float(states[i],-1.0f,1.0f);
// velocity limit 500 m/sec (could set this based on some multiple of max airspeed * EAS2TAS)
for (uint8_t i=4; i<=6; i++) states[i] = constrain_float(states[i],-5.0e2f,5.0e2f);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// position limit 1000 km - TODO apply circular limit
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t i=7; i<=8; i++) states[i] = constrain_float(states[i],-1.0e6f,1.0e6f);
// height limit covers home alt on everest through to home alt at SL and ballon drop
AP_NavEKF : fixed bug in height limit range
2014-02-23 03:15:31 -04:00
states[9] = constrain_float(states[9],-4.0e4f,1.0e4f);
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
// gyro bias limit ~6 deg/sec (this needs to be set based on manufacturers specs)
for (uint8_t i=10; i<=12; i++) states[i] = constrain_float(states[i],-0.1f*dtIMU,0.1f*dtIMU);
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
// Z accel bias limit 1.0 m/s^2 (this needs to be finalised from test data)
states[13] = constrain_float(states[13],-1.0f*dtIMU,1.0f*dtIMU);
states[22] = constrain_float(states[22],-1.0f*dtIMU,1.0f*dtIMU);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// wind velocity limit 100 m/s (could be based on some multiple of max airspeed * EAS2TAS) - TODO apply circular limit
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t i=14; i<=15; i++) states[i] = constrain_float(states[i],-100.0f,100.0f);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// earth magnetic field limit
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t i=16; i<=18; i++) states[i] = constrain_float(states[i],-1.0f,1.0f);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// body magnetic field limit
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
for (uint8_t i=19; i<=21; i++) states[i] = constrain_float(states[i],-0.5f,0.5f);
AP_NavEKF: covariance protection (not enabled) and tuning changes for stability
2014-01-02 22:10:38 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// update IMU delta angle and delta velocity measurements
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
void NavEKF::readIMUData()
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
Vector3f accel1; // acceleration vector in XYZ body axes measured by IMU1 (m/s^2)
Vector3f accel2; // acceleration vector in XYZ body axes measured by IMU2 (m/s^2)
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
// the imu sample time is sued as a common time reference throughout the filter
imuSampleTime_ms = hal.scheduler->millis();
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// limit IMU delta time to prevent numerical problems elsewhere
dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(), 0.001f, 1.0f);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
AP_NavEKF : Use average of dual IMU gyro data when available
2014-03-01 22:05:53 -04:00
// get accels and gyro data from dual sensors if healthy
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
if (_ahrs->get_ins().get_accel_health(0) && _ahrs->get_ins().get_accel_health(1)) {
accel1 = _ahrs->get_ins().get_accel(0);
accel2 = _ahrs->get_ins().get_accel(1);
AP_NavEKF: only use the active accel from DCM if fly_forward is set
2014-02-27 02:40:13 -04:00
} else {
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
accel1 = _ahrs->get_ins().get_accel();
accel2 = accel1;
AP_NavEKF: only use the active accel from DCM if fly_forward is set
2014-02-27 02:40:13 -04:00
}
AP_NavEKF: don't assume the number of gyros == number of accels
2014-03-01 22:38:35 -04:00
// average the available gyro sensors
angRate.zero();
uint8_t gyro_count = 0;
for (uint8_t i = 0; i<_ahrs->get_ins().get_gyro_count(); i++) {
if (_ahrs->get_ins().get_gyro_health(i)) {
angRate += _ahrs->get_ins().get_gyro(i);
gyro_count++;
}
}
if (gyro_count != 0) {
angRate /= gyro_count;
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
// trapezoidal integration
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
dAngIMU = (angRate + lastAngRate) * dtIMU * 0.5f;
lastAngRate = angRate;
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
dVelIMU1 = (accel1 + lastAccel1) * dtIMU * 0.5f;
lastAccel1 = accel1;
dVelIMU2 = (accel2 + lastAccel2) * dtIMU * 0.5f;
lastAccel2 = accel2;
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// check for new valid GPS data and update stored measurement if available
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
void NavEKF::readGpsData()
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// check for new GPS data
AP_NavEKF: changes for new GPS API
2014-03-30 19:52:03 -03:00
if ((_ahrs->get_gps().last_message_time_ms() != lastFixTime_ms) &&
(_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D))
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// store fix time from previous read
AP_NavEKF: Pos Vel reset after long GPS timeout
2014-01-25 17:49:25 -04:00
secondLastFixTime_ms = lastFixTime_ms;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// get current fix time
AP_NavEKF: changes for new GPS API
2014-03-30 19:52:03 -03:00
lastFixTime_ms = _ahrs->get_gps().last_message_time_ms();
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// set flag that lets other functions know that new GPS data has arrived
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
newDataGps = true;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// get state vectors that were stored at the time that is closest to when the the GPS measurement
// time after accounting for measurement delays
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
RecallStates(statesAtVelTime, (imuSampleTime_ms - constrain_int16(_msecVelDelay, 0, 500)));
RecallStates(statesAtPosTime, (imuSampleTime_ms - constrain_int16(_msecPosDelay, 0, 500)));
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// read the NED velocity from the GPS
AP_NavEKF: changes for new GPS API
2014-03-30 19:52:03 -03:00
velNED = _ahrs->get_gps().velocity();
AP_NavEKF : Prevent bad user parameter causing incorrect GPS fusion
2014-03-13 06:10:17 -03:00
AP_NavEKF : Reduce weighting on GPS when not enough satellites GPS measurement variance is doubled if only 5 satellites, and quadrupled if 4 or less. The GPS glitch rejection thresholds remain the same This will reduce the impact of GPS glitches on attitude.
2014-09-23 06:30:01 -03:00
// check if we have enough GPS satellites and increase the gps noise scaler if we don't
if (_ahrs->get_gps().num_sats() >= 6) {
gpsNoiseScaler = 1.0f;
} else if (_ahrs->get_gps().num_sats() == 5) {
gpsNoiseScaler = 1.4f;
AP_NavEKF: make it clear that all sat counts are covered
2014-09-30 23:54:27 -03:00
} else { // <= 4 satellites
AP_NavEKF : Reduce weighting on GPS when not enough satellites GPS measurement variance is doubled if only 5 satellites, and quadrupled if 4 or less. The GPS glitch rejection thresholds remain the same This will reduce the impact of GPS glitches on attitude.
2014-09-23 06:30:01 -03:00
gpsNoiseScaler = 2.0f;
}
AP_NavEKF: changes for new GPS API
2014-03-30 19:52:03 -03:00
// Check if GPS can output vertical velocity and set GPS fusion mode accordingly
if (!_ahrs->get_gps().have_vertical_velocity()) {
AP_NavEKF : Prevent bad user parameter causing incorrect GPS fusion
2014-03-13 06:10:17 -03:00
// vertical velocity should not be fused
if (_fusionModeGPS == 0) {
_fusionModeGPS = 1;
}
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// read latitutde and longitude from GPS and convert to NE position
AP_NavEKF: changes for new GPS API
2014-03-30 19:52:03 -03:00
const struct Location &gpsloc = _ahrs->get_gps().location();
AP_NavEKF: Fix bug in position reset logic
2014-04-23 04:37:07 -03:00
gpsPosNE = location_diff(_ahrs->get_home(), gpsloc);
// decay and limit the position offset which is applied to NE position wherever it is used throughout code to allow GPS position jumps to be accommodated gradually
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
decayGpsOffset();
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
}
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// check for new altitude measurement data and update stored measurement if available
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
void NavEKF::readHgtData()
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// check to see if baro measurement has changed so we know if a new measurement has arrived
AP_NavEKF : Bug fix for height drift due to timer wrap-around
2014-02-22 23:27:08 -04:00
if (_baro.get_last_update() != lastHgtMeasTime) {
// time stamp used to check for new measurement
lastHgtMeasTime = _baro.get_last_update();
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
AP_NavEKF : Bug fix for height drift due to timer wrap-around
2014-02-22 23:27:08 -04:00
// time stamp used to check for timeout
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
lastHgtTime_ms = imuSampleTime_ms;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// get measurement and set flag to let other functions know new data has arrived
AP_NavEKF: second attempt at higher rate baro fusion
2014-01-05 05:54:49 -04:00
hgtMea = _baro.get_altitude();
newDataHgt = true;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// get states that wer stored at the time closest to the measurement time, taking measurement delay into account
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
RecallStates(statesAtHgtTime, (imuSampleTime_ms - _msecHgtDelay));
AP_NavEKF: second attempt at higher rate baro fusion
2014-01-05 05:54:49 -04:00
} else {
newDataHgt = false;
AP_NavEKF: Modifed Vel Pos fusion to fuse height data whenever baro reading has changed
2014-01-05 03:15:13 -04:00
}
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// check for new magnetometer data and update store measurements if available
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
void NavEKF::readMagData()
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF: cope with compass going offline while in flight
2014-03-01 23:32:10 -04:00
if (use_compass() && _ahrs->get_compass()->last_update != lastMagUpdate) {
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// store time of last measurement update
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
lastMagUpdate = _ahrs->get_compass()->last_update;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// Read compass data
// We scale compass data to improve numerical conditioning
magData = _ahrs->get_compass()->get_field() * 0.001f;
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// get states stored at time closest to measurement time after allowance for measurement delay
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
RecallStates(statesAtMagMeasTime, (imuSampleTime_ms - _msecMagDelay));
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// let other processes know that new compass data has arrived
AP_NavEKF: debug updates
2013-12-29 21:12:09 -04:00
newDataMag = true;
AP_NavEKF: use float arrays when not doing bounds checking g++ is doing a lousy job of inlining VectorN, so avoid it when we don't need bounds checking for production code
2013-12-30 06:27:50 -04:00
} else {
AP_NavEKF: debug updates
2013-12-29 21:12:09 -04:00
newDataMag = false;
}
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// check for new airspeed data and update stored measurements if available
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
void NavEKF::readAirSpdData()
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// 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
AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration
2014-01-01 21:15:22 -04:00
const AP_Airspeed *aspeed = _ahrs->get_airspeed();
AP_NavEKF: Removed accel bias states
2014-01-03 03:52:37 -04:00
if (aspeed &&
AP_NavEKF: fixed check for airspeed sensor available
2014-01-03 06:40:45 -04:00
aspeed->use() &&
aspeed->last_update_ms() != lastAirspeedUpdate) {
VtasMeas = aspeed->get_airspeed() * aspeed->get_EAS2TAS();
AP_NavEKF: use float arrays when not doing bounds checking g++ is doing a lousy job of inlining VectorN, so avoid it when we don't need bounds checking for production code
2013-12-30 06:27:50 -04:00
lastAirspeedUpdate = aspeed->last_update_ms();
newDataTas = true;
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
RecallStates(statesAtVtasMeasTime, (imuSampleTime_ms - _msecTasDelay));
AP_NavEKF: use float arrays when not doing bounds checking g++ is doing a lousy job of inlining VectorN, so avoid it when we don't need bounds checking for production code
2013-12-30 06:27:50 -04:00
} else {
newDataTas = false;
AP_NavEKF: fixed build for APM environment
2013-12-29 03:37:55 -04:00
}
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// calculate the NED earth spin vector in rad/sec
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
void NavEKF::calcEarthRateNED(Vector3f &omega, int32_t latitude) const
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
{
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
float lat_rad = radians(latitude*1.0e-7f);
omega.x = earthRate*cosf(lat_rad);
AP_NavEKF: Explicitly defined numerical constants as single precision using f suffix
2013-12-29 05:48:15 -04:00
omega.y = 0;
AP_NavEKF: handle conversion of AHRS to handle altitude fixed accuracy of position for cm level lat/lng
2014-01-02 07:05:09 -04:00
omega.z = -earthRate*sinf(lat_rad);
AP_NavEKF: Inertial Navigation Code - 24 State EKF initial version converted from matlab
2013-12-25 18:58:02 -04:00
}
AP_NavEKF: only build on fast CPUs
2013-12-30 19:24:21 -04:00
AP_NavEKF : Clean up angle and mag field initialisation This path reduces duplicated code, eliminates unused variables and causes the earth magnetic field states to be reset when exiting static mode which will occur every time copter is armed. This enables copters to be powered on and initialised inside vehicles or houses, without bad earth field values affecting flight.
2014-03-24 04:34:05 -03:00
// initialise the earth magnetic field states using declination, suppled roll/pitch
// and magnetometer measurements and return initial attitude quaternion
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// if no magnetometer data, do not update magnetic field states and assume zero yaw angle
AP_NavEKF : Clean up angle and mag field initialisation This path reduces duplicated code, eliminates unused variables and causes the earth magnetic field states to be reset when exiting static mode which will occur every time copter is armed. This enables copters to be powered on and initialised inside vehicles or houses, without bad earth field values affecting flight.
2014-03-24 04:34:05 -03:00
Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
{
// declare local variables required to calculate initial orientation and magnetic field
float yaw;
Matrix3f Tbn;
Vector3f initMagNED;
Quaternion initQuat;
if (use_compass()) {
// calculate rotation matrix from body to NED frame
Tbn.from_euler(roll, pitch, 0.0f);
// read the magnetometer data
readMagData();
// rotate the magnetic field into NED axes
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
initMagNED = Tbn * magData;
AP_NavEKF : Clean up angle and mag field initialisation This path reduces duplicated code, eliminates unused variables and causes the earth magnetic field states to be reset when exiting static mode which will occur every time copter is armed. This enables copters to be powered on and initialised inside vehicles or houses, without bad earth field values affecting flight.
2014-03-24 04:34:05 -03:00
// calculate heading of mag field rel to body heading
float magHeading = atan2f(initMagNED.y, initMagNED.x);
// get the magnetic declination
float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
// calculate yaw angle rel to true north
AP_NavEKF: Reduce vulnerability to ground fixed magnetic interference Re-initialisation of the magnetic field states and yaw angle is now only performed a maximum of two times after start-up. Once when coming out of static modefor the first time (first arm event) Again (for copter only) when the altitude gain above the arming altitude exceeds 1.5m this prevents magnetic interference present at arming (eg arming on a metal roof)from corrupting the magnetic field states enough to cause bad heading errors and toilet bowling on copter
2014-12-02 04:32:59 -04:00
yaw = magDecAng - magHeading;
yawAligned = true;
// calculate initial filter quaternion states using yaw from magnetometer if mag heading healthy
// otherwise use existing heading
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
if (!badMag) {
initQuat.from_euler(roll, pitch, yaw);
} else {
initQuat = state.quat;
}
AP_NavEKF : Clean up angle and mag field initialisation This path reduces duplicated code, eliminates unused variables and causes the earth magnetic field states to be reset when exiting static mode which will occur every time copter is armed. This enables copters to be powered on and initialised inside vehicles or houses, without bad earth field values affecting flight.
2014-03-24 04:34:05 -03:00
// calculate initial Tbn matrix and rotate Mag measurements into NED
// to set initial NED magnetic field states
initQuat.rotation_matrix(Tbn);
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
initMagNED = Tbn* magData;
AP_NavEKF : Clean up angle and mag field initialisation This path reduces duplicated code, eliminates unused variables and causes the earth magnetic field states to be reset when exiting static mode which will occur every time copter is armed. This enables copters to be powered on and initialised inside vehicles or houses, without bad earth field values affecting flight.
2014-03-24 04:34:05 -03:00
// write to earth magnetic field state vector
state.earth_magfield = initMagNED;
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// clear bad magnetometer status
badMag = false;
AP_NavEKF : Clean up angle and mag field initialisation This path reduces duplicated code, eliminates unused variables and causes the earth magnetic field states to be reset when exiting static mode which will occur every time copter is armed. This enables copters to be powered on and initialised inside vehicles or houses, without bad earth field values affecting flight.
2014-03-24 04:34:05 -03:00
} else {
initQuat.from_euler(roll, pitch, 0.0f);
yawAligned = false;
}
// return attitude quaternion
return initQuat;
}
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// this function is used to do a forced 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.
AP_NavEKF : Clean up angle and mag field initialisation This path reduces duplicated code, eliminates unused variables and causes the earth magnetic field states to be reset when exiting static mode which will occur every time copter is armed. This enables copters to be powered on and initialised inside vehicles or houses, without bad earth field values affecting flight.
2014-03-24 04:34:05 -03:00
void NavEKF::alignYawGPS()
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
{
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
if ((sq(velNED[0]) + sq(velNED[1])) > 25.0f) {
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
float roll;
float pitch;
AP_NavEKF : improved on-ground, in-air check and GPS yaw alignment
2014-03-09 20:49:34 -03:00
float oldYaw;
float newYaw;
float yawErr;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
AP_NavEKF: Quaternion::to_euler now uses references instead of pointers
2014-10-13 00:41:59 -03:00
state.quat.to_euler(roll, pitch, oldYaw);
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// calculate course yaw angle
oldYaw = atan2f(state.velocity.y,state.velocity.x);
AP_NavEKF : improved on-ground, in-air check and GPS yaw alignment
2014-03-09 20:49:34 -03:00
// calculate yaw angle from GPS velocity
newYaw = atan2f(velNED[1],velNED[0]);
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// estimate the yaw error
yawErr = wrap_PI(newYaw - oldYaw);
// If the inertial course angle disagrees with the GPS by more than 45 degrees, we declare the compass as bad
badMag = (fabsf(yawErr) > 0.7854f);
// correct yaw angle using GPS ground course compass failed or if not previously aligned
if (badMag || !yawAligned) {
// correct the yaw angle
newYaw = oldYaw + yawErr;
AP_NavEKF : improved on-ground, in-air check and GPS yaw alignment
2014-03-09 20:49:34 -03:00
// calculate new filter quaternion states from Euler angles
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
state.quat.from_euler(roll, pitch, newYaw);
AP_NavEKF : improved on-ground, in-air check and GPS yaw alignment
2014-03-09 20:49:34 -03:00
// the yaw angle is now aligned so update its status
yawAligned = true;
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// reset the position and velocity states
ResetPosition();
ResetVelocity();
// reset the covariance for the quaternion, velocity and position states
AP_NavEKF : improved on-ground, in-air check and GPS yaw alignment
2014-03-09 20:49:34 -03:00
// zero the matrix entries
zeroRows(P,0,9);
zeroCols(P,0,9);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// quaternions - TODO maths that sets them based on different roll, yaw and pitch uncertainties
AP_NavEKF : improved on-ground, in-air check and GPS yaw alignment
2014-03-09 20:49:34 -03:00
P[0][0] = 1.0e-9f;
P[1][1] = 0.25f*sq(radians(1.0f));
P[2][2] = 0.25f*sq(radians(1.0f));
P[3][3] = 0.25f*sq(radians(1.0f));
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// velocities - we could have a big error coming out of static mode due to GPS lag
AP_NavEKF : improved on-ground, in-air check and GPS yaw alignment
2014-03-09 20:49:34 -03:00
P[4][4] = 400.0f;
P[5][5] = P[4][4];
P[6][6] = sq(0.7f);
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// positions - we could have a big error coming out of static mode due to GPS lag
AP_NavEKF : improved on-ground, in-air check and GPS yaw alignment
2014-03-09 20:49:34 -03:00
P[7][7] = 400.0f;
P[8][8] = P[7][7];
P[9][9] = sq(5.0f);
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
}
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
// Update magnetic field states if the magnetometer is bad
if (badMag) {
calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
}
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
}
}
AP_NavEKF: start with a wind estimate of 3m/s, when no direct measurement this will cope better with users with low roll/pitch gains, to ensure they get enough control on takeoff Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-13 06:42:23 -03:00
// This function is used to do a forced alignment of the wind velocity
// states so that they are set to the reciprocal of the ground speed
// and scaled to STARTUP_WIND_SPEED m/s. This is used when launching a
// fly-forward vehicle without an airspeed sensor on the assumption
// that launch will be into wind and STARTUP_WIND_SPEED is
// representative of typical launch wind
AP_NavEKF : Enable calc of wind velocity when not using airspeed sensing This patch also cleans up the logic associated with use of the synthetic sideslip measurement so that it can never be used for a non fly-forward vehicle type This patch adds functionality that initialises the wind-speed vector to the reciprocal of the ground speed vector, and scaled to 6 m/s. On average this gives a better initial wind velocity estimate on launch by assuming: a) launch will be into wind b) wind speed is equal to global average It also helps prevent a headwind causing initial underestimation of airspeed causing high autopilot gains and limit cycles on climb-out, until first turn when the EKF is able to estimate the wind.
2014-04-10 05:05:49 -03:00
void NavEKF::setWindVelStates()
{
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
float gndSpd = pythagorous2(state.velocity.x, state.velocity.y);
AP_NavEKF : Enable calc of wind velocity when not using airspeed sensing This patch also cleans up the logic associated with use of the synthetic sideslip measurement so that it can never be used for a non fly-forward vehicle type This patch adds functionality that initialises the wind-speed vector to the reciprocal of the ground speed vector, and scaled to 6 m/s. On average this gives a better initial wind velocity estimate on launch by assuming: a) launch will be into wind b) wind speed is equal to global average It also helps prevent a headwind causing initial underestimation of airspeed causing high autopilot gains and limit cycles on climb-out, until first turn when the EKF is able to estimate the wind.
2014-04-10 05:05:49 -03:00
if (gndSpd > 4.0f) {
AP_NavEKF: Add ability to inhibit in-flight mag cal and clean up moding This will enable in-flight magnetometer calibration to be inhibited unconditionally, This is required for long balloon carriage flights where ground speed can be high enough to put it into in-air state, but with very poor observability of magnetic field states causing bad state estimates and heading offsets to develop over time. The covariance matrix no longer has rows and columns artificially zeroed when in static mode. Instead booleans indicating whether wind or magentic field state estimation is active are used to: a) Set the process noise on these states to zero to stop their variances from increasing unchecked when not being updated, and b) Turn off updates for these states when measurement fusion is being performed. This reduces the likelihood of a badly conditioned covariance matrix forming during static mode operation. A filter divergence check has also been added that will declare the filter unhealthy if position, velocity and magnetic field observations are all failing their innovation consistency checks. This unhealthy status will persist for 10 seconds after the condition clears. AP_NavEKF: Remove unnecessary zeroing of wind covariances
2014-04-25 07:32:13 -03:00
// set the wind states to be the reciprocal of the velocity and scale
AP_NavEKF: start with a wind estimate of 3m/s, when no direct measurement this will cope better with users with low roll/pitch gains, to ensure they get enough control on takeoff Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-13 06:42:23 -03:00
float scaleFactor = STARTUP_WIND_SPEED / gndSpd;
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
state.wind_vel.x = - state.velocity.x * scaleFactor;
state.wind_vel.y = - state.velocity.y * scaleFactor;
AP_NavEKF : Enable calc of wind velocity when not using airspeed sensing This patch also cleans up the logic associated with use of the synthetic sideslip measurement so that it can never be used for a non fly-forward vehicle type This patch adds functionality that initialises the wind-speed vector to the reciprocal of the ground speed vector, and scaled to 6 m/s. On average this gives a better initial wind velocity estimate on launch by assuming: a) launch will be into wind b) wind speed is equal to global average It also helps prevent a headwind causing initial underestimation of airspeed causing high autopilot gains and limit cycles on climb-out, until first turn when the EKF is able to estimate the wind.
2014-04-10 05:05:49 -03:00
// reinitialise the wind state covariances
zeroRows(P,14,15);
zeroCols(P,14,15);
P[14][14] = 64.0f;
P[15][15] = P[14][14];
}
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return the transformation matrix from XYZ (body) to NED axes
AP_NavEKF: implemented getRotationBodyToNED()
2014-01-01 22:05:49 -04:00
void NavEKF::getRotationBodyToNED(Matrix3f &mat) const
{
AP_NavEKF: Added pitch and roll inertial sensor to body trim adjustment
2014-01-17 08:08:14 -04:00
Vector3f trim = _ahrs->get_trim();
AP_NavEKF: simplify variable handling in EKF use named state variables instead of states[] array where possible.
2014-09-30 23:50:53 -03:00
state.quat.rotation_matrix(mat);
AP_NavEKF: Added pitch and roll inertial sensor to body trim adjustment
2014-01-17 08:08:14 -04:00
mat.rotateXYinv(trim);
AP_NavEKF: implemented getRotationBodyToNED()
2014-01-01 22:05:49 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
void NavEKF::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov) 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];
AP_NavEKF : Removed double to float conversions
2014-01-22 22:30:20 -04:00
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
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
tasInnov = innovVtas;
}
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
// 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 current offsets applied to the GPS position measurements
AP_NavEKF: fixed return of offset in getVariances()
2014-04-02 17:46:56 -03:00
void NavEKF::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
{
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
velVar = sqrtf(velTestRatio);
posVar = sqrtf(posTestRatio);
hgtVar = sqrtf(hgtTestRatio);
magVar.x = sqrtf(magTestRatio.x);
magVar.y = sqrtf(magTestRatio.y);
magVar.z = sqrtf(magTestRatio.z);
AP_NavEKF : Fix bug in logging of airspeed innovation consistency ratio
2014-04-05 03:06:22 -03:00
tasVar = sqrtf(tasTestRatio);
AP_NavEKF: Fix bug in position reset logic
2014-04-23 04:37:07 -03:00
offset = gpsPosGlitchOffsetNE;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// zero stored variables - this needs to be called before a full filter initialisation
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
void NavEKF::ZeroVariables()
{
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
// initialise time stamps
imuSampleTime_ms = hal.scheduler->millis();
lastHealthyMagTime_ms = imuSampleTime_ms;
TASmsecPrev = imuSampleTime_ms;
BETAmsecPrev = imuSampleTime_ms;
lastMagUpdate = imuSampleTime_ms;
lastHgtMeasTime = imuSampleTime_ms;
lastHgtTime_ms = imuSampleTime_ms;
velFailTime = imuSampleTime_ms;
posFailTime = imuSampleTime_ms;
hgtFailTime = imuSampleTime_ms;
lastStateStoreTime_ms = imuSampleTime_ms;
lastFixTime_ms = imuSampleTime_ms;
secondLastFixTime_ms = imuSampleTime_ms;
lastDecayTime_ms = imuSampleTime_ms;
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
airborneDetectTime_ms = imuSampleTime_ms;
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
AP_NavEKF : Explicitly initialise gpsNoiseScaler to default value
2014-09-24 01:19:55 -03:00
gpsNoiseScaler = 1.0f;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
velTimeout = false;
posTimeout = false;
hgtTimeout = false;
AP_NavEKF : Modified compass fail logic If the vehicle can fly without a compass (a fly forward vehicle) then if the compass times out (large errors for more than 10 seconds, then it will be declared permanently failed and will not be used until the filter is reset
2014-07-31 06:16:38 -03:00
magTimeout = false;
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
badMag = false;
AP_NavEKF: Reduce vulnerability to ground fixed magnetic interference Re-initialisation of the magnetic field states and yaw angle is now only performed a maximum of two times after start-up. Once when coming out of static modefor the first time (first arm event) Again (for copter only) when the altitude gain above the arming altitude exceeds 1.5m this prevents magnetic interference present at arming (eg arming on a metal roof)from corrupting the magnetic field states enough to cause bad heading errors and toilet bowling on copter
2014-12-02 04:32:59 -04:00
firstArmComplete = false;
finalMagYawInit = false;
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
storeIndex = 0;
dtIMU = 0;
dt = 0;
AP_NavEKF: Fixed bug in pre GPS fix updates
2014-01-18 17:48:12 -04:00
hgtMea = 0;
AP_NavEKF: Reduce vulnerability to ground fixed magnetic interference Re-initialisation of the magnetic field states and yaw angle is now only performed a maximum of two times after start-up. Once when coming out of static modefor the first time (first arm event) Again (for copter only) when the altitude gain above the arming altitude exceeds 1.5m this prevents magnetic interference present at arming (eg arming on a metal roof)from corrupting the magnetic field states enough to cause bad heading errors and toilet bowling on copter
2014-12-02 04:32:59 -04:00
firstArmPosD = 0.0f;
AP_NavEKF : Improvements to staticMode robustness
2014-02-16 07:32:17 -04:00
storeIndex = 0;
AP_NavEKF: Improved divergence detection and reset Divergence is now detected by looking for very large changes in the gyro bias. This will cause the filter to be reset and declared unhealthy for 10 seconds. Don't reset filterDiverged status immediately during reset Set filterDiverged true if covariance blows up Add fault status reporting
2014-05-12 04:35:22 -03:00
lastGyroBias.zero();
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
prevDelAng.zero();
lastAngRate.zero();
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
lastAccel1.zero();
lastAccel2.zero();
AP_NavEKF : fixed bug in velocity rate of change filter This bug meant the velocity rate of change used to scale the GPS measurement variances was noisy and too small
2014-02-23 03:21:42 -04:00
velDotNEDfilt.zero();
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
summedDelAng.zero();
summedDelVel.zero();
AP_NavEKF: Fixed bug in pre GPS fix updates
2014-01-18 17:48:12 -04:00
velNED.zero();
AP_NavEKF: Fix bug in position reset logic
2014-04-23 04:37:07 -03:00
gpsPosGlitchOffsetNE.zero();
gpsPosNE.zero();
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
prevTnb.zero();
memset(&P[0][0], 0, sizeof(P));
memset(&nextP[0][0], 0, sizeof(nextP));
memset(&processNoise[0], 0, sizeof(processNoise));
AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-04 07:30:16 -03:00
memset(&storedStates[0], 0, sizeof(storedStates));
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
AP_NavEKF : Reduce ripple in estimates that can cause copter motor 'pulsing' This patch reduces the level of 5Hz and 10Hz 'pulsing' heard in motors due to GPS and altimeter fusion which cause a small 5Hz and 10Hz ripple on the output under some conditions. Attitude, velocity and position state corrections from GPS, altimeter and magnetometer measurements are applied incrementally in the interval from receiving the measurement to the predicted time of receipt of the next measurement. Averaging of attitude state corrections is not performed during periods of rapid rotation.
2014-08-23 23:02:29 -03:00
memset(&gpsIncrStateDelta[0], 0, sizeof(gpsIncrStateDelta));
memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta));
memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta));
gpsPosGlitchOffsetNE.zero();
AP_NavEKF : Added Z accel bias state and bootstrap initialisation method
2014-01-30 18:25:40 -04:00
}
AP_NavEKF: implemented getRotationBodyToNED()
2014-01-01 22:05:49 -04:00
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return true if we should use the airspeed sensor
AP_NavEKF: fixed detection of airspeed sensor the get_airspeed() AHRS call can change as the user enables/disables the airspeed sensor, plus it only gets enabled after the NavEKF constructor runs.
2014-02-18 18:19:46 -04:00
bool NavEKF::useAirspeed(void) const
{
if (_ahrs->get_airspeed() == NULL) {
return false;
}
return _ahrs->get_airspeed()->use();
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return true if the vehicle code has requested use of static mode
// in static mode, position and height are constrained to zero, allowing an attitude
// reference to be initialised and maintained when on the ground and without GPS lock
AP_NavEKF : Add accel aliasing protection
2014-02-26 04:01:51 -04:00
bool NavEKF::static_mode_demanded(void) const
AP_NavEKF: use ahrs->get_armed() for static mode demanded
2014-02-18 19:52:57 -04:00
{
return !_ahrs->get_armed() || !_ahrs->get_correct_centrifugal();
}
AP_NavEKF : Update comments and remove un-used function declarations
2014-03-11 06:18:01 -03:00
// return true if we should use the compass
AP_NavEKF: cope with compass going offline while in flight
2014-03-01 23:32:10 -04:00
bool NavEKF::use_compass(void) const
{
AP_NavEKF : Improved Magnetometer Error Handling (Plane Only) If the yaw and GPS heading disagree by more than 45 degrees on takeoff, then the magnetometer is declared as failed. The heading is then reset based on the difference between GPS ground track and stgate velocity vector. Magnetometer fusion uses corrected data and bias states are initialised to zero. This allows the compass to be switched in flight. For persistent compass errors that trigger a timeout, the compass is not permanently failed, however for non-forward fly vehicles the compass weighting is reduced.
2014-10-30 04:27:35 -03:00
return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw();
AP_NavEKF: cope with compass going offline while in flight
2014-03-01 23:32:10 -04:00
}
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
// decay GPS horizontal position offset to close to zero at a rate of 1 m/s
// limit radius to a maximum of 100m
AP_NavEKF: Fix bug in position reset logic
2014-04-23 04:37:07 -03:00
// apply glitch offset to GPS measurements
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
void NavEKF::decayGpsOffset()
{
AP_NavEKF : Clean up time stamps Time stamps are now explicitly initialised to the current IMU time to avoid unwanted activation of timeout logic on filter start and various calls to the hal.scheduler->millis() object have been consolidated.
2014-09-05 17:33:47 -03:00
float lapsedTime = 0.001f*float(imuSampleTime_ms - lastDecayTime_ms);
lastDecayTime_ms = imuSampleTime_ms;
AP_NavEKF: Fix bug in position reset logic
2014-04-23 04:37:07 -03:00
float offsetRadius = pythagorous2(gpsPosGlitchOffsetNE.x, gpsPosGlitchOffsetNE.y);
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
// decay radius if larger than velocity of 1.0 multiplied by lapsed time (plus a margin to prevent divide by zero)
if (offsetRadius > (lapsedTime + 0.1f)) {
// calculate scale factor to be applied to both offset components
float scaleFactor = constrain_float((offsetRadius - lapsedTime), 0.0f, 100.0f) / offsetRadius;
AP_NavEKF: Fix bug in position reset logic
2014-04-23 04:37:07 -03:00
gpsPosGlitchOffsetNE.x *= scaleFactor;
gpsPosGlitchOffsetNE.y *= scaleFactor;
AP_NavEKF : Updated GPS glitch protection logic This adds new functionality to the detection and compensation of GPS glitches: 1) A maximum allowable innovation is calculated using the GPS noise parameter multiplied by the gate, with an additional component allowing for growth in position uncertainty due to acceleration error since the last valid measurement 2) Includes per vehicle type values for the acceleration error limit 3) If the innovation length exceeds the maximum allowable, no fusion occurs 4) If no fusion has occurred for long enough such that the position uncertainty exceeds the maximum set by a per vehicle parameter or a maximum time, an offset is applied to the GPS data to so that it matches the value predicted by the filter 5) The offset is never allowed to be bigger than 100m 6) The offset is decayed to zero at a rate of 1.0 m/s to allow GPS jumps to be accommodated gradually 7) The default velocity innovation gate has been tightened up for copter and rover 8) The variance data logging output has been updated to make it more useful
2014-03-31 18:15:28 -03:00
}
}
AP_NavEKF: use AHRS vehicle class for sideslip calculation Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-04-21 04:12:15 -03:00
/*
should we assume zero sideslip?
*/
bool NavEKF::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;
}
AP_NavEKF: Improved divergence detection and reset Divergence is now detected by looking for very large changes in the gyro bias. This will cause the filter to be reset and declared unhealthy for 10 seconds. Don't reset filterDiverged status immediately during reset Set filterDiverged true if covariance blows up Add fault status reporting
2014-05-12 04:35:22 -03:00
/*
return the filter fault status as a bitmasked integer
AP_NavEKF : Remove ineffective numerical divergence checks
2014-10-14 04:24:32 -03:00
0 = unassigned
1 = unassigned
AP_NavEKF: Improved divergence detection and reset Divergence is now detected by looking for very large changes in the gyro bias. This will cause the filter to be reset and declared unhealthy for 10 seconds. Don't reset filterDiverged status immediately during reset Set filterDiverged true if covariance blows up Add fault status reporting
2014-05-12 04:35:22 -03:00
2 = badly conditioned X magnetometer fusion
3 = badly conditioned Y magnetometer fusion
4 = badly conditioned Z magnetometer fusion
5 = badly conditioned airspeed fusion
6 = badly conditioned synthetic sideslip fusion
7 = unassigned
return normalised delta gyro bias length used for divergence test
*/
AP_NavEKF : Remove ineffective numerical divergence checks
2014-10-14 04:24:32 -03:00
void NavEKF::getFilterFaults(uint8_t &faults) const
AP_NavEKF: Improved divergence detection and reset Divergence is now detected by looking for very large changes in the gyro bias. This will cause the filter to be reset and declared unhealthy for 10 seconds. Don't reset filterDiverged status immediately during reset Set filterDiverged true if covariance blows up Add fault status reporting
2014-05-12 04:35:22 -03:00
{
AP_NavEKF : Remove ineffective numerical divergence checks
2014-10-14 04:24:32 -03:00
faults = (faultStatus.bad_xmag<<2 |
AP_NavEKF: fixed up handling of bitfields in faultStatus Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
2014-05-15 08:42:39 -03:00
faultStatus.bad_ymag<<3 |
faultStatus.bad_zmag<<4 |
faultStatus.bad_airspeed<<5 |
faultStatus.bad_sideslip<<6);
AP_NavEKF: Improved divergence detection and reset Divergence is now detected by looking for very large changes in the gyro bias. This will cause the filter to be reset and declared unhealthy for 10 seconds. Don't reset filterDiverged status immediately during reset Set filterDiverged true if covariance blows up Add fault status reporting
2014-05-12 04:35:22 -03:00
}
AP_NavEKF: only build on fast CPUs
2013-12-30 19:24:21 -04:00
#endif // HAL_CPU_CLASS