diff options
Diffstat (limited to 'src/modules')
32 files changed, 4657 insertions, 972 deletions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 3ff3d9922..49a892609 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -61,11 +61,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); /* offset estimation - UNUSED */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f); -/* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); -PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f); -PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); - /* magnetic declination, in degrees */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); @@ -85,10 +80,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->r2 = param_find("EKF_ATT_V4_R2"); h->r3 = param_find("EKF_ATT_V4_R3"); - h->roll_off = param_find("ATT_ROLL_OFF3"); - h->pitch_off = param_find("ATT_PITCH_OFF3"); - h->yaw_off = param_find("ATT_YAW_OFF3"); - h->mag_decl = param_find("ATT_MAG_DECL"); h->acc_comp = param_find("ATT_ACC_COMP"); @@ -109,10 +100,6 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->r2, &(p->r[2])); param_get(h->r3, &(p->r[3])); - param_get(h->roll_off, &(p->roll_off)); - param_get(h->pitch_off, &(p->pitch_off)); - param_get(h->yaw_off, &(p->yaw_off)); - param_get(h->mag_decl, &(p->mag_decl)); p->mag_decl *= M_PI / 180.0f; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 74a141609..5985541ca 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -54,7 +54,6 @@ struct attitude_estimator_ekf_params { struct attitude_estimator_ekf_param_handles { param_t r0, r1, r2, r3; param_t q0, q1, q2, q3, q4; - param_t roll_off, pitch_off, yaw_off; param_t mag_decl; param_t acc_comp; }; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6a0ed041b..ff99e7baf 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -609,6 +609,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // XXX TODO } + return true; } int commander_thread_main(int argc, char *argv[]) @@ -1622,7 +1623,6 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin switch (sp_man->mode_switch) { case SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; - warnx("NONE"); break; case SWITCH_POS_OFF: // MANUAL diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index abe272e56..e4f805dc0 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -49,11 +49,11 @@ #include <math.h> #include <poll.h> #include <time.h> -#include <drivers/drv_hrt.h> +#include <float.h> #define SENSOR_COMBINED_SUB - +#include <drivers/drv_hrt.h> #include <drivers/drv_gyro.h> #include <drivers/drv_accel.h> #include <drivers/drv_mag.h> @@ -83,7 +83,7 @@ #include <mathlib/mathlib.h> #include <mavlink/mavlink_log.h> -#include "estimator.h" +#include "estimator_23states.h" @@ -96,7 +96,6 @@ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]); __EXPORT uint32_t millis(); -static uint64_t last_run = 0; static uint64_t IMUmsec = 0; static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; @@ -121,7 +120,7 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. */ int start(); @@ -135,6 +134,20 @@ public: */ int trip_nan(); + /** + * Enable logging. + * + * @param enable Set to true to enable logging, false to disable + */ + int enable_logging(bool enable); + + /** + * Set debug level. + * + * @param debug Desired debug level - 0 to disable. + */ + int set_debuglevel(unsigned debug) { _debug = debug; return 0; } + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -184,24 +197,28 @@ private: struct map_projection_reference_s _pos_ref; float _baro_ref; /**< barometer reference altitude */ - float _baro_gps_offset; /**< offset between GPS and baro */ + float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ + float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ - perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _loop_perf; ///< loop performance counter perf_counter_t _perf_gyro; ///<local performance counter for gyro updates - perf_counter_t _perf_accel; ///<local performance counter for accel updates perf_counter_t _perf_mag; ///<local performance counter for mag updates perf_counter_t _perf_gps; ///<local performance counter for gps updates perf_counter_t _perf_baro; ///<local performance counter for baro updates perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates + perf_counter_t _perf_reset; ///<local performance counter for filter resets - bool _initialized; bool _baro_init; bool _gps_initialized; - uint64_t _gps_start_time; - uint64_t _filter_start_time; + hrt_abstime _gps_start_time; + hrt_abstime _filter_start_time; + hrt_abstime _last_sensor_timestamp; + hrt_abstime _last_run; bool _gyro_valid; bool _accel_valid; bool _mag_valid; + bool _ekf_logging; ///< log EKF state + unsigned _debug; ///< debug level - default 0 int _mavlink_fd; @@ -275,9 +292,16 @@ private: static void task_main_trampoline(int argc, char *argv[]); /** - * Main sensor collection task. + * Main filter task. */ void task_main(); + + /** + * Check filter sanity state + * + * @return zero if ok, non-zero for a filter error condition. + */ + int check_filter_state(); }; namespace estimator @@ -342,24 +366,26 @@ FixedwingEstimator::FixedwingEstimator() : #endif _baro_ref(0.0f), + _baro_ref_offset(0.0f), _baro_gps_offset(0.0f), /* performance counters */ - _loop_perf(perf_alloc(PC_COUNT, "ekf_att_pos_estimator")), - _perf_gyro(perf_alloc(PC_COUNT, "ekf_att_pos_gyro_upd")), - _perf_accel(perf_alloc(PC_COUNT, "ekf_att_pos_accel_upd")), - _perf_mag(perf_alloc(PC_COUNT, "ekf_att_pos_mag_upd")), - _perf_gps(perf_alloc(PC_COUNT, "ekf_att_pos_gps_upd")), - _perf_baro(perf_alloc(PC_COUNT, "ekf_att_pos_baro_upd")), - _perf_airspeed(perf_alloc(PC_COUNT, "ekf_att_pos_aspd_upd")), + _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), + _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), + _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), + _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), + _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), + _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), + _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), /* states */ - _initialized(false), _baro_init(false), _gps_initialized(false), _gyro_valid(false), _accel_valid(false), _mag_valid(false), + _ekf_logging(true), + _debug(0), _mavlink_fd(-1), _ekf(nullptr), _velocity_xy_filtered(0.0f), @@ -367,7 +393,7 @@ FixedwingEstimator::FixedwingEstimator() : _airspeed_filtered(0.0f) { - last_run = hrt_absolute_time(); + _last_run = hrt_absolute_time(); _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); @@ -451,10 +477,20 @@ FixedwingEstimator::~FixedwingEstimator() } while (_estimator_task != -1); } + delete _ekf; + estimator::g_estimator = nullptr; } int +FixedwingEstimator::enable_logging(bool logging) +{ + _ekf_logging = logging; + + return 0; +} + +int FixedwingEstimator::parameters_update() { @@ -512,6 +548,151 @@ FixedwingEstimator::vehicle_status_poll() } } +int +FixedwingEstimator::check_filter_state() +{ + /* + * CHECK IF THE INPUT DATA IS SANE + */ + + struct ekf_status_report ekf_report; + + int check = _ekf->CheckAndBound(&ekf_report); + + const char* ekfname = "att pos estimator: "; + + switch (check) { + case 0: + /* all ok */ + break; + case 1: + { + const char* str = "NaN in states, resetting"; + warnx("%s", str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 2: + { + const char* str = "stale IMU data, resetting"; + warnx("%s", str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 3: + { + const char* str = "switching to dynamic state"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 4: + { + const char* str = "excessive gyro offsets"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 5: + { + const char* str = "GPS velocity divergence"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 6: + { + const char* str = "excessive covariances"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } + + default: + { + const char* str = "unknown reset condition"; + warnx("%s", str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + } + } + + struct estimator_status_report rep; + memset(&rep, 0, sizeof(rep)); + + // If error flag is set, we got a filter reset + if (check && ekf_report.error) { + + // Count the reset condition + perf_count(_perf_reset); + + } else if (_ekf_logging) { + _ekf->GetFilterState(&ekf_report); + } + + if (_ekf_logging || check) { + rep.timestamp = hrt_absolute_time(); + + rep.nan_flags |= (((uint8_t)ekf_report.angNaN) << 0); + rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN) << 1); + rep.nan_flags |= (((uint8_t)ekf_report.KHNaN) << 2); + rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN) << 3); + rep.nan_flags |= (((uint8_t)ekf_report.PNaN) << 4); + rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN) << 5); + rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN) << 6); + rep.nan_flags |= (((uint8_t)ekf_report.statesNaN) << 7); + + rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0); + rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); + rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); + rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); + + rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); + rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); + rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); + rep.timeout_flags |= (((uint8_t)ekf_report.imuTimeout) << 3); + + if (_debug > 10) { + + if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { + warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s", + ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); + } + + if (rep.timeout_flags) { + warnx("timeout: %s%s%s%s", + ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), + ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), + ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), + ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); + } + } + + // Copy all states or at least all that we can fit + unsigned ekf_n_states = ekf_report.n_states; + unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); + rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; + + for (unsigned i = 0; i < rep.n_states; i++) { + rep.states[i] = ekf_report.states[i]; + } + + for (unsigned i = 0; i < rep.n_states; i++) { + rep.states[i] = ekf_report.states[i]; + } + + if (_estimator_status_pub > 0) { + orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); + } else { + _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); + } + } + + return check; +} + void FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) { @@ -528,7 +709,7 @@ FixedwingEstimator::task_main() _filter_start_time = hrt_absolute_time(); if (!_ekf) { - errx(1, "failed allocating EKF filter - out of RAM!"); + errx(1, "OUT OF MEM!"); } /* @@ -556,7 +737,7 @@ FixedwingEstimator::task_main() #else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_sensor_combined_sub, 4); + orb_set_interval(_sensor_combined_sub, 9); #endif /* sets also parameters in the EKF object */ @@ -584,6 +765,13 @@ FixedwingEstimator::task_main() bool newAdsData = false; bool newDataMag = false; + float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m) + + uint64_t last_gps = 0; + _gps.vel_n_m_s = 0.0f; + _gps.vel_e_m_s = 0.0f; + _gps.vel_d_m_s = 0.0f; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -595,7 +783,7 @@ FixedwingEstimator::task_main() /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { - warn("poll error %d, %d", pret, errno); + warn("POLL ERR %d, %d", pret, errno); continue; } @@ -621,8 +809,6 @@ FixedwingEstimator::task_main() bool accel_updated; bool mag_updated; - hrt_abstime last_sensor_timestamp; - perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ @@ -646,12 +832,12 @@ FixedwingEstimator::task_main() _baro_init = false; _gps_initialized = false; - last_sensor_timestamp = hrt_absolute_time(); - last_run = last_sensor_timestamp; + _last_sensor_timestamp = hrt_absolute_time(); + _last_run = _last_sensor_timestamp; _ekf->ZeroVariables(); _ekf->dtIMU = 0.01f; - _filter_start_time = last_sensor_timestamp; + _filter_start_time = _last_sensor_timestamp; /* now skip this loop and get data on the next one, which will also re-init the filter */ continue; @@ -669,15 +855,14 @@ FixedwingEstimator::task_main() orb_check(_accel_sub, &accel_updated); if (accel_updated) { - perf_count(_perf_accel); orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } - last_sensor_timestamp = _gyro.timestamp; + _last_sensor_timestamp = _gyro.timestamp; IMUmsec = _gyro.timestamp / 1e3f; - float deltaT = (_gyro.timestamp - last_run) / 1e6f; - last_run = _gyro.timestamp; + float deltaT = (_gyro.timestamp - _last_run) / 1e6f; + _last_run = _gyro.timestamp; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { @@ -737,17 +922,17 @@ FixedwingEstimator::task_main() // Copy gyro and accel - last_sensor_timestamp = _sensor_combined.timestamp; + _last_sensor_timestamp = _sensor_combined.timestamp; IMUmsec = _sensor_combined.timestamp / 1e3f; - float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f; + float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } - last_run = _sensor_combined.timestamp; + _last_run = _sensor_combined.timestamp; // Always store data, independent of init status /* fill in last data set */ @@ -765,6 +950,7 @@ FixedwingEstimator::task_main() } _gyro_valid = true; + perf_count(_perf_gyro); } if (accel_updated) { @@ -796,6 +982,8 @@ FixedwingEstimator::task_main() #endif + //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); + bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); @@ -815,7 +1003,7 @@ FixedwingEstimator::task_main() if (gps_updated) { - uint64_t last_gps = _gps.timestamp_position; + last_gps = _gps.timestamp_position; orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); @@ -883,6 +1071,8 @@ FixedwingEstimator::task_main() warnx("ALT REF INIT"); } + perf_count(_perf_baro); + newHgtData = true; } else { newHgtData = false; @@ -933,127 +1123,39 @@ FixedwingEstimator::task_main() newDataMag = false; } - - /** - * CHECK IF THE INPUT DATA IS SANE + /* + * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY */ - int check = _ekf->CheckAndBound(); - - const char* ekfname = "[ekf] "; - - switch (check) { - case 0: - /* all ok */ - break; - case 1: - { - const char* str = "NaN in states, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 2: - { - const char* str = "stale IMU data, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 3: - { - const char* str = "switching to dynamic state"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - - default: - { - const char* str = "unknown reset condition"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - } - } - - // warn on fatal resets - if (check == 1) { - warnx("NUMERIC ERROR IN FILTER"); - } - - // If non-zero, we got a filter reset - if (check) { - - struct ekf_status_report ekf_report; - - _ekf->GetLastErrorState(&ekf_report); - - struct estimator_status_report rep; - memset(&rep, 0, sizeof(rep)); - rep.timestamp = hrt_absolute_time(); - - rep.states_nan = ekf_report.statesNaN; - rep.covariance_nan = ekf_report.covarianceNaN; - rep.kalman_gain_nan = ekf_report.kalmanGainsNaN; - - // Copy all states or at least all that we can fit - unsigned i = 0; - unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0])); - unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); - rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; - - while ((i < ekf_n_states) && (i < max_states)) { - - rep.states[i] = ekf_report.states[i]; - i++; - } - - if (_estimator_status_pub > 0) { - orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); - } else { - _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); - } - - /* set sensors to de-initialized state */ - _gyro_valid = false; - _accel_valid = false; - _mag_valid = false; - - _baro_init = false; - _gps_initialized = false; - last_sensor_timestamp = hrt_absolute_time(); - last_run = last_sensor_timestamp; - - _ekf->ZeroVariables(); - _ekf->dtIMU = 0.01f; - - // Let the system re-initialize itself + if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) { continue; - } - /** * PART TWO: EXECUTE THE FILTER + * + * We run the filter only once all data has been fetched **/ - if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) { + if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { float initVelNED[3]; + /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { - initVelNED[0] = _gps.vel_n_m_s; - initVelNED[1] = _gps.vel_e_m_s; - initVelNED[2] = _gps.vel_d_m_s; - // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; double lon = _gps.lon / 1.0e7; float gps_alt = _gps.alt / 1e3f; + initVelNED[0] = _gps.vel_n_m_s; + initVelNED[1] = _gps.vel_e_m_s; + initVelNED[2] = _gps.vel_d_m_s; + // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_gps_offset = _baro_ref - _baro.altitude; + _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame + _baro_gps_offset = _baro.altitude - gps_alt; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); @@ -1077,10 +1179,13 @@ FixedwingEstimator::task_main() map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + + #if 0 warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset); + warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); + #endif _gps_initialized = true; @@ -1089,287 +1194,312 @@ FixedwingEstimator::task_main() initVelNED[0] = 0.0f; initVelNED[1] = 0.0f; initVelNED[2] = 0.0f; - _ekf->posNED[0] = 0.0f; - _ekf->posNED[1] = 0.0f; - _ekf->posNED[2] = 0.0f; - - _ekf->posNE[0] = _ekf->posNED[0]; - _ekf->posNE[1] = _ekf->posNED[1]; + _ekf->posNE[0] = posNED[0]; + _ekf->posNE[1] = posNED[1]; _local_pos.ref_alt = _baro_ref; + _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); - } - } - - // If valid IMU data and states initialised, predict states and covariances - if (_ekf->statesInitialised) { - // Run the strapdown INS equations every IMU update - _ekf->UpdateStrapdownEquationsNED(); -#if 0 - // debug code - could be tunred into a filter mnitoring/watchdog function - float tempQuat[4]; - - for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; + } else if (_ekf->statesInitialised) { - quat2eul(eulerEst, tempQuat); + // We're apparently initialized in this case now - for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; + int check = check_filter_state(); - if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; - - if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; - -#endif - // store the predicted states for subsequent use by measurement fusion - _ekf->StoreStates(IMUmsec); - // Check if on ground - status is used by covariance prediction - _ekf->OnGroundCheck(); - // sum delta angles and time used by covariance prediction - _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; - _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; - dt += _ekf->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 >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { - _ekf->CovariancePrediction(dt); - _ekf->summedDelAng.zero(); - _ekf->summedDelVel.zero(); - dt = 0.0f; - } - - _initialized = true; - } - - // Fuse GPS Measurements - if (newDataGps && _gps_initialized) { - // Convert GPS measurements to Pos NE, hgt and Vel NED - _ekf->velNED[0] = _gps.vel_n_m_s; - _ekf->velNED[1] = _gps.vel_e_m_s; - _ekf->velNED[2] = _gps.vel_d_m_s; - _ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - - _ekf->posNE[0] = _ekf->posNED[0]; - _ekf->posNE[1] = _ekf->posNED[1]; - // set fusion flags - _ekf->fuseVelData = true; - _ekf->fusePosData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); - - } else if (_ekf->statesInitialised) { - // Convert GPS measurements to Pos NE, hgt and Vel NED - _ekf->velNED[0] = 0.0f; - _ekf->velNED[1] = 0.0f; - _ekf->velNED[2] = 0.0f; - _ekf->posNED[0] = 0.0f; - _ekf->posNED[1] = 0.0f; - _ekf->posNED[2] = 0.0f; - - _ekf->posNE[0] = _ekf->posNED[0]; - _ekf->posNE[1] = _ekf->posNED[1]; - // set fusion flags - _ekf->fuseVelData = true; - _ekf->fusePosData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); + if (check) { + // Let the system re-initialize itself + continue; + } - } else { - _ekf->fuseVelData = false; - _ekf->fusePosData = false; - } - if (newHgtData && _ekf->statesInitialised) { - // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); - _ekf->fuseHgtData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); + // Run the strapdown INS equations every IMU update + _ekf->UpdateStrapdownEquationsNED(); + #if 0 + // debug code - could be tunred into a filter mnitoring/watchdog function + float tempQuat[4]; - } else { - _ekf->fuseHgtData = false; - } + for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; - // Fuse Magnetometer Measurements - if (newDataMag && _ekf->statesInitialised) { - _ekf->fuseMagData = true; - _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + quat2eul(eulerEst, tempQuat); - } else { - _ekf->fuseMagData = false; - } + for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; - if (_ekf->statesInitialised) _ekf->FuseMagnetometer(); + if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; - // Fuse Airspeed Measurements - if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { - _ekf->fuseVtasData = true; - _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - _ekf->FuseAirspeed(); + if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; - } else { - _ekf->fuseVtasData = false; - } + #endif + // store the predicted states for subsequent use by measurement fusion + _ekf->StoreStates(IMUmsec); + // Check if on ground - status is used by covariance prediction + _ekf->OnGroundCheck(); + // sum delta angles and time used by covariance prediction + _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; + _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; + dt += _ekf->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 >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { + _ekf->CovariancePrediction(dt); + _ekf->summedDelAng.zero(); + _ekf->summedDelVel.zero(); + dt = 0.0f; + } - // Publish results - if (_initialized && (check == OK)) { - - - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) - - math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); - math::Matrix<3, 3> R = q.to_dcm(); - math::Vector<3> euler = R.to_euler(); - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _att.R[i][j] = R(i, j); - - _att.timestamp = last_sensor_timestamp; - _att.q[0] = _ekf->states[0]; - _att.q[1] = _ekf->states[1]; - _att.q[2] = _ekf->states[2]; - _att.q[3] = _ekf->states[3]; - _att.q_valid = true; - _att.R_valid = true; - - _att.timestamp = last_sensor_timestamp; - _att.roll = euler(0); - _att.pitch = euler(1); - _att.yaw = euler(2); - - _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; - // gyro offsets - _att.rate_offsets[0] = _ekf->states[10]; - _att.rate_offsets[1] = _ekf->states[11]; - _att.rate_offsets[2] = _ekf->states[12]; - - /* lazily publish the attitude only once available */ - if (_att_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); + // Fuse GPS Measurements + if (newDataGps && _gps_initialized) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + + float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f; + + // Calculate acceleration predicted by GPS velocity change + if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) || + (fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) || + (fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) { + + _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; + _ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt; + _ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt; + } + + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; + _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); + + _ekf->posNE[0] = posNED[0]; + _ekf->posNE[1] = posNED[1]; + // set fusion flags + _ekf->fuseVelData = true; + _ekf->fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + // run the fusion step + _ekf->FuseVelposNED(); + + } else if (_ekf->statesInitialised) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + _ekf->velNED[0] = 0.0f; + _ekf->velNED[1] = 0.0f; + _ekf->velNED[2] = 0.0f; + + _ekf->posNE[0] = 0.0f; + _ekf->posNE[1] = 0.0f; + // set fusion flags + _ekf->fuseVelData = true; + _ekf->fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + // run the fusion step + _ekf->FuseVelposNED(); - } else { - /* advertise and publish */ - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); - } - } + } else { + _ekf->fuseVelData = false; + _ekf->fusePosData = false; + } - if (_gps_initialized) { - _local_pos.timestamp = last_sensor_timestamp; - _local_pos.x = _ekf->states[7]; - _local_pos.y = _ekf->states[8]; - // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_gps_offset; - - _local_pos.vx = _ekf->states[4]; - _local_pos.vy = _ekf->states[5]; - _local_pos.vz = _ekf->states[6]; - - _local_pos.xy_valid = _gps_initialized; - _local_pos.z_valid = true; - _local_pos.v_xy_valid = _gps_initialized; - _local_pos.v_z_valid = true; - _local_pos.xy_global = true; - - _local_pos.z_global = false; - _local_pos.yaw = _att.yaw; - - _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); - _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); - _airspeed_filtered = 0.95*_airspeed_filtered + + 0.05*_airspeed.true_airspeed_m_s; - - - /* crude land detector for fixedwing only, - * TODO: adapt so that it works for both, maybe move to another location - */ - if (_velocity_xy_filtered < 5 - && _velocity_z_filtered < 10 - && _airspeed_filtered < 10) { - _local_pos.landed = true; - } else { - _local_pos.landed = false; - } + if (newHgtData && _ekf->statesInitialised) { + // Could use a blend of GPS and baro alt data if desired + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + _ekf->fuseHgtData = true; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + // run the fusion step + _ekf->FuseVelposNED(); - /* lazily publish the local position only once available */ - if (_local_pos_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); + } else { + _ekf->fuseHgtData = false; + } - } else { - /* advertise and publish */ - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); - } + // Fuse Magnetometer Measurements + if (newDataMag && _ekf->statesInitialised) { + _ekf->fuseMagData = true; + _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data - _global_pos.timestamp = _local_pos.timestamp; + _ekf->magstate.obsIndex = 0; + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); - if (_local_pos.xy_global) { - double est_lat, est_lon; - map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); - _global_pos.lat = est_lat; - _global_pos.lon = est_lon; - _global_pos.time_gps_usec = _gps.time_gps_usec; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; - } + } else { + _ekf->fuseMagData = false; + } - if (_local_pos.v_xy_valid) { - _global_pos.vel_n = _local_pos.vx; - _global_pos.vel_e = _local_pos.vy; - } else { - _global_pos.vel_n = 0.0f; - _global_pos.vel_e = 0.0f; - } + // Fuse Airspeed Measurements + if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { + _ekf->fuseVtasData = true; + _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + _ekf->FuseAirspeed(); - /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z); + } else { + _ekf->fuseVtasData = false; + } - if (_local_pos.v_z_valid) { - _global_pos.vel_d = _local_pos.vz; - } - _global_pos.yaw = _local_pos.yaw; + // Output results + math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); + math::Matrix<3, 3> R = q.to_dcm(); + math::Vector<3> euler = R.to_euler(); + + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + _att.R[i][j] = R(i, j); + + _att.timestamp = _last_sensor_timestamp; + _att.q[0] = _ekf->states[0]; + _att.q[1] = _ekf->states[1]; + _att.q[2] = _ekf->states[2]; + _att.q[3] = _ekf->states[3]; + _att.q_valid = true; + _att.R_valid = true; + + _att.timestamp = _last_sensor_timestamp; + _att.roll = euler(0); + _att.pitch = euler(1); + _att.yaw = euler(2); + + _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; + // gyro offsets + _att.rate_offsets[0] = _ekf->states[10]; + _att.rate_offsets[1] = _ekf->states[11]; + _att.rate_offsets[2] = _ekf->states[12]; + + /* lazily publish the attitude only once available */ + if (_att_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; + } else { + /* advertise and publish */ + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + } - _global_pos.timestamp = _local_pos.timestamp; + if (_gps_initialized) { + _local_pos.timestamp = _last_sensor_timestamp; + _local_pos.x = _ekf->states[7]; + _local_pos.y = _ekf->states[8]; + // XXX need to announce change of Z reference somehow elegantly + _local_pos.z = _ekf->states[9] - _baro_ref_offset; + + _local_pos.vx = _ekf->states[4]; + _local_pos.vy = _ekf->states[5]; + _local_pos.vz = _ekf->states[6]; + + _local_pos.xy_valid = _gps_initialized; + _local_pos.z_valid = true; + _local_pos.v_xy_valid = _gps_initialized; + _local_pos.v_z_valid = true; + _local_pos.xy_global = true; + + _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); + _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); + _airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s; + + + /* crude land detector for fixedwing only, + * TODO: adapt so that it works for both, maybe move to another location + */ + if (_velocity_xy_filtered < 5 + && _velocity_z_filtered < 10 + && _airspeed_filtered < 10) { + _local_pos.landed = true; + } else { + _local_pos.landed = false; + } + + _local_pos.z_global = false; + _local_pos.yaw = _att.yaw; + + /* lazily publish the local position only once available */ + if (_local_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); + + } else { + /* advertise and publish */ + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); + } + + _global_pos.timestamp = _local_pos.timestamp; + + if (_local_pos.xy_global) { + double est_lat, est_lon; + map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); + _global_pos.lat = est_lat; + _global_pos.lon = est_lon; + _global_pos.time_gps_usec = _gps.time_gps_usec; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; + } + + if (_local_pos.v_xy_valid) { + _global_pos.vel_n = _local_pos.vx; + _global_pos.vel_e = _local_pos.vy; + } else { + _global_pos.vel_n = 0.0f; + _global_pos.vel_e = 0.0f; + } + + /* local pos alt is negative, change sign and add alt offsets */ + _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; + + if (_local_pos.v_z_valid) { + _global_pos.vel_d = _local_pos.vz; + } + + + _global_pos.yaw = _local_pos.yaw; + + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; + + _global_pos.timestamp = _local_pos.timestamp; + + /* lazily publish the global position only once available */ + if (_global_pos_pub > 0) { + /* publish the global position */ + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + + } else { + /* advertise and publish */ + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } + + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + _wind.covariance_north = 0.0f; // XXX get form filter + _wind.covariance_east = 0.0f; + + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); + + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } + + } - /* lazily publish the global position only once available */ - if (_global_pos_pub > 0) { - /* publish the global position */ - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + } - } else { - /* advertise and publish */ - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); } if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; _wind.windspeed_north = _ekf->states[14]; _wind.windspeed_east = _ekf->states[15]; - _wind.covariance_north = 0.0f; // XXX get form filter - _wind.covariance_east = 0.0f; + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; /* lazily publish the wind estimate only once available */ if (_wind_pub > 0) { @@ -1430,34 +1560,44 @@ FixedwingEstimator::print_status() // 4-6: Velocity - m/sec (North, East, Down) // 7-9: Position - m (North, East, Down) // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) + // 13: Accelerometer offset + // 14-15: Wind Vector - m/sec (North,East) + // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) + // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); - printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt); + printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); + printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, (double)_baro_gps_offset); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); - printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); - printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); - printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); - printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); - printf("states (accel offs) [14]: %8.4f\n", (double)_ekf->states[13]); - printf("states (wind) [15-16]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); - printf("states (earth mag) [17-19]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); - printf("states (body mag) [20-22]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); - printf("states (terrain) [23]: %8.4f\n", (double)_ekf->states[22]); + printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); + printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); + printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); + printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); + + if (n_states == 23) { + printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); + printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); + printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); + printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); + + } else { + printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); + printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); + printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); + } printf("states: %s %s %s %s %s %s %s %s %s %s\n", - (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", - (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", - (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", - (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", - (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", + (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", + (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", + (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } int FixedwingEstimator::trip_nan() { @@ -1512,7 +1652,7 @@ int FixedwingEstimator::trip_nan() { int ekf_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 1) - errx(1, "usage: ekf_att_pos_estimator {start|stop|status}"); + errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}"); if (!strcmp(argv[1], "start")) { @@ -1566,6 +1706,29 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) } } + if (!strcmp(argv[1], "logging")) { + if (estimator::g_estimator) { + int ret = estimator::g_estimator->enable_logging(true); + + exit(ret); + + } else { + errx(1, "not running"); + } + } + + if (!strcmp(argv[1], "debug")) { + if (estimator::g_estimator) { + int debug = strtoul(argv[2], NULL, 10); + int ret = estimator::g_estimator->set_debuglevel(debug); + + exit(ret); + + } else { + errx(1, "not running"); + } + } + warnx("unrecognized command"); return 1; } diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp new file mode 100644 index 000000000..67bfec4ea --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp @@ -0,0 +1,2142 @@ +#include "estimator_21states.h" + +#include <string.h> + +AttPosEKF::AttPosEKF() : + fusionModeGPS(0), + covSkipCount(0), + EAS2TAS(1.0f), + statesInitialised(false), + fuseVelData(false), + fusePosData(false), + fuseHgtData(false), + fuseMagData(false), + fuseVtasData(false), + onGround(true), + staticMode(true), + useAirspeed(true), + useCompass(true), + numericalProtection(true), + storeIndex(0), + magDeclination(0.0f) +{ + InitialiseParameters(); +} + +AttPosEKF::~AttPosEKF() +{ +} + +void AttPosEKF::UpdateStrapdownEquationsNED() +{ + Vector3f delVelNav; + float q00; + float q11; + float q22; + float q33; + float q01; + float q02; + float q03; + float q12; + float q13; + float q23; + Mat3f Tbn; + Mat3f Tnb; + float rotationMag; + float qUpdated[4]; + float quatMag; + double deltaQuat[4]; + const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; + +// Remove sensor bias errors + correctedDelAng.x = dAngIMU.x - states[10]; + correctedDelAng.y = dAngIMU.y - states[11]; + correctedDelAng.z = dAngIMU.z - states[12]; + dVelIMU.x = dVelIMU.x; + dVelIMU.y = dVelIMU.y; + dVelIMU.z = dVelIMU.z; + +// Save current measurements + Vector3f prevDelAng = correctedDelAng; + +// Apply corrections for earths rotation rate and coning errors +// * and + operators have been overloaded + correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); +// Convert the rotation vector to its equivalent quaternion + rotationMag = correctedDelAng.length(); + if (rotationMag < 1e-12f) + { + deltaQuat[0] = 1.0; + deltaQuat[1] = 0.0; + deltaQuat[2] = 0.0; + deltaQuat[3] = 0.0; + } + else + { + deltaQuat[0] = cos(0.5f*rotationMag); + double rotScaler = (sin(0.5f*rotationMag))/rotationMag; + deltaQuat[1] = correctedDelAng.x*rotScaler; + deltaQuat[2] = correctedDelAng.y*rotScaler; + deltaQuat[3] = correctedDelAng.z*rotScaler; + } + +// Update the quaternions by rotating from the previous attitude through +// 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]; + +// Normalise the quaternions and update the quaternion states + quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); + if (quatMag > 1e-16f) + { + float quatMagInv = 1.0f/quatMag; + states[0] = quatMagInv*qUpdated[0]; + states[1] = quatMagInv*qUpdated[1]; + states[2] = quatMagInv*qUpdated[2]; + states[3] = quatMagInv*qUpdated[3]; + } + +// Calculate the body to nav cosine matrix + q00 = sq(states[0]); + q11 = sq(states[1]); + q22 = sq(states[2]); + q33 = sq(states[3]); + q01 = states[0]*states[1]; + q02 = states[0]*states[2]; + q03 = states[0]*states[3]; + q12 = states[1]*states[2]; + q13 = states[1]*states[3]; + q23 = states[2]*states[3]; + + Tbn.x.x = q00 + q11 - q22 - q33; + Tbn.y.y = q00 - q11 + q22 - q33; + Tbn.z.z = q00 - q11 - q22 + q33; + Tbn.x.y = 2*(q12 - q03); + Tbn.x.z = 2*(q13 + q02); + Tbn.y.x = 2*(q12 + q03); + Tbn.y.z = 2*(q23 - q01); + Tbn.z.x = 2*(q13 - q02); + Tbn.z.y = 2*(q23 + q01); + + Tnb = Tbn.transpose(); + +// transform body delta velocities to delta velocities in the nav frame +// * and + operators have been overloaded + //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; + delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; + delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; + delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; + +// calculate the magnitude of the nav acceleration (required for GPS +// variance estimation) + accNavMag = delVelNav.length()/dtIMU; + +// If calculating position save previous velocity + float lastVelocity[3]; + lastVelocity[0] = states[4]; + lastVelocity[1] = states[5]; + lastVelocity[2] = states[6]; + +// Sum delta velocities to get velocity + states[4] = states[4] + delVelNav.x; + states[5] = states[5] + delVelNav.y; + states[6] = states[6] + delVelNav.z; + +// If calculating postions, do a trapezoidal integration for position + states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; + states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; + states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; + + // Constrain states (to protect against filter divergence) + //ConstrainStates(); +} + +void AttPosEKF::CovariancePrediction(float dt) +{ + // scalars + float daxCov; + float dayCov; + float dazCov; + float dvxCov; + float dvyCov; + float dvzCov; + float dvx; + float dvy; + float dvz; + float dax; + float day; + float daz; + float q0; + float q1; + float q2; + float q3; + float dax_b; + float day_b; + float daz_b; + + // arrays + float processNoise[21]; + float SF[14]; + float SG[8]; + float SQ[11]; + float SPP[13] = {0}; + float nextP[21][21]; + + // calculate covariance prediction process noise + windVelSigma = dt*0.1f; + dAngBiasSigma = dt*5.0e-7f; + magEarthSigma = dt*3.0e-4f; + magBodySigma = dt*3.0e-4f; + for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma; + if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale; + for (uint8_t i=13; i<=14; i++) processNoise[i] = windVelSigma; + for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma; + for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma; + for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]); + + // 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; + q0 = states[0]; + q1 = states[1]; + q2 = states[2]; + q3 = states[3]; + dax_b = states[10]; + day_b = states[11]; + daz_b = states[12]; + daxCov = sq(dt*1.4544411e-2f); + dayCov = sq(dt*1.4544411e-2f); + dazCov = sq(dt*1.4544411e-2f); + if (onGround) dazCov = dazCov * sq(yawVarScale); + dvxCov = sq(dt*0.5f); + dvyCov = sq(dt*0.5f); + dvzCov = sq(dt*0.5f); + + // Predicted covariance calculation + SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3; + SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; + SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; + SF[3] = day/2 - day_b/2; + SF[4] = daz/2 - daz_b/2; + SF[5] = dax/2 - dax_b/2; + SF[6] = dax_b/2 - dax/2; + SF[7] = daz_b/2 - daz/2; + SF[8] = day_b/2 - day/2; + SF[9] = q1/2; + SF[10] = q2/2; + SF[11] = q3/2; + SF[12] = 2*dvz*q0; + SF[13] = 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[12] + SF[13] - 2*dvx*q2; + SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; + SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; + SPP[3] = SF[11]; + SPP[4] = SF[10]; + SPP[5] = SF[9]; + SPP[6] = SF[7]; + SPP[7] = SF[8]; + + nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; + nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2; + nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2; + nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3] + SF[4]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[3]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[6]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[4]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) - SPP[5]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - (q0*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]))/2; + nextP[0][4] = P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3] + SF[2]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) - SPP[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]); + nextP[0][5] = P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3] + SF[1]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SF[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) - SPP[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]); + nextP[0][6] = P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3] + SF[1]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[0]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[0]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) - SPP[1]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]); + nextP[0][7] = P[0][7] + P[1][7]*SF[6] + P[2][7]*SPP[7] + P[3][7]*SPP[6] + P[10][7]*SPP[5] + P[11][7]*SPP[4] + P[12][7]*SPP[3] + dt*(P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3]); + nextP[0][8] = P[0][8] + P[1][8]*SF[6] + P[2][8]*SPP[7] + P[3][8]*SPP[6] + P[10][8]*SPP[5] + P[11][8]*SPP[4] + P[12][8]*SPP[3] + dt*(P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3]); + nextP[0][9] = P[0][9] + P[1][9]*SF[6] + P[2][9]*SPP[7] + P[3][9]*SPP[6] + P[10][9]*SPP[5] + P[11][9]*SPP[4] + P[12][9]*SPP[3] + dt*(P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3]); + nextP[0][10] = P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]; + nextP[0][11] = P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]; + nextP[0][12] = P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]; + nextP[0][13] = P[0][13] + P[1][13]*SF[6] + P[2][13]*SPP[7] + P[3][13]*SPP[6] + P[10][13]*SPP[5] + P[11][13]*SPP[4] + P[12][13]*SPP[3]; + nextP[0][14] = P[0][14] + P[1][14]*SF[6] + P[2][14]*SPP[7] + P[3][14]*SPP[6] + P[10][14]*SPP[5] + P[11][14]*SPP[4] + P[12][14]*SPP[3]; + nextP[0][15] = P[0][15] + P[1][15]*SF[6] + P[2][15]*SPP[7] + P[3][15]*SPP[6] + P[10][15]*SPP[5] + P[11][15]*SPP[4] + P[12][15]*SPP[3]; + nextP[0][16] = P[0][16] + P[1][16]*SF[6] + P[2][16]*SPP[7] + P[3][16]*SPP[6] + P[10][16]*SPP[5] + P[11][16]*SPP[4] + P[12][16]*SPP[3]; + nextP[0][17] = P[0][17] + P[1][17]*SF[6] + P[2][17]*SPP[7] + P[3][17]*SPP[6] + P[10][17]*SPP[5] + P[11][17]*SPP[4] + P[12][17]*SPP[3]; + nextP[0][18] = P[0][18] + P[1][18]*SF[6] + P[2][18]*SPP[7] + P[3][18]*SPP[6] + P[10][18]*SPP[5] + P[11][18]*SPP[4] + P[12][18]*SPP[3]; + nextP[0][19] = P[0][19] + P[1][19]*SF[6] + P[2][19]*SPP[7] + P[3][19]*SPP[6] + P[10][19]*SPP[5] + P[11][19]*SPP[4] + P[12][19]*SPP[3]; + nextP[0][20] = P[0][20] + P[1][20]*SF[6] + P[2][20]*SPP[7] + P[3][20]*SPP[6] + P[10][20]*SPP[5] + P[11][20]*SPP[4] + P[12][20]*SPP[3]; + nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2 + SF[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[7]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[6]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[5]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[4]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) + SPP[3]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2); + nextP[1][1] = P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[4]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[7]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[3]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - SPP[4]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2))/2; + nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) - SPP[3]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[5]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2))/2; + nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[6]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[4]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) - SPP[5]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2))/2; + nextP[1][4] = P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2); + nextP[1][5] = P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2 + SF[1]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SF[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2); + nextP[1][6] = P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2 + SF[1]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[0]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2); + nextP[1][7] = P[1][7] + P[0][7]*SF[5] + P[2][7]*SF[4] + P[3][7]*SPP[7] + P[11][7]*SPP[3] - P[12][7]*SPP[4] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2); + nextP[1][8] = P[1][8] + P[0][8]*SF[5] + P[2][8]*SF[4] + P[3][8]*SPP[7] + P[11][8]*SPP[3] - P[12][8]*SPP[4] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2); + nextP[1][9] = P[1][9] + P[0][9]*SF[5] + P[2][9]*SF[4] + P[3][9]*SPP[7] + P[11][9]*SPP[3] - P[12][9]*SPP[4] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2); + nextP[1][10] = P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2; + nextP[1][11] = P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2; + nextP[1][12] = P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2; + nextP[1][13] = P[1][13] + P[0][13]*SF[5] + P[2][13]*SF[4] + P[3][13]*SPP[7] + P[11][13]*SPP[3] - P[12][13]*SPP[4] - (P[10][13]*q0)/2; + nextP[1][14] = P[1][14] + P[0][14]*SF[5] + P[2][14]*SF[4] + P[3][14]*SPP[7] + P[11][14]*SPP[3] - P[12][14]*SPP[4] - (P[10][14]*q0)/2; + nextP[1][15] = P[1][15] + P[0][15]*SF[5] + P[2][15]*SF[4] + P[3][15]*SPP[7] + P[11][15]*SPP[3] - P[12][15]*SPP[4] - (P[10][15]*q0)/2; + nextP[1][16] = P[1][16] + P[0][16]*SF[5] + P[2][16]*SF[4] + P[3][16]*SPP[7] + P[11][16]*SPP[3] - P[12][16]*SPP[4] - (P[10][16]*q0)/2; + nextP[1][17] = P[1][17] + P[0][17]*SF[5] + P[2][17]*SF[4] + P[3][17]*SPP[7] + P[11][17]*SPP[3] - P[12][17]*SPP[4] - (P[10][17]*q0)/2; + nextP[1][18] = P[1][18] + P[0][18]*SF[5] + P[2][18]*SF[4] + P[3][18]*SPP[7] + P[11][18]*SPP[3] - P[12][18]*SPP[4] - (P[10][18]*q0)/2; + nextP[1][19] = P[1][19] + P[0][19]*SF[5] + P[2][19]*SF[4] + P[3][19]*SPP[7] + P[11][19]*SPP[3] - P[12][19]*SPP[4] - (P[10][19]*q0)/2; + nextP[1][20] = P[1][20] + P[0][20]*SF[5] + P[2][20]*SF[4] + P[3][20]*SPP[7] + P[11][20]*SPP[3] - P[12][20]*SPP[4] - (P[10][20]*q0)/2; + nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2 + SF[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[7]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[6]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[5]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[4]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) + SPP[3]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2); + nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[4]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[7]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[3]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - SPP[4]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2))/2; + nextP[2][2] = P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) - SPP[3]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[5]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2))/2; + nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[6]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[4]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) - SPP[5]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2))/2; + nextP[2][4] = P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2); + nextP[2][5] = P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2 + SF[1]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SF[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2); + nextP[2][6] = P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2 + SF[1]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[0]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2); + nextP[2][7] = P[2][7] + P[0][7]*SF[3] + P[3][7]*SF[5] + P[1][7]*SPP[6] - P[10][7]*SPP[3] + P[12][7]*SPP[5] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2); + nextP[2][8] = P[2][8] + P[0][8]*SF[3] + P[3][8]*SF[5] + P[1][8]*SPP[6] - P[10][8]*SPP[3] + P[12][8]*SPP[5] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2); + nextP[2][9] = P[2][9] + P[0][9]*SF[3] + P[3][9]*SF[5] + P[1][9]*SPP[6] - P[10][9]*SPP[3] + P[12][9]*SPP[5] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2); + nextP[2][10] = P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2; + nextP[2][11] = P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2; + nextP[2][12] = P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2; + nextP[2][13] = P[2][13] + P[0][13]*SF[3] + P[3][13]*SF[5] + P[1][13]*SPP[6] - P[10][13]*SPP[3] + P[12][13]*SPP[5] - (P[11][13]*q0)/2; + nextP[2][14] = P[2][14] + P[0][14]*SF[3] + P[3][14]*SF[5] + P[1][14]*SPP[6] - P[10][14]*SPP[3] + P[12][14]*SPP[5] - (P[11][14]*q0)/2; + nextP[2][15] = P[2][15] + P[0][15]*SF[3] + P[3][15]*SF[5] + P[1][15]*SPP[6] - P[10][15]*SPP[3] + P[12][15]*SPP[5] - (P[11][15]*q0)/2; + nextP[2][16] = P[2][16] + P[0][16]*SF[3] + P[3][16]*SF[5] + P[1][16]*SPP[6] - P[10][16]*SPP[3] + P[12][16]*SPP[5] - (P[11][16]*q0)/2; + nextP[2][17] = P[2][17] + P[0][17]*SF[3] + P[3][17]*SF[5] + P[1][17]*SPP[6] - P[10][17]*SPP[3] + P[12][17]*SPP[5] - (P[11][17]*q0)/2; + nextP[2][18] = P[2][18] + P[0][18]*SF[3] + P[3][18]*SF[5] + P[1][18]*SPP[6] - P[10][18]*SPP[3] + P[12][18]*SPP[5] - (P[11][18]*q0)/2; + nextP[2][19] = P[2][19] + P[0][19]*SF[3] + P[3][19]*SF[5] + P[1][19]*SPP[6] - P[10][19]*SPP[3] + P[12][19]*SPP[5] - (P[11][19]*q0)/2; + nextP[2][20] = P[2][20] + P[0][20]*SF[3] + P[3][20]*SF[5] + P[1][20]*SPP[6] - P[10][20]*SPP[3] + P[12][20]*SPP[5] - (P[11][20]*q0)/2; + nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2 + SF[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[7]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[6]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[5]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[4]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + SPP[3]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2); + nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[4]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[7]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[3]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) - SPP[4]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2))/2; + nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) - SPP[3]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[5]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2))/2; + nextP[3][3] = P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[6]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[4]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) - SPP[5]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2))/2; + nextP[3][4] = P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2); + nextP[3][5] = P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2 + SF[1]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SF[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2); + nextP[3][6] = P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2 + SF[1]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[0]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2); + nextP[3][7] = P[3][7] + P[0][7]*SF[4] + P[1][7]*SF[3] + P[2][7]*SF[6] + P[10][7]*SPP[4] - P[11][7]*SPP[5] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2); + nextP[3][8] = P[3][8] + P[0][8]*SF[4] + P[1][8]*SF[3] + P[2][8]*SF[6] + P[10][8]*SPP[4] - P[11][8]*SPP[5] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2); + nextP[3][9] = P[3][9] + P[0][9]*SF[4] + P[1][9]*SF[3] + P[2][9]*SF[6] + P[10][9]*SPP[4] - P[11][9]*SPP[5] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2); + nextP[3][10] = P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2; + nextP[3][11] = P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2; + nextP[3][12] = P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2; + nextP[3][13] = P[3][13] + P[0][13]*SF[4] + P[1][13]*SF[3] + P[2][13]*SF[6] + P[10][13]*SPP[4] - P[11][13]*SPP[5] - (P[12][13]*q0)/2; + nextP[3][14] = P[3][14] + P[0][14]*SF[4] + P[1][14]*SF[3] + P[2][14]*SF[6] + P[10][14]*SPP[4] - P[11][14]*SPP[5] - (P[12][14]*q0)/2; + nextP[3][15] = P[3][15] + P[0][15]*SF[4] + P[1][15]*SF[3] + P[2][15]*SF[6] + P[10][15]*SPP[4] - P[11][15]*SPP[5] - (P[12][15]*q0)/2; + nextP[3][16] = P[3][16] + P[0][16]*SF[4] + P[1][16]*SF[3] + P[2][16]*SF[6] + P[10][16]*SPP[4] - P[11][16]*SPP[5] - (P[12][16]*q0)/2; + nextP[3][17] = P[3][17] + P[0][17]*SF[4] + P[1][17]*SF[3] + P[2][17]*SF[6] + P[10][17]*SPP[4] - P[11][17]*SPP[5] - (P[12][17]*q0)/2; + nextP[3][18] = P[3][18] + P[0][18]*SF[4] + P[1][18]*SF[3] + P[2][18]*SF[6] + P[10][18]*SPP[4] - P[11][18]*SPP[5] - (P[12][18]*q0)/2; + nextP[3][19] = P[3][19] + P[0][19]*SF[4] + P[1][19]*SF[3] + P[2][19]*SF[6] + P[10][19]*SPP[4] - P[11][19]*SPP[5] - (P[12][19]*q0)/2; + nextP[3][20] = P[3][20] + P[0][20]*SF[4] + P[1][20]*SF[3] + P[2][20]*SF[6] + P[10][20]*SPP[4] - P[11][20]*SPP[5] - (P[12][20]*q0)/2; + nextP[4][0] = P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2] + SF[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[7]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[6]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[5]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[4]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) + SPP[3]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]); + nextP[4][1] = P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2] + SF[5]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[4]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[7]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[3]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - SPP[4]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]))/2; + nextP[4][2] = P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2] + SF[3]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[5]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) - SPP[3]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[5]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]))/2; + nextP[4][3] = P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2] + SF[4]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[3]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[6]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[4]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) - SPP[5]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - (q0*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]))/2; + nextP[4][4] = P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[2]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) - SPP[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); + nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2] + SF[1]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SF[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) - SPP[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]); + nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2] + SF[1]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[0]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[0]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) - SPP[1]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]); + nextP[4][7] = P[4][7] + P[0][7]*SF[2] + P[1][7]*SF[0] + P[2][7]*SPP[0] - P[3][7]*SPP[2] + dt*(P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2]); + nextP[4][8] = P[4][8] + P[0][8]*SF[2] + P[1][8]*SF[0] + P[2][8]*SPP[0] - P[3][8]*SPP[2] + dt*(P[4][5] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2]); + nextP[4][9] = P[4][9] + P[0][9]*SF[2] + P[1][9]*SF[0] + P[2][9]*SPP[0] - P[3][9]*SPP[2] + dt*(P[4][6] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2]); + nextP[4][10] = P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]; + nextP[4][11] = P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]; + nextP[4][12] = P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]; + nextP[4][13] = P[4][13] + P[0][13]*SF[2] + P[1][13]*SF[0] + P[2][13]*SPP[0] - P[3][13]*SPP[2]; + nextP[4][14] = P[4][14] + P[0][14]*SF[2] + P[1][14]*SF[0] + P[2][14]*SPP[0] - P[3][14]*SPP[2]; + nextP[4][15] = P[4][15] + P[0][15]*SF[2] + P[1][15]*SF[0] + P[2][15]*SPP[0] - P[3][15]*SPP[2]; + nextP[4][16] = P[4][16] + P[0][16]*SF[2] + P[1][16]*SF[0] + P[2][16]*SPP[0] - P[3][16]*SPP[2]; + nextP[4][17] = P[4][17] + P[0][17]*SF[2] + P[1][17]*SF[0] + P[2][17]*SPP[0] - P[3][17]*SPP[2]; + nextP[4][18] = P[4][18] + P[0][18]*SF[2] + P[1][18]*SF[0] + P[2][18]*SPP[0] - P[3][18]*SPP[2]; + nextP[4][19] = P[4][19] + P[0][19]*SF[2] + P[1][19]*SF[0] + P[2][19]*SPP[0] - P[3][19]*SPP[2]; + nextP[4][20] = P[4][20] + P[0][20]*SF[2] + P[1][20]*SF[0] + P[2][20]*SPP[0] - P[3][20]*SPP[2]; + nextP[5][0] = P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0] + SF[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[7]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[6]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[5]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[4]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) + SPP[3]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]); + nextP[5][1] = P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0] + SF[5]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[4]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[7]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[3]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - SPP[4]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]))/2; + nextP[5][2] = P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0] + SF[3]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[5]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) - SPP[3]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[5]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]))/2; + nextP[5][3] = P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0] + SF[4]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[3]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[6]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[4]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) - SPP[5]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - (q0*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]))/2; + nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0] + SF[2]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) - SPP[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]); + nextP[5][5] = P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[1]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SF[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) - SPP[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); + nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0] + SF[1]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[0]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[0]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) - SPP[1]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]); + nextP[5][7] = P[5][7] + P[0][7]*SF[1] + P[2][7]*SF[0] + P[3][7]*SF[2] - P[1][7]*SPP[0] + dt*(P[5][4] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0]); + nextP[5][8] = P[5][8] + P[0][8]*SF[1] + P[2][8]*SF[0] + P[3][8]*SF[2] - P[1][8]*SPP[0] + dt*(P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0]); + nextP[5][9] = P[5][9] + P[0][9]*SF[1] + P[2][9]*SF[0] + P[3][9]*SF[2] - P[1][9]*SPP[0] + dt*(P[5][6] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0]); + nextP[5][10] = P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]; + nextP[5][11] = P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]; + nextP[5][12] = P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]; + nextP[5][13] = P[5][13] + P[0][13]*SF[1] + P[2][13]*SF[0] + P[3][13]*SF[2] - P[1][13]*SPP[0]; + nextP[5][14] = P[5][14] + P[0][14]*SF[1] + P[2][14]*SF[0] + P[3][14]*SF[2] - P[1][14]*SPP[0]; + nextP[5][15] = P[5][15] + P[0][15]*SF[1] + P[2][15]*SF[0] + P[3][15]*SF[2] - P[1][15]*SPP[0]; + nextP[5][16] = P[5][16] + P[0][16]*SF[1] + P[2][16]*SF[0] + P[3][16]*SF[2] - P[1][16]*SPP[0]; + nextP[5][17] = P[5][17] + P[0][17]*SF[1] + P[2][17]*SF[0] + P[3][17]*SF[2] - P[1][17]*SPP[0]; + nextP[5][18] = P[5][18] + P[0][18]*SF[1] + P[2][18]*SF[0] + P[3][18]*SF[2] - P[1][18]*SPP[0]; + nextP[5][19] = P[5][19] + P[0][19]*SF[1] + P[2][19]*SF[0] + P[3][19]*SF[2] - P[1][19]*SPP[0]; + nextP[5][20] = P[5][20] + P[0][20]*SF[1] + P[2][20]*SF[0] + P[3][20]*SF[2] - P[1][20]*SPP[0]; + nextP[6][0] = P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1] + SF[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[7]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[6]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[5]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[4]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) + SPP[3]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]); + nextP[6][1] = P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1] + SF[5]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[4]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[7]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[3]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - SPP[4]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]))/2; + nextP[6][2] = P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1] + SF[3]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[5]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) - SPP[3]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[5]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]))/2; + nextP[6][3] = P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1] + SF[4]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[3]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[6]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[4]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) - SPP[5]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - (q0*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]))/2; + nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1] + SF[2]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) - SPP[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]); + nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1] + SF[1]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SF[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) - SPP[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]); + nextP[6][6] = P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1] + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) + SF[1]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[0]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) - SPP[1]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); + nextP[6][7] = P[6][7] + P[1][7]*SF[1] + P[3][7]*SF[0] + P[0][7]*SPP[0] - P[2][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1]); + nextP[6][8] = P[6][8] + P[1][8]*SF[1] + P[3][8]*SF[0] + P[0][8]*SPP[0] - P[2][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1]); + nextP[6][9] = P[6][9] + P[1][9]*SF[1] + P[3][9]*SF[0] + P[0][9]*SPP[0] - P[2][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1]); + nextP[6][10] = P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]; + nextP[6][11] = P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]; + nextP[6][12] = P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]; + nextP[6][13] = P[6][13] + P[1][13]*SF[1] + P[3][13]*SF[0] + P[0][13]*SPP[0] - P[2][13]*SPP[1]; + nextP[6][14] = P[6][14] + P[1][14]*SF[1] + P[3][14]*SF[0] + P[0][14]*SPP[0] - P[2][14]*SPP[1]; + nextP[6][15] = P[6][15] + P[1][15]*SF[1] + P[3][15]*SF[0] + P[0][15]*SPP[0] - P[2][15]*SPP[1]; + nextP[6][16] = P[6][16] + P[1][16]*SF[1] + P[3][16]*SF[0] + P[0][16]*SPP[0] - P[2][16]*SPP[1]; + nextP[6][17] = P[6][17] + P[1][17]*SF[1] + P[3][17]*SF[0] + P[0][17]*SPP[0] - P[2][17]*SPP[1]; + nextP[6][18] = P[6][18] + P[1][18]*SF[1] + P[3][18]*SF[0] + P[0][18]*SPP[0] - P[2][18]*SPP[1]; + nextP[6][19] = P[6][19] + P[1][19]*SF[1] + P[3][19]*SF[0] + P[0][19]*SPP[0] - P[2][19]*SPP[1]; + nextP[6][20] = P[6][20] + P[1][20]*SF[1] + P[3][20]*SF[0] + P[0][20]*SPP[0] - P[2][20]*SPP[1]; + nextP[7][0] = P[7][0] + P[4][0]*dt + SF[6]*(P[7][1] + P[4][1]*dt) + SPP[7]*(P[7][2] + P[4][2]*dt) + SPP[6]*(P[7][3] + P[4][3]*dt) + SPP[5]*(P[7][10] + P[4][10]*dt) + SPP[4]*(P[7][11] + P[4][11]*dt) + SPP[3]*(P[7][12] + P[4][12]*dt); + nextP[7][1] = P[7][1] + P[4][1]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][2] + P[4][2]*dt) + SPP[7]*(P[7][3] + P[4][3]*dt) + SPP[3]*(P[7][11] + P[4][11]*dt) - SPP[4]*(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[3]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][1] + P[4][1]*dt) - SPP[3]*(P[7][10] + P[4][10]*dt) + SPP[5]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; + nextP[7][3] = P[7][3] + P[4][3]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[3]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][2] + P[4][2]*dt) + SPP[4]*(P[7][10] + P[4][10]*dt) - SPP[5]*(P[7][11] + P[4][11]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; + nextP[7][4] = P[7][4] + P[4][4]*dt + SF[0]*(P[7][1] + P[4][1]*dt) + SF[2]*(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); + nextP[7][5] = P[7][5] + P[4][5]*dt + SF[1]*(P[7][0] + P[4][0]*dt) + SF[0]*(P[7][2] + P[4][2]*dt) + SF[2]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt); + nextP[7][6] = P[7][6] + P[4][6]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[0]*(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); + 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[8][0] = P[8][0] + P[5][0]*dt + SF[6]*(P[8][1] + P[5][1]*dt) + SPP[7]*(P[8][2] + P[5][2]*dt) + SPP[6]*(P[8][3] + P[5][3]*dt) + SPP[5]*(P[8][10] + P[5][10]*dt) + SPP[4]*(P[8][11] + P[5][11]*dt) + SPP[3]*(P[8][12] + P[5][12]*dt); + nextP[8][1] = P[8][1] + P[5][1]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][2] + P[5][2]*dt) + SPP[7]*(P[8][3] + P[5][3]*dt) + SPP[3]*(P[8][11] + P[5][11]*dt) - SPP[4]*(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[3]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][1] + P[5][1]*dt) - SPP[3]*(P[8][10] + P[5][10]*dt) + SPP[5]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; + nextP[8][3] = P[8][3] + P[5][3]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[3]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][2] + P[5][2]*dt) + SPP[4]*(P[8][10] + P[5][10]*dt) - SPP[5]*(P[8][11] + P[5][11]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; + nextP[8][4] = P[8][4] + P[5][4]*dt + SF[0]*(P[8][1] + P[5][1]*dt) + SF[2]*(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); + nextP[8][5] = P[8][5] + P[5][5]*dt + SF[1]*(P[8][0] + P[5][0]*dt) + SF[0]*(P[8][2] + P[5][2]*dt) + SF[2]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt); + nextP[8][6] = P[8][6] + P[5][6]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[0]*(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); + 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[9][0] = P[9][0] + P[6][0]*dt + SF[6]*(P[9][1] + P[6][1]*dt) + SPP[7]*(P[9][2] + P[6][2]*dt) + SPP[6]*(P[9][3] + P[6][3]*dt) + SPP[5]*(P[9][10] + P[6][10]*dt) + SPP[4]*(P[9][11] + P[6][11]*dt) + SPP[3]*(P[9][12] + P[6][12]*dt); + nextP[9][1] = P[9][1] + P[6][1]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][2] + P[6][2]*dt) + SPP[7]*(P[9][3] + P[6][3]*dt) + SPP[3]*(P[9][11] + P[6][11]*dt) - SPP[4]*(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[3]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][1] + P[6][1]*dt) - SPP[3]*(P[9][10] + P[6][10]*dt) + SPP[5]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; + nextP[9][3] = P[9][3] + P[6][3]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[3]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][2] + P[6][2]*dt) + SPP[4]*(P[9][10] + P[6][10]*dt) - SPP[5]*(P[9][11] + P[6][11]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; + nextP[9][4] = P[9][4] + P[6][4]*dt + SF[0]*(P[9][1] + P[6][1]*dt) + SF[2]*(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); + nextP[9][5] = P[9][5] + P[6][5]*dt + SF[1]*(P[9][0] + P[6][0]*dt) + SF[0]*(P[9][2] + P[6][2]*dt) + SF[2]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt); + nextP[9][6] = P[9][6] + P[6][6]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[0]*(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); + 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; + nextP[9][16] = P[9][16] + P[6][16]*dt; + nextP[9][17] = P[9][17] + P[6][17]*dt; + 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[10][0] = P[10][0] + P[10][1]*SF[6] + P[10][2]*SPP[7] + P[10][3]*SPP[6] + P[10][10]*SPP[5] + P[10][11]*SPP[4] + P[10][12]*SPP[3]; + nextP[10][1] = P[10][1] + P[10][0]*SF[5] + P[10][2]*SF[4] + P[10][3]*SPP[7] + P[10][11]*SPP[3] - P[10][12]*SPP[4] - (P[10][10]*q0)/2; + nextP[10][2] = P[10][2] + P[10][0]*SF[3] + P[10][3]*SF[5] + P[10][1]*SPP[6] - P[10][10]*SPP[3] + P[10][12]*SPP[5] - (P[10][11]*q0)/2; + nextP[10][3] = P[10][3] + P[10][0]*SF[4] + P[10][1]*SF[3] + P[10][2]*SF[6] + P[10][10]*SPP[4] - P[10][11]*SPP[5] - (P[10][12]*q0)/2; + nextP[10][4] = P[10][4] + P[10][1]*SF[0] + P[10][0]*SF[2] + P[10][2]*SPP[0] - P[10][3]*SPP[2]; + nextP[10][5] = P[10][5] + P[10][0]*SF[1] + P[10][2]*SF[0] + P[10][3]*SF[2] - P[10][1]*SPP[0]; + nextP[10][6] = P[10][6] + P[10][1]*SF[1] + P[10][3]*SF[0] + P[10][0]*SPP[0] - P[10][2]*SPP[1]; + 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[11][0] = P[11][0] + P[11][1]*SF[6] + P[11][2]*SPP[7] + P[11][3]*SPP[6] + P[11][10]*SPP[5] + P[11][11]*SPP[4] + P[11][12]*SPP[3]; + nextP[11][1] = P[11][1] + P[11][0]*SF[5] + P[11][2]*SF[4] + P[11][3]*SPP[7] + P[11][11]*SPP[3] - P[11][12]*SPP[4] - (P[11][10]*q0)/2; + nextP[11][2] = P[11][2] + P[11][0]*SF[3] + P[11][3]*SF[5] + P[11][1]*SPP[6] - P[11][10]*SPP[3] + P[11][12]*SPP[5] - (P[11][11]*q0)/2; + nextP[11][3] = P[11][3] + P[11][0]*SF[4] + P[11][1]*SF[3] + P[11][2]*SF[6] + P[11][10]*SPP[4] - P[11][11]*SPP[5] - (P[11][12]*q0)/2; + nextP[11][4] = P[11][4] + P[11][1]*SF[0] + P[11][0]*SF[2] + P[11][2]*SPP[0] - P[11][3]*SPP[2]; + nextP[11][5] = P[11][5] + P[11][0]*SF[1] + P[11][2]*SF[0] + P[11][3]*SF[2] - P[11][1]*SPP[0]; + nextP[11][6] = P[11][6] + P[11][1]*SF[1] + P[11][3]*SF[0] + P[11][0]*SPP[0] - P[11][2]*SPP[1]; + 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[12][0] = P[12][0] + P[12][1]*SF[6] + P[12][2]*SPP[7] + P[12][3]*SPP[6] + P[12][10]*SPP[5] + P[12][11]*SPP[4] + P[12][12]*SPP[3]; + nextP[12][1] = P[12][1] + P[12][0]*SF[5] + P[12][2]*SF[4] + P[12][3]*SPP[7] + P[12][11]*SPP[3] - P[12][12]*SPP[4] - (P[12][10]*q0)/2; + nextP[12][2] = P[12][2] + P[12][0]*SF[3] + P[12][3]*SF[5] + P[12][1]*SPP[6] - P[12][10]*SPP[3] + P[12][12]*SPP[5] - (P[12][11]*q0)/2; + nextP[12][3] = P[12][3] + P[12][0]*SF[4] + P[12][1]*SF[3] + P[12][2]*SF[6] + P[12][10]*SPP[4] - P[12][11]*SPP[5] - (P[12][12]*q0)/2; + nextP[12][4] = P[12][4] + P[12][1]*SF[0] + P[12][0]*SF[2] + P[12][2]*SPP[0] - P[12][3]*SPP[2]; + nextP[12][5] = P[12][5] + P[12][0]*SF[1] + P[12][2]*SF[0] + P[12][3]*SF[2] - P[12][1]*SPP[0]; + nextP[12][6] = P[12][6] + P[12][1]*SF[1] + P[12][3]*SF[0] + P[12][0]*SPP[0] - P[12][2]*SPP[1]; + 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[13][0] = P[13][0] + P[13][1]*SF[6] + P[13][2]*SPP[7] + P[13][3]*SPP[6] + P[13][10]*SPP[5] + P[13][11]*SPP[4] + P[13][12]*SPP[3]; + nextP[13][1] = P[13][1] + P[13][0]*SF[5] + P[13][2]*SF[4] + P[13][3]*SPP[7] + P[13][11]*SPP[3] - P[13][12]*SPP[4] - (P[13][10]*q0)/2; + nextP[13][2] = P[13][2] + P[13][0]*SF[3] + P[13][3]*SF[5] + P[13][1]*SPP[6] - P[13][10]*SPP[3] + P[13][12]*SPP[5] - (P[13][11]*q0)/2; + nextP[13][3] = P[13][3] + P[13][0]*SF[4] + P[13][1]*SF[3] + P[13][2]*SF[6] + P[13][10]*SPP[4] - P[13][11]*SPP[5] - (P[13][12]*q0)/2; + nextP[13][4] = P[13][4] + P[13][1]*SF[0] + P[13][0]*SF[2] + P[13][2]*SPP[0] - P[13][3]*SPP[2]; + nextP[13][5] = P[13][5] + P[13][0]*SF[1] + P[13][2]*SF[0] + P[13][3]*SF[2] - P[13][1]*SPP[0]; + nextP[13][6] = P[13][6] + P[13][1]*SF[1] + P[13][3]*SF[0] + P[13][0]*SPP[0] - P[13][2]*SPP[1]; + 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[14][0] = P[14][0] + P[14][1]*SF[6] + P[14][2]*SPP[7] + P[14][3]*SPP[6] + P[14][10]*SPP[5] + P[14][11]*SPP[4] + P[14][12]*SPP[3]; + nextP[14][1] = P[14][1] + P[14][0]*SF[5] + P[14][2]*SF[4] + P[14][3]*SPP[7] + P[14][11]*SPP[3] - P[14][12]*SPP[4] - (P[14][10]*q0)/2; + nextP[14][2] = P[14][2] + P[14][0]*SF[3] + P[14][3]*SF[5] + P[14][1]*SPP[6] - P[14][10]*SPP[3] + P[14][12]*SPP[5] - (P[14][11]*q0)/2; + nextP[14][3] = P[14][3] + P[14][0]*SF[4] + P[14][1]*SF[3] + P[14][2]*SF[6] + P[14][10]*SPP[4] - P[14][11]*SPP[5] - (P[14][12]*q0)/2; + nextP[14][4] = P[14][4] + P[14][1]*SF[0] + P[14][0]*SF[2] + P[14][2]*SPP[0] - P[14][3]*SPP[2]; + nextP[14][5] = P[14][5] + P[14][0]*SF[1] + P[14][2]*SF[0] + P[14][3]*SF[2] - P[14][1]*SPP[0]; + nextP[14][6] = P[14][6] + P[14][1]*SF[1] + P[14][3]*SF[0] + P[14][0]*SPP[0] - P[14][2]*SPP[1]; + 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[15][0] = P[15][0] + P[15][1]*SF[6] + P[15][2]*SPP[7] + P[15][3]*SPP[6] + P[15][10]*SPP[5] + P[15][11]*SPP[4] + P[15][12]*SPP[3]; + nextP[15][1] = P[15][1] + P[15][0]*SF[5] + P[15][2]*SF[4] + P[15][3]*SPP[7] + P[15][11]*SPP[3] - P[15][12]*SPP[4] - (P[15][10]*q0)/2; + nextP[15][2] = P[15][2] + P[15][0]*SF[3] + P[15][3]*SF[5] + P[15][1]*SPP[6] - P[15][10]*SPP[3] + P[15][12]*SPP[5] - (P[15][11]*q0)/2; + nextP[15][3] = P[15][3] + P[15][0]*SF[4] + P[15][1]*SF[3] + P[15][2]*SF[6] + P[15][10]*SPP[4] - P[15][11]*SPP[5] - (P[15][12]*q0)/2; + nextP[15][4] = P[15][4] + P[15][1]*SF[0] + P[15][0]*SF[2] + P[15][2]*SPP[0] - P[15][3]*SPP[2]; + nextP[15][5] = P[15][5] + P[15][0]*SF[1] + P[15][2]*SF[0] + P[15][3]*SF[2] - P[15][1]*SPP[0]; + nextP[15][6] = P[15][6] + P[15][1]*SF[1] + P[15][3]*SF[0] + P[15][0]*SPP[0] - P[15][2]*SPP[1]; + 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[16][0] = P[16][0] + P[16][1]*SF[6] + P[16][2]*SPP[7] + P[16][3]*SPP[6] + P[16][10]*SPP[5] + P[16][11]*SPP[4] + P[16][12]*SPP[3]; + nextP[16][1] = P[16][1] + P[16][0]*SF[5] + P[16][2]*SF[4] + P[16][3]*SPP[7] + P[16][11]*SPP[3] - P[16][12]*SPP[4] - (P[16][10]*q0)/2; + nextP[16][2] = P[16][2] + P[16][0]*SF[3] + P[16][3]*SF[5] + P[16][1]*SPP[6] - P[16][10]*SPP[3] + P[16][12]*SPP[5] - (P[16][11]*q0)/2; + nextP[16][3] = P[16][3] + P[16][0]*SF[4] + P[16][1]*SF[3] + P[16][2]*SF[6] + P[16][10]*SPP[4] - P[16][11]*SPP[5] - (P[16][12]*q0)/2; + nextP[16][4] = P[16][4] + P[16][1]*SF[0] + P[16][0]*SF[2] + P[16][2]*SPP[0] - P[16][3]*SPP[2]; + nextP[16][5] = P[16][5] + P[16][0]*SF[1] + P[16][2]*SF[0] + P[16][3]*SF[2] - P[16][1]*SPP[0]; + nextP[16][6] = P[16][6] + P[16][1]*SF[1] + P[16][3]*SF[0] + P[16][0]*SPP[0] - P[16][2]*SPP[1]; + 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[17][0] = P[17][0] + P[17][1]*SF[6] + P[17][2]*SPP[7] + P[17][3]*SPP[6] + P[17][10]*SPP[5] + P[17][11]*SPP[4] + P[17][12]*SPP[3]; + nextP[17][1] = P[17][1] + P[17][0]*SF[5] + P[17][2]*SF[4] + P[17][3]*SPP[7] + P[17][11]*SPP[3] - P[17][12]*SPP[4] - (P[17][10]*q0)/2; + nextP[17][2] = P[17][2] + P[17][0]*SF[3] + P[17][3]*SF[5] + P[17][1]*SPP[6] - P[17][10]*SPP[3] + P[17][12]*SPP[5] - (P[17][11]*q0)/2; + nextP[17][3] = P[17][3] + P[17][0]*SF[4] + P[17][1]*SF[3] + P[17][2]*SF[6] + P[17][10]*SPP[4] - P[17][11]*SPP[5] - (P[17][12]*q0)/2; + nextP[17][4] = P[17][4] + P[17][1]*SF[0] + P[17][0]*SF[2] + P[17][2]*SPP[0] - P[17][3]*SPP[2]; + nextP[17][5] = P[17][5] + P[17][0]*SF[1] + P[17][2]*SF[0] + P[17][3]*SF[2] - P[17][1]*SPP[0]; + nextP[17][6] = P[17][6] + P[17][1]*SF[1] + P[17][3]*SF[0] + P[17][0]*SPP[0] - P[17][2]*SPP[1]; + 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[18][0] = P[18][0] + P[18][1]*SF[6] + P[18][2]*SPP[7] + P[18][3]*SPP[6] + P[18][10]*SPP[5] + P[18][11]*SPP[4] + P[18][12]*SPP[3]; + nextP[18][1] = P[18][1] + P[18][0]*SF[5] + P[18][2]*SF[4] + P[18][3]*SPP[7] + P[18][11]*SPP[3] - P[18][12]*SPP[4] - (P[18][10]*q0)/2; + nextP[18][2] = P[18][2] + P[18][0]*SF[3] + P[18][3]*SF[5] + P[18][1]*SPP[6] - P[18][10]*SPP[3] + P[18][12]*SPP[5] - (P[18][11]*q0)/2; + nextP[18][3] = P[18][3] + P[18][0]*SF[4] + P[18][1]*SF[3] + P[18][2]*SF[6] + P[18][10]*SPP[4] - P[18][11]*SPP[5] - (P[18][12]*q0)/2; + nextP[18][4] = P[18][4] + P[18][1]*SF[0] + P[18][0]*SF[2] + P[18][2]*SPP[0] - P[18][3]*SPP[2]; + nextP[18][5] = P[18][5] + P[18][0]*SF[1] + P[18][2]*SF[0] + P[18][3]*SF[2] - P[18][1]*SPP[0]; + nextP[18][6] = P[18][6] + P[18][1]*SF[1] + P[18][3]*SF[0] + P[18][0]*SPP[0] - P[18][2]*SPP[1]; + 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[19][0] = P[19][0] + P[19][1]*SF[6] + P[19][2]*SPP[7] + P[19][3]*SPP[6] + P[19][10]*SPP[5] + P[19][11]*SPP[4] + P[19][12]*SPP[3]; + nextP[19][1] = P[19][1] + P[19][0]*SF[5] + P[19][2]*SF[4] + P[19][3]*SPP[7] + P[19][11]*SPP[3] - P[19][12]*SPP[4] - (P[19][10]*q0)/2; + nextP[19][2] = P[19][2] + P[19][0]*SF[3] + P[19][3]*SF[5] + P[19][1]*SPP[6] - P[19][10]*SPP[3] + P[19][12]*SPP[5] - (P[19][11]*q0)/2; + nextP[19][3] = P[19][3] + P[19][0]*SF[4] + P[19][1]*SF[3] + P[19][2]*SF[6] + P[19][10]*SPP[4] - P[19][11]*SPP[5] - (P[19][12]*q0)/2; + nextP[19][4] = P[19][4] + P[19][1]*SF[0] + P[19][0]*SF[2] + P[19][2]*SPP[0] - P[19][3]*SPP[2]; + nextP[19][5] = P[19][5] + P[19][0]*SF[1] + P[19][2]*SF[0] + P[19][3]*SF[2] - P[19][1]*SPP[0]; + nextP[19][6] = P[19][6] + P[19][1]*SF[1] + P[19][3]*SF[0] + P[19][0]*SPP[0] - P[19][2]*SPP[1]; + 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[20][0] = P[20][0] + P[20][1]*SF[6] + P[20][2]*SPP[7] + P[20][3]*SPP[6] + P[20][10]*SPP[5] + P[20][11]*SPP[4] + P[20][12]*SPP[3]; + nextP[20][1] = P[20][1] + P[20][0]*SF[5] + P[20][2]*SF[4] + P[20][3]*SPP[7] + P[20][11]*SPP[3] - P[20][12]*SPP[4] - (P[20][10]*q0)/2; + nextP[20][2] = P[20][2] + P[20][0]*SF[3] + P[20][3]*SF[5] + P[20][1]*SPP[6] - P[20][10]*SPP[3] + P[20][12]*SPP[5] - (P[20][11]*q0)/2; + nextP[20][3] = P[20][3] + P[20][0]*SF[4] + P[20][1]*SF[3] + P[20][2]*SF[6] + P[20][10]*SPP[4] - P[20][11]*SPP[5] - (P[20][12]*q0)/2; + nextP[20][4] = P[20][4] + P[20][1]*SF[0] + P[20][0]*SF[2] + P[20][2]*SPP[0] - P[20][3]*SPP[2]; + nextP[20][5] = P[20][5] + P[20][0]*SF[1] + P[20][2]*SF[0] + P[20][3]*SF[2] - P[20][1]*SPP[0]; + nextP[20][6] = P[20][6] + P[20][1]*SF[1] + P[20][3]*SF[0] + P[20][0]*SPP[0] - P[20][2]*SPP[1]; + 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]; + + for (unsigned i = 0; i < n_states; i++) + { + nextP[i][i] = nextP[i][i] + processNoise[i]; + } + + // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by + // setting the coresponding covariance terms to zero. + if (onGround || !useCompass) + { + zeroRows(nextP,15,20); + zeroCols(nextP,15,20); + } + + // If on ground or not using airspeed sensing, inhibit wind velocity + // covariance growth. + if (onGround || !useAirspeed) + { + zeroRows(nextP,13,14); + zeroCols(nextP,13,14); + } + + // If the total position variance exceds 1E6 (1000m), then stop covariance + // growth by setting the predicted to the previous values + // This prevent an ill conditioned matrix from occurring for long periods + // without GPS + if ((P[7][7] + P[8][8]) > 1E6f) + { + for (uint8_t i=7; i<=8; i++) + { + for (unsigned j = 0; j < n_states; j++) + { + nextP[i][j] = P[i][j]; + nextP[j][i] = P[j][i]; + } + } + } + + if (onGround || staticMode) { + // copy the portion of the variances we want to + // propagate + for (unsigned i = 0; i < 14; i++) { + P[i][i] = nextP[i][i]; + + // force symmetry for observable states + // force zero for non-observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + if ((i > 12) || (j > 12)) { + P[i][j] = 0.0f; + } else { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + } + P[j][i] = P[i][j]; + } + } + } + + } else { + + // Copy covariance + for (unsigned i = 0; i < n_states; i++) { + P[i][i] = nextP[i][i]; + } + + // force symmetry for observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + P[j][i] = P[i][j]; + } + } + + } + + ConstrainVariances(); +} + +void AttPosEKF::FuseVelposNED() +{ + +// declare variables used by fault isolation logic + uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure + uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available + uint32_t hgtRetryTime = 5000; // height measurement retry time + uint32_t horizRetryTime; + +// declare variables used to check measurement errors + float velInnov[3] = {0.0f,0.0f,0.0f}; + float posInnov[2] = {0.0f,0.0f}; + float hgtInnov = 0.0f; + +// declare variables used to control access to arrays + bool fuseData[6] = {false,false,false,false,false,false}; + uint8_t stateIndex; + uint8_t obsIndex; + uint8_t indexLimit; + +// declare variables used by state and covariance update calculations + float velErr; + float posErr; + float R_OBS[6]; + float observation[6]; + float SK; + float quatMag; + +// Perform sequential fusion of GPS measurements. This assumes that the +// errors in the different velocity and position components are +// uncorrelated which is not true, however in the absence of covariance +// data from the GPS receiver it is the only assumption we can make +// so we might as well take advantage of the computational efficiencies +// associated with sequential fusion + if (fuseVelData || fusePosData || fuseHgtData) + { + // set the GPS data timeout depending on whether airspeed data is present + if (useAirspeed) horizRetryTime = gpsRetryTime; + else horizRetryTime = gpsRetryTimeNoTAS; + + // Form the observation vector + for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; + for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; + observation[5] = -(hgtMea); + + // Estimate the GPS Velocity, GPS horiz position and height measurement variances. + velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring + posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring + R_OBS[0] = 0.04f + sq(velErr); + R_OBS[1] = R_OBS[0]; + R_OBS[2] = 0.08f + sq(velErr); + R_OBS[3] = R_OBS[2]; + R_OBS[4] = 4.0f + sq(posErr); + R_OBS[5] = 4.0f; + + // Set innovation variances to zero default + for (uint8_t i = 0; i<=5; i++) + { + varInnovVelPos[i] = 0.0f; + } + // calculate innovations and check GPS data validity using an innovation consistency check + if (fuseVelData) + { + // test velocity measurements + uint8_t imax = 2; + if (fusionModeGPS == 1) imax = 1; + for (uint8_t i = 0; i<=imax; i++) + { + velInnov[i] = statesAtVelTime[i+4] - velNED[i]; + stateIndex = 4 + i; + varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; + } + // apply a 5-sigma threshold + current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); + current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; + if (current_ekf_state.velHealth || current_ekf_state.velTimeout) + { + current_ekf_state.velHealth = true; + current_ekf_state.velFailTime = millis(); + } + else + { + current_ekf_state.velHealth = false; + } + } + if (fusePosData) + { + // test horizontal position measurements + posInnov[0] = statesAtPosTime[7] - posNE[0]; + posInnov[1] = statesAtPosTime[8] - posNE[1]; + varInnovVelPos[3] = P[7][7] + R_OBS[3]; + varInnovVelPos[4] = P[8][8] + R_OBS[4]; + // apply a 10-sigma threshold + current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); + current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime; + if (current_ekf_state.posHealth || current_ekf_state.posTimeout) + { + current_ekf_state.posHealth = true; + current_ekf_state.posFailTime = millis(); + } + else + { + current_ekf_state.posHealth = false; + } + } + // test height measurements + if (fuseHgtData) + { + hgtInnov = statesAtHgtTime[9] + hgtMea; + varInnovVelPos[5] = P[9][9] + R_OBS[5]; + // apply a 10-sigma threshold + current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; + current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; + if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) + { + current_ekf_state.hgtHealth = true; + current_ekf_state.hgtFailTime = millis(); + } + else + { + current_ekf_state.hgtHealth = false; + } + } + // Set range for sequential fusion of velocity and position measurements depending + // on which data is available and its health + if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth) + { + fuseData[0] = true; + fuseData[1] = true; + fuseData[2] = true; + } + if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth) + { + fuseData[0] = true; + fuseData[1] = true; + } + if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth) + { + fuseData[3] = true; + fuseData[4] = true; + } + if (fuseHgtData && current_ekf_state.hgtHealth) + { + fuseData[5] = true; + } + // Limit range of states modified when on ground + if(!onGround) + { + indexLimit = 20; + } + else + { + indexLimit = 12; + } + // Fuse measurements sequentially + for (obsIndex=0; obsIndex<=5; obsIndex++) + { + if (fuseData[obsIndex]) + { + stateIndex = 4 + obsIndex; + // Calculate the measurement innovation, using states from a + // different time coordinate if fusing height data + if (obsIndex >= 0 && obsIndex <= 2) + { + innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; + } + else if (obsIndex == 3 || obsIndex == 4) + { + innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex]; + } + else if (obsIndex == 5) + { + innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex]; + } + // Calculate the Kalman Gain + // Calculate innovation variances - also used for data logging + varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; + SK = 1.0/varInnovVelPos[obsIndex]; + for (uint8_t i= 0; i<=indexLimit; i++) + { + Kfusion[i] = P[i][stateIndex]*SK; + } + // Calculate state corrections and re-normalise the quaternions + for (uint8_t i = 0; i<=indexLimit; i++) + { + states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; + } + quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) // divide by 0 protection + { + for (uint8_t i = 0; i<=3; i++) + { + states[i] = states[i] / quatMag; + } + } + // Update the covariance - take advantage of direct observation of a + // single state at index = stateIndex to reduce computations + // Optimised implementation of standard equation P = (I - K*H)*P; + for (uint8_t i= 0; i<=indexLimit; i++) + { + for (uint8_t j= 0; j<=indexLimit; j++) + { + KHP[i][j] = Kfusion[i] * P[stateIndex][j]; + } + } + for (uint8_t i= 0; i<=indexLimit; i++) + { + for (uint8_t j= 0; j<=indexLimit; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + } + + ForceSymmetry(); + ConstrainVariances(); + + //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); +} + +void AttPosEKF::FuseMagnetometer() +{ + uint8_t obsIndex; + uint8_t indexLimit; + float DCM[3][3] = + { + {1.0f,0.0f,0.0f} , + {0.0f,1.0f,0.0f} , + {0.0f,0.0f,1.0f} + }; + float MagPred[3] = {0.0f,0.0f,0.0f}; + float SK_MX[6]; + float SK_MY[5]; + float SK_MZ[6]; + float SH_MAG[9] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; + +// Perform sequential fusion of Magnetometer measurements. +// This assumes that the errors in the different components are +// uncorrelated which is not true, however in the absence of covariance +// data fit is the only assumption we can make +// so we might as well take advantage of the computational efficiencies +// associated with sequential fusion + if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) + { + // Limit range of states modified when on ground + if(!onGround) + { + indexLimit = 20; + } + else + { + indexLimit = 12; + } + + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float magN = 0.4f; + static float magE = 0.0f; + static float magD = 0.3f; + + static float R_MAG = 0.0025f; + + float H_MAG[21] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; + + // Sequential fusion of XYZ components to spread processing load across + // three prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseMagData) + { + static float magXbias = 0.0f; + static float magYbias = 0.0f; + static float magZbias = 0.0f; + + // Copy required states to local variable names + q0 = statesAtMagMeasTime[0]; + q1 = statesAtMagMeasTime[1]; + q2 = statesAtMagMeasTime[2]; + q3 = statesAtMagMeasTime[3]; + magN = statesAtMagMeasTime[15]; + magE = statesAtMagMeasTime[16]; + magD = statesAtMagMeasTime[17]; + magXbias = statesAtMagMeasTime[18]; + magYbias = statesAtMagMeasTime[19]; + magZbias = statesAtMagMeasTime[20]; + + // rotate predicted earth components into body axes and calculate + // predicted measurments + 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 + R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU); + + // Calculate observation jacobians + 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<=20; 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[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[16] = 2*q0*q3 + 2*q1*q2; + H_MAG[17] = 2*q1*q3 - 2*q0*q2; + H_MAG[18] = 1.0f; + + // Calculate Kalman gain + SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][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[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][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[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][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[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][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[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][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)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][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)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; + 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; + Kfusion[0] = SK_MX[0]*(P[0][18] + 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][15]*SK_MX[1] + P[0][16]*SK_MX[5] - P[0][17]*SK_MX[4]); + Kfusion[1] = SK_MX[0]*(P[1][18] + 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][15]*SK_MX[1] + P[1][16]*SK_MX[5] - P[1][17]*SK_MX[4]); + Kfusion[2] = SK_MX[0]*(P[2][18] + 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][15]*SK_MX[1] + P[2][16]*SK_MX[5] - P[2][17]*SK_MX[4]); + Kfusion[3] = SK_MX[0]*(P[3][18] + 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][15]*SK_MX[1] + P[3][16]*SK_MX[5] - P[3][17]*SK_MX[4]); + Kfusion[4] = SK_MX[0]*(P[4][18] + 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][15]*SK_MX[1] + P[4][16]*SK_MX[5] - P[4][17]*SK_MX[4]); + Kfusion[5] = SK_MX[0]*(P[5][18] + 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][15]*SK_MX[1] + P[5][16]*SK_MX[5] - P[5][17]*SK_MX[4]); + Kfusion[6] = SK_MX[0]*(P[6][18] + 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][15]*SK_MX[1] + P[6][16]*SK_MX[5] - P[6][17]*SK_MX[4]); + Kfusion[7] = SK_MX[0]*(P[7][18] + 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][15]*SK_MX[1] + P[7][16]*SK_MX[5] - P[7][17]*SK_MX[4]); + Kfusion[8] = SK_MX[0]*(P[8][18] + 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][15]*SK_MX[1] + P[8][16]*SK_MX[5] - P[8][17]*SK_MX[4]); + Kfusion[9] = SK_MX[0]*(P[9][18] + 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][15]*SK_MX[1] + P[9][16]*SK_MX[5] - P[9][17]*SK_MX[4]); + Kfusion[10] = SK_MX[0]*(P[10][18] + 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][15]*SK_MX[1] + P[10][16]*SK_MX[5] - P[10][17]*SK_MX[4]); + Kfusion[11] = SK_MX[0]*(P[11][18] + 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][15]*SK_MX[1] + P[11][16]*SK_MX[5] - P[11][17]*SK_MX[4]); + Kfusion[12] = SK_MX[0]*(P[12][18] + 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][15]*SK_MX[1] + P[12][16]*SK_MX[5] - P[12][17]*SK_MX[4]); + Kfusion[13] = SK_MX[0]*(P[13][18] + 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][15]*SK_MX[1] + P[13][16]*SK_MX[5] - P[13][17]*SK_MX[4]); + Kfusion[14] = SK_MX[0]*(P[14][18] + 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][15]*SK_MX[1] + P[14][16]*SK_MX[5] - P[14][17]*SK_MX[4]); + Kfusion[15] = SK_MX[0]*(P[15][18] + 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][15]*SK_MX[1] + P[15][16]*SK_MX[5] - P[15][17]*SK_MX[4]); + Kfusion[16] = SK_MX[0]*(P[16][18] + 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][15]*SK_MX[1] + P[16][16]*SK_MX[5] - P[16][17]*SK_MX[4]); + Kfusion[17] = SK_MX[0]*(P[17][18] + 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][15]*SK_MX[1] + P[17][16]*SK_MX[5] - P[17][17]*SK_MX[4]); + Kfusion[18] = SK_MX[0]*(P[18][18] + 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][15]*SK_MX[1] + P[18][16]*SK_MX[5] - P[18][17]*SK_MX[4]); + Kfusion[19] = SK_MX[0]*(P[19][18] + 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][15]*SK_MX[1] + P[19][16]*SK_MX[5] - P[19][17]*SK_MX[4]); + Kfusion[20] = SK_MX[0]*(P[20][18] + 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][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]); + varInnovMag[0] = 1.0f/SK_MX[0]; + innovMag[0] = MagPred[0] - magData.x; + + // reset the observation index to 0 (we start by fusing the X + // measurement) + obsIndex = 0; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (unsigned int i=0; i<n_states; 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[15] = 2*q1*q2 - 2*q0*q3; + H_MAG[16] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; + H_MAG[17] = 2*q0*q1 + 2*q2*q3; + H_MAG[19] = 1; + + // Calculate Kalman gain + SK_MY[0] = 1/(P[19][19] + R_MAG + P[0][19]*SH_MAG[2] + P[1][19]*SH_MAG[1] + P[2][19]*SH_MAG[0] - P[16][19]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[19][15] + P[0][15]*SH_MAG[2] + P[1][15]*SH_MAG[1] + P[2][15]*SH_MAG[0] - P[16][15]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][15]*(2*q0*q3 - 2*q1*q2) + P[17][15]*(2*q0*q1 + 2*q2*q3) - P[3][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[19][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[16][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][17]*(2*q0*q3 - 2*q1*q2) + P[17][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[16][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][3]*(2*q0*q3 - 2*q1*q2) + P[17][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[15][19]*(2*q0*q3 - 2*q1*q2) + P[17][19]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[19][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[16][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][0]*(2*q0*q3 - 2*q1*q2) + P[17][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[19][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[16][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][1]*(2*q0*q3 - 2*q1*q2) + P[17][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[16][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][2]*(2*q0*q3 - 2*q1*q2) + P[17][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[19][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[16][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][16]*(2*q0*q3 - 2*q1*q2) + P[17][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; + SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MY[3] = 2*q0*q3 - 2*q1*q2; + SK_MY[4] = 2*q0*q1 + 2*q2*q3; + Kfusion[0] = SK_MY[0]*(P[0][19] + 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][16]*SK_MY[1] - P[0][15]*SK_MY[3] + P[0][17]*SK_MY[4]); + Kfusion[1] = SK_MY[0]*(P[1][19] + 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][16]*SK_MY[1] - P[1][15]*SK_MY[3] + P[1][17]*SK_MY[4]); + Kfusion[2] = SK_MY[0]*(P[2][19] + 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][16]*SK_MY[1] - P[2][15]*SK_MY[3] + P[2][17]*SK_MY[4]); + Kfusion[3] = SK_MY[0]*(P[3][19] + 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][16]*SK_MY[1] - P[3][15]*SK_MY[3] + P[3][17]*SK_MY[4]); + Kfusion[4] = SK_MY[0]*(P[4][19] + 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][16]*SK_MY[1] - P[4][15]*SK_MY[3] + P[4][17]*SK_MY[4]); + Kfusion[5] = SK_MY[0]*(P[5][19] + 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][16]*SK_MY[1] - P[5][15]*SK_MY[3] + P[5][17]*SK_MY[4]); + Kfusion[6] = SK_MY[0]*(P[6][19] + 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][16]*SK_MY[1] - P[6][15]*SK_MY[3] + P[6][17]*SK_MY[4]); + Kfusion[7] = SK_MY[0]*(P[7][19] + 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][16]*SK_MY[1] - P[7][15]*SK_MY[3] + P[7][17]*SK_MY[4]); + Kfusion[8] = SK_MY[0]*(P[8][19] + 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][16]*SK_MY[1] - P[8][15]*SK_MY[3] + P[8][17]*SK_MY[4]); + Kfusion[9] = SK_MY[0]*(P[9][19] + 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][16]*SK_MY[1] - P[9][15]*SK_MY[3] + P[9][17]*SK_MY[4]); + Kfusion[10] = SK_MY[0]*(P[10][19] + 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][16]*SK_MY[1] - P[10][15]*SK_MY[3] + P[10][17]*SK_MY[4]); + Kfusion[11] = SK_MY[0]*(P[11][19] + 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][16]*SK_MY[1] - P[11][15]*SK_MY[3] + P[11][17]*SK_MY[4]); + Kfusion[12] = SK_MY[0]*(P[12][19] + 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][16]*SK_MY[1] - P[12][15]*SK_MY[3] + P[12][17]*SK_MY[4]); + Kfusion[13] = SK_MY[0]*(P[13][19] + 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][16]*SK_MY[1] - P[13][15]*SK_MY[3] + P[13][17]*SK_MY[4]); + Kfusion[14] = SK_MY[0]*(P[14][19] + 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][16]*SK_MY[1] - P[14][15]*SK_MY[3] + P[14][17]*SK_MY[4]); + Kfusion[15] = SK_MY[0]*(P[15][19] + 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][16]*SK_MY[1] - P[15][15]*SK_MY[3] + P[15][17]*SK_MY[4]); + Kfusion[16] = SK_MY[0]*(P[16][19] + 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][16]*SK_MY[1] - P[16][15]*SK_MY[3] + P[16][17]*SK_MY[4]); + Kfusion[17] = SK_MY[0]*(P[17][19] + 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][16]*SK_MY[1] - P[17][15]*SK_MY[3] + P[17][17]*SK_MY[4]); + Kfusion[18] = SK_MY[0]*(P[18][19] + 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][16]*SK_MY[1] - P[18][15]*SK_MY[3] + P[18][17]*SK_MY[4]); + Kfusion[19] = SK_MY[0]*(P[19][19] + 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][16]*SK_MY[1] - P[19][15]*SK_MY[3] + P[19][17]*SK_MY[4]); + Kfusion[20] = SK_MY[0]*(P[20][19] + 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][16]*SK_MY[1] - P[20][15]*SK_MY[3] + P[20][17]*SK_MY[4]); + varInnovMag[1] = 1.0f/SK_MY[0]; + innovMag[1] = MagPred[1] - magData.y; + } + else if (obsIndex == 2) // we are now fusing the Z measurement + { + // Calculate observation jacobians + for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + 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[15] = 2*q0*q2 + 2*q1*q3; + H_MAG[16] = 2*q2*q3 - 2*q0*q1; + H_MAG[17] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + H_MAG[20] = 1; + + // Calculate Kalman gain + SK_MZ[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[1] + P[3][20]*SH_MAG[0] + P[17][20]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[20][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[17][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][1]*(2*q0*q2 + 2*q1*q3) - P[16][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[20][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[17][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][2]*(2*q0*q2 + 2*q1*q3) - P[16][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[20][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[17][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][0]*(2*q0*q2 + 2*q1*q3) - P[16][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[20][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[17][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][3]*(2*q0*q2 + 2*q1*q3) - P[16][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[20][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[17][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][17]*(2*q0*q2 + 2*q1*q3) - P[16][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[15][20]*(2*q0*q2 + 2*q1*q3) - P[16][20]*(2*q0*q1 - 2*q2*q3) - P[1][20]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[20][15] + P[0][15]*SH_MAG[1] + P[3][15]*SH_MAG[0] + P[17][15]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][15]*(2*q0*q2 + 2*q1*q3) - P[16][15]*(2*q0*q1 - 2*q2*q3) - P[1][15]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[20][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[17][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][16]*(2*q0*q2 + 2*q1*q3) - P[16][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)) + P[2][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + 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][20] + 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][17]*SK_MZ[1] + P[0][15]*SK_MZ[5] - P[0][16]*SK_MZ[4]); + Kfusion[1] = SK_MZ[0]*(P[1][20] + 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][17]*SK_MZ[1] + P[1][15]*SK_MZ[5] - P[1][16]*SK_MZ[4]); + Kfusion[2] = SK_MZ[0]*(P[2][20] + 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][17]*SK_MZ[1] + P[2][15]*SK_MZ[5] - P[2][16]*SK_MZ[4]); + Kfusion[3] = SK_MZ[0]*(P[3][20] + 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][17]*SK_MZ[1] + P[3][15]*SK_MZ[5] - P[3][16]*SK_MZ[4]); + Kfusion[4] = SK_MZ[0]*(P[4][20] + 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][17]*SK_MZ[1] + P[4][15]*SK_MZ[5] - P[4][16]*SK_MZ[4]); + Kfusion[5] = SK_MZ[0]*(P[5][20] + 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][17]*SK_MZ[1] + P[5][15]*SK_MZ[5] - P[5][16]*SK_MZ[4]); + Kfusion[6] = SK_MZ[0]*(P[6][20] + 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][17]*SK_MZ[1] + P[6][15]*SK_MZ[5] - P[6][16]*SK_MZ[4]); + Kfusion[7] = SK_MZ[0]*(P[7][20] + 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][17]*SK_MZ[1] + P[7][15]*SK_MZ[5] - P[7][16]*SK_MZ[4]); + Kfusion[8] = SK_MZ[0]*(P[8][20] + 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][17]*SK_MZ[1] + P[8][15]*SK_MZ[5] - P[8][16]*SK_MZ[4]); + Kfusion[9] = SK_MZ[0]*(P[9][20] + 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][17]*SK_MZ[1] + P[9][15]*SK_MZ[5] - P[9][16]*SK_MZ[4]); + Kfusion[10] = SK_MZ[0]*(P[10][20] + 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][17]*SK_MZ[1] + P[10][15]*SK_MZ[5] - P[10][16]*SK_MZ[4]); + Kfusion[11] = SK_MZ[0]*(P[11][20] + 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][17]*SK_MZ[1] + P[11][15]*SK_MZ[5] - P[11][16]*SK_MZ[4]); + Kfusion[12] = SK_MZ[0]*(P[12][20] + 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][17]*SK_MZ[1] + P[12][15]*SK_MZ[5] - P[12][16]*SK_MZ[4]); + Kfusion[13] = SK_MZ[0]*(P[13][20] + 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][17]*SK_MZ[1] + P[13][15]*SK_MZ[5] - P[13][16]*SK_MZ[4]); + Kfusion[14] = SK_MZ[0]*(P[14][20] + 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][17]*SK_MZ[1] + P[14][15]*SK_MZ[5] - P[14][16]*SK_MZ[4]); + Kfusion[15] = SK_MZ[0]*(P[15][20] + 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][17]*SK_MZ[1] + P[15][15]*SK_MZ[5] - P[15][16]*SK_MZ[4]); + Kfusion[16] = SK_MZ[0]*(P[16][20] + 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][17]*SK_MZ[1] + P[16][15]*SK_MZ[5] - P[16][16]*SK_MZ[4]); + Kfusion[17] = SK_MZ[0]*(P[17][20] + 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][17]*SK_MZ[1] + P[17][15]*SK_MZ[5] - P[17][16]*SK_MZ[4]); + Kfusion[18] = SK_MZ[0]*(P[18][20] + 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][17]*SK_MZ[1] + P[18][15]*SK_MZ[5] - P[18][16]*SK_MZ[4]); + Kfusion[19] = SK_MZ[0]*(P[19][20] + 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][17]*SK_MZ[1] + P[19][15]*SK_MZ[5] - P[19][16]*SK_MZ[4]); + Kfusion[20] = SK_MZ[0]*(P[20][20] + 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][17]*SK_MZ[1] + P[20][15]*SK_MZ[5] - P[20][16]*SK_MZ[4]); + varInnovMag[2] = 1.0f/SK_MZ[0]; + innovMag[2] = MagPred[2] - magData.z; + + } + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) + { + // correct the state vector + for (uint8_t j= 0; j<=indexLimit; j++) + { + states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=3; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + for (uint8_t j = 4; j<=17; j++) KH[i][j] = 0.0f; + if (!onGround) + { + for (uint8_t j = 15; j<=20; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + } + else + { + for (uint8_t j = 15; j<=20; j++) + { + KH[i][j] = 0.0f; + } + } + } + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=indexLimit; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k<=3; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + if (!onGround) + { + for (uint8_t k = 15; k<=20; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + } + } + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=indexLimit; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + obsIndex = obsIndex + 1; + + ForceSymmetry(); + ConstrainVariances(); +} + +void AttPosEKF::FuseAirspeed() +{ + float vn; + float ve; + float vd; + float vwn; + float vwe; + const float R_TAS = 2.0f; + float SH_TAS[3]; + float Kfusion[21]; + float VtasPred; + + // Copy required states to local variable names + vn = statesAtVtasMeasTime[4]; + ve = statesAtVtasMeasTime[5]; + vd = statesAtVtasMeasTime[6]; + vwn = statesAtVtasMeasTime[13]; + vwe = statesAtVtasMeasTime[14]; + + // Need to check that it is flying before fusing airspeed data + // Calculate the predicted airspeed + VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); + // Perform fusion of True Airspeed measurement + if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) + { + // Calculate observation jacobians + SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); + SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; + SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; + + float H_TAS[21]; + for (uint8_t i=0; i<=20; 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[13] = -SH_TAS[2]; + H_TAS[14] = -SH_TAS[1]; + + // Calculate Kalman gains + float SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][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[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); + Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][13]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][14]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); + Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][13]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][14]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]); + Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][13]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][14]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]); + Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][13]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][14]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]); + Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][13]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][14]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]); + Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][13]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][14]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]); + Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][13]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][14]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]); + Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][13]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][14]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]); + Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][13]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][14]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]); + Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][13]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][14]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]); + Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][13]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][14]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]); + Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][13]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][14]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); + Kfusion[13] = SK_TAS*(P[13][4]*SH_TAS[2] - P[13][13]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][14]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); + Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][13]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][14]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); + Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][13]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][14]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); + Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][13]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][14]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); + Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][13]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][14]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); + Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][13]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][14]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); + Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); + Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); + varInnovVtas = 1.0f/SK_TAS; + + // Calculate the measurement innovation + innovVtas = VtasPred - VtasMeas; + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovVtas*innovVtas*SK_TAS) < 25.0) + { + // correct the state vector + for (uint8_t j=0; j<=20; j++) + { + states[j] = states[j] - Kfusion[j] * innovVtas; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // 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<=20; i++) + { + 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]; + } + for (uint8_t j = 7; j<=12; j++) KH[i][j] = 0.0; + for (uint8_t j = 13; j<=14; j++) + { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0; + } + for (uint8_t i = 0; i<=20; i++) + { + for (uint8_t j = 0; j<=20; j++) + { + KHP[i][j] = 0.0; + for (uint8_t k = 4; k<=6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + for (uint8_t k = 13; k<=14; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + for (uint8_t i = 0; i<=20; i++) + { + for (uint8_t j = 0; j<=20; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + + ForceSymmetry(); + ConstrainVariances(); +} + +void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +{ + uint8_t row; + uint8_t col; + for (row=first; row<=last; row++) + { + for (col=0; col<n_states; col++) + { + covMat[row][col] = 0.0; + } + } +} + +void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +{ + uint8_t row; + uint8_t col; + for (col=first; col<=last; col++) + { + for (row=0; row < n_states; row++) + { + covMat[row][col] = 0.0; + } + } +} + +float AttPosEKF::sq(float valIn) +{ + return valIn*valIn; +} + +// Store states in a history array along with time stamp +void AttPosEKF::StoreStates(uint64_t timestamp_ms) +{ + for (unsigned i=0; i<n_states; i++) + storedStates[i][storeIndex] = states[i]; + statetimeStamp[storeIndex] = timestamp_ms; + storeIndex++; + if (storeIndex == data_buffer_size) + storeIndex = 0; +} + +void AttPosEKF::ResetStoredStates() +{ + // reset all stored states + memset(&storedStates[0][0], 0, sizeof(storedStates)); + memset(&statetimeStamp[0], 0, sizeof(statetimeStamp)); + + // reset store index to first + storeIndex = 0; + + // overwrite all existing states + for (unsigned i = 0; i < n_states; i++) { + storedStates[i][storeIndex] = states[i]; + } + + statetimeStamp[storeIndex] = millis(); + + // increment to next storage index + storeIndex++; +} + +// Output the state vector stored at the time that best matches that specified by msec +int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec) +{ + int ret = 0; + + // int64_t bestTimeDelta = 200; + // unsigned bestStoreIndex = 0; + // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + // { + // // The time delta can also end up as negative number, + // // since we might compare future to past or past to future + // // therefore cast to int64. + // int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; + // if (timeDelta < 0) { + // timeDelta = -timeDelta; + // } + + // if (timeDelta < bestTimeDelta) + // { + // bestStoreIndex = storeIndex; + // bestTimeDelta = timeDelta; + // } + // } + // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + // { + // for (uint8_t i=0; i < n_states; i++) { + // if (isfinite(storedStates[i][bestStoreIndex])) { + // statesForFusion[i] = storedStates[i][bestStoreIndex]; + // } else if (isfinite(states[i])) { + // statesForFusion[i] = states[i]; + // } else { + // // There is not much we can do here, except reporting the error we just + // // found. + // ret++; + // } + // } + // else // otherwise output current state + // { + for (uint8_t i=0; i < n_states; i++) { + if (isfinite(states[i])) { + statesForFusion[i] = states[i]; + } else { + ret++; + } + } + // } + + return ret; +} + +void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) +{ + // Calculate the nav to body cosine matrix + float q00 = sq(quat[0]); + float q11 = sq(quat[1]); + float q22 = sq(quat[2]); + float q33 = sq(quat[3]); + float q01 = quat[0]*quat[1]; + float q02 = quat[0]*quat[2]; + float q03 = quat[0]*quat[3]; + float q12 = quat[1]*quat[2]; + float q13 = quat[1]*quat[3]; + float q23 = quat[2]*quat[3]; + + Tnb.x.x = q00 + q11 - q22 - q33; + Tnb.y.y = q00 - q11 + q22 - q33; + Tnb.z.z = q00 - q11 - q22 + q33; + Tnb.y.x = 2*(q12 - q03); + Tnb.z.x = 2*(q13 + q02); + Tnb.x.y = 2*(q12 + q03); + Tnb.z.y = 2*(q23 - q01); + Tnb.x.z = 2*(q13 - q02); + Tnb.y.z = 2*(q23 + q01); +} + +void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) +{ + // Calculate the body to nav cosine matrix + float q00 = sq(quat[0]); + float q11 = sq(quat[1]); + float q22 = sq(quat[2]); + float q33 = sq(quat[3]); + float q01 = quat[0]*quat[1]; + float q02 = quat[0]*quat[2]; + float q03 = quat[0]*quat[3]; + float q12 = quat[1]*quat[2]; + float q13 = quat[1]*quat[3]; + float q23 = quat[2]*quat[3]; + + Tbn.x.x = q00 + q11 - q22 - q33; + Tbn.y.y = q00 - q11 + q22 - q33; + Tbn.z.z = q00 - q11 - q22 + q33; + Tbn.x.y = 2*(q12 - q03); + Tbn.x.z = 2*(q13 + q02); + Tbn.y.x = 2*(q12 + q03); + Tbn.y.z = 2*(q23 - q01); + Tbn.z.x = 2*(q13 - q02); + Tbn.z.y = 2*(q23 + q01); +} + +void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3]) +{ + float u1 = cos(0.5f*eul[0]); + float u2 = cos(0.5f*eul[1]); + float u3 = cos(0.5f*eul[2]); + float u4 = sin(0.5f*eul[0]); + float u5 = sin(0.5f*eul[1]); + float u6 = sin(0.5f*eul[2]); + quat[0] = u1*u2*u3+u4*u5*u6; + quat[1] = u4*u2*u3-u1*u5*u6; + quat[2] = u1*u5*u3+u4*u2*u6; + quat[3] = u1*u2*u6-u4*u5*u3; +} + +void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4]) +{ + y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3])); + y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2])); + y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); +} + +void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) +{ + velNED[0] = gpsGndSpd*cosf(gpsCourse); + velNED[1] = gpsGndSpd*sinf(gpsCourse); + velNED[2] = gpsVelD; +} + +void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +{ + posNED[0] = earthRadius * (lat - latRef); + posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); + posNED[2] = -(hgt - hgtRef); +} + +void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +{ + lat = latRef + posNED[0] * earthRadiusInv; + lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); + hgt = hgtRef - posNED[2]; +} + +void AttPosEKF::OnGroundCheck() +{ + onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); + if (staticMode) { + staticMode = !(GPSstatus > GPS_FIX_2D); + } +} + +void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) +{ + //Define Earth rotation vector in the NED navigation frame + omega.x = earthRate*cosf(latitude); + omega.y = 0.0f; + omega.z = -earthRate*sinf(latitude); +} + +void AttPosEKF::CovarianceInit() +{ + // Calculate the initial covariance matrix P + P[0][0] = 0.25f * sq(1.0f*deg2rad); + P[1][1] = 0.25f * sq(1.0f*deg2rad); + P[2][2] = 0.25f * sq(1.0f*deg2rad); + P[3][3] = 0.25f * sq(10.0f*deg2rad); + P[4][4] = sq(0.7); + P[5][5] = P[4][4]; + P[6][6] = sq(0.7); + P[7][7] = sq(15.0); + P[8][8] = P[7][7]; + P[9][9] = sq(5.0); + P[10][10] = sq(0.1*deg2rad*dtIMU); + P[11][11] = P[10][10]; + P[12][12] = P[10][10]; + P[13][13] = sq(8.0f); + P[14][4] = P[13][13]; + P[15][15] = sq(0.02f); + P[16][16] = P[15][15]; + P[17][17] = P[15][15]; + P[18][18] = sq(0.02f); + P[19][19] = P[18][18]; + P[20][20] = P[18][18]; +} + +float AttPosEKF::ConstrainFloat(float val, float min, float max) +{ + return (val > max) ? max : ((val < min) ? min : val); +} + +void AttPosEKF::ConstrainVariances() +{ + if (!numericalProtection) { + return; + } + + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) + + // Constrain quaternion variances + for (unsigned i = 0; i < 4; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Constrain velocitie variances + for (unsigned i = 4; i < 7; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Constrain position variances + for (unsigned i = 7; i < 10; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); + } + + // Angle bias variances + for (unsigned i = 10; i < 13; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.175f * dtIMU)); + } + + // Wind velocity variances + for (unsigned i = 13; i < 15; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Earth magnetic field variances + for (unsigned i = 15; i < 18; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Body magnetic field variances + for (unsigned i = 18; i < 21; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + +} + +void AttPosEKF::ConstrainStates() +{ + if (!numericalProtection) { + return; + } + + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) + + + // Constrain quaternion + for (unsigned i = 0; i < 4; i++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Constrain velocities to what GPS can do for us + for (unsigned i = 4; i < 7; i++) { + states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); + } + + // Constrain position to a reasonable vehicle range (in meters) + for (unsigned i = 7; i < 9; i++) { + states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); + } + + // Constrain altitude + states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); + + // Angle bias limit - set to 8 degrees / sec + for (unsigned i = 10; i < 13; i++) { + states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); + } + + // Wind velocity limits - assume 120 m/s max velocity + for (unsigned i = 13; i < 15; i++) { + states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); + } + + // Earth magnetic field limits (in Gauss) + for (unsigned i = 15; i < 18; i++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Body magnetic field variances (in Gauss). + // the max offset should be in this range. + for (unsigned i = 18; i < 21; i++) { + states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); + } + +} + +void AttPosEKF::ForceSymmetry() +{ + if (!numericalProtection) { + return; + } + + // Force symmetry on the covariance matrix to prevent ill-conditioning + // of the matrix which would cause the filter to blow-up + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (P[i][j] + P[j][i]); + P[j][i] = P[i][j]; + } + } +} + +bool AttPosEKF::FilterHealthy() +{ + if (!statesInitialised) { + return false; + } + + // XXX Check state vector for NaNs and ill-conditioning + + // Check if any of the major inputs timed out + if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) { + return false; + } + + // Nothing fired, return ok. + return true; +} + +/** + * Reset the filter position states + * + * This resets the position to the last GPS measurement + * or to zero in case of static position. + */ +void AttPosEKF::ResetPosition(void) +{ + if (staticMode) { + states[7] = 0; + states[8] = 0; + } else if (GPSstatus >= GPS_FIX_3D) { + + // reset the states from the GPS measurements + states[7] = posNE[0]; + states[8] = posNE[1]; + } +} + +/** + * Reset the height state. + * + * This resets the height state with the last altitude measurements + */ +void AttPosEKF::ResetHeight(void) +{ + // write to the state vector + states[9] = -hgtMea; +} + +/** + * Reset the velocity state. + */ +void AttPosEKF::ResetVelocity(void) +{ + if (staticMode) { + states[4] = 0.0f; + states[5] = 0.0f; + states[6] = 0.0f; + } else if (GPSstatus >= GPS_FIX_3D) { + + states[4] = velNED[0]; // north velocity from last reading + states[5] = velNED[1]; // east velocity from last reading + states[6] = velNED[2]; // down velocity from last reading + } +} + + +void AttPosEKF::FillErrorReport(struct ekf_status_report *err) +{ + for (int i = 0; i < n_states; i++) + { + err->states[i] = states[i]; + } + + err->velHealth = current_ekf_state.velHealth; + err->posHealth = current_ekf_state.posHealth; + err->hgtHealth = current_ekf_state.hgtHealth; + err->velTimeout = current_ekf_state.velTimeout; + err->posTimeout = current_ekf_state.posTimeout; + err->hgtTimeout = current_ekf_state.hgtTimeout; +} + +bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { + bool err = false; + + // check all states and covariance matrices + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + if (!isfinite(KH[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // intermediate result used for covariance updates + if (!isfinite(KHP[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // intermediate result used for covariance updates + if (!isfinite(P[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // covariance matrix + } + + if (!isfinite(Kfusion[i])) { + + err_report->kalmanGainsNaN = true; + err = true; + } // Kalman gains + + if (!isfinite(states[i])) { + + err_report->statesNaN = true; + err = true; + } // state matrix + } + + if (err) { + FillErrorReport(err_report); + } + + return err; + +} + +/** + * Check the filter inputs and bound its operational state + * + * This check will reset the filter states if required + * due to a failure of consistency or timeout checks. + * it should be run after the measurement data has been + * updated, but before any of the fusion steps are + * executed. + */ +int AttPosEKF::CheckAndBound() +{ + + // Store the old filter state + bool currStaticMode = staticMode; + + // Reset the filter if the states went NaN + if (StatesNaN(&last_ekf_error)) { + + InitializeDynamic(velNED, magDeclination); + + return 1; + } + + // Reset the filter if the IMU data is too old + if (dtIMU > 0.2f) { + + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + + // that's all we can do here, return + return 2; + } + + // Check if we're on ground - this also sets static mode. + OnGroundCheck(); + + // Check if we switched between states + if (currStaticMode != staticMode) { + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + + return 3; + } + + return 0; +} + +void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) +{ + float initialRoll, initialPitch; + float cosRoll, sinRoll, cosPitch, sinPitch; + float magX, magY; + float initialHdg, cosHeading, sinHeading; + + initialRoll = atan2(-ay, -az); + initialPitch = atan2(ax, -az); + + cosRoll = cosf(initialRoll); + sinRoll = sinf(initialRoll); + cosPitch = cosf(initialPitch); + sinPitch = sinf(initialPitch); + + magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; + + magY = my * cosRoll - mz * sinRoll; + + initialHdg = atan2f(-magY, magX); + /* true heading is the mag heading minus declination */ + initialHdg += declination; + + cosRoll = cosf(initialRoll * 0.5f); + sinRoll = sinf(initialRoll * 0.5f); + + cosPitch = cosf(initialPitch * 0.5f); + sinPitch = sinf(initialPitch * 0.5f); + + cosHeading = cosf(initialHdg * 0.5f); + sinHeading = sinf(initialHdg * 0.5f); + + initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; + initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; + initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; + initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; +} + +void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) +{ + + // Clear the init flag + statesInitialised = false; + + magDeclination = declination; + + ZeroVariables(); + + // Calculate initial filter quaternion states from raw measurements + float initQuat[4]; + Vector3f initMagXYZ; + initMagXYZ = magData - magBias; + AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, magDeclination, initQuat); + + // Calculate initial Tbn matrix and rotate Mag measurements into NED + // to set initial NED magnetic field states + Mat3f DCM; + quat2Tbn(DCM, initQuat); + Vector3f initMagNED; + initMagXYZ = magData - magBias; + initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; + initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; + initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; + + + + // write to state vector + for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions + for (uint8_t j=0; j<=2; j++) states[j+4] = initvelNED[j]; // velocities + for (uint8_t j=0; j<=7; j++) states[j+7] = 0.0f; // positiions, dAngBias, windVel + states[15] = initMagNED.x; // Magnetic Field North + states[16] = initMagNED.y; // Magnetic Field East + states[17] = initMagNED.z; // Magnetic Field Down + states[18] = magBias.x; // Magnetic Field Bias X + states[19] = magBias.y; // Magnetic Field Bias Y + states[20] = magBias.z; // Magnetic Field Bias Z + + statesInitialised = true; + + // initialise the covariance matrix + CovarianceInit(); + + //Define Earth rotation vector in the NED navigation frame + calcEarthRateNED(earthRateNED, latRef); + + //Initialise summed variables used by covariance prediction + summedDelAng.x = 0.0f; + summedDelAng.y = 0.0f; + summedDelAng.z = 0.0f; + summedDelVel.x = 0.0f; + summedDelVel.y = 0.0f; + summedDelVel.z = 0.0f; +} + +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) +{ + //store initial lat,long and height + latRef = referenceLat; + lonRef = referenceLon; + hgtRef = referenceHgt; + + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); + + InitializeDynamic(initvelNED, declination); +} + +void AttPosEKF::ZeroVariables() +{ + // Do the data structure init + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + KH[i][j] = 0.0f; // intermediate result used for covariance updates + KHP[i][j] = 0.0f; // intermediate result used for covariance updates + P[i][j] = 0.0f; // covariance matrix + } + + Kfusion[i] = 0.0f; // Kalman gains + states[i] = 0.0f; // state matrix + } + + for (unsigned i = 0; i < data_buffer_size; i++) { + + for (unsigned j = 0; j < n_states; j++) { + storedStates[j][i] = 0.0f; + } + + statetimeStamp[i] = 0; + } + + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); +} + +void AttPosEKF::GetFilterState(struct ekf_status_report *state) +{ + memcpy(state, ¤t_ekf_state, sizeof(state)); +} + +void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) +{ + memcpy(last_error, &last_ekf_error, sizeof(last_error)); +} diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.h b/src/modules/ekf_att_pos_estimator/estimator_21states.h new file mode 100644 index 000000000..a19ff1995 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_21states.h @@ -0,0 +1,247 @@ +#pragma once + +#include "estimator_utilities.h" + +class AttPosEKF { + +public: + + AttPosEKF(); + ~AttPosEKF(); + + /* ############################################## + * + * M A I N F I L T E R P A R A M E T E R S + * + * ########################################### */ + + /* + * parameters are defined here and initialised in + * the InitialiseParameters() (which is just 20 lines down) + */ + + float covTimeStepMax; // maximum time allowed between covariance predictions + float covDelAngMax; // maximum delta angle between covariance predictions + float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + + float yawVarScale; + float windVelSigma; + float dAngBiasSigma; + float dVelBiasSigma; + float magEarthSigma; + float magBodySigma; + float gndHgtSigma; + + float vneSigma; + float vdSigma; + float posNeSigma; + float posDSigma; + float magMeasurementSigma; + float airspeedMeasurementSigma; + + float gyroProcessNoise; + float accelProcessNoise; + + float EAS2TAS; // ratio f true to equivalent airspeed + + void InitialiseParameters() + { + covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions + covDelAngMax = 0.02f; // maximum delta angle between covariance predictions + rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + EAS2TAS = 1.0f; + + yawVarScale = 1.0f; + windVelSigma = 0.1f; + dAngBiasSigma = 5.0e-7f; + dVelBiasSigma = 1e-4f; + magEarthSigma = 3.0e-4f; + magBodySigma = 3.0e-4f; + gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma + + vneSigma = 0.2f; + vdSigma = 0.3f; + posNeSigma = 2.0f; + posDSigma = 2.0f; + + magMeasurementSigma = 0.05; + airspeedMeasurementSigma = 1.4f; + gyroProcessNoise = 1.4544411e-2f; + accelProcessNoise = 0.5f; + } + + // Global variables + float KH[n_states][n_states]; // intermediate result used for covariance updates + float KHP[n_states][n_states]; // intermediate result used for covariance updates + float P[n_states][n_states]; // covariance matrix + float Kfusion[n_states]; // Kalman gains + float states[n_states]; // state matrix + float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps + uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored + + float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements + float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements + float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement + float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time + float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time + + Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) + Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) + Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) + Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) + float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) + Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) + Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) + Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) + Vector3f dVelIMU; + Vector3f dAngIMU; + float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) + uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity + float innovVelPos[6]; // innovation output + float varInnovVelPos[6]; // innovation variance output + + float velNED[3]; // North, East, Down velocity obs (m/s) + float posNE[2]; // North, East position obs (m) + float hgtMea; // measured height (m) + float posNED[3]; // North, East Down position (m) + + float innovMag[3]; // innovation output + float varInnovMag[3]; // innovation variance output + Vector3f magData; // magnetometer flux radings in X,Y,Z body axes + float innovVtas; // innovation output + float varInnovVtas; // innovation variance output + float VtasMeas; // true airspeed measurement (m/s) + float magDeclination; + float latRef; // WGS-84 latitude of reference point (rad) + float lonRef; // WGS-84 longitude of reference point (rad) + float hgtRef; // WGS-84 height of reference point (m) + Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes + uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction + + // GPS input data variables + float gpsCourse; + float gpsVelD; + float gpsLat; + float gpsLon; + float gpsHgt; + uint8_t GPSstatus; + + // Baro input + float baroHgt; + + bool statesInitialised; + + bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused + bool fusePosData; // this boolean causes the posNE and velNED obs to be fused + bool fuseHgtData; // this boolean causes the hgtMea obs to be fused + bool fuseMagData; // boolean true when magnetometer data is to be fused + bool fuseVtasData; // boolean true when airspeed data is to be fused + + bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) + bool staticMode; ///< boolean true if no position feedback is fused + bool useAirspeed; ///< boolean true if airspeed data is being used + bool useCompass; ///< boolean true if magnetometer data is being used + + struct ekf_status_report current_ekf_state; + struct ekf_status_report last_ekf_error; + + bool numericalProtection; + + unsigned storeIndex; + + +void UpdateStrapdownEquationsNED(); + +void CovariancePrediction(float dt); + +void FuseVelposNED(); + +void FuseMagnetometer(); + +void FuseAirspeed(); + +void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); + +void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); + +void quatNorm(float (&quatOut)[4], const float quatIn[4]); + +// store staes along with system time stamp in msces +void StoreStates(uint64_t timestamp_ms); + +/** + * Recall the state vector. + * + * Recalls the vector stored at closest time to the one specified by msec + * + * @return zero on success, integer indicating the number of invalid states on failure. + * Does only copy valid states, if the statesForFusion vector was initialized + * correctly by the caller, the result can be safely used, but is a mixture + * time-wise where valid states were updated and invalid remained at the old + * value. + */ +int RecallStates(float statesForFusion[n_states], uint64_t msec); + +void ResetStoredStates(); + +void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); + +void calcEarthRateNED(Vector3f &omega, float latitude); + +static void eul2quat(float (&quat)[4], const float (&eul)[3]); + +static void quat2eul(float (&eul)[3], const float (&quat)[4]); + +static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); + +static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); + +static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); + +static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); + +static float sq(float valIn); + +void OnGroundCheck(); + +void CovarianceInit(); + +void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); + +float ConstrainFloat(float val, float min, float max); + +void ConstrainVariances(); + +void ConstrainStates(); + +void ForceSymmetry(); + +int CheckAndBound(); + +void ResetPosition(); + +void ResetVelocity(); + +void ZeroVariables(); + +void GetFilterState(struct ekf_status_report *state); + +void GetLastErrorState(struct ekf_status_report *last_error); + +bool StatesNaN(struct ekf_status_report *err_report); +void FillErrorReport(struct ekf_status_report *err); + +void InitializeDynamic(float (&initvelNED)[3], float declination); + +protected: + +bool FilterHealthy(); + +void ResetHeight(void); + +void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); + +}; + +uint32_t millis(); + diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 1320b4668..9622f7e40 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1,143 +1,9 @@ -#include "estimator.h" +#include "estimator_23states.h" #include <string.h> -#include <stdarg.h> - -// Define EKF_DEBUG here to enable the debug print calls -// if the macro is not set, these will be completely -// optimized out by the compiler. -//#define EKF_DEBUG - -#ifdef EKF_DEBUG #include <stdio.h> +#include <stdarg.h> -static void -ekf_debug_print(const char *fmt, va_list args) -{ - fprintf(stderr, "%s: ", "[ekf]"); - vfprintf(stderr, fmt, args); - - fprintf(stderr, "\n"); -} - -static void -ekf_debug(const char *fmt, ...) -{ - va_list args; - - va_start(args, fmt); - ekf_debug_print(fmt, args); -} - -#else - -static void ekf_debug(const char *fmt, ...) { while(0){} } -#endif - -float Vector3f::length(void) const -{ - return sqrt(x*x + y*y + z*z); -} - -void Vector3f::zero(void) -{ - x = 0.0f; - y = 0.0f; - z = 0.0f; -} - -Mat3f::Mat3f() { - identity(); -} - -void Mat3f::identity() { - x.x = 1.0f; - x.y = 0.0f; - x.z = 0.0f; - - y.x = 0.0f; - y.y = 1.0f; - y.z = 0.0f; - - z.x = 0.0f; - z.y = 0.0f; - z.z = 1.0f; -} - -Mat3f Mat3f::transpose(void) const -{ - Mat3f ret = *this; - swap_var(ret.x.y, ret.y.x); - swap_var(ret.x.z, ret.z.x); - swap_var(ret.y.z, ret.z.y); - return ret; -} - -// overload + operator to provide a vector addition -Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x + vecIn2.x; - vecOut.y = vecIn1.y + vecIn2.y; - vecOut.z = vecIn1.z + vecIn2.z; - return vecOut; -} - -// overload - operator to provide a vector subtraction -Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x - vecIn2.x; - vecOut.y = vecIn1.y - vecIn2.y; - vecOut.z = vecIn1.z - vecIn2.z; - return vecOut; -} - -// overload * operator to provide a matrix vector product -Vector3f operator*( Mat3f matIn, Vector3f vecIn) -{ - Vector3f vecOut; - vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z; - vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z; - vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; - return vecOut; -} - -// overload % operator to provide a vector cross product -Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) -{ - Vector3f vecOut; - vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y; - vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z; - vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x; - return vecOut; -} - -// overload * operator to provide a vector scaler product -Vector3f operator*(Vector3f vecIn1, float sclIn1) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x * sclIn1; - vecOut.y = vecIn1.y * sclIn1; - vecOut.z = vecIn1.z * sclIn1; - return vecOut; -} - -// overload * operator to provide a vector scaler product -Vector3f operator*(float sclIn1, Vector3f vecIn1) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x * sclIn1; - vecOut.y = vecIn1.y * sclIn1; - vecOut.z = vecIn1.z * sclIn1; - return vecOut; -} - -void swap_var(float &d1, float &d2) -{ - float tmp = d1; - d1 = d2; - d2 = tmp; -} +#define EKF_COVARIANCE_DIVERGED 1.0e8f AttPosEKF::AttPosEKF() @@ -145,7 +11,42 @@ AttPosEKF::AttPosEKF() * instead to allow clean in-air re-initialization. */ { + summedDelAng.zero(); + summedDelVel.zero(); + + fusionModeGPS = 0; + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; + onGround = true; + staticMode = true; + useAirspeed = true; + useCompass = true; + useRangeFinder = true; + numericalProtection = true; + refSet = false; + storeIndex = 0; + gpsHgt = 0.0f; + baroHgt = 0.0f; + GPSstatus = 0; + VtasMeas = 0.0f; + magDeclination = 0.0f; + dAngIMU.zero(); + dVelIMU.zero(); + velNED[0] = 0.0f; + velNED[1] = 0.0f; + velNED[2] = 0.0f; + accelGPSNED[0] = 0.0f; + accelGPSNED[1] = 0.0f; + accelGPSNED[2] = 0.0f; + delAngTotal.zero(); + ekfDiverged = false; + lastReset = 0; + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); ZeroVariables(); InitialiseParameters(); } @@ -181,6 +82,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED() dVelIMU.y = dVelIMU.y; dVelIMU.z = dVelIMU.z - states[13]; + delAngTotal.x += correctedDelAng.x; + delAngTotal.y += correctedDelAng.y; + delAngTotal.z += correctedDelAng.z; + // Save current measurements Vector3f prevDelAng = correctedDelAng; @@ -199,8 +104,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED() } else { - deltaQuat[0] = cosf(0.5f*rotationMag); - float rotScaler = (sinf(0.5f*rotationMag))/rotationMag; + // We are using double here as we are unsure how small + // the angle differences are and if we get into numeric + // issues with float. The runtime impact is not measurable + // for these quantities. + deltaQuat[0] = cos(0.5*(double)rotationMag); + float rotScaler = (sin(0.5*(double)rotationMag))/(double)rotationMag; deltaQuat[1] = correctedDelAng.x*rotScaler; deltaQuat[2] = correctedDelAng.y*rotScaler; deltaQuat[3] = correctedDelAng.z*rotScaler; @@ -312,7 +221,8 @@ void AttPosEKF::CovariancePrediction(float dt) float nextP[n_states][n_states]; // calculate covariance prediction process noise - for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i= 4; i<10; i++) processNoise[i] = 1.0e-9f; for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; // scale gyro bias noise when on ground to allow for faster bias estimation for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; @@ -977,20 +887,20 @@ void AttPosEKF::CovariancePrediction(float dt) // propagate for (unsigned i = 0; i <= 13; i++) { P[i][i] = nextP[i][i]; + } - // force symmetry for observable states - // force zero for non-observable states - for (unsigned i = 1; i < n_states; i++) + // force symmetry for observable states + // force zero for non-observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) { - for (uint8_t j = 0; j < i; j++) - { - if ((i > 13) || (j > 13)) { - P[i][j] = 0.0f; - } else { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - } - P[j][i] = P[i][j]; + if ((i > 13) || (j > 13)) { + P[i][j] = 0.0f; + } else { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); } + P[j][i] = P[i][j]; } } @@ -1020,9 +930,9 @@ void AttPosEKF::FuseVelposNED() { // declare variables used by fault isolation logic - uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure - uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available - uint32_t hgtRetryTime = 5000; // height measurement retry time + uint32_t gpsRetryTime = 3000; // time in msec before GPS fusion will be retried following innovation consistency failure + uint32_t gpsRetryTimeNoTAS = 500; // retry time if no TAS measurement available + uint32_t hgtRetryTime = 500; // height measurement retry time uint32_t horizRetryTime; // declare variables used to check measurement errors @@ -1178,7 +1088,7 @@ void AttPosEKF::FuseVelposNED() stateIndex = 4 + obsIndex; // Calculate the measurement innovation, using states from a // different time coordinate if fusing height data - if (obsIndex >= 0 && obsIndex <= 2) + if (obsIndex <= 2) { innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; } @@ -1193,7 +1103,7 @@ void AttPosEKF::FuseVelposNED() // Calculate the Kalman Gain // Calculate innovation variances - also used for data logging varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; - SK = 1.0/varInnovVelPos[obsIndex]; + SK = 1.0/(double)varInnovVelPos[obsIndex]; for (uint8_t i= 0; i<=indexLimit; i++) { Kfusion[i] = P[i][stateIndex]*SK; @@ -1277,7 +1187,7 @@ void AttPosEKF::FuseMagnetometer() // data fit is the only assumption we can make // so we might as well take advantage of the computational efficiencies // associated with sequential fusion - if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) + if (useCompass && fuseMagData && (obsIndex < 3)) { // Limit range of states modified when on ground if(!onGround) @@ -1293,7 +1203,7 @@ void AttPosEKF::FuseMagnetometer() // three prediction time steps. // Calculate observation jacobians and Kalman gains - if (fuseMagData) + if (obsIndex == 0) { // Copy required states to local variable names q0 = statesAtMagMeasTime[0]; @@ -1388,11 +1298,6 @@ void AttPosEKF::FuseMagnetometer() Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); varInnovMag[0] = 1.0f/SK_MX[0]; innovMag[0] = MagPred[0] - magData.x; - - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; - fuseMagData = false; } else if (obsIndex == 1) // we are now fusing the Y measurement { @@ -1508,7 +1413,7 @@ void AttPosEKF::FuseMagnetometer() } // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) + if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f) { // correct the state vector for (uint8_t j= 0; j < indexLimit; j++) @@ -1517,7 +1422,7 @@ void AttPosEKF::FuseMagnetometer() } // normalise the quaternion states float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12) + if (quatMag > 1e-12f) { for (uint8_t j= 0; j<=3; j++) { @@ -1612,7 +1517,7 @@ void AttPosEKF::FuseAirspeed() SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; - + float H_TAS[n_states]; for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f; H_TAS[4] = SH_TAS[2]; @@ -1661,7 +1566,7 @@ void AttPosEKF::FuseAirspeed() // Calculate the measurement innovation innovVtas = VtasPred - VtasMeas; // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovVtas*innovVtas*SK_TAS) < 25.0) + if ((innovVtas*innovVtas*SK_TAS) < 25.0f) { // correct the state vector for (uint8_t j=0; j <= 22; j++) @@ -1758,7 +1663,7 @@ void AttPosEKF::FuseRangeFinder() // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data SH_RNG[4] = sin(rngFinderPitch); - cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cos(rngFinderPitch); + cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch); if (useRangeFinder && cosRngTilt > 0.87f) { // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset @@ -1855,21 +1760,21 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) int64_t bestTimeDelta = 200; unsigned bestStoreIndex = 0; - for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + for (unsigned storeIndexLocal = 0; storeIndexLocal < data_buffer_size; storeIndexLocal++) { // Work around a GCC compiler bug - we know 64bit support on ARM is // sketchy in GCC. uint64_t timeDelta; - if (msec > statetimeStamp[storeIndex]) { - timeDelta = msec - statetimeStamp[storeIndex]; + if (msec > statetimeStamp[storeIndexLocal]) { + timeDelta = msec - statetimeStamp[storeIndexLocal]; } else { - timeDelta = statetimeStamp[storeIndex] - msec; + timeDelta = statetimeStamp[storeIndexLocal] - msec; } - if (timeDelta < bestTimeDelta) + if (timeDelta < (uint64_t)bestTimeDelta) { - bestStoreIndex = storeIndex; + bestStoreIndex = storeIndexLocal; bestTimeDelta = timeDelta; } } @@ -1926,7 +1831,7 @@ void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) Tnb.y.z = 2*(q23 + q01); } -void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) +void AttPosEKF::quat2Tbn(Mat3f &Tbn_ret, const float (&quat)[4]) { // Calculate the body to nav cosine matrix float q00 = sq(quat[0]); @@ -1940,15 +1845,15 @@ void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) float q13 = quat[1]*quat[3]; float q23 = quat[2]*quat[3]; - Tbn.x.x = q00 + q11 - q22 - q33; - Tbn.y.y = q00 - q11 + q22 - q33; - Tbn.z.z = q00 - q11 - q22 + q33; - Tbn.x.y = 2*(q12 - q03); - Tbn.x.z = 2*(q13 + q02); - Tbn.y.x = 2*(q12 + q03); - Tbn.y.z = 2*(q23 - q01); - Tbn.z.x = 2*(q13 - q02); - Tbn.z.y = 2*(q23 + q01); + Tbn_ret.x.x = q00 + q11 - q22 - q33; + Tbn_ret.y.y = q00 - q11 + q22 - q33; + Tbn_ret.z.z = q00 - q11 - q22 + q33; + Tbn_ret.x.y = 2*(q12 - q03); + Tbn_ret.x.z = 2*(q13 + q02); + Tbn_ret.y.x = 2*(q12 + q03); + Tbn_ret.y.z = 2*(q23 - q01); + Tbn_ret.z.x = 2*(q13 - q02); + Tbn_ret.z.y = 2*(q23 + q01); } void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3]) @@ -1979,17 +1884,17 @@ void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, velNED[2] = gpsVelD; } -void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef) +void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference) { - posNED[0] = earthRadius * (lat - latRef); - posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); - posNED[2] = -(hgt - hgtRef); + posNED[0] = earthRadius * (lat - latReference); + posNED[1] = earthRadius * cos(latReference) * (lon - lonReference); + posNED[2] = -(hgt - hgtReference); } -void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef) { - lat = latRef + posNED[0] * earthRadiusInv; - lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); + lat = latRef + (double)posNED[0] * earthRadiusInv; + lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef); hgt = hgtRef - posNED[2]; } @@ -2194,10 +2099,71 @@ void AttPosEKF::ForceSymmetry() { P[i][j] = 0.5f * (P[i][j] + P[j][i]); P[j][i] = P[i][j]; + + if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) || + (fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) { + current_ekf_state.covariancesExcessive = true; + current_ekf_state.error |= true; + InitializeDynamic(velNED, magDeclination); + return; + } + + float symmetric = 0.5f * (P[i][j] + P[j][i]); + P[i][j] = symmetric; + P[j][i] = symmetric; } } } +bool AttPosEKF::GyroOffsetsDiverged() +{ + // Detect divergence by looking for rapid changes of the gyro offset + Vector3f current_bias; + current_bias.x = states[10]; + current_bias.y = states[11]; + current_bias.z = states[12]; + + Vector3f delta = current_bias - lastGyroOffset; + float delta_len = delta.length(); + float delta_len_scaled = 0.0f; + + // Protect against division by zero + if (delta_len > 0.0f) { + float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f); + delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU; + } + + bool diverged = (delta_len_scaled > 1.0f); + lastGyroOffset = current_bias; + current_ekf_state.error |= diverged; + current_ekf_state.gyroOffsetsExcessive = diverged; + + return diverged; +} + +bool AttPosEKF::VelNEDDiverged() +{ + Vector3f current_vel; + current_vel.x = states[4]; + current_vel.y = states[5]; + current_vel.z = states[6]; + + Vector3f gps_vel; + gps_vel.x = velNED[0]; + gps_vel.y = velNED[1]; + gps_vel.z = velNED[2]; + + Vector3f delta = current_vel - gps_vel; + float delta_len = delta.length(); + + bool excessive = (delta_len > 20.0f); + + current_ekf_state.error |= excessive; + current_ekf_state.velOffsetExcessive = excessive; + + return excessive; +} + bool AttPosEKF::FilterHealthy() { if (!statesInitialised) { @@ -2262,42 +2228,26 @@ void AttPosEKF::ResetVelocity(void) } } - -void AttPosEKF::FillErrorReport(struct ekf_status_report *err) -{ - for (unsigned i = 0; i < n_states; i++) - { - err->states[i] = states[i]; - } - - err->velHealth = current_ekf_state.velHealth; - err->posHealth = current_ekf_state.posHealth; - err->hgtHealth = current_ekf_state.hgtHealth; - err->velTimeout = current_ekf_state.velTimeout; - err->posTimeout = current_ekf_state.posTimeout; - err->hgtTimeout = current_ekf_state.hgtTimeout; -} - -bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { +bool AttPosEKF::StatesNaN() { bool err = false; // check all integrators if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { - err_report->statesNaN = true; + current_ekf_state.angNaN = true; ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { - err_report->statesNaN = true; + current_ekf_state.angNaN = true; ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { - err_report->statesNaN = true; + current_ekf_state.summedDelVelNaN = true; ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); err = true; goto out; @@ -2308,7 +2258,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { for (unsigned j = 0; j < n_states; j++) { if (!isfinite(KH[i][j])) { - err_report->covarianceNaN = true; + current_ekf_state.KHNaN = true; err = true; ekf_debug("KH NaN"); goto out; @@ -2316,7 +2266,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(KHP[i][j])) { - err_report->covarianceNaN = true; + current_ekf_state.KHPNaN = true; err = true; ekf_debug("KHP NaN"); goto out; @@ -2324,7 +2274,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(P[i][j])) { - err_report->covarianceNaN = true; + current_ekf_state.covarianceNaN = true; err = true; ekf_debug("P NaN"); } // covariance matrix @@ -2332,7 +2282,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(Kfusion[i])) { - err_report->kalmanGainsNaN = true; + current_ekf_state.kalmanGainsNaN = true; ekf_debug("Kfusion NaN"); err = true; goto out; @@ -2340,7 +2290,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(states[i])) { - err_report->statesNaN = true; + current_ekf_state.statesNaN = true; ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); err = true; goto out; @@ -2349,7 +2299,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { out: if (err) { - FillErrorReport(err_report); + current_ekf_state.error |= true; } return err; @@ -2365,47 +2315,105 @@ out: * updated, but before any of the fusion steps are * executed. */ -int AttPosEKF::CheckAndBound() +int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) { // Store the old filter state bool currStaticMode = staticMode; + // Limit reset rate to 5 Hz to allow the filter + // to settle + if (millis() - lastReset < 200) { + return 0; + } + + if (ekfDiverged) { + ekfDiverged = false; + } + + int ret = 0; + + // Check if we're on ground - this also sets static mode. + OnGroundCheck(); + // Reset the filter if the states went NaN - if (StatesNaN(&last_ekf_error)) { + if (StatesNaN()) { ekf_debug("re-initializing dynamic"); - InitializeDynamic(velNED, magDeclination); + // Reset and fill error report + InitializeDynamic(velNED, magDeclination); - return 1; + ret = 1; } // Reset the filter if the IMU data is too old if (dtIMU > 0.3f) { + current_ekf_state.imuTimeout = true; + + // Fill error report + GetFilterState(&last_ekf_error); + ResetVelocity(); ResetPosition(); ResetHeight(); ResetStoredStates(); + // Timeout cleared with this reset + current_ekf_state.imuTimeout = false; + // that's all we can do here, return - return 2; + ret = 2; } - // Check if we're on ground - this also sets static mode. - OnGroundCheck(); - // Check if we switched between states if (currStaticMode != staticMode) { + // Fill error report, but not setting error flag + GetFilterState(&last_ekf_error); + ResetVelocity(); ResetPosition(); ResetHeight(); ResetStoredStates(); - return 3; + ret = 3; + } + + // Reset the filter if gyro offsets are excessive + if (GyroOffsetsDiverged()) { + + // Reset and fill error report + InitializeDynamic(velNED, magDeclination); + + // that's all we can do here, return + ret = 4; + } + + // Reset the filter if it diverges too far from GPS + if (VelNEDDiverged()) { + + // Reset and fill error report + InitializeDynamic(velNED, magDeclination); + + // that's all we can do here, return + ret = 5; + } + + // The excessive covariance detection already + // reset the filter. Just need to report here. + if (last_ekf_error.covariancesExcessive) { + ret = 6; + } + + if (ret) { + ekfDiverged = true; + lastReset = millis(); + + // This reads the last error and clears it + GetLastErrorState(last_error); } - return 0; + return ret; } void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) @@ -2456,6 +2464,30 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { + if (current_ekf_state.error) { + GetFilterState(&last_ekf_error); + } + + ZeroVariables(); + + // Reset error states + current_ekf_state.error = false; + current_ekf_state.angNaN = false; + current_ekf_state.summedDelVelNaN = false; + current_ekf_state.KHNaN = false; + current_ekf_state.KHPNaN = false; + current_ekf_state.PNaN = false; + current_ekf_state.covarianceNaN = false; + current_ekf_state.kalmanGainsNaN = false; + current_ekf_state.statesNaN = false; + + current_ekf_state.velHealth = true; + //current_ekf_state.posHealth = ?; + //current_ekf_state.hgtHealth = ?; + + current_ekf_state.velTimeout = false; + //current_ekf_state.posTimeout = ?; + //current_ekf_state.hgtTimeout = ?; // Fill variables with valid data velNED[0] = initvelNED[0]; @@ -2494,7 +2526,11 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities - for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel + // positions: + states[7] = posNE[0]; + states[8] = posNE[1]; + states[9] = -hgtMea; + for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel states[16] = initMagNED.x; // Magnetic Field North states[17] = initMagNED.y; // Magnetic Field East states[18] = initMagNED.z; // Magnetic Field Down @@ -2525,14 +2561,16 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do hgtRef = referenceHgt; refSet = true; - // we are at reference altitude, so measurement must be zero - hgtMea = 0.0f; + // we are at reference position, so measurement must be zero + posNE[0] = 0.0f; + posNE[1] = 0.0f; + + // we are at an unknown, possibly non-zero altitude - so altitude + // is not reset (hgtMea) // the baro offset must be this difference now baroHgtOffset = baroHgt - referenceHgt; - memset(&last_ekf_error, 0, sizeof(last_ekf_error)); - InitializeDynamic(initvelNED, declination); } @@ -2540,27 +2578,8 @@ void AttPosEKF::ZeroVariables() { // Initialize on-init initialized variables - fusionModeGPS = 0; - covSkipCount = 0; - statesInitialised = false; - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - fuseMagData = false; - fuseVtasData = false; - onGround = true; - staticMode = true; - useAirspeed = true; - useCompass = true; - useRangeFinder = true; - numericalProtection = true; - refSet = false; + storeIndex = 0; - gpsHgt = 0.0f; - baroHgt = 0.0f; - GPSstatus = 0; - VtasMeas = 0.0f; - magDeclination = 0.0f; // Do the data structure init for (unsigned i = 0; i < n_states; i++) { @@ -2577,9 +2596,7 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); - - dAngIMU.zero(); - dVelIMU.zero(); + lastGyroOffset.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { @@ -2598,12 +2615,27 @@ void AttPosEKF::ZeroVariables() } -void AttPosEKF::GetFilterState(struct ekf_status_report *state) +void AttPosEKF::GetFilterState(struct ekf_status_report *err) { - memcpy(state, ¤t_ekf_state, sizeof(*state)); + + // Copy states + for (unsigned i = 0; i < n_states; i++) { + current_ekf_state.states[i] = states[i]; + } + current_ekf_state.n_states = n_states; + + memcpy(err, ¤t_ekf_state, sizeof(*err)); + + // err->velHealth = current_ekf_state.velHealth; + // err->posHealth = current_ekf_state.posHealth; + // err->hgtHealth = current_ekf_state.hgtHealth; + // err->velTimeout = current_ekf_state.velTimeout; + // err->posTimeout = current_ekf_state.posTimeout; + // err->hgtTimeout = current_ekf_state.hgtTimeout; } void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) { memcpy(last_error, &last_ekf_error, sizeof(*last_error)); + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index e821089f2..7aad849f9 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -1,76 +1,10 @@ -#include <math.h> -#include <stdint.h> - #pragma once -#define GRAVITY_MSS 9.80665f -#define deg2rad 0.017453292f -#define rad2deg 57.295780f -#define pi 3.141592657f -#define earthRate 0.000072921f -#define earthRadius 6378145.0f -#define earthRadiusInv 1.5678540e-7f - -class Vector3f -{ -private: -public: - float x; - float y; - float z; - - float length(void) const; - void zero(void); -}; - -class Mat3f -{ -private: -public: - Vector3f x; - Vector3f y; - Vector3f z; - - Mat3f(); - - void identity(); - Mat3f transpose(void) const; -}; - -Vector3f operator*(float sclIn1, Vector3f vecIn1); -Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); -Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); -Vector3f operator*( Mat3f matIn, Vector3f vecIn); -Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); -Vector3f operator*(Vector3f vecIn1, float sclIn1); - -void swap_var(float &d1, float &d2); +#include "estimator_utilities.h" const unsigned int n_states = 23; const unsigned int data_buffer_size = 50; -enum GPS_FIX { - GPS_FIX_NOFIX = 0, - GPS_FIX_2D = 2, - GPS_FIX_3D = 3 -}; - -struct ekf_status_report { - bool velHealth; - bool posHealth; - bool hgtHealth; - bool velTimeout; - bool posTimeout; - bool hgtTimeout; - uint32_t velFailTime; - uint32_t posFailTime; - uint32_t hgtFailTime; - float states[n_states]; - bool statesNaN; - bool covarianceNaN; - bool kalmanGainsNaN; -}; - class AttPosEKF { public: @@ -141,7 +75,7 @@ public: accelProcessNoise = 0.5f; } - struct { + struct mag_state_struct { unsigned obsIndex; float MagPred[3]; float SH_MAG[9]; @@ -157,7 +91,12 @@ public: float magZbias; float R_MAG; Mat3f DCM; - } magstate; + }; + + struct mag_state_struct magstate; + struct mag_state_struct resetMagState; + + // Global variables @@ -166,6 +105,7 @@ public: float P[n_states][n_states]; // covariance matrix float Kfusion[n_states]; // Kalman gains float states[n_states]; // state matrix + float resetStates[n_states]; float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored @@ -183,6 +123,8 @@ public: float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) + Vector3f lastGyroOffset; // Last gyro offset + Vector3f delAngTotal; Mat3f Tbn; // transformation matrix from body to NED coordinates Mat3f Tnb; // transformation amtrix from NED to body coordinates @@ -196,11 +138,11 @@ public: float varInnovVelPos[6]; // innovation variance output float velNED[3]; // North, East, Down velocity obs (m/s) + float accelGPSNED[3]; // Acceleration predicted by GPS in earth frame float posNE[2]; // North, East position obs (m) float hgtMea; // measured height (m) float baroHgtOffset; ///< the baro (weather) offset from normalized altitude float rngMea; // Ground distance - float posNED[3]; // North, East Down position (m) float innovMag[3]; // innovation output float varInnovMag[3]; // innovation variance output @@ -243,6 +185,9 @@ public: bool useCompass; ///< boolean true if magnetometer data is being used bool useRangeFinder; ///< true when rangefinder is being used + bool ekfDiverged; + uint64_t lastReset; + struct ekf_status_report current_ekf_state; struct ekf_status_report last_ekf_error; @@ -299,9 +244,9 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]); static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); +void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); -static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); @@ -321,7 +266,7 @@ void ConstrainStates(); void ForceSymmetry(); -int CheckAndBound(); +int CheckAndBound(struct ekf_status_report *last_error); void ResetPosition(); @@ -333,8 +278,7 @@ void GetFilterState(struct ekf_status_report *state); void GetLastErrorState(struct ekf_status_report *last_error); -bool StatesNaN(struct ekf_status_report *err_report); -void FillErrorReport(struct ekf_status_report *err); +bool StatesNaN(); void InitializeDynamic(float (&initvelNED)[3], float declination); @@ -342,6 +286,10 @@ protected: bool FilterHealthy(); +bool GyroOffsetsDiverged(); + +bool VelNEDDiverged(); + void ResetHeight(void); void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp new file mode 100644 index 000000000..b4767a0d3 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -0,0 +1,139 @@ + +#include "estimator_utilities.h" + +// Define EKF_DEBUG here to enable the debug print calls +// if the macro is not set, these will be completely +// optimized out by the compiler. +//#define EKF_DEBUG + +#ifdef EKF_DEBUG +#include <stdio.h> + +static void +ekf_debug_print(const char *fmt, va_list args) +{ + fprintf(stderr, "%s: ", "[ekf]"); + vfprintf(stderr, fmt, args); + + fprintf(stderr, "\n"); +} + +void +ekf_debug(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + ekf_debug_print(fmt, args); +} + +#else + +void ekf_debug(const char *fmt, ...) { while(0){} } +#endif + +float Vector3f::length(void) const +{ + return sqrt(x*x + y*y + z*z); +} + +void Vector3f::zero(void) +{ + x = 0.0f; + y = 0.0f; + z = 0.0f; +} + +Mat3f::Mat3f() { + identity(); +} + +void Mat3f::identity() { + x.x = 1.0f; + x.y = 0.0f; + x.z = 0.0f; + + y.x = 0.0f; + y.y = 1.0f; + y.z = 0.0f; + + z.x = 0.0f; + z.y = 0.0f; + z.z = 1.0f; +} + +Mat3f Mat3f::transpose(void) const +{ + Mat3f ret = *this; + swap_var(ret.x.y, ret.y.x); + swap_var(ret.x.z, ret.z.x); + swap_var(ret.y.z, ret.z.y); + return ret; +} + +// overload + operator to provide a vector addition +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x + vecIn2.x; + vecOut.y = vecIn1.y + vecIn2.y; + vecOut.z = vecIn1.z + vecIn2.z; + return vecOut; +} + +// overload - operator to provide a vector subtraction +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x - vecIn2.x; + vecOut.y = vecIn1.y - vecIn2.y; + vecOut.z = vecIn1.z - vecIn2.z; + return vecOut; +} + +// overload * operator to provide a matrix vector product +Vector3f operator*( Mat3f matIn, Vector3f vecIn) +{ + Vector3f vecOut; + vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z; + vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z; + vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; + return vecOut; +} + +// overload % operator to provide a vector cross product +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y; + vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z; + vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x; + return vecOut; +} + +// overload * operator to provide a vector scaler product +Vector3f operator*(Vector3f vecIn1, float sclIn1) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x * sclIn1; + vecOut.y = vecIn1.y * sclIn1; + vecOut.z = vecIn1.z * sclIn1; + return vecOut; +} + +// overload * operator to provide a vector scaler product +Vector3f operator*(float sclIn1, Vector3f vecIn1) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x * sclIn1; + vecOut.y = vecIn1.y * sclIn1; + vecOut.z = vecIn1.z * sclIn1; + return vecOut; +} + +void swap_var(float &d1, float &d2) +{ + float tmp = d1; + d1 = d2; + d2 = tmp; +} diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h new file mode 100644 index 000000000..d47568b62 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -0,0 +1,82 @@ +#include <math.h> +#include <stdint.h> + +#pragma once + +#define GRAVITY_MSS 9.80665f +#define deg2rad 0.017453292f +#define rad2deg 57.295780f +#define pi 3.141592657f +#define earthRate 0.000072921f +#define earthRadius 6378145.0 +#define earthRadiusInv 1.5678540e-7 + +class Vector3f +{ +private: +public: + float x; + float y; + float z; + + float length(void) const; + void zero(void); +}; + +class Mat3f +{ +private: +public: + Vector3f x; + Vector3f y; + Vector3f z; + + Mat3f(); + + void identity(); + Mat3f transpose(void) const; +}; + +Vector3f operator*(float sclIn1, Vector3f vecIn1); +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*( Mat3f matIn, Vector3f vecIn); +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*(Vector3f vecIn1, float sclIn1); + +void swap_var(float &d1, float &d2); + +enum GPS_FIX { + GPS_FIX_NOFIX = 0, + GPS_FIX_2D = 2, + GPS_FIX_3D = 3 +}; + +struct ekf_status_report { + bool error; + bool velHealth; + bool posHealth; + bool hgtHealth; + bool velTimeout; + bool posTimeout; + bool hgtTimeout; + bool imuTimeout; + uint32_t velFailTime; + uint32_t posFailTime; + uint32_t hgtFailTime; + float states[32]; + unsigned n_states; + bool angNaN; + bool summedDelVelNaN; + bool KHNaN; + bool KHPNaN; + bool PNaN; + bool covarianceNaN; + bool kalmanGainsNaN; + bool statesNaN; + bool gyroOffsetsExcessive; + bool covariancesExcessive; + bool velOffsetExcessive; +}; + +void ekf_debug(const char *fmt, ...);
\ No newline at end of file diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 6fefec2c2..dc5220bf0 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -39,4 +39,5 @@ MODULE_COMMAND = ekf_att_pos_estimator SRCS = ekf_att_pos_estimator_main.cpp \ ekf_att_pos_estimator_params.c \ - estimator.cpp + estimator_23states.cpp \ + estimator_utilities.cpp diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 49d812e7e..000c02e3d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -43,8 +43,8 @@ * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Original implementation for total energy control class: - * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) + * Implementation for total energy control class: + * Thomas Gubler * * More details and acknowledgements in the referenced library headers. * @@ -88,7 +88,6 @@ #include <mavlink/mavlink_log.h> #include <launchdetection/LaunchDetector.h> #include <ecl/l1/ecl_l1_pos_controller.h> -#include <external_lgpl/tecs/tecs.h> #include <drivers/drv_range_finder.h> #include "landingslope.h" #include "mtecs/mTecs.h" @@ -199,7 +198,6 @@ private: math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; - TECS _tecs; fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; @@ -565,23 +563,6 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); - _tecs.set_time_const(_parameters.time_const); - _tecs.set_min_sink_rate(_parameters.min_sink_rate); - _tecs.set_max_sink_rate(_parameters.max_sink_rate); - _tecs.set_throttle_damp(_parameters.throttle_damp); - _tecs.set_integrator_gain(_parameters.integrator_gain); - _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); - _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); - _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); - _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); - _tecs.set_speed_weight(_parameters.speed_weight); - _tecs.set_pitch_damping(_parameters.pitch_damping); - _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); - _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); - _tecs.set_max_climb_rate(_parameters.max_climb_rate); - _tecs.set_heightrate_p(_parameters.heightrate_p); - _tecs.set_speedrate_p(_parameters.speedrate_p); - /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || _parameters.airspeed_max < 5.0f || @@ -653,9 +634,6 @@ FixedwingPositionControl::vehicle_airspeed_poll() } } - /* update TECS state */ - _tecs.enable_airspeed(_airspeed_valid); - return airspeed_updated; } @@ -835,10 +813,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - if (!_mTecs.getEnabled()) { - _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - } - float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ @@ -863,9 +837,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); - /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ - _tecs.set_speed_weight(_parameters.speed_weight); - /* current waypoint (the one currently heading for) */ math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); @@ -1228,8 +1199,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* user switched off throttle */ if (_manual.z < 0.1f) { throttle_max = 0.0f; - /* switch to pure pitch based altitude control, give up speed */ - _tecs.set_speed_weight(0.0f); } /* climb out control */ @@ -1269,9 +1238,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max); } - _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); + _att_sp.pitch_body = _mTecs.getPitchSetpoint(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1455,29 +1424,20 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, tecs_mode mode) { - if (_mTecs.getEnabled()) { - /* Using mtecs library: prepare arguments for mtecs call */ - float flightPathAngle = 0.0f; - float ground_speed_length = ground_speed.length(); - if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); - } - fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { - limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); - } else { - limitOverride.disablePitchMinOverride(); - } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, - limitOverride); + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); } else { - /* Using tecs library */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, - _airspeed.indicated_airspeed_m_s, eas2tas, - climbout_mode, climbout_pitch_min_rad, - throttle_min, throttle_max, throttle_cruise, - pitch_min_rad, pitch_max_rad); + limitOverride.disablePitchMinOverride(); } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); } int diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 48c9079a4..fc0a2551c 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -58,6 +58,7 @@ mTecs::mTecs() : _controlEnergyDistribution(this, "PIT", true), _controlAltitude(this, "FPA", true), _controlAirSpeed(this, "ACC"), + _flightPathAngleLowpass(this, "FPA_LP"), _airspeedLowpass(this, "A_LP"), _airspeedDerivative(this, "AD"), _throttleSp(0.0f), @@ -123,7 +124,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng /* time measurement */ updateTimeMeasurement(); - /* Filter arispeed */ + /* Filter airspeed */ float airspeedFiltered = _airspeedLowpass.update(airspeed); /* calculate longitudinal acceleration setpoint from airspeed setpoint*/ @@ -138,8 +139,6 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng } /* Write part of the status message */ - _status.flightPathAngleSp = flightPathAngleSp; - _status.flightPathAngle = flightPathAngle; _status.airspeedSp = airspeedSp; _status.airspeed = airspeed; _status.airspeedFiltered = airspeedFiltered; @@ -164,8 +163,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* update parameters first */ updateParams(); + /* Filter flightpathangle */ + float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle); + /* calculate values (energies) */ - float flightPathAngleError = flightPathAngleSp - flightPathAngle; + float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered; float airspeedDerivative = 0.0f; if(_airspeedDerivative.getDt() > 0.0f) { airspeedDerivative = _airspeedDerivative.update(airspeedFiltered); @@ -175,12 +177,12 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G; float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm; - float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm; + float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm; float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError; float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp; float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate; - float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm; + float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm; float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError; float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp; float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; @@ -202,7 +204,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); if (mode == TECS_MODE_TAKEOFF) { - outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector + outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; } else if (mode == TECS_MODE_LAND) { // only limit pitch but do not limit throttle @@ -221,6 +223,9 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); /* Write part of the status message */ + _status.flightPathAngleSp = flightPathAngleSp; + _status.flightPathAngle = flightPathAngle; + _status.flightPathAngleFiltered = flightPathAngleFiltered; _status.airspeedDerivativeSp = airspeedDerivativeSp; _status.airspeedDerivative = airspeedDerivative; _status.totalEnergyRateSp = totalEnergyRateSp; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index c102f5dee..efa89a5d3 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -104,12 +104,17 @@ protected: uORB::Publication<tecs_status_s> _status; /**< publish internal values for logging */ /* control blocks */ - BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ - BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ - BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight path angle setpoint */ - BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration setpoint */ + BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output + is throttle */ + BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: + output is pitch */ + BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight + path angle setpoint */ + BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration + setpoint */ /* Other calculation Blocks */ + control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */ control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ @@ -118,21 +123,22 @@ protected: float _pitchSp; /**< Pitch Setpoint from -pi to pi */ /* Output Limits in special modes */ - BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ - BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ - BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */ - BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */ - BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in last phase)*/ - BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */ + BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ + BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in + last phase)*/ + BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */ /* Time measurements */ - hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ + hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ - bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ - bool _dtCalculated; /**< True if dt has been calculated in this iteration */ + bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + bool _dtCalculated; /**< True if dt has been calculated in this iteration */ int _counter; - bool _debug; ///< Set true to enable debug output + bool _debug; ///< Set true to enable debug output static void debug_print(const char *fmt, va_list args); diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index bbbb8f9db..5b9238780 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -175,6 +175,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); /** + * Lowpass (cutoff freq.) for the flight path angle + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f); + +/** * P gain for the altitude control * Maps the altitude error to the flight path angle setpoint * diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 839a7890b..7758faed7 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -181,16 +181,13 @@ int gpio_led_main(int argc, char *argv[]) } else { gpio_led_started = true; warnx("start, using pin: %s", pin_name); + exit(0); } - - exit(0); - - } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; warnx("stop"); - + exit(0); } else { errx(1, "not running"); } diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp new file mode 100644 index 000000000..ca846a465 --- /dev/null +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -0,0 +1,415 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include <crc32.h> +#include <unistd.h> +#include <stdio.h> +#include <fcntl.h> + +#include "mavlink_ftp.h" + +MavlinkFTP *MavlinkFTP::_server; + +MavlinkFTP * +MavlinkFTP::getServer() +{ + // XXX this really cries out for some locking... + if (_server == nullptr) { + _server = new MavlinkFTP; + } + return _server; +} + +MavlinkFTP::MavlinkFTP() +{ + // initialise the request freelist + dq_init(&_workFree); + sem_init(&_lock, 0, 1); + + // initialize session list + for (size_t i=0; i<kMaxSession; i++) { + _session_fds[i] = -1; + } + + // drop work entries onto the free list + for (unsigned i = 0; i < kRequestQueueSize; i++) { + _qFree(&_workBufs[i]); + } + +} + +void +MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg) +{ + // get a free request + auto req = _dqFree(); + + // if we couldn't get a request slot, just drop it + if (req != nullptr) { + + // decode the request + if (req->decode(mavlink, msg)) { + + // and queue it for the worker + work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); + } else { + _qFree(req); + } + } +} + +void +MavlinkFTP::_workerTrampoline(void *arg) +{ + auto req = reinterpret_cast<Request *>(arg); + auto server = MavlinkFTP::getServer(); + + // call the server worker with the work item + server->_worker(req); +} + +void +MavlinkFTP::_worker(Request *req) +{ + auto hdr = req->header(); + ErrorCode errorCode = kErrNone; + uint32_t messageCRC; + + // basic sanity checks; must validate length before use + if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) { + errorCode = kErrNoRequest; + goto out; + } + + // check request CRC to make sure this is one of ours + messageCRC = hdr->crc32; + hdr->crc32 = 0; + if (crc32(req->rawData(), req->dataSize()) != messageCRC) { + errorCode = kErrNoRequest; + goto out; + printf("ftp: bad crc\n"); + } + + printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); + + switch (hdr->opcode) { + case kCmdNone: + break; + + case kCmdTerminate: + errorCode = _workTerminate(req); + break; + + case kCmdReset: + errorCode = _workReset(); + break; + + case kCmdList: + errorCode = _workList(req); + break; + + case kCmdOpen: + errorCode = _workOpen(req, false); + break; + + case kCmdCreate: + errorCode = _workOpen(req, true); + break; + + case kCmdRead: + errorCode = _workRead(req); + break; + + case kCmdWrite: + errorCode = _workWrite(req); + break; + + case kCmdRemove: + errorCode = _workRemove(req); + break; + + default: + errorCode = kErrNoRequest; + break; + } + +out: + // handle success vs. error + if (errorCode == kErrNone) { + hdr->opcode = kRspAck; + printf("FTP: ack\n"); + } else { + printf("FTP: nak %u\n", errorCode); + hdr->opcode = kRspNak; + hdr->size = 1; + hdr->data[0] = errorCode; + } + + // respond to the request + _reply(req); + + // free the request buffer back to the freelist + _qFree(req); +} + +void +MavlinkFTP::_reply(Request *req) +{ + auto hdr = req->header(); + + // message is assumed to be already constructed in the request buffer, so generate the CRC + hdr->crc32 = 0; + hdr->crc32 = crc32(req->rawData(), req->dataSize()); + + // then pack and send the reply back to the request source + req->reply(); +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workList(Request *req) +{ + auto hdr = req->header(); + DIR *dp = opendir(req->dataAsCString()); + + if (dp == nullptr) { + printf("FTP: can't open path '%s'\n", req->dataAsCString()); + return kErrNotDir; + } + + ErrorCode errorCode = kErrNone; + struct dirent entry, *result = nullptr; + unsigned offset = 0; + + // move to the requested offset + seekdir(dp, hdr->offset); + + for (;;) { + // read the directory entry + if (readdir_r(dp, &entry, &result)) { + errorCode = kErrIO; + break; + } + + // no more entries? + if (result == nullptr) { + if (hdr->offset != 0 && offset == 0) { + // User is requesting subsequent dir entries but there were none. This means the user asked + // to seek past EOF. + errorCode = kErrEOF; + } + // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that + break; + } + + // name too big to fit? + if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) { + break; + } + + // store the type marker + switch (entry.d_type) { + case DTYPE_FILE: + hdr->data[offset++] = kDirentFile; + break; + case DTYPE_DIRECTORY: + hdr->data[offset++] = kDirentDir; + break; + default: + hdr->data[offset++] = kDirentUnknown; + break; + } + + // copy the name, which we know will fit + strcpy((char *)&hdr->data[offset], entry.d_name); + offset += strlen(entry.d_name) + 1; + printf("FTP: list %s\n", entry.d_name); + } + + closedir(dp); + hdr->size = offset; + + return errorCode; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workOpen(Request *req, bool create) +{ + auto hdr = req->header(); + + int session_index = _findUnusedSession(); + if (session_index < 0) { + return kErrNoSession; + } + + int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; + + int fd = ::open(req->dataAsCString(), oflag); + if (fd < 0) { + return create ? kErrPerm : kErrNotFile; + } + _session_fds[session_index] = fd; + + hdr->session = session_index; + hdr->size = 0; + + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workRead(Request *req) +{ + auto hdr = req->header(); + + int session_index = hdr->session; + + if (!_validSession(session_index)) { + return kErrNoSession; + } + + // Seek to the specified position + printf("Seek %d\n", hdr->offset); + if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { + // Unable to see to the specified location + return kErrEOF; + } + + int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength); + if (bytes_read < 0) { + // Negative return indicates error other than eof + return kErrIO; + } + + printf("Read success %d\n", bytes_read); + hdr->size = bytes_read; + + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workWrite(Request *req) +{ +#if 0 + // NYI: Coming soon + auto hdr = req->header(); + + // look up session + auto session = getSession(hdr->session); + if (session == nullptr) { + return kErrNoSession; + } + + // append to file + int result = session->append(hdr->offset, &hdr->data[0], hdr->size); + + if (result < 0) { + // XXX might also be no space, I/O, etc. + return kErrNotAppend; + } + + hdr->size = result; + return kErrNone; +#else + return kErrPerm; +#endif +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemove(Request *req) +{ + auto hdr = req->header(); + + // for now, send error reply + return kErrPerm; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workTerminate(Request *req) +{ + auto hdr = req->header(); + + if (!_validSession(hdr->session)) { + return kErrNoSession; + } + + ::close(_session_fds[hdr->session]); + + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workReset(void) +{ + for (size_t i=0; i<kMaxSession; i++) { + if (_session_fds[i] != -1) { + ::close(_session_fds[i]); + _session_fds[i] = -1; + } + } + + return kErrNone; +} + +bool +MavlinkFTP::_validSession(unsigned index) +{ + if ((index >= kMaxSession) || (_session_fds[index] < 0)) { + return false; + } + return true; +} + +int +MavlinkFTP::_findUnusedSession(void) +{ + for (size_t i=0; i<kMaxSession; i++) { + if (_session_fds[i] == -1) { + return i; + } + } + + return -1; +} + +char * +MavlinkFTP::Request::dataAsCString() +{ + // guarantee nul termination + if (header()->size < kMaxDataLength) { + requestData()[header()->size] = '\0'; + } else { + requestData()[kMaxDataLength - 1] = '\0'; + } + + // and return data + return (char *)&(header()->data[0]); +} diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h new file mode 100644 index 000000000..e22e61553 --- /dev/null +++ b/src/modules/mavlink/mavlink_ftp.h @@ -0,0 +1,226 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +/** + * @file mavlink_ftp.h + * + * MAVLink remote file server. + * + * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes + * a session ID and sequence number. + * + * A limited number of requests (currently 2) may be outstanding at a time. + * Additional messages will be discarded. + * + * Messages consist of a fixed header, followed by a data area. + * + */ + +#include <dirent.h> +#include <queue.h> + +#include <nuttx/wqueue.h> +#include <systemlib/err.h> + +#include "mavlink_messages.h" + +class MavlinkFTP +{ +public: + MavlinkFTP(); + + static MavlinkFTP *getServer(); + + // static interface + void handle_message(Mavlink* mavlink, + mavlink_message_t *msg); + +private: + + static const unsigned kRequestQueueSize = 2; + + static MavlinkFTP *_server; + + struct RequestHeader + { + uint8_t magic; + uint8_t session; + uint8_t opcode; + uint8_t size; + uint32_t crc32; + uint32_t offset; + uint8_t data[]; + }; + + enum Opcode : uint8_t + { + kCmdNone, // ignored, always acked + kCmdTerminate, // releases sessionID, closes file + kCmdReset, // terminates all sessions + kCmdList, // list files in <path> from <offset> + kCmdOpen, // opens <path> for reading, returns <session> + kCmdRead, // reads <size> bytes from <offset> in <session> + kCmdCreate, // creates <path> for writing, returns <session> + kCmdWrite, // appends <size> bytes at <offset> in <session> + kCmdRemove, // remove file (only if created by server?) + + kRspAck, + kRspNak + }; + + enum ErrorCode : uint8_t + { + kErrNone, + kErrNoRequest, + kErrNoSession, + kErrSequence, + kErrNotDir, + kErrNotFile, + kErrEOF, + kErrNotAppend, + kErrTooBig, + kErrIO, + kErrPerm + }; + + int _findUnusedSession(void); + bool _validSession(unsigned index); + + static const unsigned kMaxSession = 2; + int _session_fds[kMaxSession]; + + class Request + { + public: + union { + dq_entry_t entry; + work_s work; + }; + + bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) { + if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) { + _mavlink = mavlink; + mavlink_msg_encapsulated_data_decode(fromMessage, &_message); + return true; + } + return false; + } + + void reply() { + + // XXX the proper way would be an IOCTL / uORB call, rather than exploiting the + // flat memory architecture, as we're operating between threads here. + mavlink_message_t msg; + msg.checksum = 0; + unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), + _mavlink->get_channel(), &msg, sequence(), rawData()); + + _mavlink->lockMessageBufferMutex(); + bool fError = _mavlink->message_buffer_write(&msg, len); + _mavlink->unlockMessageBufferMutex(); + + if (!fError) { + warnx("FTP TX ERR"); + } else { + warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", + _mavlink->get_system_id(), + _mavlink->get_component_id(), + _mavlink->get_channel(), + len, + msg.checksum); + } + } + + uint8_t *rawData() { return &_message.data[0]; } + RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.data[0]); } + uint8_t *requestData() { return &(header()->data[0]); } + unsigned dataSize() { return header()->size + sizeof(RequestHeader); } + uint16_t sequence() const { return _message.seqnr; } + mavlink_channel_t channel() { return _mavlink->get_channel(); } + + char *dataAsCString(); + + private: + Mavlink *_mavlink; + mavlink_encapsulated_data_t _message; + + }; + + static const uint8_t kProtocolMagic = 'f'; + static const char kDirentFile = 'F'; + static const char kDirentDir = 'D'; + static const char kDirentUnknown = 'U'; + static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader); + + /// Request worker; runs on the low-priority work queue to service + /// remote requests. + /// + static void _workerTrampoline(void *arg); + void _worker(Request *req); + + /// Reply to a request (XXX should be a Request method) + /// + void _reply(Request *req); + + ErrorCode _workList(Request *req); + ErrorCode _workOpen(Request *req, bool create); + ErrorCode _workRead(Request *req); + ErrorCode _workWrite(Request *req); + ErrorCode _workRemove(Request *req); + ErrorCode _workTerminate(Request *req); + ErrorCode _workReset(); + + // work freelist + Request _workBufs[kRequestQueueSize]; + dq_queue_t _workFree; + sem_t _lock; + + void _qLock() { do {} while (sem_wait(&_lock) != 0); } + void _qUnlock() { sem_post(&_lock); } + + void _qFree(Request *req) { + _qLock(); + dq_addlast(&req->entry, &_workFree); + _qUnlock(); + } + + Request *_dqFree() { + _qLock(); + auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree)); + _qUnlock(); + return req; + } + +}; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5710f0523..9a5e31ef4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -83,6 +83,10 @@ #include "mavlink_rate_limiter.h" #include "mavlink_commands.h" +#ifndef MAVLINK_CRC_EXTRA + #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#endif + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -114,6 +118,7 @@ static uint64_t last_write_try_times[6] = {0}; void mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) { + Mavlink *instance; switch (channel) { @@ -192,13 +197,14 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (buf_free < desired) { /* we don't want to send anything just in half, so return */ + instance->count_txerr(); return; } } ssize_t ret = write(uart, ch, desired); if (ret != desired) { - warnx("TX FAIL"); + instance->count_txerr(); } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } @@ -230,6 +236,7 @@ Mavlink::Mavlink() : _verbose(false), _forwarding_on(false), _passing_on(false), + _ftp_on(false), _uart_fd(-1), _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), @@ -243,7 +250,8 @@ Mavlink::Mavlink() : _param_use_hil_gps(0), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { _wpm = &_wpm_s; mission.count = 0; @@ -296,6 +304,7 @@ Mavlink::Mavlink() : Mavlink::~Mavlink() { perf_free(_loop_perf); + perf_free(_txerr_perf); if (_task_running) { /* task wakes up every 10ms or so at the longest */ @@ -321,6 +330,12 @@ Mavlink::~Mavlink() } void +Mavlink::count_txerr() +{ + perf_count(_txerr_perf); +} + +void Mavlink::set_mode(enum MAVLINK_MODE mode) { _mode = mode; @@ -459,7 +474,7 @@ Mavlink::get_instance_id() return _instance_id; } -mavlink_channel_t +const mavlink_channel_t Mavlink::get_channel() { return _channel; @@ -537,6 +552,16 @@ void Mavlink::mavlink_update_system(void) _use_hil_gps = (bool)use_hil_gps; } +int Mavlink::get_system_id() +{ + return mavlink_system.sysid; +} + +int Mavlink::get_component_id() +{ + return mavlink_system.compid; +} + int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { /* process baud rate */ @@ -1631,11 +1656,21 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) int Mavlink::message_buffer_init(int size) { + _message_buffer.size = size; _message_buffer.write_ptr = 0; _message_buffer.read_ptr = 0; _message_buffer.data = (char*)malloc(_message_buffer.size); - return (_message_buffer.data == 0) ? ERROR : OK; + + int ret; + if (_message_buffer.data == 0) { + ret = ERROR; + _message_buffer.size = 0; + } else { + ret = OK; + } + + return ret; } void @@ -1763,7 +1798,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1819,6 +1854,10 @@ Mavlink::task_main(int argc, char *argv[]) _wait_to_transmit = true; break; + case 'x': + _ftp_on = true; + break; + default: err_flag = true; break; @@ -1884,9 +1923,12 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_logbuffer_init(&_logbuffer, 5); /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ - if (_passing_on) { - /* initialize message buffer if multiplexing is on */ - if (OK != message_buffer_init(300)) { + if (_passing_on || _ftp_on) { + /* initialize message buffer if multiplexing is on or its needed for FTP. + * make space for two messages plus off-by-one space as we use the empty element + * marker ring buffer approach. + */ + if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) { errx(1, "can't allocate message buffer, exiting"); } @@ -2049,32 +2091,50 @@ Mavlink::task_main(int argc, char *argv[]) } } - /* pass messages from other UARTs */ - if (_passing_on) { + /* pass messages from other UARTs or FTP worker */ + if (_passing_on || _ftp_on) { bool is_part; - void *read_ptr; + uint8_t *read_ptr; + uint8_t *write_ptr; - /* guard get ptr by mutex */ pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr(&read_ptr, &is_part); + int available = message_buffer_get_ptr((void**)&read_ptr, &is_part); pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { - /* write first part of buffer */ - _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); - message_buffer_mark_read(available); + // Reconstruct message from buffer + + mavlink_message_t msg; + write_ptr = (uint8_t*)&msg; + + // Pull a single message from the buffer + int read_count = available; + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); + + message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ - if (is_part) { - /* guard get ptr by mutex */ - pthread_mutex_lock(&_message_buffer_mutex); - available = message_buffer_get_ptr(&read_ptr, &is_part); - pthread_mutex_unlock(&_message_buffer_mutex); - - _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + if (is_part && read_count < sizeof(mavlink_message_t)) { + write_ptr += read_count; + available = message_buffer_get_ptr((void**)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } + + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, &msg); } } @@ -2124,7 +2184,7 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ close(_mavlink_fd); - if (_passing_on) { + if (_passing_on || _ftp_on) { message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } @@ -2142,11 +2202,20 @@ int Mavlink::start_helper(int argc, char *argv[]) /* create the instance in task context */ Mavlink *instance = new Mavlink(); - /* this will actually only return once MAVLink exits */ - int res = instance->task_main(argc, argv); + int res; - /* delete instance on main thread end */ - delete instance; + if (!instance) { + + /* out of memory */ + res = -ENOMEM; + warnx("OUT OF MEM"); + } else { + /* this will actually only return once MAVLink exits */ + res = instance->task_main(argc, argv); + + /* delete instance on main thread end */ + delete instance; + } return res; } @@ -2266,7 +2335,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 85a88442c..6777d56c3 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -123,27 +123,41 @@ public: /** * Display the mavlink status. */ - void status(); + void status(); - static int stream(int argc, char *argv[]); + static int stream(int argc, char *argv[]); - static int instance_count(); + static int instance_count(); - static Mavlink *new_instance(); + static Mavlink *new_instance(); - static Mavlink *get_instance(unsigned instance); + static Mavlink *get_instance(unsigned instance); - static Mavlink *get_instance_for_device(const char *device_name); + static Mavlink *get_instance_for_device(const char *device_name); - static int destroy_all_instances(); + static int destroy_all_instances(); - static bool instance_exists(const char *device_name, Mavlink *self); + static bool instance_exists(const char *device_name, Mavlink *self); - static void forward_message(mavlink_message_t *msg, Mavlink *self); + static void forward_message(mavlink_message_t *msg, Mavlink *self); - static int get_uart_fd(unsigned index); + static int get_uart_fd(unsigned index); - int get_uart_fd(); + int get_uart_fd(); + + /** + * Get the MAVLink system id. + * + * @return The system ID of this vehicle + */ + int get_system_id(); + + /** + * Get the MAVLink component id. + * + * @return The component ID of this vehicle + */ + int get_component_id(); const char *_device_name; @@ -153,30 +167,30 @@ public: MAVLINK_MODE_CAMERA }; - void set_mode(enum MAVLINK_MODE); - enum MAVLINK_MODE get_mode() { return _mode; } + void set_mode(enum MAVLINK_MODE); + enum MAVLINK_MODE get_mode() { return _mode; } - bool get_hil_enabled() { return _hil_enabled; } + bool get_hil_enabled() { return _hil_enabled; } - bool get_use_hil_gps() { return _use_hil_gps; } + bool get_use_hil_gps() { return _use_hil_gps; } - bool get_flow_control_enabled() { return _flow_control_enabled; } + bool get_flow_control_enabled() { return _flow_control_enabled; } - bool get_forwarding_on() { return _forwarding_on; } + bool get_forwarding_on() { return _forwarding_on; } /** * Handle waypoint related messages. */ - void mavlink_wpm_message_handler(const mavlink_message_t *msg); + void mavlink_wpm_message_handler(const mavlink_message_t *msg); - static int start_helper(int argc, char *argv[]); + static int start_helper(int argc, char *argv[]); /** * Handle parameter related messages. */ - void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); /** * Enable / disable Hardware in the Loop simulation mode. @@ -186,90 +200,105 @@ public: * requested change could not be made or was * redundant. */ - int set_hil_enabled(bool hil_enabled); + int set_hil_enabled(bool hil_enabled); - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); - int get_instance_id(); + int get_instance_id(); /** * Enable / disable hardware flow control. * * @param enabled True if hardware flow control should be enabled */ - int enable_flow_control(bool enabled); + int enable_flow_control(bool enabled); - mavlink_channel_t get_channel(); + const mavlink_channel_t get_channel(); + + void configure_stream_threadsafe(const char *stream_name, const float rate); - bool _task_should_exit; /**< if true, mavlink task should exit */ + bool _task_should_exit; /**< if true, mavlink task should exit */ - int get_mavlink_fd() { return _mavlink_fd; } + int get_mavlink_fd() { return _mavlink_fd; } + + MavlinkStream * get_streams() { return _streams; } const /* Functions for waiting to start transmission until message received. */ - void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } - bool get_has_received_messages() { return _received_messages; } - void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } - bool get_wait_to_transmit() { return _wait_to_transmit; } - bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + + bool message_buffer_write(void *ptr, int size); + + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } + + /** + * Count a transmision error + */ + void count_txerr(); protected: - Mavlink *next; + Mavlink *next; private: - int _instance_id; + int _instance_id; - int _mavlink_fd; - bool _task_running; + int _mavlink_fd; + bool _task_running; /* states */ - bool _hil_enabled; /**< Hardware In the Loop mode */ - bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ - bool _is_usb_uart; /**< Port is USB */ - bool _wait_to_transmit; /**< Wait to transmit until received messages. */ - bool _received_messages; /**< Whether we've received valid mavlink messages. */ + bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ + bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ - unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ + unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ - MavlinkOrbSubscription *_subscriptions; - MavlinkStream *_streams; + MavlinkOrbSubscription *_subscriptions; + MavlinkStream *_streams; - orb_advert_t _mission_pub; - struct mission_s mission; - MAVLINK_MODE _mode; + orb_advert_t _mission_pub; + struct mission_s mission; + MAVLINK_MODE _mode; - uint8_t _mavlink_wpm_comp_id; - mavlink_channel_t _channel; + uint8_t _mavlink_wpm_comp_id; + mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; - unsigned int _total_counter; + unsigned int _total_counter; - pthread_t _receive_thread; + pthread_t _receive_thread; /* Allocate storage space for waypoints */ - mavlink_wpm_storage _wpm_s; - mavlink_wpm_storage *_wpm; + mavlink_wpm_storage _wpm_s; + mavlink_wpm_storage *_wpm; - bool _verbose; - bool _forwarding_on; - bool _passing_on; - int _uart_fd; - int _baudrate; - int _datarate; + bool _verbose; + bool _forwarding_on; + bool _passing_on; + bool _ftp_on; + int _uart_fd; + int _baudrate; + int _datarate; /** * If the queue index is not at 0, the queue sending * logic will send parameters from the current index * to len - 1, the end of the param list. */ - unsigned int _mavlink_param_queue_index; + unsigned int _mavlink_param_queue_index; - bool mavlink_link_termination_allowed; + bool mavlink_link_termination_allowed; - char *_subscribe_to_stream; - float _subscribe_to_stream_rate; + char *_subscribe_to_stream; + float _subscribe_to_stream_rate; - bool _flow_control_enabled; + bool _flow_control_enabled; struct mavlink_message_buffer { int write_ptr; @@ -277,11 +306,14 @@ private: int size; char *data; }; + mavlink_message_buffer _message_buffer; pthread_mutex_t _message_buffer_mutex; - perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _txerr_perf; /**< TX error counter */ + bool _param_initialized; param_t _param_system_id; param_t _param_component_id; @@ -294,7 +326,7 @@ private: * @param param The parameter id to send. * @return zero on success, nonzero on failure. */ - int mavlink_pm_send_param(param_t param); + int mavlink_pm_send_param(param_t param); /** * Send one parameter identified by index. @@ -302,7 +334,7 @@ private: * @param index The index of the parameter to send. * @return zero on success, nonzero else. */ - int mavlink_pm_send_param_for_index(uint16_t index); + int mavlink_pm_send_param_for_index(uint16_t index); /** * Send one parameter identified by name. @@ -310,14 +342,14 @@ private: * @param name The index of the parameter to send. * @return zero on success, nonzero else. */ - int mavlink_pm_send_param_for_name(const char *name); + int mavlink_pm_send_param_for_name(const char *name); /** * Send a queue of parameters, one parameter per function call. * * @return zero on success, nonzero on failure */ - int mavlink_pm_queued_send(void); + int mavlink_pm_queued_send(void); /** * Start sending the parameter queue. @@ -327,12 +359,12 @@ private: * mavlink_pm_queued_send(). * @see mavlink_pm_queued_send() */ - void mavlink_pm_start_queued_send(); + void mavlink_pm_start_queued_send(); - void mavlink_update_system(); + void mavlink_update_system(); - void mavlink_waypoint_eventloop(uint64_t now); - void mavlink_wpm_send_waypoint_reached(uint16_t seq); + void mavlink_waypoint_eventloop(uint64_t now); + void mavlink_wpm_send_waypoint_reached(uint16_t seq); void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); @@ -349,7 +381,6 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); int configure_stream(const char *stream_name, const float rate); - void configure_stream_threadsafe(const char *stream_name, const float rate); int message_buffer_init(int size); @@ -359,8 +390,6 @@ private: int message_buffer_is_empty(); - bool message_buffer_write(void *ptr, int size); - int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fef7a5c89..e1ebc16cc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -232,6 +232,11 @@ public: return "HEARTBEAT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HEARTBEAT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamHeartbeat(); @@ -254,8 +259,15 @@ protected: struct position_setpoint_triplet_s pos_sp_triplet; /* always send the heartbeat, independent of the update status of the topics */ - (void)status_sub->update(&status); - (void)pos_sp_triplet_sub->update(&pos_sp_triplet); + if (!status_sub->update(&status)) { + /* if topic update failed fill it with defaults */ + memset(&status, 0, sizeof(status)); + } + + if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) { + /* if topic update failed fill it with defaults */ + memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + } uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; @@ -285,6 +297,11 @@ public: return "SYS_STATUS"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_SYS_STATUS; + } + static MavlinkStream *new_instance() { return new MavlinkStreamSysStatus(); @@ -336,6 +353,11 @@ public: return "HIGHRES_IMU"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIGHRES_IMU; + } + static MavlinkStream *new_instance() { return new MavlinkStreamHighresIMU(); @@ -421,6 +443,11 @@ public: return "ATTITUDE"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE; + } + static MavlinkStream *new_instance() { return new MavlinkStreamAttitude(); @@ -467,6 +494,11 @@ public: return "ATTITUDE_QUATERNION"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; + } + static MavlinkStream *new_instance() { return new MavlinkStreamAttitudeQuaternion(); @@ -519,6 +551,11 @@ public: return "VFR_HUD"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_VFR_HUD; + } + static MavlinkStream *new_instance() { return new MavlinkStreamVFRHUD(); @@ -602,6 +639,11 @@ public: return "GPS_RAW_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_RAW_INT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGPSRawInt(); @@ -655,6 +697,11 @@ public: return "GLOBAL_POSITION_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGlobalPositionInt(); @@ -716,6 +763,11 @@ public: return "LOCAL_POSITION_NED"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED; + } + static MavlinkStream *new_instance() { return new MavlinkStreamLocalPositionNED(); @@ -767,6 +819,11 @@ public: return "VICON_POSITION_ESTIMATE"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + } + static MavlinkStream *new_instance() { return new MavlinkStreamViconPositionEstimate(); @@ -817,6 +874,11 @@ public: return "GPS_GLOBAL_ORIGIN"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGPSGlobalOrigin(); @@ -857,6 +919,11 @@ public: return MavlinkStreamServoOutputRaw<N>::get_name_static(); } + uint8_t get_id() + { + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + } + static const char *get_name_static() { switch (N) { @@ -934,6 +1001,11 @@ public: return "HIL_CONTROLS"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIL_CONTROLS; + } + static MavlinkStream *new_instance() { return new MavlinkStreamHILControls(); @@ -1071,6 +1143,11 @@ public: return "GLOBAL_POSITION_SETPOINT_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGlobalPositionSetpointInt(); @@ -1114,6 +1191,11 @@ public: return "LOCAL_POSITION_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamLocalPositionSetpoint(); @@ -1162,6 +1244,11 @@ public: return "ROLL_PITCH_YAW_THRUST_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamRollPitchYawThrustSetpoint(); @@ -1210,6 +1297,11 @@ public: return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); @@ -1258,6 +1350,11 @@ public: return "RC_CHANNELS_RAW"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_RC_CHANNELS_RAW; + } + static MavlinkStream *new_instance() { return new MavlinkStreamRCChannelsRaw(); @@ -1342,6 +1439,11 @@ public: return "MANUAL_CONTROL"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_MANUAL_CONTROL; + } + static MavlinkStream *new_instance() { return new MavlinkStreamManualControl(); @@ -1391,6 +1493,11 @@ public: return "OPTICAL_FLOW"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_OPTICAL_FLOW; + } + static MavlinkStream *new_instance() { return new MavlinkStreamOpticalFlow(); @@ -1439,6 +1546,11 @@ public: return "ATTITUDE_CONTROLS"; } + uint8_t get_id() + { + return 0; + } + static MavlinkStream *new_instance() { return new MavlinkStreamAttitudeControls(); @@ -1497,6 +1609,11 @@ public: return "NAMED_VALUE_FLOAT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamNamedValueFloat(); @@ -1545,6 +1662,11 @@ public: return "CAMERA_CAPTURE"; } + uint8_t get_id() + { + return 0; + } + static MavlinkStream *new_instance() { return new MavlinkStreamCameraCapture(); @@ -1590,6 +1712,11 @@ public: return "DISTANCE_SENSOR"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_DISTANCE_SENSOR; + } + static MavlinkStream *new_instance() { return new MavlinkStreamDistanceSensor(); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3b9bd207b..6aba72161 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -121,6 +121,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); memset(&hil_local_pos, 0, sizeof(hil_local_pos)); memset(&_control_mode, 0, sizeof(_control_mode)); + + // make sure the FTP server is started + (void)MavlinkFTP::getServer(); } MavlinkReceiver::~MavlinkReceiver() @@ -163,6 +166,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_heartbeat(msg); break; + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: + handle_message_request_data_stream(msg); + break; + + case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + MavlinkFTP::getServer()->handle_message(_mavlink, msg); + break; + default: break; } @@ -469,6 +480,24 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) } void +MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) +{ + mavlink_request_data_stream_t req; + mavlink_msg_request_data_stream_decode(msg, &req); + + if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) { + float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f; + + MavlinkStream *stream; + LL_FOREACH(_mavlink->get_streams(), stream) { + if (req.req_stream_id == stream->get_id()) { + _mavlink->configure_stream_threadsafe(stream->get_name(), rate); + } + } + } +} + +void MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { mavlink_hil_sensor_t imu; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 5d9364fcc..65ef884a2 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -71,6 +71,8 @@ #include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> +#include "mavlink_ftp.h" + class Mavlink; class MavlinkReceiver @@ -116,6 +118,7 @@ private: void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); + void handle_message_request_data_stream(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index a41ace48e..69809a386 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -67,6 +67,7 @@ public: static const char *get_name_static(); virtual void subscribe(Mavlink *mavlink) = 0; virtual const char *get_name() const = 0; + virtual uint8_t get_id() = 0; protected: mavlink_channel_t _channel; diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f532e26fe..a4d8bfbfb 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -43,7 +43,8 @@ SRCS += mavlink_main.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ mavlink_rate_limiter.cpp \ - mavlink_commands.cpp + mavlink_commands.cpp \ + mavlink_ftp.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 19872bbd0..e483b35e8 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -976,7 +976,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; struct log_TELE_s log_TELE; - struct log_ESTM_s log_ESTM; + struct log_EST0_s log_EST0; + struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; struct log_GS0A_s log_GS0A; @@ -1489,15 +1490,21 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { - log_msg.msg_type = LOG_ESTM_MSG; - unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s); - memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s)); - memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy); - log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states; - log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan; - log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan; - log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan; - LOGBUFFER_WRITE_AND_COUNT(ESTM); + log_msg.msg_type = LOG_EST0_MSG; + unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s); + memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s)); + memcpy(&(log_msg.body.log_EST0.s), buf.estimator_status.states, maxcopy0); + log_msg.body.log_EST0.n_states = buf.estimator_status.n_states; + log_msg.body.log_EST0.nan_flags = buf.estimator_status.nan_flags; + log_msg.body.log_EST0.health_flags = buf.estimator_status.health_flags; + log_msg.body.log_EST0.timeout_flags = buf.estimator_status.timeout_flags; + LOGBUFFER_WRITE_AND_COUNT(EST0); + + log_msg.msg_type = LOG_EST1_MSG; + unsigned maxcopy1 = ((sizeof(buf.estimator_status.states) - maxcopy0) < sizeof(log_msg.body.log_EST1.s)) ? (sizeof(buf.estimator_status.states) - maxcopy0) : sizeof(log_msg.body.log_EST1.s); + memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s)); + memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1); + LOGBUFFER_WRITE_AND_COUNT(EST1); } /* --- TECS STATUS --- */ @@ -1507,6 +1514,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; + log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5645ebcf1..8c05e87c5 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -288,15 +288,7 @@ struct log_TELE_s { uint8_t txbuf; }; -/* --- ESTM - ESTIMATOR STATUS --- */ -#define LOG_ESTM_MSG 23 -struct log_ESTM_s { - float s[10]; - uint8_t n_states; - uint8_t states_nan; - uint8_t covariance_nan; - uint8_t kalman_gain_nan; -}; +// ID 23 available /* --- PWR - ONBOARD POWER SYSTEM --- */ #define LOG_PWR_MSG 24 @@ -353,6 +345,7 @@ struct log_TECS_s { float altitude; float flightPathAngleSp; float flightPathAngle; + float flightPathAngleFiltered; float airspeedSp; float airspeed; float airspeedFiltered; @@ -376,6 +369,22 @@ struct log_WIND_s { float cov_y; }; +/* --- EST0 - ESTIMATOR STATUS --- */ +#define LOG_EST0_MSG 32 +struct log_EST0_s { + float s[12]; + uint8_t n_states; + uint8_t nan_flags; + uint8_t health_flags; + uint8_t timeout_flags; +}; + +/* --- EST1 - ESTIMATOR STATUS --- */ +#define LOG_EST1_MSG 33 +struct log_EST1_s { + float s[16]; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -424,14 +433,15 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), - LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"), + LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), + LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index dad81d8e1..5deb471d6 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -243,6 +243,36 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** + * Board rotation Y (Pitch) offset + * + * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration + */ + PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); + +/** + * Board rotation X (Roll) offset + * + * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); + +/** + * Board rotation Z (YAW) offset + * + * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); + +/** * External magnetometer rotation * * This parameter defines the rotation of the external magnetometer relative diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 1a1d153f7..cc96f8a53 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -229,7 +229,7 @@ private: math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ - + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -252,6 +252,8 @@ private: int board_rotation; int external_mag_rotation; + + float board_offset[3]; int rc_map_roll; int rc_map_pitch; @@ -346,6 +348,8 @@ private: param_t board_rotation; param_t external_mag_rotation; + + param_t board_offset[3]; } _parameter_handles; /**< handles for interesting parameters */ @@ -594,6 +598,11 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); + + /* rotation offsets */ + _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); + _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); + _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); /* fetch initial parameter values */ parameters_update(); @@ -806,6 +815,18 @@ Sensors::parameters_update() get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); + + param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); + param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); + param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); + + /** fine tune board offset on parameter update **/ + math::Matrix<3, 3> board_rotation_offset; + board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], + M_DEG_TO_RAD_F * _parameters.board_offset[1], + M_DEG_TO_RAD_F * _parameters.board_offset[2]); + + _board_rotation = _board_rotation * board_rotation_offset; return OK; } diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 6835ee4a2..668d9dfdf 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -75,7 +75,7 @@ __EXPORT extern void perf_free(perf_counter_t handle); /** * Count a performance event. * - * This call only affects counters that take single events; PC_COUNT etc. + * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc. * * @param handle The handle returned from perf_alloc. */ diff --git a/src/modules/uORB/topics/estimator_status.h b/src/modules/uORB/topics/estimator_status.h index 5530cdb21..7f26b505b 100644 --- a/src/modules/uORB/topics/estimator_status.h +++ b/src/modules/uORB/topics/estimator_status.h @@ -64,9 +64,9 @@ struct estimator_status_report { uint64_t timestamp; /**< Timestamp in microseconds since boot */ float states[32]; /**< Internal filter states */ float n_states; /**< Number of states effectively used */ - bool states_nan; /**< If set to true, one of the states is NaN */ - bool covariance_nan; /**< If set to true, the covariance matrix went NaN */ - bool kalman_gain_nan; /**< If set to true, the Kalman gain matrix went NaN */ + uint8_t nan_flags; /**< Bitmask to indicate NaN states */ + uint8_t health_flags; /**< Bitmask to indicate sensor health states (vel, pos, hgt) */ + uint8_t timeout_flags; /**< Bitmask to indicate timeout flags (vel, pos, hgt) */ }; diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h index 05310e906..c4d0c1874 100644 --- a/src/modules/uORB/topics/tecs_status.h +++ b/src/modules/uORB/topics/tecs_status.h @@ -68,6 +68,7 @@ struct tecs_status_s { float altitude; float flightPathAngleSp; float flightPathAngle; + float flightPathAngleFiltered; float airspeedSp; float airspeed; float airspeedFiltered; |