From edd16afead95bbf236d974ad895e10cc2ef70033 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Apr 2014 15:49:29 +0200 Subject: [PATCH 01/58] Add filter parameters and multicopter defaults to parametrize Pauls estimator correctly when running for multicopters. Estimator itself not updated yet, will be next step. --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 6 + .../fw_att_pos_estimator_main.cpp | 44 ++++++ .../fw_att_pos_estimator_params.c | 131 ++++++++++++++++++ 3 files changed, 181 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 4db62607a9..c1f9db7d1c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -35,6 +35,12 @@ then param set MPC_TILT_MAX 1.0 param set MPC_LAND_SPEED 1.0 param set MPC_LAND_TILT 0.3 + + param set PE_VELNE_NOISE 0.5 + param set PE_VELNE_NOISE 0.7 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 1.0 + fi set PWM_RATE 400 diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 840cd585ec..96db3f20cc 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -199,6 +199,17 @@ private: int32_t height_delay_ms; int32_t mag_delay_ms; int32_t tas_delay_ms; + float velne_noise; + float veld_noise; + float posne_noise; + float posd_noise; + float mag_noise; + float gyro_pnoise; + float acc_pnoise; + float gbias_pnoise; + float abias_pnoise; + float mage_pnoise; + float magb_pnoise; } _parameters; /**< local copies of interesting parameters */ struct { @@ -207,6 +218,17 @@ private: param_t height_delay_ms; param_t mag_delay_ms; param_t tas_delay_ms; + param_t velne_noise; + param_t veld_noise; + param_t posne_noise; + param_t posd_noise; + param_t mag_noise; + param_t gyro_pnoise; + param_t acc_pnoise; + param_t gbias_pnoise; + param_t abias_pnoise; + param_t mage_pnoise; + param_t magb_pnoise; } _parameter_handles; /**< handles for interesting parameters */ AttPosEKF *_ekf; @@ -302,6 +324,17 @@ FixedwingEstimator::FixedwingEstimator() : _parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS"); _parameter_handles.mag_delay_ms = param_find("PE_MAG_DELAY_MS"); _parameter_handles.tas_delay_ms = param_find("PE_TAS_DELAY_MS"); + _parameter_handles.velne_noise = param_find("PE_VELNE_NOISE"); + _parameter_handles.veld_noise = param_find("PE_VELD_NOISE"); + _parameter_handles.posne_noise = param_find("PE_POSNE_NOISE"); + _parameter_handles.posd_noise = param_find("PE_POSD_NOISE"); + _parameter_handles.mag_noise = param_find("PE_MAG_NOISE"); + _parameter_handles.gyro_pnoise = param_find("PE_GYRO_PNOISE"); + _parameter_handles.acc_pnoise = param_find("PE_ACC_PNOISE"); + _parameter_handles.gbias_pnoise = param_find("PE_GBIAS_PNOISE"); + _parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE"); + _parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE"); + _parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE"); /* fetch initial parameter values */ parameters_update(); @@ -366,6 +399,17 @@ FixedwingEstimator::parameters_update() param_get(_parameter_handles.height_delay_ms, &(_parameters.height_delay_ms)); param_get(_parameter_handles.mag_delay_ms, &(_parameters.mag_delay_ms)); param_get(_parameter_handles.tas_delay_ms, &(_parameters.tas_delay_ms)); + param_get(_parameter_handles.velne_noise, &(_parameters.velne_noise)); + param_get(_parameter_handles.veld_noise, &(_parameters.veld_noise)); + param_get(_parameter_handles.posne_noise, &(_parameters.posne_noise)); + param_get(_parameter_handles.posd_noise, &(_parameters.posd_noise)); + param_get(_parameter_handles.mag_noise, &(_parameters.mag_noise)); + param_get(_parameter_handles.gyro_pnoise, &(_parameters.gyro_pnoise)); + param_get(_parameter_handles.acc_pnoise, &(_parameters.acc_pnoise)); + param_get(_parameter_handles.gbias_pnoise, &(_parameters.gbias_pnoise)); + param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise)); + param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise)); + param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise)); return OK; } diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c index 6138ef39cd..9d01a095c8 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c @@ -115,3 +115,134 @@ PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210); */ PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f); +/** + * Velocity noise in north-east (horizontal) direction. + * + * Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5 + * + * @min 0.05 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f); + +/** + * Velocity noise in down (vertical) direction + * + * Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7 + * + * @min 0.05 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f); + +/** + * Position noise in north-east (horizontal) direction + * + * Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f); + +/** + * Position noise in down (vertical) direction + * + * Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f); + +/** + * Magnetometer measurement noise + * + * Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f); + +/** + * Gyro process noise + * + * Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015. + * This noise controls how much the filter trusts the gyro measurements. + * Increasing it makes the filter trust the gyro less and other sensors more. + * + * @min 0.001 + * @max 0.05 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f); + +/** + * Accelerometer process noise + * + * Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25. + * Increasing this value makes the filter trust the accelerometer less + * and other sensors more. + * + * @min 0.05 + * @max 1.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f); + +/** + * Gyro bias estimate process noise + * + * Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f. + * Increasing this value will make the gyro bias converge faster but noisier. + * + * @min 0.0000001 + * @max 0.00001 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); + +/** + * Accelerometer bias estimate process noise + * + * Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f. + * Increasing this value makes the bias estimation faster and noisier. + * + * @min 0.0001 + * @max 0.001 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0001f); + +/** + * Magnetometer earth frame offsets process noise + * + * Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001. + * Increasing this value makes the magnetometer earth bias estimate converge + * faster but also noisier. + * + * @min 0.0001 + * @max 0.01 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f); + +/** + * Magnetometer body frame offsets process noise + * + * Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003. + * Increasing this value makes the magnetometer body bias estimate converge faster + * but also noisier. + * + * @min 0.0001 + * @max 0.01 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f); + From 7b61c927f0420cb3b519972221654176e7c9274b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Apr 2014 16:13:13 +0200 Subject: [PATCH 02/58] Renamed FW filter to EKF to express its generic properties, switched multicopters over to this filter for first tests. --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_apps | 3 ++- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 2 +- .../estimator.cpp | 0 .../estimator.h | 0 .../fw_att_pos_estimator_main.cpp | 6 +++--- .../fw_att_pos_estimator_params.c | 0 .../module.mk | 2 +- 9 files changed, 9 insertions(+), 8 deletions(-) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/estimator.cpp (100%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/estimator.h (100%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/fw_att_pos_estimator_main.cpp (99%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/fw_att_pos_estimator_params.c (100%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/module.mk (97%) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 429abc5ec8..9aca3fc5f4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -6,7 +6,7 @@ # # Start the attitude and position estimator # -fw_att_pos_estimator start +ekf_att_pos_estimator start # # Start attitude controller diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index ed3939757a..c75281fcdd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -5,7 +5,8 @@ # attitude_estimator_ekf start -position_estimator_inav start +ekf_att_pos_estimator start +#position_estimator_inav start mc_att_control start mc_pos_control start diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 532e978d04..1daf8277ea 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -70,7 +70,7 @@ MODULES += modules/gpio_led # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/fw_att_pos_estimator +MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav #MODULES += examples/flow_position_estimator diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13421acc5..7f0c59515a 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -79,7 +79,7 @@ MODULES += modules/gpio_led # MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3 -MODULES += modules/fw_att_pos_estimator +MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp similarity index 100% rename from src/modules/fw_att_pos_estimator/estimator.cpp rename to src/modules/ekf_att_pos_estimator/estimator.cpp diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h similarity index 100% rename from src/modules/fw_att_pos_estimator/estimator.h rename to src/modules/ekf_att_pos_estimator/estimator.h diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp similarity index 99% rename from src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp rename to src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 96db3f20cc..7857a04693 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -90,7 +90,7 @@ * * @ingroup apps */ -extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]); +extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]); __EXPORT uint32_t millis(); @@ -1255,10 +1255,10 @@ int FixedwingEstimator::trip_nan() { return ret; } -int fw_att_pos_estimator_main(int argc, char *argv[]) +int ekf_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 1) - errx(1, "usage: fw_att_pos_estimator {start|stop|status}"); + errx(1, "usage: ekf_att_pos_estimator {start|stop|status}"); if (!strcmp(argv[1], "start")) { diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c similarity index 100% rename from src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c rename to src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c diff --git a/src/modules/fw_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk similarity index 97% rename from src/modules/fw_att_pos_estimator/module.mk rename to src/modules/ekf_att_pos_estimator/module.mk index c992959e0d..30955d0ddc 100644 --- a/src/modules/fw_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -35,7 +35,7 @@ # Main Attitude and Position Estimator for Fixed Wing Aircraft # -MODULE_COMMAND = fw_att_pos_estimator +MODULE_COMMAND = ekf_att_pos_estimator SRCS = fw_att_pos_estimator_main.cpp \ fw_att_pos_estimator_params.c \ From dca1e7fc611bb44caf1fc586e45105d170955de2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Apr 2014 22:40:37 +0200 Subject: [PATCH 03/58] Decomission unmaintained position estimator --- .../att_pos_estimator_ekf/KalmanNav.cpp | 815 ------------------ .../att_pos_estimator_ekf/KalmanNav.hpp | 192 ----- .../att_pos_estimator_ekf/kalman_main.cpp | 157 ---- src/modules/att_pos_estimator_ekf/module.mk | 42 - src/modules/att_pos_estimator_ekf/params.c | 49 -- 5 files changed, 1255 deletions(-) delete mode 100644 src/modules/att_pos_estimator_ekf/KalmanNav.cpp delete mode 100644 src/modules/att_pos_estimator_ekf/KalmanNav.hpp delete mode 100644 src/modules/att_pos_estimator_ekf/kalman_main.cpp delete mode 100644 src/modules/att_pos_estimator_ekf/module.mk delete mode 100644 src/modules/att_pos_estimator_ekf/params.c diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp deleted file mode 100644 index 668bac5d9c..0000000000 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ /dev/null @@ -1,815 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 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. - * - ****************************************************************************/ - -/** - * @file KalmanNav.cpp - * - * Kalman filter navigation code - */ - -#include - -#include "KalmanNav.hpp" -#include -#include - -// constants -// Titterton pg. 52 -static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s -static const float R0 = 6378137.0f; // earth radius, m -static const float g0 = 9.806f; // standard gravitational accel. m/s^2 -static const int8_t ret_ok = 0; // no error in function -static const int8_t ret_error = -1; // error occurred - -KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - // subscriptions - _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz - _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz - _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz - // publications - _pos(&getPublications(), ORB_ID(vehicle_global_position)), - _localPos(&getPublications(), ORB_ID(vehicle_local_position)), - _att(&getPublications(), ORB_ID(vehicle_attitude)), - // timestamps - _pubTimeStamp(hrt_absolute_time()), - _predictTimeStamp(hrt_absolute_time()), - _attTimeStamp(hrt_absolute_time()), - _outTimeStamp(hrt_absolute_time()), - // frame count - _navFrames(0), - // miss counts - _miss(0), - // accelerations - fN(0), fE(0), fD(0), - // state - phi(0), theta(0), psi(0), - vN(0), vE(0), vD(0), - lat(0), lon(0), alt(0), - lat0(0), lon0(0), alt0(0), - // parameters for ground station - _vGyro(this, "V_GYRO"), - _vAccel(this, "V_ACCEL"), - _rMag(this, "R_MAG"), - _rGpsVel(this, "R_GPS_VEL"), - _rGpsPos(this, "R_GPS_POS"), - _rGpsAlt(this, "R_GPS_ALT"), - _rPressAlt(this, "R_PRESS_ALT"), - _rAccel(this, "R_ACCEL"), - _magDip(this, "ENV_MAG_DIP"), - _magDec(this, "ENV_MAG_DEC"), - _g(this, "ENV_G"), - _faultPos(this, "FAULT_POS"), - _faultAtt(this, "FAULT_ATT"), - _attitudeInitialized(false), - _positionInitialized(false), - _attitudeInitCounter(0) -{ - using namespace math; - - F.zero(); - G.zero(); - V.zero(); - HAtt.zero(); - RAtt.zero(); - HPos.zero(); - RPos.zero(); - - // initial state covariance matrix - P0.identity(); - P0 *= 0.01f; - P = P0; - - // initial state - phi = 0.0f; - theta = 0.0f; - psi = 0.0f; - vN = 0.0f; - vE = 0.0f; - vD = 0.0f; - lat = 0.0f; - lon = 0.0f; - alt = 0.0f; - - // initialize rotation quaternion with a single raw sensor measurement - _sensors.update(); - q = init( - _sensors.accelerometer_m_s2[0], - _sensors.accelerometer_m_s2[1], - _sensors.accelerometer_m_s2[2], - _sensors.magnetometer_ga[0], - _sensors.magnetometer_ga[1], - _sensors.magnetometer_ga[2]); - - // initialize dcm - C_nb = q.to_dcm(); - - // HPos is constant - HPos(0, 3) = 1.0f; - HPos(1, 4) = 1.0f; - HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F; - HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F; - HPos(4, 8) = 1.0f; - HPos(5, 8) = 1.0f; - - // initialize all parameters - updateParams(); -} - -math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz) -{ - 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); - - 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); - - float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; - float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; - float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; - float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; - - return math::Quaternion(q0, q1, q2, q3); - -} - -void KalmanNav::update() -{ - using namespace math; - - struct pollfd fds[1]; - fds[0].fd = _sensors.getHandle(); - fds[0].events = POLLIN; - - // poll for new data - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - // XXX this is seriously bad - should be an emergency - return; - - } else if (ret == 0) { // timeout - return; - } - - // get new timestamp - uint64_t newTimeStamp = hrt_absolute_time(); - - // check updated subscriptions - if (_param_update.updated()) updateParams(); - - bool gpsUpdate = _gps.updated(); - bool sensorsUpdate = _sensors.updated(); - - // get new information from subscriptions - // this clears update flag - updateSubscriptions(); - - // initialize attitude when sensors online - if (!_attitudeInitialized && sensorsUpdate) { - if (correctAtt() == ret_ok) _attitudeInitCounter++; - - if (_attitudeInitCounter > 100) { - warnx("initialized EKF attitude"); - warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f", - double(phi), double(theta), double(psi)); - _attitudeInitialized = true; - } - } - - // initialize position when gps received - if (!_positionInitialized && - _attitudeInitialized && // wait for attitude first - gpsUpdate && - _gps.fix_type > 2 - //&& _gps.counter_pos_valid > 10 - ) { - vN = _gps.vel_n_m_s; - vE = _gps.vel_e_m_s; - vD = _gps.vel_d_m_s; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); - // set reference position for - // local position - lat0 = lat; - lon0 = lon; - alt0 = alt; - // XXX map_projection has internal global - // states that multiple things could change, - // should make map_projection take reference - // lat/lon and not have init - map_projection_init(lat0, lon0); - _positionInitialized = true; - warnx("initialized EKF state with GPS"); - warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", - double(vN), double(vE), double(vD), - lat, lon, double(alt)); - } - - // prediction step - // using sensors timestamp so we can account for packet lag - float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; - //printf("dt: %15.10f\n", double(dt)); - _predictTimeStamp = _sensors.timestamp; - - // don't predict if time greater than a second - if (dt < 1.0f) { - predictState(dt); - predictStateCovariance(dt); - // count fast frames - _navFrames += 1; - } - - // count times 100 Hz rate isn't met - if (dt > 0.01f) _miss++; - - // gps correction step - if (_positionInitialized && gpsUpdate) { - correctPos(); - } - - // attitude correction step - if (_attitudeInitialized // initialized - && sensorsUpdate // new data - && _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz - ) { - _attTimeStamp = _sensors.timestamp; - correctAtt(); - } - - // publication - if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz - _pubTimeStamp = newTimeStamp; - - updatePublications(); - } - - // output - if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz - _outTimeStamp = newTimeStamp; - //printf("nav: %4d Hz, miss #: %4d\n", - // _navFrames / 10, _miss / 10); - _navFrames = 0; - _miss = 0; - } -} - -void KalmanNav::updatePublications() -{ - using namespace math; - - // global position publication - _pos.timestamp = _pubTimeStamp; - _pos.time_gps_usec = _gps.timestamp_position; - _pos.global_valid = true; - _pos.lat = lat * M_RAD_TO_DEG; - _pos.lon = lon * M_RAD_TO_DEG; - _pos.alt = float(alt); - _pos.vel_n = vN; - _pos.vel_e = vE; - _pos.vel_d = vD; - _pos.yaw = psi; - - // local position publication - float x; - float y; - bool landed = alt < (alt0 + 0.1); // XXX improve? - map_projection_project(lat, lon, &x, &y); - _localPos.timestamp = _pubTimeStamp; - _localPos.xy_valid = true; - _localPos.z_valid = true; - _localPos.v_xy_valid = true; - _localPos.v_z_valid = true; - _localPos.x = x; - _localPos.y = y; - _localPos.z = alt0 - alt; - _localPos.vx = vN; - _localPos.vy = vE; - _localPos.vz = vD; - _localPos.yaw = psi; - _localPos.xy_global = true; - _localPos.z_global = true; - _localPos.ref_timestamp = _pubTimeStamp; - _localPos.ref_lat = getLatDegE7(); - _localPos.ref_lon = getLonDegE7(); - _localPos.ref_alt = 0; - _localPos.landed = landed; - - // attitude publication - _att.timestamp = _pubTimeStamp; - _att.roll = phi; - _att.pitch = theta; - _att.yaw = psi; - _att.rollspeed = _sensors.gyro_rad_s[0]; - _att.pitchspeed = _sensors.gyro_rad_s[1]; - _att.yawspeed = _sensors.gyro_rad_s[2]; - // TODO, add gyro offsets to filter - _att.rate_offsets[0] = 0.0f; - _att.rate_offsets[1] = 0.0f; - _att.rate_offsets[2] = 0.0f; - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _att.R[i][j] = C_nb(i, j); - - for (int i = 0; i < 4; i++) _att.q[i] = q(i); - - _att.R_valid = true; - _att.q_valid = true; - - // selectively update publications, - // do NOT call superblock do-all method - if (_positionInitialized) { - _pos.update(); - _localPos.update(); - } - - if (_attitudeInitialized) - _att.update(); -} - -int KalmanNav::predictState(float dt) -{ - using namespace math; - - // trig - float sinL = sinf(lat); - float cosL = cosf(lat); - float cosLSing = cosf(lat); - - // prevent singularity - if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } - - // attitude prediction - if (_attitudeInitialized) { - Vector<3> w(_sensors.gyro_rad_s); - - // attitude - q = q + q.derivative(w) * dt; - - // renormalize quaternion if needed - if (fabsf(q.length() - 1.0f) > 1e-4f) { - q.normalize(); - } - - // C_nb update - C_nb = q.to_dcm(); - - // euler update - Vector<3> euler = C_nb.to_euler(); - phi = euler.data[0]; - theta = euler.data[1]; - psi = euler.data[2]; - - // specific acceleration in nav frame - Vector<3> accelB(_sensors.accelerometer_m_s2); - Vector<3> accelN = C_nb * accelB; - fN = accelN(0); - fE = accelN(1); - fD = accelN(2); - } - - // position prediction - if (_positionInitialized) { - // neglects angular deflections in local gravity - // see Titerton pg. 70 - float R = R0 + float(alt); - float LDot = vN / R; - float lDot = vE / (cosLSing * R); - float rotRate = 2 * omega + lDot; - - // XXX position prediction using speed - float vNDot = fN - vE * rotRate * sinL + - vD * LDot; - float vDDot = fD - vE * rotRate * cosL - - vN * LDot + _g.get(); - float vEDot = fE + vN * rotRate * sinL + - vDDot * rotRate * cosL; - - // rectangular integration - vN += vNDot * dt; - vE += vEDot * dt; - vD += vDDot * dt; - lat += double(LDot * dt); - lon += double(lDot * dt); - alt += double(-vD * dt); - } - - return ret_ok; -} - -int KalmanNav::predictStateCovariance(float dt) -{ - using namespace math; - - // trig - float sinL = sinf(lat); - float cosL = cosf(lat); - float cosLSq = cosL * cosL; - float tanL = tanf(lat); - - // prepare for matrix - float R = R0 + float(alt); - float RSq = R * R; - - // F Matrix - // Titterton pg. 291 - - F(0, 1) = -(omega * sinL + vE * tanL / R); - F(0, 2) = vN / R; - F(0, 4) = 1.0f / R; - F(0, 6) = -omega * sinL; - F(0, 8) = -vE / RSq; - - F(1, 0) = omega * sinL + vE * tanL / R; - F(1, 2) = omega * cosL + vE / R; - F(1, 3) = -1.0f / R; - F(1, 8) = vN / RSq; - - F(2, 0) = -vN / R; - F(2, 1) = -omega * cosL - vE / R; - F(2, 4) = -tanL / R; - F(2, 6) = -omega * cosL - vE / (R * cosLSq); - F(2, 8) = vE * tanL / RSq; - - F(3, 1) = -fD; - F(3, 2) = fE; - F(3, 3) = vD / R; - F(3, 4) = -2 * (omega * sinL + vE * tanL / R); - F(3, 5) = vN / R; - F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq)); - F(3, 8) = (vE * vE * tanL - vN * vD) / RSq; - - F(4, 0) = fD; - F(4, 2) = -fN; - F(4, 3) = 2 * omega * sinL + vE * tanL / R; - F(4, 4) = (vN * tanL + vD) / R; - F(4, 5) = 2 * omega * cosL + vE / R; - F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) + - vN * vE / (R * cosLSq); - F(4, 8) = -vE * (vN * tanL + vD) / RSq; - - F(5, 0) = -fE; - F(5, 1) = fN; - F(5, 3) = -2 * vN / R; - F(5, 4) = -2 * (omega * cosL + vE / R); - F(5, 6) = 2 * omega * vE * sinL; - F(5, 8) = (vN * vN + vE * vE) / RSq; - - F(6, 3) = 1 / R; - F(6, 8) = -vN / RSq; - - F(7, 4) = 1 / (R * cosL); - F(7, 6) = vE * tanL / (R * cosL); - F(7, 8) = -vE / (cosL * RSq); - - F(8, 5) = -1; - - // G Matrix - // Titterton pg. 291 - G(0, 0) = -C_nb(0, 0); - G(0, 1) = -C_nb(0, 1); - G(0, 2) = -C_nb(0, 2); - G(1, 0) = -C_nb(1, 0); - G(1, 1) = -C_nb(1, 1); - G(1, 2) = -C_nb(1, 2); - G(2, 0) = -C_nb(2, 0); - G(2, 1) = -C_nb(2, 1); - G(2, 2) = -C_nb(2, 2); - - G(3, 3) = C_nb(0, 0); - G(3, 4) = C_nb(0, 1); - G(3, 5) = C_nb(0, 2); - G(4, 3) = C_nb(1, 0); - G(4, 4) = C_nb(1, 1); - G(4, 5) = C_nb(1, 2); - G(5, 3) = C_nb(2, 0); - G(5, 4) = C_nb(2, 1); - G(5, 5) = C_nb(2, 2); - - // continuous prediction equations - // for discrete time EKF - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; - - return ret_ok; -} - -int KalmanNav::correctAtt() -{ - using namespace math; - - // trig - float cosPhi = cosf(phi); - float cosTheta = cosf(theta); - // float cosPsi = cosf(psi); - float sinPhi = sinf(phi); - float sinTheta = sinf(theta); - // float sinPsi = sinf(psi); - - // mag predicted measurement - // choosing some typical magnetic field properties, - // TODO dip/dec depend on lat/ lon/ time - //float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level - float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north - - // compensate roll and pitch, but not yaw - // XXX take the vectors out of the C_nb matrix to avoid singularities - math::Matrix<3,3> C_rp; - C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); - - // mag measurement - Vector<3> magBody(_sensors.magnetometer_ga); - - // transform to earth frame - Vector<3> magNav = C_rp * magBody; - - // calculate error between estimate and measurement - // apply declination correction for true heading as well. - float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec; - if (yMag > M_PI_F) yMag -= 2*M_PI_F; - if (yMag < -M_PI_F) yMag += 2*M_PI_F; - - // accel measurement - Vector<3> zAccel(_sensors.accelerometer_m_s2); - float accelMag = zAccel.length(); - zAccel.normalize(); - - // ignore accel correction when accel mag not close to g - Matrix<4,4> RAttAdjust = RAtt; - - bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; - - if (ignoreAccel) { - RAttAdjust(1, 1) = 1.0e10; - RAttAdjust(2, 2) = 1.0e10; - RAttAdjust(3, 3) = 1.0e10; - - } else { - //printf("correcting attitude with accel\n"); - } - - // accel predicted measurement - Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); - - // calculate residual - Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); - - // HMag - HAtt(0, 2) = 1; - - // HAccel - HAtt(1, 1) = cosTheta; - HAtt(2, 0) = -cosPhi * cosTheta; - HAtt(2, 1) = sinPhi * sinTheta; - HAtt(3, 0) = sinPhi * cosTheta; - HAtt(3, 1) = cosPhi * sinTheta; - - // compute correction - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance - Matrix<9, 4> K = P * HAtt.transposed() * S.inversed(); - Vector<9> xCorrect = K * y; - - // check correciton is sane - for (size_t i = 0; i < xCorrect.get_size(); i++) { - float val = xCorrect(i); - - if (isnan(val) || isinf(val)) { - // abort correction and return - warnx("numerical failure in att correction"); - // reset P matrix to P0 - P = P0; - return ret_error; - } - } - - // correct state - if (!ignoreAccel) { - phi += xCorrect(PHI); - theta += xCorrect(THETA); - } - - psi += xCorrect(PSI); - - // attitude also affects nav velocities - if (_positionInitialized) { - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); - } - - // update state covariance - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P - K * HAtt * P; - - // fault detection - float beta = y * (S.inversed() * y); - - if (beta > _faultAtt.get()) { - warnx("fault in attitude: beta = %8.4f", (double)beta); - warnx("y:"); y.print(); - } - - // update quaternions from euler - // angle correction - q.from_euler(phi, theta, psi); - - return ret_ok; -} - -int KalmanNav::correctPos() -{ - using namespace math; - - // residual - Vector<6> y; - y(0) = _gps.vel_n_m_s - vN; - y(1) = _gps.vel_e_m_s - vE; - y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; - y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG; - y(4) = _gps.alt / 1.0e3f - alt; - y(5) = _sensors.baro_alt_meter - alt; - - // compute correction - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance - Matrix<9,6> K = P * HPos.transposed() * S.inversed(); - Vector<9> xCorrect = K * y; - - // check correction is sane - for (size_t i = 0; i < xCorrect.get_size(); i++) { - float val = xCorrect(i); - - if (!isfinite(val)) { - // abort correction and return - warnx("numerical failure in gps correction"); - // fallback to GPS - vN = _gps.vel_n_m_s; - vE = _gps.vel_e_m_s; - vD = _gps.vel_d_m_s; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); - // reset P matrix to P0 - P = P0; - return ret_error; - } - } - - // correct state - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); - lat += double(xCorrect(LAT)); - lon += double(xCorrect(LON)); - alt += xCorrect(ALT); - - // update state covariance - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P - K * HPos * P; - - // fault detetcion - float beta = y * (S.inversed() * y); - - static int counter = 0; - if (beta > _faultPos.get() && (counter % 10 == 0)) { - warnx("fault in gps: beta = %8.4f", (double)beta); - warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f", - double(y(0) / sqrtf(RPos(0, 0))), - double(y(1) / sqrtf(RPos(1, 1))), - double(y(2) / sqrtf(RPos(2, 2))), - double(y(3) / sqrtf(RPos(3, 3))), - double(y(4) / sqrtf(RPos(4, 4))), - double(y(5) / sqrtf(RPos(5, 5)))); - } - counter++; - - return ret_ok; -} - -void KalmanNav::updateParams() -{ - using namespace math; - using namespace control; - SuperBlock::updateParams(); - - // gyro noise - V(0, 0) = _vGyro.get(); // gyro x, rad/s - V(1, 1) = _vGyro.get(); // gyro y - V(2, 2) = _vGyro.get(); // gyro z - - // accel noise - V(3, 3) = _vAccel.get(); // accel x, m/s^2 - V(4, 4) = _vAccel.get(); // accel y - V(5, 5) = _vAccel.get(); // accel z - - // magnetometer noise - float noiseMin = 1e-6f; - float noiseMagSq = _rMag.get() * _rMag.get(); - - if (noiseMagSq < noiseMin) noiseMagSq = noiseMin; - - RAtt(0, 0) = noiseMagSq; // normalized direction - - // accelerometer noise - float noiseAccelSq = _rAccel.get() * _rAccel.get(); - - // bound noise to prevent singularities - if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin; - - RAtt(1, 1) = noiseAccelSq; // normalized direction - RAtt(2, 2) = noiseAccelSq; - RAtt(3, 3) = noiseAccelSq; - - // gps noise - float R = R0 + float(alt); - float cosLSing = cosf(lat); - - // prevent singularity - if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } - - float noiseVel = _rGpsVel.get(); - float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; - float noiseLonDegE7 = noiseLatDegE7 / cosLSing; - float noiseGpsAlt = _rGpsAlt.get(); - float noisePressAlt = _rPressAlt.get(); - - // bound noise to prevent singularities - if (noiseVel < noiseMin) noiseVel = noiseMin; - - if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin; - - if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin; - - if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin; - - if (noisePressAlt < noiseMin) noisePressAlt = noiseMin; - - RPos(0, 0) = noiseVel * noiseVel; // vn - RPos(1, 1) = noiseVel * noiseVel; // ve - RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat - RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon - RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h - RPos(5, 5) = noisePressAlt * noisePressAlt; // h - // XXX, note that RPos depends on lat, so updateParams should - // be called if lat changes significantly -} diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp deleted file mode 100644 index caf93bc787..0000000000 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ /dev/null @@ -1,192 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 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. - * - ****************************************************************************/ - -/** - * @file KalmanNav.hpp - * - * kalman filter navigation code - */ - -#pragma once - -//#define MATRIX_ASSERT -//#define VECTOR_ASSERT - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -/** - * Kalman filter navigation class - * http://en.wikipedia.org/wiki/Extended_Kalman_filter - * Discrete-time extended Kalman filter - */ -class KalmanNav : public control::SuperBlock -{ -public: - /** - * Constructor - */ - KalmanNav(SuperBlock *parent, const char *name); - - /** - * Deconstuctor - */ - - virtual ~KalmanNav() {}; - - math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz); - - /** - * The main callback function for the class - */ - void update(); - - - /** - * Publication update - */ - virtual void updatePublications(); - - /** - * State prediction - * Continuous, non-linear - */ - int predictState(float dt); - - /** - * State covariance prediction - * Continuous, linear - */ - int predictStateCovariance(float dt); - - /** - * Attitude correction - */ - int correctAtt(); - - /** - * Position correction - */ - int correctPos(); - - /** - * Overloaded update parameters - */ - virtual void updateParams(); -protected: - // kalman filter - math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ - math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */ - math::Matrix<9,9> P; /**< state covariance matrix */ - math::Matrix<9,9> P0; /**< initial state covariance matrix */ - math::Matrix<6,6> V; /**< gyro/ accel noise matrix */ - math::Matrix<4,9> HAtt; /**< attitude measurement matrix */ - math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */ - math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */ - math::Matrix<6,6> RPos; /**< position measurement noise matrix */ - // attitude - math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ - math::Quaternion q; /**< quaternion from body to nav frame */ - // subscriptions - uORB::Subscription _sensors; /**< sensors sub. */ - uORB::Subscription _gps; /**< gps sub. */ - uORB::Subscription _param_update; /**< parameter update sub. */ - // publications - uORB::Publication _pos; /**< position pub. */ - uORB::Publication _localPos; /**< local position pub. */ - uORB::Publication _att; /**< attitude pub. */ - // time stamps - uint64_t _pubTimeStamp; /**< output data publication time stamp */ - uint64_t _predictTimeStamp; /**< prediction time stamp */ - uint64_t _attTimeStamp; /**< attitude correction time stamp */ - uint64_t _outTimeStamp; /**< output time stamp */ - // frame count - uint16_t _navFrames; /**< navigation frames completed in output cycle */ - // miss counts - uint16_t _miss; /**< number of times fast prediction loop missed */ - // accelerations - float fN, fE, fD; /**< navigation frame acceleration */ - // states - enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */ - float phi, theta, psi; /**< 3-2-1 euler angles */ - float vN, vE, vD; /**< navigation velocity, m/s */ - double lat, lon; /**< lat, lon radians */ - // parameters - float alt; /**< altitude, meters */ - double lat0, lon0; /**< reference latitude and longitude */ - float alt0; /**< refeerence altitude (ground height) */ - control::BlockParamFloat _vGyro; /**< gyro process noise */ - control::BlockParamFloat _vAccel; /**< accelerometer process noise */ - control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ - control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */ - control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */ - control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */ - control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */ - control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */ - control::BlockParamFloat _magDip; /**< magnetic inclination with level */ - control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */ - control::BlockParamFloat _g; /**< gravitational constant */ - control::BlockParamFloat _faultPos; /**< fault detection threshold for position */ - control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */ - // status - bool _attitudeInitialized; - bool _positionInitialized; - uint16_t _attitudeInitCounter; - // accessors - int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } - void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; } - int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); } - void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; } - int32_t getAltE3() { return int32_t(alt * 1.0e3); } - void setAltE3(int32_t val) { alt = double(val) / 1.0e3; } -}; diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp deleted file mode 100644 index 3d20d4d2d6..0000000000 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: James Goppert - * - * 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. - * - ****************************************************************************/ - -/** - * @file kalman_main.cpp - * Combined attitude / position estimator. - * - * @author James Goppert - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "KalmanNav.hpp" - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ - -/** - * Deamon management function. - */ -extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int kalman_demo_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p ]"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int att_pos_estimator_ekf_main(int argc, char *argv[]) -{ - - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - - daemon_task = task_spawn_cmd("att_pos_estimator_ekf", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 30, - 8192, - kalman_demo_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("is running\n"); - exit(0); - - } else { - warnx("not started\n"); - exit(1); - } - - } - - usage("unrecognized command"); - exit(1); -} - -int kalman_demo_thread_main(int argc, char *argv[]) -{ - - warnx("starting"); - - using namespace math; - - thread_running = true; - - KalmanNav nav(NULL, "KF"); - - while (!thread_should_exit) { - nav.update(); - } - - warnx("exiting."); - - thread_running = false; - - return 0; -} diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/modules/att_pos_estimator_ekf/module.mk deleted file mode 100644 index 8d4a40d950..0000000000 --- a/src/modules/att_pos_estimator_ekf/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 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. -# -############################################################################ - -# -# Full attitude / position Extended Kalman Filter -# - -MODULE_COMMAND = att_pos_estimator_ekf - -SRCS = kalman_main.cpp \ - KalmanNav.cpp \ - params.c diff --git a/src/modules/att_pos_estimator_ekf/params.c b/src/modules/att_pos_estimator_ekf/params.c deleted file mode 100644 index 4af5edeade..0000000000 --- a/src/modules/att_pos_estimator_ekf/params.c +++ /dev/null @@ -1,49 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 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 - -/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f); -PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f); -PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f); -PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f); -PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f); -PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); -PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); -PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); -PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); From ce56d75bc606015728f59a3e811fa48ff9db2979 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 01:37:31 +0200 Subject: [PATCH 04/58] Updated filter to most recent version with accel scale estimation, exposed crucial parameters for cross-vehicle support --- .../ekf_att_pos_estimator/estimator.cpp | 1610 +++++++++++------ src/modules/ekf_att_pos_estimator/estimator.h | 120 +- .../fw_att_pos_estimator_main.cpp | 25 +- .../fw_att_pos_estimator_params.c | 14 +- 4 files changed, 1188 insertions(+), 581 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index c312173938..ebe30cae0e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1,5 +1,5 @@ #include "estimator.h" - +//#include #include float Vector3f::length(void) const @@ -69,6 +69,25 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn) return vecOut; } +// overload * operator to provide a matrix product +Mat3f operator*( Mat3f matIn1, Mat3f matIn2) +{ + Mat3f matOut; + matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x; + matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y; + matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z; + + matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x; + matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y; + matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z; + + matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x; + matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y; + matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z; + + return matOut; +} + // overload % operator to provide a vector cross product Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) { @@ -109,7 +128,6 @@ void swap_var(float &d1, float &d2) AttPosEKF::AttPosEKF() : fusionModeGPS(0), covSkipCount(0), - EAS2TAS(1.0f), statesInitialised(false), fuseVelData(false), fusePosData(false), @@ -120,10 +138,11 @@ AttPosEKF::AttPosEKF() : staticMode(true), useAirspeed(true), useCompass(true), + useRangeFinder(true), numericalProtection(true), storeIndex(0) { - + InitialiseParameters(); } AttPosEKF::~AttPosEKF() @@ -143,8 +162,6 @@ void AttPosEKF::UpdateStrapdownEquationsNED() float q12; float q13; float q23; - Mat3f Tbn; - Mat3f Tnb; float rotationMag; float qUpdated[4]; float quatMag; @@ -157,7 +174,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() correctedDelAng.z = dAngIMU.z - states[12]; dVelIMU.x = dVelIMU.x; dVelIMU.y = dVelIMU.y; - dVelIMU.z = dVelIMU.z; + dVelIMU.z = dVelIMU.z - states[13]; // Save current measurements Vector3f prevDelAng = correctedDelAng; @@ -177,8 +194,8 @@ void AttPosEKF::UpdateStrapdownEquationsNED() } else { - deltaQuat[0] = cos(0.5f*rotationMag); - float rotScaler = (sin(0.5f*rotationMag))/rotationMag; + deltaQuat[0] = cosf(0.5f*rotationMag); + float rotScaler = (sinf(0.5f*rotationMag))/rotationMag; deltaQuat[1] = correctedDelAng.x*rotScaler; deltaQuat[2] = correctedDelAng.y*rotScaler; deltaQuat[3] = correctedDelAng.z*rotScaler; @@ -260,11 +277,6 @@ void AttPosEKF::UpdateStrapdownEquationsNED() void AttPosEKF::CovariancePrediction(float dt) { // scalars - float windVelSigma; - float dAngBiasSigma; - // float dVelBiasSigma; - float magEarthSigma; - float magBodySigma; float daxCov; float dayCov; float dazCov; @@ -284,28 +296,29 @@ void AttPosEKF::CovariancePrediction(float dt) float dax_b; float day_b; float daz_b; + float dvz_b; // arrays - float processNoise[21]; - float SF[14]; + float processNoise[n_states]; + float SF[15]; float SG[8]; float SQ[11]; - float SPP[13] = {0}; - float nextP[21][21]; + float SPP[8] = {0}; + float nextP[n_states][n_states]; // calculate covariance prediction process noise - const float yawVarScale = 1.0f; - 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]); + for (uint8_t i= 0; i<=9; 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; + processNoise[13] = dVelBiasSigma; + for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; + for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; + for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; + processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; + + // square all sigmas + for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); // set variables used to calculate covariance growth dvx = summedDelVel.x; @@ -321,29 +334,33 @@ void AttPosEKF::CovariancePrediction(float dt) 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); + dvz_b = states[13]; + gyroProcessNoise = ConstrainFloat(gyroProcessNoise, 1e-3f, 5e-2f); + daxCov = sq(dt*gyroProcessNoise); + dayCov = sq(dt*gyroProcessNoise); + dazCov = sq(dt*gyroProcessNoise); if (onGround) dazCov = dazCov * sq(yawVarScale); - dvxCov = sq(dt*0.5f); - dvyCov = sq(dt*0.5f); - dvzCov = sq(dt*0.5f); + accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f); + dvxCov = sq(dt*accelProcessNoise); + dvyCov = sq(dt*accelProcessNoise); + dvzCov = sq(dt*accelProcessNoise); // 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; + SF[0] = dvz - dvz_b; + SF[1] = 2*q3*SF[0] + 2*dvx*q1 + 2*dvy*q2; + SF[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; + SF[3] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3; + SF[4] = day/2 - day_b/2; + SF[5] = daz/2 - daz_b/2; + SF[6] = dax/2 - dax_b/2; + SF[7] = dax_b/2 - dax/2; + SF[8] = daz_b/2 - daz/2; + SF[9] = day_b/2 - day/2; + SF[10] = 2*q0*SF[0]; + SF[11] = q1/2; + SF[12] = q2/2; + SF[13] = q3/2; + SF[14] = 2*dvy*q1; SG[0] = q0/2; SG[1] = sq(q3); @@ -366,169 +383,183 @@ void AttPosEKF::CovariancePrediction(float dt) 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]; + SPP[0] = SF[10] + SF[14] - 2*dvx*q2; + SPP[1] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3; + SPP[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; + SPP[3] = 2*q0*q1 - 2*q2*q3; + SPP[4] = 2*q0*q2 + 2*q1*q3; + SPP[5] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SPP[6] = SF[13]; + SPP[7] = SF[12]; - nextP[0][0] = P[0][0] + P[1][0]*SF[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[0][0] = P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6] + (daxCov*SQ[10])/4 + SF[7]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[9]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[8]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) + SPP[7]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[6]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; + nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6] + SF[6]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[5]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[9]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[6]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) - SPP[7]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - (q0*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]))/2; + nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6] + SF[4]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[8]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[6]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - SPP[6]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]))/2; + nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6] + SF[5]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[4]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[7]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SF[11]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[7]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]))/2; + nextP[0][4] = P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6] + SF[3]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[0]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SPP[2]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[4]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][5] = P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6] + SF[2]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[3]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[0]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[3]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][6] = P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6] + SF[2]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[1]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[0]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) - SPP[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][7] = P[0][7] + P[1][7]*SF[7] + P[2][7]*SF[9] + P[3][7]*SF[8] + P[10][7]*SF[11] + P[11][7]*SPP[7] + P[12][7]*SPP[6] + dt*(P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6]); + nextP[0][8] = P[0][8] + P[1][8]*SF[7] + P[2][8]*SF[9] + P[3][8]*SF[8] + P[10][8]*SF[11] + P[11][8]*SPP[7] + P[12][8]*SPP[6] + dt*(P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6]); + nextP[0][9] = P[0][9] + P[1][9]*SF[7] + P[2][9]*SF[9] + P[3][9]*SF[8] + P[10][9]*SF[11] + P[11][9]*SPP[7] + P[12][9]*SPP[6] + dt*(P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6]); + nextP[0][10] = P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]; + nextP[0][11] = P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]; + nextP[0][12] = P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]; + nextP[0][13] = P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]; + nextP[0][14] = P[0][14] + P[1][14]*SF[7] + P[2][14]*SF[9] + P[3][14]*SF[8] + P[10][14]*SF[11] + P[11][14]*SPP[7] + P[12][14]*SPP[6]; + nextP[0][15] = P[0][15] + P[1][15]*SF[7] + P[2][15]*SF[9] + P[3][15]*SF[8] + P[10][15]*SF[11] + P[11][15]*SPP[7] + P[12][15]*SPP[6]; + nextP[0][16] = P[0][16] + P[1][16]*SF[7] + P[2][16]*SF[9] + P[3][16]*SF[8] + P[10][16]*SF[11] + P[11][16]*SPP[7] + P[12][16]*SPP[6]; + nextP[0][17] = P[0][17] + P[1][17]*SF[7] + P[2][17]*SF[9] + P[3][17]*SF[8] + P[10][17]*SF[11] + P[11][17]*SPP[7] + P[12][17]*SPP[6]; + nextP[0][18] = P[0][18] + P[1][18]*SF[7] + P[2][18]*SF[9] + P[3][18]*SF[8] + P[10][18]*SF[11] + P[11][18]*SPP[7] + P[12][18]*SPP[6]; + nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6]; + nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6]; + nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6]; + nextP[0][22] = P[0][22] + P[1][22]*SF[7] + P[2][22]*SF[9] + P[3][22]*SF[8] + P[10][22]*SF[11] + P[11][22]*SPP[7] + P[12][22]*SPP[6]; + nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2 + SF[7]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) + SPP[7]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[6]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2); + nextP[1][1] = P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[5]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[9]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[6]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) - SPP[7]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2))/2; + nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[8]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[6]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) - SPP[6]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2))/2; + nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[4]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SF[11]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[7]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2))/2; + nextP[1][4] = P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[4]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][5] = P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][6] = P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2 + SF[2]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[1]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][7] = P[1][7] + P[0][7]*SF[6] + P[2][7]*SF[5] + P[3][7]*SF[9] + P[11][7]*SPP[6] - P[12][7]*SPP[7] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2); + nextP[1][8] = P[1][8] + P[0][8]*SF[6] + P[2][8]*SF[5] + P[3][8]*SF[9] + P[11][8]*SPP[6] - P[12][8]*SPP[7] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2); + nextP[1][9] = P[1][9] + P[0][9]*SF[6] + P[2][9]*SF[5] + P[3][9]*SF[9] + P[11][9]*SPP[6] - P[12][9]*SPP[7] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2); + nextP[1][10] = P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2; + nextP[1][11] = P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2; + nextP[1][12] = P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2; + nextP[1][13] = P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2; + nextP[1][14] = P[1][14] + P[0][14]*SF[6] + P[2][14]*SF[5] + P[3][14]*SF[9] + P[11][14]*SPP[6] - P[12][14]*SPP[7] - (P[10][14]*q0)/2; + nextP[1][15] = P[1][15] + P[0][15]*SF[6] + P[2][15]*SF[5] + P[3][15]*SF[9] + P[11][15]*SPP[6] - P[12][15]*SPP[7] - (P[10][15]*q0)/2; + nextP[1][16] = P[1][16] + P[0][16]*SF[6] + P[2][16]*SF[5] + P[3][16]*SF[9] + P[11][16]*SPP[6] - P[12][16]*SPP[7] - (P[10][16]*q0)/2; + nextP[1][17] = P[1][17] + P[0][17]*SF[6] + P[2][17]*SF[5] + P[3][17]*SF[9] + P[11][17]*SPP[6] - P[12][17]*SPP[7] - (P[10][17]*q0)/2; + nextP[1][18] = P[1][18] + P[0][18]*SF[6] + P[2][18]*SF[5] + P[3][18]*SF[9] + P[11][18]*SPP[6] - P[12][18]*SPP[7] - (P[10][18]*q0)/2; + nextP[1][19] = P[1][19] + P[0][19]*SF[6] + P[2][19]*SF[5] + P[3][19]*SF[9] + P[11][19]*SPP[6] - P[12][19]*SPP[7] - (P[10][19]*q0)/2; + nextP[1][20] = P[1][20] + P[0][20]*SF[6] + P[2][20]*SF[5] + P[3][20]*SF[9] + P[11][20]*SPP[6] - P[12][20]*SPP[7] - (P[10][20]*q0)/2; + nextP[1][21] = P[1][21] + P[0][21]*SF[6] + P[2][21]*SF[5] + P[3][21]*SF[9] + P[11][21]*SPP[6] - P[12][21]*SPP[7] - (P[10][21]*q0)/2; + nextP[1][22] = P[1][22] + P[0][22]*SF[6] + P[2][22]*SF[5] + P[3][22]*SF[9] + P[11][22]*SPP[6] - P[12][22]*SPP[7] - (P[10][22]*q0)/2; + nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2 + SF[7]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + SPP[7]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[6]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2); + nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[5]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[9]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[6]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) - SPP[7]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2))/2; + nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[8]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[6]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - SPP[6]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2))/2; + nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[4]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[7]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SF[11]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[7]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2))/2; + nextP[2][4] = P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[4]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][5] = P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][6] = P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2 + SF[2]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[1]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][7] = P[2][7] + P[0][7]*SF[4] + P[1][7]*SF[8] + P[3][7]*SF[6] + P[12][7]*SF[11] - P[10][7]*SPP[6] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2); + nextP[2][8] = P[2][8] + P[0][8]*SF[4] + P[1][8]*SF[8] + P[3][8]*SF[6] + P[12][8]*SF[11] - P[10][8]*SPP[6] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2); + nextP[2][9] = P[2][9] + P[0][9]*SF[4] + P[1][9]*SF[8] + P[3][9]*SF[6] + P[12][9]*SF[11] - P[10][9]*SPP[6] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2); + nextP[2][10] = P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2; + nextP[2][11] = P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2; + nextP[2][12] = P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2; + nextP[2][13] = P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2; + nextP[2][14] = P[2][14] + P[0][14]*SF[4] + P[1][14]*SF[8] + P[3][14]*SF[6] + P[12][14]*SF[11] - P[10][14]*SPP[6] - (P[11][14]*q0)/2; + nextP[2][15] = P[2][15] + P[0][15]*SF[4] + P[1][15]*SF[8] + P[3][15]*SF[6] + P[12][15]*SF[11] - P[10][15]*SPP[6] - (P[11][15]*q0)/2; + nextP[2][16] = P[2][16] + P[0][16]*SF[4] + P[1][16]*SF[8] + P[3][16]*SF[6] + P[12][16]*SF[11] - P[10][16]*SPP[6] - (P[11][16]*q0)/2; + nextP[2][17] = P[2][17] + P[0][17]*SF[4] + P[1][17]*SF[8] + P[3][17]*SF[6] + P[12][17]*SF[11] - P[10][17]*SPP[6] - (P[11][17]*q0)/2; + nextP[2][18] = P[2][18] + P[0][18]*SF[4] + P[1][18]*SF[8] + P[3][18]*SF[6] + P[12][18]*SF[11] - P[10][18]*SPP[6] - (P[11][18]*q0)/2; + nextP[2][19] = P[2][19] + P[0][19]*SF[4] + P[1][19]*SF[8] + P[3][19]*SF[6] + P[12][19]*SF[11] - P[10][19]*SPP[6] - (P[11][19]*q0)/2; + nextP[2][20] = P[2][20] + P[0][20]*SF[4] + P[1][20]*SF[8] + P[3][20]*SF[6] + P[12][20]*SF[11] - P[10][20]*SPP[6] - (P[11][20]*q0)/2; + nextP[2][21] = P[2][21] + P[0][21]*SF[4] + P[1][21]*SF[8] + P[3][21]*SF[6] + P[12][21]*SF[11] - P[10][21]*SPP[6] - (P[11][21]*q0)/2; + nextP[2][22] = P[2][22] + P[0][22]*SF[4] + P[1][22]*SF[8] + P[3][22]*SF[6] + P[12][22]*SF[11] - P[10][22]*SPP[6] - (P[11][22]*q0)/2; + nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2 + SF[7]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[8]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + SPP[7]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[6]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2); + nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2 + SF[6]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[5]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[9]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[6]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) - SPP[7]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2))/2; + nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[8]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[6]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - SPP[6]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2))/2; + nextP[3][3] = P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[4]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[7]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SF[11]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[7]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2))/2; + nextP[3][4] = P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[4]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][5] = P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][6] = P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2 + SF[2]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[1]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][7] = P[3][7] + P[0][7]*SF[5] + P[1][7]*SF[4] + P[2][7]*SF[7] - P[11][7]*SF[11] + P[10][7]*SPP[7] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2); + nextP[3][8] = P[3][8] + P[0][8]*SF[5] + P[1][8]*SF[4] + P[2][8]*SF[7] - P[11][8]*SF[11] + P[10][8]*SPP[7] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2); + nextP[3][9] = P[3][9] + P[0][9]*SF[5] + P[1][9]*SF[4] + P[2][9]*SF[7] - P[11][9]*SF[11] + P[10][9]*SPP[7] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2); + nextP[3][10] = P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2; + nextP[3][11] = P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2; + nextP[3][12] = P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2; + nextP[3][13] = P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2; + nextP[3][14] = P[3][14] + P[0][14]*SF[5] + P[1][14]*SF[4] + P[2][14]*SF[7] - P[11][14]*SF[11] + P[10][14]*SPP[7] - (P[12][14]*q0)/2; + nextP[3][15] = P[3][15] + P[0][15]*SF[5] + P[1][15]*SF[4] + P[2][15]*SF[7] - P[11][15]*SF[11] + P[10][15]*SPP[7] - (P[12][15]*q0)/2; + nextP[3][16] = P[3][16] + P[0][16]*SF[5] + P[1][16]*SF[4] + P[2][16]*SF[7] - P[11][16]*SF[11] + P[10][16]*SPP[7] - (P[12][16]*q0)/2; + nextP[3][17] = P[3][17] + P[0][17]*SF[5] + P[1][17]*SF[4] + P[2][17]*SF[7] - P[11][17]*SF[11] + P[10][17]*SPP[7] - (P[12][17]*q0)/2; + nextP[3][18] = P[3][18] + P[0][18]*SF[5] + P[1][18]*SF[4] + P[2][18]*SF[7] - P[11][18]*SF[11] + P[10][18]*SPP[7] - (P[12][18]*q0)/2; + nextP[3][19] = P[3][19] + P[0][19]*SF[5] + P[1][19]*SF[4] + P[2][19]*SF[7] - P[11][19]*SF[11] + P[10][19]*SPP[7] - (P[12][19]*q0)/2; + nextP[3][20] = P[3][20] + P[0][20]*SF[5] + P[1][20]*SF[4] + P[2][20]*SF[7] - P[11][20]*SF[11] + P[10][20]*SPP[7] - (P[12][20]*q0)/2; + nextP[3][21] = P[3][21] + P[0][21]*SF[5] + P[1][21]*SF[4] + P[2][21]*SF[7] - P[11][21]*SF[11] + P[10][21]*SPP[7] - (P[12][21]*q0)/2; + nextP[3][22] = P[3][22] + P[0][22]*SF[5] + P[1][22]*SF[4] + P[2][22]*SF[7] - P[11][22]*SF[11] + P[10][22]*SPP[7] - (P[12][22]*q0)/2; + nextP[4][0] = P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4] + SF[7]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[9]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[8]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) + SPP[7]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[6]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]); + nextP[4][1] = P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4] + SF[6]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[5]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[9]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[6]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) - SPP[7]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - (q0*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]))/2; + nextP[4][2] = P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4] + SF[4]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[8]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[6]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - SPP[6]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]))/2; + nextP[4][3] = P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4] + SF[5]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[4]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[7]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SF[11]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[7]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]))/2; + nextP[4][4] = P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[3]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[0]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SPP[2]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[4]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); + nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4] + SF[2]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[3]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[0]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[3]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]); + nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4] + SF[2]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[1]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[0]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) - SPP[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]); + nextP[4][7] = P[4][7] + P[0][7]*SF[3] + P[1][7]*SF[1] + P[2][7]*SPP[0] - P[3][7]*SPP[2] - P[13][7]*SPP[4] + dt*(P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4]); + nextP[4][8] = P[4][8] + P[0][8]*SF[3] + P[1][8]*SF[1] + P[2][8]*SPP[0] - P[3][8]*SPP[2] - P[13][8]*SPP[4] + dt*(P[4][5] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4]); + nextP[4][9] = P[4][9] + P[0][9]*SF[3] + P[1][9]*SF[1] + P[2][9]*SPP[0] - P[3][9]*SPP[2] - P[13][9]*SPP[4] + dt*(P[4][6] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4]); + nextP[4][10] = P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]; + nextP[4][11] = P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]; + nextP[4][12] = P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]; + nextP[4][13] = P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]; + nextP[4][14] = P[4][14] + P[0][14]*SF[3] + P[1][14]*SF[1] + P[2][14]*SPP[0] - P[3][14]*SPP[2] - P[13][14]*SPP[4]; + nextP[4][15] = P[4][15] + P[0][15]*SF[3] + P[1][15]*SF[1] + P[2][15]*SPP[0] - P[3][15]*SPP[2] - P[13][15]*SPP[4]; + nextP[4][16] = P[4][16] + P[0][16]*SF[3] + P[1][16]*SF[1] + P[2][16]*SPP[0] - P[3][16]*SPP[2] - P[13][16]*SPP[4]; + nextP[4][17] = P[4][17] + P[0][17]*SF[3] + P[1][17]*SF[1] + P[2][17]*SPP[0] - P[3][17]*SPP[2] - P[13][17]*SPP[4]; + nextP[4][18] = P[4][18] + P[0][18]*SF[3] + P[1][18]*SF[1] + P[2][18]*SPP[0] - P[3][18]*SPP[2] - P[13][18]*SPP[4]; + nextP[4][19] = P[4][19] + P[0][19]*SF[3] + P[1][19]*SF[1] + P[2][19]*SPP[0] - P[3][19]*SPP[2] - P[13][19]*SPP[4]; + nextP[4][20] = P[4][20] + P[0][20]*SF[3] + P[1][20]*SF[1] + P[2][20]*SPP[0] - P[3][20]*SPP[2] - P[13][20]*SPP[4]; + nextP[4][21] = P[4][21] + P[0][21]*SF[3] + P[1][21]*SF[1] + P[2][21]*SPP[0] - P[3][21]*SPP[2] - P[13][21]*SPP[4]; + nextP[4][22] = P[4][22] + P[0][22]*SF[3] + P[1][22]*SF[1] + P[2][22]*SPP[0] - P[3][22]*SPP[2] - P[13][22]*SPP[4]; + nextP[5][0] = P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3] + SF[7]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[9]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[8]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) + SPP[7]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[6]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]); + nextP[5][1] = P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3] + SF[6]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[5]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[9]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[6]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) - SPP[7]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - (q0*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]))/2; + nextP[5][2] = P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3] + SF[4]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[8]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[6]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - SPP[6]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]))/2; + nextP[5][3] = P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3] + SF[5]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[4]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[7]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SF[11]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[7]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]))/2; + nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3] + SF[3]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[0]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SPP[2]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[4]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]); + nextP[5][5] = P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[2]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[3]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[0]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[3]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); + nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3] + SF[2]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[1]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[0]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) - SPP[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]); + nextP[5][7] = P[5][7] + P[0][7]*SF[2] + P[2][7]*SF[1] + P[3][7]*SF[3] - P[1][7]*SPP[0] + P[13][7]*SPP[3] + dt*(P[5][4] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3]); + nextP[5][8] = P[5][8] + P[0][8]*SF[2] + P[2][8]*SF[1] + P[3][8]*SF[3] - P[1][8]*SPP[0] + P[13][8]*SPP[3] + dt*(P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3]); + nextP[5][9] = P[5][9] + P[0][9]*SF[2] + P[2][9]*SF[1] + P[3][9]*SF[3] - P[1][9]*SPP[0] + P[13][9]*SPP[3] + dt*(P[5][6] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3]); + nextP[5][10] = P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]; + nextP[5][11] = P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]; + nextP[5][12] = P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]; + nextP[5][13] = P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]; + nextP[5][14] = P[5][14] + P[0][14]*SF[2] + P[2][14]*SF[1] + P[3][14]*SF[3] - P[1][14]*SPP[0] + P[13][14]*SPP[3]; + nextP[5][15] = P[5][15] + P[0][15]*SF[2] + P[2][15]*SF[1] + P[3][15]*SF[3] - P[1][15]*SPP[0] + P[13][15]*SPP[3]; + nextP[5][16] = P[5][16] + P[0][16]*SF[2] + P[2][16]*SF[1] + P[3][16]*SF[3] - P[1][16]*SPP[0] + P[13][16]*SPP[3]; + nextP[5][17] = P[5][17] + P[0][17]*SF[2] + P[2][17]*SF[1] + P[3][17]*SF[3] - P[1][17]*SPP[0] + P[13][17]*SPP[3]; + nextP[5][18] = P[5][18] + P[0][18]*SF[2] + P[2][18]*SF[1] + P[3][18]*SF[3] - P[1][18]*SPP[0] + P[13][18]*SPP[3]; + nextP[5][19] = P[5][19] + P[0][19]*SF[2] + P[2][19]*SF[1] + P[3][19]*SF[3] - P[1][19]*SPP[0] + P[13][19]*SPP[3]; + nextP[5][20] = P[5][20] + P[0][20]*SF[2] + P[2][20]*SF[1] + P[3][20]*SF[3] - P[1][20]*SPP[0] + P[13][20]*SPP[3]; + nextP[5][21] = P[5][21] + P[0][21]*SF[2] + P[2][21]*SF[1] + P[3][21]*SF[3] - P[1][21]*SPP[0] + P[13][21]*SPP[3]; + nextP[5][22] = P[5][22] + P[0][22]*SF[2] + P[2][22]*SF[1] + P[3][22]*SF[3] - P[1][22]*SPP[0] + P[13][22]*SPP[3]; + nextP[6][0] = P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[7]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][1] = P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[6]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[5]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[7]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][2] = P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[4]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[6]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[6]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][3] = P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[5]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[4]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[7]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SF[11]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[3]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[2]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[4]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[2]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[3]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[0]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[3]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][6] = P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) - SPP[5]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]) + SF[2]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); + nextP[6][7] = P[6][7] + P[1][7]*SF[2] + P[3][7]*SF[1] + P[0][7]*SPP[0] - P[2][7]*SPP[1] - P[13][7]*SPP[5] + dt*(P[6][4] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*SPP[5]); + nextP[6][8] = P[6][8] + P[1][8]*SF[2] + P[3][8]*SF[1] + P[0][8]*SPP[0] - P[2][8]*SPP[1] - P[13][8]*SPP[5] + dt*(P[6][5] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*SPP[5]); + nextP[6][9] = P[6][9] + P[1][9]*SF[2] + P[3][9]*SF[1] + P[0][9]*SPP[0] - P[2][9]*SPP[1] - P[13][9]*SPP[5] + dt*(P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*SPP[5]); + nextP[6][10] = P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*SPP[5]; + nextP[6][11] = P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*SPP[5]; + nextP[6][12] = P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*SPP[5]; + nextP[6][13] = P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]; + nextP[6][14] = P[6][14] + P[1][14]*SF[2] + P[3][14]*SF[1] + P[0][14]*SPP[0] - P[2][14]*SPP[1] - P[13][14]*SPP[5]; + nextP[6][15] = P[6][15] + P[1][15]*SF[2] + P[3][15]*SF[1] + P[0][15]*SPP[0] - P[2][15]*SPP[1] - P[13][15]*SPP[5]; + nextP[6][16] = P[6][16] + P[1][16]*SF[2] + P[3][16]*SF[1] + P[0][16]*SPP[0] - P[2][16]*SPP[1] - P[13][16]*SPP[5]; + nextP[6][17] = P[6][17] + P[1][17]*SF[2] + P[3][17]*SF[1] + P[0][17]*SPP[0] - P[2][17]*SPP[1] - P[13][17]*SPP[5]; + nextP[6][18] = P[6][18] + P[1][18]*SF[2] + P[3][18]*SF[1] + P[0][18]*SPP[0] - P[2][18]*SPP[1] - P[13][18]*SPP[5]; + nextP[6][19] = P[6][19] + P[1][19]*SF[2] + P[3][19]*SF[1] + P[0][19]*SPP[0] - P[2][19]*SPP[1] - P[13][19]*SPP[5]; + nextP[6][20] = P[6][20] + P[1][20]*SF[2] + P[3][20]*SF[1] + P[0][20]*SPP[0] - P[2][20]*SPP[1] - P[13][20]*SPP[5]; + nextP[6][21] = P[6][21] + P[1][21]*SF[2] + P[3][21]*SF[1] + P[0][21]*SPP[0] - P[2][21]*SPP[1] - P[13][21]*SPP[5]; + nextP[6][22] = P[6][22] + P[1][22]*SF[2] + P[3][22]*SF[1] + P[0][22]*SPP[0] - P[2][22]*SPP[1] - P[13][22]*SPP[5]; + nextP[7][0] = P[7][0] + P[4][0]*dt + SF[7]*(P[7][1] + P[4][1]*dt) + SF[9]*(P[7][2] + P[4][2]*dt) + SF[8]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][10] + P[4][10]*dt) + SPP[7]*(P[7][11] + P[4][11]*dt) + SPP[6]*(P[7][12] + P[4][12]*dt); + nextP[7][1] = P[7][1] + P[4][1]*dt + SF[6]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][2] + P[4][2]*dt) + SF[9]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][11] + P[4][11]*dt) - SPP[7]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; + nextP[7][2] = P[7][2] + P[4][2]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[8]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][12] + P[4][12]*dt) - SPP[6]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; + nextP[7][3] = P[7][3] + P[4][3]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][1] + P[4][1]*dt) + SF[7]*(P[7][2] + P[4][2]*dt) - SF[11]*(P[7][11] + P[4][11]*dt) + SPP[7]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; + nextP[7][4] = P[7][4] + P[4][4]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[3]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt) - SPP[4]*(P[7][13] + P[4][13]*dt); + nextP[7][5] = P[7][5] + P[4][5]*dt + SF[2]*(P[7][0] + P[4][0]*dt) + SF[1]*(P[7][2] + P[4][2]*dt) + SF[3]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt) + SPP[3]*(P[7][13] + P[4][13]*dt); + nextP[7][6] = P[7][6] + P[4][6]*dt + SF[2]*(P[7][1] + P[4][1]*dt) + SF[1]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt) - SPP[5]*(P[7][13] + P[4][13]*dt); nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); @@ -543,13 +574,15 @@ void AttPosEKF::CovariancePrediction(float 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[7][21] = P[7][21] + P[4][21]*dt; + nextP[7][22] = P[7][22] + P[4][22]*dt; + nextP[8][0] = P[8][0] + P[5][0]*dt + SF[7]*(P[8][1] + P[5][1]*dt) + SF[9]*(P[8][2] + P[5][2]*dt) + SF[8]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][10] + P[5][10]*dt) + SPP[7]*(P[8][11] + P[5][11]*dt) + SPP[6]*(P[8][12] + P[5][12]*dt); + nextP[8][1] = P[8][1] + P[5][1]*dt + SF[6]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][2] + P[5][2]*dt) + SF[9]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][11] + P[5][11]*dt) - SPP[7]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; + nextP[8][2] = P[8][2] + P[5][2]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[8]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][12] + P[5][12]*dt) - SPP[6]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; + nextP[8][3] = P[8][3] + P[5][3]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][1] + P[5][1]*dt) + SF[7]*(P[8][2] + P[5][2]*dt) - SF[11]*(P[8][11] + P[5][11]*dt) + SPP[7]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; + nextP[8][4] = P[8][4] + P[5][4]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[3]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt) - SPP[4]*(P[8][13] + P[5][13]*dt); + nextP[8][5] = P[8][5] + P[5][5]*dt + SF[2]*(P[8][0] + P[5][0]*dt) + SF[1]*(P[8][2] + P[5][2]*dt) + SF[3]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt) + SPP[3]*(P[8][13] + P[5][13]*dt); + nextP[8][6] = P[8][6] + P[5][6]*dt + SF[2]*(P[8][1] + P[5][1]*dt) + SF[1]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt) - SPP[5]*(P[8][13] + P[5][13]*dt); nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt); nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); @@ -564,13 +597,15 @@ void AttPosEKF::CovariancePrediction(float 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[8][21] = P[8][21] + P[5][21]*dt; + nextP[8][22] = P[8][22] + P[5][22]*dt; + nextP[9][0] = P[9][0] + P[6][0]*dt + SF[7]*(P[9][1] + P[6][1]*dt) + SF[9]*(P[9][2] + P[6][2]*dt) + SF[8]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][10] + P[6][10]*dt) + SPP[7]*(P[9][11] + P[6][11]*dt) + SPP[6]*(P[9][12] + P[6][12]*dt); + nextP[9][1] = P[9][1] + P[6][1]*dt + SF[6]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][2] + P[6][2]*dt) + SF[9]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][11] + P[6][11]*dt) - SPP[7]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; + nextP[9][2] = P[9][2] + P[6][2]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[8]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][12] + P[6][12]*dt) - SPP[6]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; + nextP[9][3] = P[9][3] + P[6][3]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][1] + P[6][1]*dt) + SF[7]*(P[9][2] + P[6][2]*dt) - SF[11]*(P[9][11] + P[6][11]*dt) + SPP[7]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; + nextP[9][4] = P[9][4] + P[6][4]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[3]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt) - SPP[4]*(P[9][13] + P[6][13]*dt); + nextP[9][5] = P[9][5] + P[6][5]*dt + SF[2]*(P[9][0] + P[6][0]*dt) + SF[1]*(P[9][2] + P[6][2]*dt) + SF[3]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt) + SPP[3]*(P[9][13] + P[6][13]*dt); + nextP[9][6] = P[9][6] + P[6][6]*dt + SF[2]*(P[9][1] + P[6][1]*dt) + SF[1]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt) - SPP[5]*(P[9][13] + P[6][13]*dt); nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt); nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt); nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); @@ -585,13 +620,15 @@ void AttPosEKF::CovariancePrediction(float 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[9][21] = P[9][21] + P[6][21]*dt; + nextP[9][22] = P[9][22] + P[6][22]*dt; + nextP[10][0] = P[10][0] + P[10][1]*SF[7] + P[10][2]*SF[9] + P[10][3]*SF[8] + P[10][10]*SF[11] + P[10][11]*SPP[7] + P[10][12]*SPP[6]; + nextP[10][1] = P[10][1] + P[10][0]*SF[6] + P[10][2]*SF[5] + P[10][3]*SF[9] + P[10][11]*SPP[6] - P[10][12]*SPP[7] - (P[10][10]*q0)/2; + nextP[10][2] = P[10][2] + P[10][0]*SF[4] + P[10][1]*SF[8] + P[10][3]*SF[6] + P[10][12]*SF[11] - P[10][10]*SPP[6] - (P[10][11]*q0)/2; + nextP[10][3] = P[10][3] + P[10][0]*SF[5] + P[10][1]*SF[4] + P[10][2]*SF[7] - P[10][11]*SF[11] + P[10][10]*SPP[7] - (P[10][12]*q0)/2; + nextP[10][4] = P[10][4] + P[10][1]*SF[1] + P[10][0]*SF[3] + P[10][2]*SPP[0] - P[10][3]*SPP[2] - P[10][13]*SPP[4]; + nextP[10][5] = P[10][5] + P[10][0]*SF[2] + P[10][2]*SF[1] + P[10][3]*SF[3] - P[10][1]*SPP[0] + P[10][13]*SPP[3]; + nextP[10][6] = P[10][6] + P[10][1]*SF[2] + P[10][3]*SF[1] + P[10][0]*SPP[0] - P[10][2]*SPP[1] - P[10][13]*SPP[5]; nextP[10][7] = P[10][7] + P[10][4]*dt; nextP[10][8] = P[10][8] + P[10][5]*dt; nextP[10][9] = P[10][9] + P[10][6]*dt; @@ -606,13 +643,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[10][21] = P[10][21]; + nextP[10][22] = P[10][22]; + nextP[11][0] = P[11][0] + P[11][1]*SF[7] + P[11][2]*SF[9] + P[11][3]*SF[8] + P[11][10]*SF[11] + P[11][11]*SPP[7] + P[11][12]*SPP[6]; + nextP[11][1] = P[11][1] + P[11][0]*SF[6] + P[11][2]*SF[5] + P[11][3]*SF[9] + P[11][11]*SPP[6] - P[11][12]*SPP[7] - (P[11][10]*q0)/2; + nextP[11][2] = P[11][2] + P[11][0]*SF[4] + P[11][1]*SF[8] + P[11][3]*SF[6] + P[11][12]*SF[11] - P[11][10]*SPP[6] - (P[11][11]*q0)/2; + nextP[11][3] = P[11][3] + P[11][0]*SF[5] + P[11][1]*SF[4] + P[11][2]*SF[7] - P[11][11]*SF[11] + P[11][10]*SPP[7] - (P[11][12]*q0)/2; + nextP[11][4] = P[11][4] + P[11][1]*SF[1] + P[11][0]*SF[3] + P[11][2]*SPP[0] - P[11][3]*SPP[2] - P[11][13]*SPP[4]; + nextP[11][5] = P[11][5] + P[11][0]*SF[2] + P[11][2]*SF[1] + P[11][3]*SF[3] - P[11][1]*SPP[0] + P[11][13]*SPP[3]; + nextP[11][6] = P[11][6] + P[11][1]*SF[2] + P[11][3]*SF[1] + P[11][0]*SPP[0] - P[11][2]*SPP[1] - P[11][13]*SPP[5]; nextP[11][7] = P[11][7] + P[11][4]*dt; nextP[11][8] = P[11][8] + P[11][5]*dt; nextP[11][9] = P[11][9] + P[11][6]*dt; @@ -627,13 +666,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[11][21] = P[11][21]; + nextP[11][22] = P[11][22]; + nextP[12][0] = P[12][0] + P[12][1]*SF[7] + P[12][2]*SF[9] + P[12][3]*SF[8] + P[12][10]*SF[11] + P[12][11]*SPP[7] + P[12][12]*SPP[6]; + nextP[12][1] = P[12][1] + P[12][0]*SF[6] + P[12][2]*SF[5] + P[12][3]*SF[9] + P[12][11]*SPP[6] - P[12][12]*SPP[7] - (P[12][10]*q0)/2; + nextP[12][2] = P[12][2] + P[12][0]*SF[4] + P[12][1]*SF[8] + P[12][3]*SF[6] + P[12][12]*SF[11] - P[12][10]*SPP[6] - (P[12][11]*q0)/2; + nextP[12][3] = P[12][3] + P[12][0]*SF[5] + P[12][1]*SF[4] + P[12][2]*SF[7] - P[12][11]*SF[11] + P[12][10]*SPP[7] - (P[12][12]*q0)/2; + nextP[12][4] = P[12][4] + P[12][1]*SF[1] + P[12][0]*SF[3] + P[12][2]*SPP[0] - P[12][3]*SPP[2] - P[12][13]*SPP[4]; + nextP[12][5] = P[12][5] + P[12][0]*SF[2] + P[12][2]*SF[1] + P[12][3]*SF[3] - P[12][1]*SPP[0] + P[12][13]*SPP[3]; + nextP[12][6] = P[12][6] + P[12][1]*SF[2] + P[12][3]*SF[1] + P[12][0]*SPP[0] - P[12][2]*SPP[1] - P[12][13]*SPP[5]; nextP[12][7] = P[12][7] + P[12][4]*dt; nextP[12][8] = P[12][8] + P[12][5]*dt; nextP[12][9] = P[12][9] + P[12][6]*dt; @@ -648,13 +689,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[12][21] = P[12][21]; + nextP[12][22] = P[12][22]; + nextP[13][0] = P[13][0] + P[13][1]*SF[7] + P[13][2]*SF[9] + P[13][3]*SF[8] + P[13][10]*SF[11] + P[13][11]*SPP[7] + P[13][12]*SPP[6]; + nextP[13][1] = P[13][1] + P[13][0]*SF[6] + P[13][2]*SF[5] + P[13][3]*SF[9] + P[13][11]*SPP[6] - P[13][12]*SPP[7] - (P[13][10]*q0)/2; + nextP[13][2] = P[13][2] + P[13][0]*SF[4] + P[13][1]*SF[8] + P[13][3]*SF[6] + P[13][12]*SF[11] - P[13][10]*SPP[6] - (P[13][11]*q0)/2; + nextP[13][3] = P[13][3] + P[13][0]*SF[5] + P[13][1]*SF[4] + P[13][2]*SF[7] - P[13][11]*SF[11] + P[13][10]*SPP[7] - (P[13][12]*q0)/2; + nextP[13][4] = P[13][4] + P[13][1]*SF[1] + P[13][0]*SF[3] + P[13][2]*SPP[0] - P[13][3]*SPP[2] - P[13][13]*SPP[4]; + nextP[13][5] = P[13][5] + P[13][0]*SF[2] + P[13][2]*SF[1] + P[13][3]*SF[3] - P[13][1]*SPP[0] + P[13][13]*SPP[3]; + nextP[13][6] = P[13][6] + P[13][1]*SF[2] + P[13][3]*SF[1] + P[13][0]*SPP[0] - P[13][2]*SPP[1] - P[13][13]*SPP[5]; nextP[13][7] = P[13][7] + P[13][4]*dt; nextP[13][8] = P[13][8] + P[13][5]*dt; nextP[13][9] = P[13][9] + P[13][6]*dt; @@ -669,13 +712,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[13][21] = P[13][21]; + nextP[13][22] = P[13][22]; + nextP[14][0] = P[14][0] + P[14][1]*SF[7] + P[14][2]*SF[9] + P[14][3]*SF[8] + P[14][10]*SF[11] + P[14][11]*SPP[7] + P[14][12]*SPP[6]; + nextP[14][1] = P[14][1] + P[14][0]*SF[6] + P[14][2]*SF[5] + P[14][3]*SF[9] + P[14][11]*SPP[6] - P[14][12]*SPP[7] - (P[14][10]*q0)/2; + nextP[14][2] = P[14][2] + P[14][0]*SF[4] + P[14][1]*SF[8] + P[14][3]*SF[6] + P[14][12]*SF[11] - P[14][10]*SPP[6] - (P[14][11]*q0)/2; + nextP[14][3] = P[14][3] + P[14][0]*SF[5] + P[14][1]*SF[4] + P[14][2]*SF[7] - P[14][11]*SF[11] + P[14][10]*SPP[7] - (P[14][12]*q0)/2; + nextP[14][4] = P[14][4] + P[14][1]*SF[1] + P[14][0]*SF[3] + P[14][2]*SPP[0] - P[14][3]*SPP[2] - P[14][13]*SPP[4]; + nextP[14][5] = P[14][5] + P[14][0]*SF[2] + P[14][2]*SF[1] + P[14][3]*SF[3] - P[14][1]*SPP[0] + P[14][13]*SPP[3]; + nextP[14][6] = P[14][6] + P[14][1]*SF[2] + P[14][3]*SF[1] + P[14][0]*SPP[0] - P[14][2]*SPP[1] - P[14][13]*SPP[5]; nextP[14][7] = P[14][7] + P[14][4]*dt; nextP[14][8] = P[14][8] + P[14][5]*dt; nextP[14][9] = P[14][9] + P[14][6]*dt; @@ -690,13 +735,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[14][21] = P[14][21]; + nextP[14][22] = P[14][22]; + nextP[15][0] = P[15][0] + P[15][1]*SF[7] + P[15][2]*SF[9] + P[15][3]*SF[8] + P[15][10]*SF[11] + P[15][11]*SPP[7] + P[15][12]*SPP[6]; + nextP[15][1] = P[15][1] + P[15][0]*SF[6] + P[15][2]*SF[5] + P[15][3]*SF[9] + P[15][11]*SPP[6] - P[15][12]*SPP[7] - (P[15][10]*q0)/2; + nextP[15][2] = P[15][2] + P[15][0]*SF[4] + P[15][1]*SF[8] + P[15][3]*SF[6] + P[15][12]*SF[11] - P[15][10]*SPP[6] - (P[15][11]*q0)/2; + nextP[15][3] = P[15][3] + P[15][0]*SF[5] + P[15][1]*SF[4] + P[15][2]*SF[7] - P[15][11]*SF[11] + P[15][10]*SPP[7] - (P[15][12]*q0)/2; + nextP[15][4] = P[15][4] + P[15][1]*SF[1] + P[15][0]*SF[3] + P[15][2]*SPP[0] - P[15][3]*SPP[2] - P[15][13]*SPP[4]; + nextP[15][5] = P[15][5] + P[15][0]*SF[2] + P[15][2]*SF[1] + P[15][3]*SF[3] - P[15][1]*SPP[0] + P[15][13]*SPP[3]; + nextP[15][6] = P[15][6] + P[15][1]*SF[2] + P[15][3]*SF[1] + P[15][0]*SPP[0] - P[15][2]*SPP[1] - P[15][13]*SPP[5]; nextP[15][7] = P[15][7] + P[15][4]*dt; nextP[15][8] = P[15][8] + P[15][5]*dt; nextP[15][9] = P[15][9] + P[15][6]*dt; @@ -711,13 +758,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[15][21] = P[15][21]; + nextP[15][22] = P[15][22]; + nextP[16][0] = P[16][0] + P[16][1]*SF[7] + P[16][2]*SF[9] + P[16][3]*SF[8] + P[16][10]*SF[11] + P[16][11]*SPP[7] + P[16][12]*SPP[6]; + nextP[16][1] = P[16][1] + P[16][0]*SF[6] + P[16][2]*SF[5] + P[16][3]*SF[9] + P[16][11]*SPP[6] - P[16][12]*SPP[7] - (P[16][10]*q0)/2; + nextP[16][2] = P[16][2] + P[16][0]*SF[4] + P[16][1]*SF[8] + P[16][3]*SF[6] + P[16][12]*SF[11] - P[16][10]*SPP[6] - (P[16][11]*q0)/2; + nextP[16][3] = P[16][3] + P[16][0]*SF[5] + P[16][1]*SF[4] + P[16][2]*SF[7] - P[16][11]*SF[11] + P[16][10]*SPP[7] - (P[16][12]*q0)/2; + nextP[16][4] = P[16][4] + P[16][1]*SF[1] + P[16][0]*SF[3] + P[16][2]*SPP[0] - P[16][3]*SPP[2] - P[16][13]*SPP[4]; + nextP[16][5] = P[16][5] + P[16][0]*SF[2] + P[16][2]*SF[1] + P[16][3]*SF[3] - P[16][1]*SPP[0] + P[16][13]*SPP[3]; + nextP[16][6] = P[16][6] + P[16][1]*SF[2] + P[16][3]*SF[1] + P[16][0]*SPP[0] - P[16][2]*SPP[1] - P[16][13]*SPP[5]; nextP[16][7] = P[16][7] + P[16][4]*dt; nextP[16][8] = P[16][8] + P[16][5]*dt; nextP[16][9] = P[16][9] + P[16][6]*dt; @@ -732,13 +781,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[16][21] = P[16][21]; + nextP[16][22] = P[16][22]; + nextP[17][0] = P[17][0] + P[17][1]*SF[7] + P[17][2]*SF[9] + P[17][3]*SF[8] + P[17][10]*SF[11] + P[17][11]*SPP[7] + P[17][12]*SPP[6]; + nextP[17][1] = P[17][1] + P[17][0]*SF[6] + P[17][2]*SF[5] + P[17][3]*SF[9] + P[17][11]*SPP[6] - P[17][12]*SPP[7] - (P[17][10]*q0)/2; + nextP[17][2] = P[17][2] + P[17][0]*SF[4] + P[17][1]*SF[8] + P[17][3]*SF[6] + P[17][12]*SF[11] - P[17][10]*SPP[6] - (P[17][11]*q0)/2; + nextP[17][3] = P[17][3] + P[17][0]*SF[5] + P[17][1]*SF[4] + P[17][2]*SF[7] - P[17][11]*SF[11] + P[17][10]*SPP[7] - (P[17][12]*q0)/2; + nextP[17][4] = P[17][4] + P[17][1]*SF[1] + P[17][0]*SF[3] + P[17][2]*SPP[0] - P[17][3]*SPP[2] - P[17][13]*SPP[4]; + nextP[17][5] = P[17][5] + P[17][0]*SF[2] + P[17][2]*SF[1] + P[17][3]*SF[3] - P[17][1]*SPP[0] + P[17][13]*SPP[3]; + nextP[17][6] = P[17][6] + P[17][1]*SF[2] + P[17][3]*SF[1] + P[17][0]*SPP[0] - P[17][2]*SPP[1] - P[17][13]*SPP[5]; nextP[17][7] = P[17][7] + P[17][4]*dt; nextP[17][8] = P[17][8] + P[17][5]*dt; nextP[17][9] = P[17][9] + P[17][6]*dt; @@ -753,13 +804,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[17][21] = P[17][21]; + nextP[17][22] = P[17][22]; + nextP[18][0] = P[18][0] + P[18][1]*SF[7] + P[18][2]*SF[9] + P[18][3]*SF[8] + P[18][10]*SF[11] + P[18][11]*SPP[7] + P[18][12]*SPP[6]; + nextP[18][1] = P[18][1] + P[18][0]*SF[6] + P[18][2]*SF[5] + P[18][3]*SF[9] + P[18][11]*SPP[6] - P[18][12]*SPP[7] - (P[18][10]*q0)/2; + nextP[18][2] = P[18][2] + P[18][0]*SF[4] + P[18][1]*SF[8] + P[18][3]*SF[6] + P[18][12]*SF[11] - P[18][10]*SPP[6] - (P[18][11]*q0)/2; + nextP[18][3] = P[18][3] + P[18][0]*SF[5] + P[18][1]*SF[4] + P[18][2]*SF[7] - P[18][11]*SF[11] + P[18][10]*SPP[7] - (P[18][12]*q0)/2; + nextP[18][4] = P[18][4] + P[18][1]*SF[1] + P[18][0]*SF[3] + P[18][2]*SPP[0] - P[18][3]*SPP[2] - P[18][13]*SPP[4]; + nextP[18][5] = P[18][5] + P[18][0]*SF[2] + P[18][2]*SF[1] + P[18][3]*SF[3] - P[18][1]*SPP[0] + P[18][13]*SPP[3]; + nextP[18][6] = P[18][6] + P[18][1]*SF[2] + P[18][3]*SF[1] + P[18][0]*SPP[0] - P[18][2]*SPP[1] - P[18][13]*SPP[5]; nextP[18][7] = P[18][7] + P[18][4]*dt; nextP[18][8] = P[18][8] + P[18][5]*dt; nextP[18][9] = P[18][9] + P[18][6]*dt; @@ -774,13 +827,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[18][21] = P[18][21]; + nextP[18][22] = P[18][22]; + nextP[19][0] = P[19][0] + P[19][1]*SF[7] + P[19][2]*SF[9] + P[19][3]*SF[8] + P[19][10]*SF[11] + P[19][11]*SPP[7] + P[19][12]*SPP[6]; + nextP[19][1] = P[19][1] + P[19][0]*SF[6] + P[19][2]*SF[5] + P[19][3]*SF[9] + P[19][11]*SPP[6] - P[19][12]*SPP[7] - (P[19][10]*q0)/2; + nextP[19][2] = P[19][2] + P[19][0]*SF[4] + P[19][1]*SF[8] + P[19][3]*SF[6] + P[19][12]*SF[11] - P[19][10]*SPP[6] - (P[19][11]*q0)/2; + nextP[19][3] = P[19][3] + P[19][0]*SF[5] + P[19][1]*SF[4] + P[19][2]*SF[7] - P[19][11]*SF[11] + P[19][10]*SPP[7] - (P[19][12]*q0)/2; + nextP[19][4] = P[19][4] + P[19][1]*SF[1] + P[19][0]*SF[3] + P[19][2]*SPP[0] - P[19][3]*SPP[2] - P[19][13]*SPP[4]; + nextP[19][5] = P[19][5] + P[19][0]*SF[2] + P[19][2]*SF[1] + P[19][3]*SF[3] - P[19][1]*SPP[0] + P[19][13]*SPP[3]; + nextP[19][6] = P[19][6] + P[19][1]*SF[2] + P[19][3]*SF[1] + P[19][0]*SPP[0] - P[19][2]*SPP[1] - P[19][13]*SPP[5]; nextP[19][7] = P[19][7] + P[19][4]*dt; nextP[19][8] = P[19][8] + P[19][5]*dt; nextP[19][9] = P[19][9] + P[19][6]*dt; @@ -795,13 +850,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[19][21] = P[19][21]; + nextP[19][22] = P[19][22]; + nextP[20][0] = P[20][0] + P[20][1]*SF[7] + P[20][2]*SF[9] + P[20][3]*SF[8] + P[20][10]*SF[11] + P[20][11]*SPP[7] + P[20][12]*SPP[6]; + nextP[20][1] = P[20][1] + P[20][0]*SF[6] + P[20][2]*SF[5] + P[20][3]*SF[9] + P[20][11]*SPP[6] - P[20][12]*SPP[7] - (P[20][10]*q0)/2; + nextP[20][2] = P[20][2] + P[20][0]*SF[4] + P[20][1]*SF[8] + P[20][3]*SF[6] + P[20][12]*SF[11] - P[20][10]*SPP[6] - (P[20][11]*q0)/2; + nextP[20][3] = P[20][3] + P[20][0]*SF[5] + P[20][1]*SF[4] + P[20][2]*SF[7] - P[20][11]*SF[11] + P[20][10]*SPP[7] - (P[20][12]*q0)/2; + nextP[20][4] = P[20][4] + P[20][1]*SF[1] + P[20][0]*SF[3] + P[20][2]*SPP[0] - P[20][3]*SPP[2] - P[20][13]*SPP[4]; + nextP[20][5] = P[20][5] + P[20][0]*SF[2] + P[20][2]*SF[1] + P[20][3]*SF[3] - P[20][1]*SPP[0] + P[20][13]*SPP[3]; + nextP[20][6] = P[20][6] + P[20][1]*SF[2] + P[20][3]*SF[1] + P[20][0]*SPP[0] - P[20][2]*SPP[1] - P[20][13]*SPP[5]; nextP[20][7] = P[20][7] + P[20][4]*dt; nextP[20][8] = P[20][8] + P[20][5]*dt; nextP[20][9] = P[20][9] + P[20][6]*dt; @@ -816,6 +873,54 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[20][18] = P[20][18]; nextP[20][19] = P[20][19]; nextP[20][20] = P[20][20]; + nextP[20][21] = P[20][21]; + nextP[20][22] = P[20][22]; + nextP[21][0] = P[21][0] + P[21][1]*SF[7] + P[21][2]*SF[9] + P[21][3]*SF[8] + P[21][10]*SF[11] + P[21][11]*SPP[7] + P[21][12]*SPP[6]; + nextP[21][1] = P[21][1] + P[21][0]*SF[6] + P[21][2]*SF[5] + P[21][3]*SF[9] + P[21][11]*SPP[6] - P[21][12]*SPP[7] - (P[21][10]*q0)/2; + nextP[21][2] = P[21][2] + P[21][0]*SF[4] + P[21][1]*SF[8] + P[21][3]*SF[6] + P[21][12]*SF[11] - P[21][10]*SPP[6] - (P[21][11]*q0)/2; + nextP[21][3] = P[21][3] + P[21][0]*SF[5] + P[21][1]*SF[4] + P[21][2]*SF[7] - P[21][11]*SF[11] + P[21][10]*SPP[7] - (P[21][12]*q0)/2; + nextP[21][4] = P[21][4] + P[21][1]*SF[1] + P[21][0]*SF[3] + P[21][2]*SPP[0] - P[21][3]*SPP[2] - P[21][13]*SPP[4]; + nextP[21][5] = P[21][5] + P[21][0]*SF[2] + P[21][2]*SF[1] + P[21][3]*SF[3] - P[21][1]*SPP[0] + P[21][13]*SPP[3]; + nextP[21][6] = P[21][6] + P[21][1]*SF[2] + P[21][3]*SF[1] + P[21][0]*SPP[0] - P[21][2]*SPP[1] - P[21][13]*SPP[5]; + nextP[21][7] = P[21][7] + P[21][4]*dt; + nextP[21][8] = P[21][8] + P[21][5]*dt; + nextP[21][9] = P[21][9] + P[21][6]*dt; + nextP[21][10] = P[21][10]; + nextP[21][11] = P[21][11]; + nextP[21][12] = P[21][12]; + nextP[21][13] = P[21][13]; + nextP[21][14] = P[21][14]; + nextP[21][15] = P[21][15]; + nextP[21][16] = P[21][16]; + nextP[21][17] = P[21][17]; + nextP[21][18] = P[21][18]; + nextP[21][19] = P[21][19]; + nextP[21][20] = P[21][20]; + nextP[21][21] = P[21][21]; + nextP[21][22] = P[21][22]; + nextP[22][0] = P[22][0] + P[22][1]*SF[7] + P[22][2]*SF[9] + P[22][3]*SF[8] + P[22][10]*SF[11] + P[22][11]*SPP[7] + P[22][12]*SPP[6]; + nextP[22][1] = P[22][1] + P[22][0]*SF[6] + P[22][2]*SF[5] + P[22][3]*SF[9] + P[22][11]*SPP[6] - P[22][12]*SPP[7] - (P[22][10]*q0)/2; + nextP[22][2] = P[22][2] + P[22][0]*SF[4] + P[22][1]*SF[8] + P[22][3]*SF[6] + P[22][12]*SF[11] - P[22][10]*SPP[6] - (P[22][11]*q0)/2; + nextP[22][3] = P[22][3] + P[22][0]*SF[5] + P[22][1]*SF[4] + P[22][2]*SF[7] - P[22][11]*SF[11] + P[22][10]*SPP[7] - (P[22][12]*q0)/2; + nextP[22][4] = P[22][4] + P[22][1]*SF[1] + P[22][0]*SF[3] + P[22][2]*SPP[0] - P[22][3]*SPP[2] - P[22][13]*SPP[4]; + nextP[22][5] = P[22][5] + P[22][0]*SF[2] + P[22][2]*SF[1] + P[22][3]*SF[3] - P[22][1]*SPP[0] + P[22][13]*SPP[3]; + nextP[22][6] = P[22][6] + P[22][1]*SF[2] + P[22][3]*SF[1] + P[22][0]*SPP[0] - P[22][2]*SPP[1] - P[22][13]*SPP[5]; + nextP[22][7] = P[22][7] + P[22][4]*dt; + nextP[22][8] = P[22][8] + P[22][5]*dt; + nextP[22][9] = P[22][9] + P[22][6]*dt; + nextP[22][10] = P[22][10]; + nextP[22][11] = P[22][11]; + nextP[22][12] = P[22][12]; + nextP[22][13] = P[22][13]; + nextP[22][14] = P[22][14]; + nextP[22][15] = P[22][15]; + nextP[22][16] = P[22][16]; + nextP[22][17] = P[22][17]; + nextP[22][18] = P[22][18]; + nextP[22][19] = P[22][19]; + nextP[22][20] = P[22][20]; + nextP[22][21] = P[22][21]; + nextP[22][22] = P[22][22]; for (unsigned i = 0; i < n_states; i++) { @@ -826,16 +931,24 @@ void AttPosEKF::CovariancePrediction(float dt) // setting the coresponding covariance terms to zero. if (onGround || !useCompass) { - zeroRows(nextP,15,20); - zeroCols(nextP,15,20); + zeroRows(nextP,16,21); + zeroCols(nextP,16,21); } // If on ground or not using airspeed sensing, inhibit wind velocity // covariance growth. if (onGround || !useAirspeed) { - zeroRows(nextP,13,14); - zeroCols(nextP,13,14); + zeroRows(nextP,14,15); + zeroCols(nextP,14,15); + } + + // If on ground, inhibit terrain offset updates by + // setting the coresponding covariance terms to zero. + if (onGround) + { + zeroRows(nextP,22,22); + zeroCols(nextP,22,22); } // If the total position variance exceds 1E6 (1000m), then stop covariance @@ -857,7 +970,7 @@ void AttPosEKF::CovariancePrediction(float dt) if (onGround || staticMode) { // copy the portion of the variances we want to // propagate - for (unsigned i = 0; i < 14; i++) { + for (unsigned i = 0; i <= 13; i++) { P[i][i] = nextP[i][i]; // force symmetry for observable states @@ -866,7 +979,7 @@ void AttPosEKF::CovariancePrediction(float dt) { for (uint8_t j = 0; j < i; j++) { - if ((i > 12) || (j > 12)) { + if ((i > 13) || (j > 13)) { P[i][j] = 0.0f; } else { P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); @@ -946,12 +1059,12 @@ void AttPosEKF::FuseVelposNED() // 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[0] = sq(vneSigma) + 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; + R_OBS[2] = sq(vdSigma) + sq(velErr); + R_OBS[3] = sq(posNeSigma) + sq(posErr); + R_OBS[4] = R_OBS[3]; + R_OBS[5] = sq(posDSigma) + sq(posErr); // Set innovation variances to zero default for (uint8_t i = 0; i<=5; i++) @@ -1046,11 +1159,11 @@ void AttPosEKF::FuseVelposNED() // Limit range of states modified when on ground if(!onGround) { - indexLimit = 20; + indexLimit = 22; } else { - indexLimit = 12; + indexLimit = 13; } // Fuse measurements sequentially for (obsIndex=0; obsIndex<=5; obsIndex++) @@ -1080,6 +1193,12 @@ void AttPosEKF::FuseVelposNED() { Kfusion[i] = P[i][stateIndex]*SK; } + + // Don't update Z accel bias state unless using a height observation (GPS velocities can be biased) + if (obsIndex != 5) { + Kfusion[13] = 0; + } + // Calculate state corrections and re-normalise the quaternions for (uint8_t i = 0; i<=indexLimit; i++) { @@ -1117,24 +1236,38 @@ void AttPosEKF::FuseVelposNED() 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; + static uint8_t obsIndex; + static float MagPred[3]; + static float SH_MAG[9]; + 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 magXbias = 0.0f; + static float magYbias = 0.0f; + static float magZbias = 0.0f; + float R_MAG = sq(magMeasurementSigma); 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}; + float H_MAG[n_states]; + for (uint8_t i = 0; i < n_states; i++) { + H_MAG[i] = 0.0f; + } + uint8_t indexLimit; // Perform sequential fusion of Magnetometer measurements. // This assumes that the errors in the different components are @@ -1147,46 +1280,30 @@ void AttPosEKF::FuseMagnetometer() // Limit range of states modified when on ground if(!onGround) { - indexLimit = 20; + indexLimit = 22; } else { - indexLimit = 12; + indexLimit = 13; } - 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]; + magN = statesAtMagMeasTime[16]; + magE = statesAtMagMeasTime[17]; + magD = statesAtMagMeasTime[18]; + magXbias = statesAtMagMeasTime[19]; + magYbias = statesAtMagMeasTime[20]; + magZbias = statesAtMagMeasTime[21]; // rotate predicted earth components into body axes and calculate // predicted measurments @@ -1204,7 +1321,7 @@ void AttPosEKF::FuseMagnetometer() 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); + R_MAG = sq(magMeasurementSigma) + sq(0.05f*dAngIMU.length()/dtIMU); // Calculate observation jacobians SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; @@ -1217,135 +1334,145 @@ void AttPosEKF::FuseMagnetometer() SH_MAG[7] = 2*magN*q0; SH_MAG[8] = 2*magE*q3; - for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + for (uint8_t i = 0; i < n_states; 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; + H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[17] = 2*q0*q3 + 2*q1*q2; + H_MAG[18] = 2*q1*q3 - 2*q0*q2; + H_MAG[19] = 1.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[0] = 1/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); 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]); + Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[5] - P[0][18]*SK_MX[4]); + Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[5] - P[1][18]*SK_MX[4]); + Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[5] - P[2][18]*SK_MX[4]); + Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[5] - P[3][18]*SK_MX[4]); + Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[5] - P[4][18]*SK_MX[4]); + Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[5] - P[5][18]*SK_MX[4]); + Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[5] - P[6][18]*SK_MX[4]); + Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[5] - P[7][18]*SK_MX[4]); + Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[5] - P[8][18]*SK_MX[4]); + Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[5] - P[9][18]*SK_MX[4]); + Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[5] - P[10][18]*SK_MX[4]); + Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[5] - P[11][18]*SK_MX[4]); + Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]); + Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); + Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); + Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]); + Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]); + Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]); + Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]); + Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); + Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); + 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 { // Calculate observation jacobians - for (unsigned int i=0; i 1e-12f) { - for (uint8_t j= 0; j<=3; j++) + for (uint8_t j= 0; j <= 3; j++) { float quatMagInv = 1.0f/quatMag; states[j] = states[j] * quatMagInv; @@ -1513,38 +1642,38 @@ void AttPosEKF::FuseAirspeed() // 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 i = 0; i <= 22; i++) { - for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0; - for (uint8_t j = 4; j<=6; j++) + 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++) + for (uint8_t j = 7; j <= 13; j++) KH[i][j] = 0.0; + for (uint8_t j = 14; j <= 15; j++) { KH[i][j] = Kfusion[i] * H_TAS[j]; } - for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0; + for (uint8_t j = 16; j <= 22; j++) KH[i][j] = 0.0; } - for (uint8_t i = 0; i<=20; i++) + for (uint8_t i = 0; i <= 22; i++) { - for (uint8_t j = 0; j<=20; j++) + for (uint8_t j = 0; j <= 22; j++) { KHP[i][j] = 0.0; - for (uint8_t k = 4; k<=6; k++) + 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++) + for (uint8_t k = 14; k <= 15; k++) { KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; } } } - for (uint8_t i = 0; i<=20; i++) + for (uint8_t i = 0; i <= 22; i++) { - for (uint8_t j = 0; j<=20; j++) + for (uint8_t j = 0; j <= 22; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1556,6 +1685,343 @@ void AttPosEKF::FuseAirspeed() ConstrainVariances(); } +void AttPosEKF::FuseRangeFinder() +{ + + // Local variables + float rngPred; + float SH_RNG[5]; + float H_RNG[23]; + float SK_RNG[6]; + float cosRngTilt; + const float R_RNG = 0.25f; // 0.5 m2 rangefinder measurement variance + + // Copy required states to local variable names + float q0 = statesAtRngTime[0]; + float q1 = statesAtRngTime[1]; + float q2 = statesAtRngTime[2]; + float q3 = statesAtRngTime[3]; + float pd = statesAtRngTime[9]; + float ptd = statesAtRngTime[22]; + + // 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); + if (useRangeFinder && cosRngTilt > 0.87f) + { + // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset + // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations + SH_RNG[0] = SH_RNG[4]*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); + SH_RNG[1] = pd - ptd; + SH_RNG[2] = 1/sq(SH_RNG[0]); + SH_RNG[3] = 1/SH_RNG[0]; + for (uint8_t i = 0; i < n_states; i++) { + H_RNG[i] = 0.0f; + Kfusion[i] = 0.0f; + } + H_RNG[22] = -SH_RNG[3]; + SK_RNG[0] = 1/(R_RNG + SH_RNG[3]*(P[9][9]*SH_RNG[3] - P[22][9]*SH_RNG[3] + P[0][9]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][9]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][9]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][9]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[3]*(P[9][22]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[0][22]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][22]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][22]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][22]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4])*(P[9][0]*SH_RNG[3] - P[22][0]*SH_RNG[3] + P[0][0]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][0]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][0]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][0]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4])*(P[9][1]*SH_RNG[3] - P[22][1]*SH_RNG[3] + P[0][1]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][1]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][1]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][1]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4])*(P[9][2]*SH_RNG[3] - P[22][2]*SH_RNG[3] + P[0][2]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][2]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][2]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][2]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])*(P[9][3]*SH_RNG[3] - P[22][3]*SH_RNG[3] + P[0][3]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][3]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][3]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][3]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4]))); + SK_RNG[1] = 2*q1 - 2*q3*SH_RNG[4]; + SK_RNG[2] = 2*q0 - 2*q2*SH_RNG[4]; + SK_RNG[3] = 2*q3 + 2*q1*SH_RNG[4]; + SK_RNG[4] = 2*q2 + 2*q0*SH_RNG[4]; + SK_RNG[5] = SH_RNG[2]; + Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]); + + // Calculate the measurement innovation + rngPred = (ptd - pd)/cosRngTilt; + innovRng = rngPred - rngMea; + //printf("mea=%5.1f, pred=%5.1f, pd=%5.1f, ptd=%5.2f\n", rngMea, rngPred, pd, ptd); + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovRng*innovRng*SK_RNG[0]) < 25) + { + // correct the state vector + states[22] = states[22] - Kfusion[22] * innovRng; + + // correct the covariance P = (I - K*H)*P + P[22][22] = P[22][22] - Kfusion[22] * H_RNG[22] * P[22][22]; + P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); + } + } + +} + +void AttPosEKF::FuseOpticalFlow() +{ + static uint8_t obsIndex; + static float losPred[2]; + static float SH_LOS[13]; + static float SK_LOS[17]; + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float vn = 0.0f; + static float ve = 0.0f; + static float vd = 0.0f; + static float pd = 0.0f; + static float ptd = 0.0f; + static float R_LOS = 0.01f; + // Transformation matrix from body to navigation axes + Mat3f DCM; + // Transformation matrix from sensor to body axes + // assume camera is aligned with Z body axis plus a misalignment + // defined by 3 small angles about X, Y and Z body axis + Mat3f Tsb; + Tsb.x.y = -a3; + Tsb.y.x = a3; + Tsb.x.z = a2; + Tsb.z.x = -a2; + Tsb.y.z = -a1; + Tsb.z.y = a1; + // Transformation matrix from navigation to sensor axes + Mat3f Tns; + float H_LOS[n_states]; + for (uint8_t i = 0; i < n_states; i++) { + H_LOS[i] = 0.0f; + } + Vector3f velNED; + Vector3f relVelSensor; + +// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. + if (useOpticalFlow && (fuseOptData || obsIndex == 1) && !onGround && Tbn.z.z > 0.866f) + { + // Sequential fusion of XY components to spread processing load across + // three prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseOptData) + { + // Copy required states to local variable names + q0 = statesAtLosMeasTime[0]; + q1 = statesAtLosMeasTime[1]; + q2 = statesAtLosMeasTime[2]; + q3 = statesAtLosMeasTime[3]; + vn = statesAtLosMeasTime[4]; + ve = statesAtLosMeasTime[5]; + vd = statesAtLosMeasTime[6]; + pd = statesAtLosMeasTime[9]; + ptd = statesAtLosMeasTime[2]; + velNED.x = vn; + velNED.y = ve; + velNED.z = vd; + + // rotate predicted earth components into body axes and calculate + // predicted measurments + float q01 = q0 * q1; + float q02 = q0 * q2; + float q03 = q0 * q3; + float q12 = q1 * q2; + float q13 = q1 * q3; + float q23 = q2 * q3; + DCM.x.x = q0 + q1 - q2 - q3; + DCM.y.y = q0 - q1 + q2 - q3; + DCM.z.z = q0 - q1 - q2 + q3; + DCM.x.y = 2*(q12 - q03); + DCM.x.z = 2*(q13 + q02); + DCM.y.x = 2*(q12 + q03); + DCM.y.z = 2*(q23 - q01); + DCM.z.x = 2*(q13 - q02); + DCM.z.y = 2*(q23 + q01); + // calculate transformation from NED to sensor axes + Tns = DCM*Tsb; + Tns = Tns.transpose(); + // calculate range from ground plain to centre of sensor fov assuming flat earth + float range = ((ptd - pd)/Tns.z.z); + // calculate relative velocity in sensor frame + relVelSensor = Tns*velNED; + // divide by range to get predicted angular LOS rates relative to X and Y axes + losPred[0] = relVelSensor.y/range; + losPred[1] = -relVelSensor.x/range; + + // scale optical flow observation error with total angular rate + R_LOS = 0.01f;// + sq(0.05f*dAngIMU.length()/dtIMU); + + // Calculate observation jacobians + SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); + SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); + SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); + SH_LOS[3] = 1/(pd - ptd); + SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; + SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; + SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; + SH_LOS[7] = 1/sq(pd - ptd); + SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; + SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; + SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; + SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; + SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; + + for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; + H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); + H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); + H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); + H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); + H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); + H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); + H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + + // Calculate Kalman gain + SK_LOS[0] = 1/(R_LOS + (SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]))*(P[0][0]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][0]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][0]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][0]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][0]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][0]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]))*(P[0][1]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][1]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][1]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][1]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][1]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][1]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]))*(P[0][2]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][2]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][2]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][2]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][2]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][2]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - (SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]))*(P[0][3]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][3]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][3]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][3]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][3]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][3]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][9]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][9]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][9]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][9]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][9]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][9]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][22]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][22]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][22]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][22]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][22]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][22]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3))*(P[0][4]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][4]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][4]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][4]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][4]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][4]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3))*(P[0][5]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][5]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][5]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][5]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][5]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][5]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))*(P[0][6]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][6]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][6]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][6]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][6]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][6]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)))); + SK_LOS[1] = 1/(R_LOS + (SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]))*(P[0][0]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][0]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][0]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]))*(P[0][1]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][1]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][1]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]))*(P[0][2]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][2]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][2]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][2]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][2]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - (SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]))*(P[0][3]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][3]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][3]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][3]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][3]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][9]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][9]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][9]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][9]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][9]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][22]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][22]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][22]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][22]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][22]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3))*(P[0][4]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][4]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][4]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][4]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][4]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3))*(P[0][5]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][5]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][5]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][5]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][5]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))*(P[0][6]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][6]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][6]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][6]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][6]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)))); + SK_LOS[2] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); + SK_LOS[3] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); + SK_LOS[4] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); + SK_LOS[5] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); + SK_LOS[6] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); + SK_LOS[7] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); + SK_LOS[8] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); + SK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); + SK_LOS[10] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); + SK_LOS[11] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); + SK_LOS[12] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); + SK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); + SK_LOS[14] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); + SK_LOS[15] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); + SK_LOS[16] = SH_LOS[0]; + + Kfusion[0] = -SK_LOS[0]*(P[0][0]*SK_LOS[11] + P[0][1]*SK_LOS[10] - P[0][3]*SK_LOS[9] + P[0][2]*SK_LOS[12] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[0][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[0][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[1] = -SK_LOS[0]*(P[1][0]*SK_LOS[11] + P[1][1]*SK_LOS[10] - P[1][3]*SK_LOS[9] + P[1][2]*SK_LOS[12] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[1][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[1][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[2] = -SK_LOS[0]*(P[2][0]*SK_LOS[11] + P[2][1]*SK_LOS[10] - P[2][3]*SK_LOS[9] + P[2][2]*SK_LOS[12] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[2][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[2][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[3] = -SK_LOS[0]*(P[3][0]*SK_LOS[11] + P[3][1]*SK_LOS[10] - P[3][3]*SK_LOS[9] + P[3][2]*SK_LOS[12] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[3][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[3][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[4] = -SK_LOS[0]*(P[4][0]*SK_LOS[11] + P[4][1]*SK_LOS[10] - P[4][3]*SK_LOS[9] + P[4][2]*SK_LOS[12] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[4][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[4][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[5] = -SK_LOS[0]*(P[5][0]*SK_LOS[11] + P[5][1]*SK_LOS[10] - P[5][3]*SK_LOS[9] + P[5][2]*SK_LOS[12] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[5][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[5][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[6] = -SK_LOS[0]*(P[6][0]*SK_LOS[11] + P[6][1]*SK_LOS[10] - P[6][3]*SK_LOS[9] + P[6][2]*SK_LOS[12] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[6][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[6][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[7] = -SK_LOS[0]*(P[7][0]*SK_LOS[11] + P[7][1]*SK_LOS[10] - P[7][3]*SK_LOS[9] + P[7][2]*SK_LOS[12] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[7][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[7][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[8] = -SK_LOS[0]*(P[8][0]*SK_LOS[11] + P[8][1]*SK_LOS[10] - P[8][3]*SK_LOS[9] + P[8][2]*SK_LOS[12] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[8][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[8][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[9] = -SK_LOS[0]*(P[9][0]*SK_LOS[11] + P[9][1]*SK_LOS[10] - P[9][3]*SK_LOS[9] + P[9][2]*SK_LOS[12] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[9][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[9][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[10] = -SK_LOS[0]*(P[10][0]*SK_LOS[11] + P[10][1]*SK_LOS[10] - P[10][3]*SK_LOS[9] + P[10][2]*SK_LOS[12] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[10][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[10][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[11] = -SK_LOS[0]*(P[11][0]*SK_LOS[11] + P[11][1]*SK_LOS[10] - P[11][3]*SK_LOS[9] + P[11][2]*SK_LOS[12] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[11][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[11][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[12] = -SK_LOS[0]*(P[12][0]*SK_LOS[11] + P[12][1]*SK_LOS[10] - P[12][3]*SK_LOS[9] + P[12][2]*SK_LOS[12] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[12][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[12][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[13] = -SK_LOS[0]*(P[13][0]*SK_LOS[11] + P[13][1]*SK_LOS[10] - P[13][3]*SK_LOS[9] + P[13][2]*SK_LOS[12] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[13][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[13][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[14] = -SK_LOS[0]*(P[14][0]*SK_LOS[11] + P[14][1]*SK_LOS[10] - P[14][3]*SK_LOS[9] + P[14][2]*SK_LOS[12] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[14][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[14][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[15] = -SK_LOS[0]*(P[15][0]*SK_LOS[11] + P[15][1]*SK_LOS[10] - P[15][3]*SK_LOS[9] + P[15][2]*SK_LOS[12] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[15][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[15][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[16] = -SK_LOS[0]*(P[16][0]*SK_LOS[11] + P[16][1]*SK_LOS[10] - P[16][3]*SK_LOS[9] + P[16][2]*SK_LOS[12] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[16][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[16][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[17] = -SK_LOS[0]*(P[17][0]*SK_LOS[11] + P[17][1]*SK_LOS[10] - P[17][3]*SK_LOS[9] + P[17][2]*SK_LOS[12] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[17][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[17][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[18] = -SK_LOS[0]*(P[18][0]*SK_LOS[11] + P[18][1]*SK_LOS[10] - P[18][3]*SK_LOS[9] + P[18][2]*SK_LOS[12] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[18][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[18][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[19] = -SK_LOS[0]*(P[19][0]*SK_LOS[11] + P[19][1]*SK_LOS[10] - P[19][3]*SK_LOS[9] + P[19][2]*SK_LOS[12] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[19][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[19][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[20] = -SK_LOS[0]*(P[20][0]*SK_LOS[11] + P[20][1]*SK_LOS[10] - P[20][3]*SK_LOS[9] + P[20][2]*SK_LOS[12] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[20][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[20][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[21] = -SK_LOS[0]*(P[21][0]*SK_LOS[11] + P[21][1]*SK_LOS[10] - P[21][3]*SK_LOS[9] + P[21][2]*SK_LOS[12] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[21][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[21][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[22] = -SK_LOS[0]*(P[22][0]*SK_LOS[11] + P[22][1]*SK_LOS[10] - P[22][3]*SK_LOS[9] + P[22][2]*SK_LOS[12] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[22][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[22][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + varInnovLOS[0] = 1.0f/SK_LOS[0]; + innovLOS[0] = losPred[0] - losData[0]; + + // reset the observation index to 0 (we start by fusing the X + // measurement) + obsIndex = 0; + fuseOptData = false; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; + H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); + H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); + H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); + H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); + H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); + H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); + H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + + // Calculate Kalman gains + Kfusion[0] = SK_LOS[1]*(P[0][0]*SK_LOS[13] - P[0][3]*SK_LOS[6] + P[0][1]*SK_LOS[14] + P[0][2]*SK_LOS[15] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[0][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[1] = SK_LOS[1]*(P[1][0]*SK_LOS[13] - P[1][3]*SK_LOS[6] + P[1][1]*SK_LOS[14] + P[1][2]*SK_LOS[15] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[1][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[2] = SK_LOS[1]*(P[2][0]*SK_LOS[13] - P[2][3]*SK_LOS[6] + P[2][1]*SK_LOS[14] + P[2][2]*SK_LOS[15] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[2][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[3] = SK_LOS[1]*(P[3][0]*SK_LOS[13] - P[3][3]*SK_LOS[6] + P[3][1]*SK_LOS[14] + P[3][2]*SK_LOS[15] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[3][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[4] = SK_LOS[1]*(P[4][0]*SK_LOS[13] - P[4][3]*SK_LOS[6] + P[4][1]*SK_LOS[14] + P[4][2]*SK_LOS[15] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[4][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[5] = SK_LOS[1]*(P[5][0]*SK_LOS[13] - P[5][3]*SK_LOS[6] + P[5][1]*SK_LOS[14] + P[5][2]*SK_LOS[15] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[5][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[6] = SK_LOS[1]*(P[6][0]*SK_LOS[13] - P[6][3]*SK_LOS[6] + P[6][1]*SK_LOS[14] + P[6][2]*SK_LOS[15] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[6][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[7] = SK_LOS[1]*(P[7][0]*SK_LOS[13] - P[7][3]*SK_LOS[6] + P[7][1]*SK_LOS[14] + P[7][2]*SK_LOS[15] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[7][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[8] = SK_LOS[1]*(P[8][0]*SK_LOS[13] - P[8][3]*SK_LOS[6] + P[8][1]*SK_LOS[14] + P[8][2]*SK_LOS[15] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[8][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[9] = SK_LOS[1]*(P[9][0]*SK_LOS[13] - P[9][3]*SK_LOS[6] + P[9][1]*SK_LOS[14] + P[9][2]*SK_LOS[15] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[9][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[10] = SK_LOS[1]*(P[10][0]*SK_LOS[13] - P[10][3]*SK_LOS[6] + P[10][1]*SK_LOS[14] + P[10][2]*SK_LOS[15] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[10][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[11] = SK_LOS[1]*(P[11][0]*SK_LOS[13] - P[11][3]*SK_LOS[6] + P[11][1]*SK_LOS[14] + P[11][2]*SK_LOS[15] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[11][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[12] = SK_LOS[1]*(P[12][0]*SK_LOS[13] - P[12][3]*SK_LOS[6] + P[12][1]*SK_LOS[14] + P[12][2]*SK_LOS[15] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[12][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[13] = SK_LOS[1]*(P[13][0]*SK_LOS[13] - P[13][3]*SK_LOS[6] + P[13][1]*SK_LOS[14] + P[13][2]*SK_LOS[15] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[13][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[14] = SK_LOS[1]*(P[14][0]*SK_LOS[13] - P[14][3]*SK_LOS[6] + P[14][1]*SK_LOS[14] + P[14][2]*SK_LOS[15] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[14][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[15] = SK_LOS[1]*(P[15][0]*SK_LOS[13] - P[15][3]*SK_LOS[6] + P[15][1]*SK_LOS[14] + P[15][2]*SK_LOS[15] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[15][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[16] = SK_LOS[1]*(P[16][0]*SK_LOS[13] - P[16][3]*SK_LOS[6] + P[16][1]*SK_LOS[14] + P[16][2]*SK_LOS[15] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[16][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[17] = SK_LOS[1]*(P[17][0]*SK_LOS[13] - P[17][3]*SK_LOS[6] + P[17][1]*SK_LOS[14] + P[17][2]*SK_LOS[15] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[17][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[18] = SK_LOS[1]*(P[18][0]*SK_LOS[13] - P[18][3]*SK_LOS[6] + P[18][1]*SK_LOS[14] + P[18][2]*SK_LOS[15] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[18][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[19] = SK_LOS[1]*(P[19][0]*SK_LOS[13] - P[19][3]*SK_LOS[6] + P[19][1]*SK_LOS[14] + P[19][2]*SK_LOS[15] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[19][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[20] = SK_LOS[1]*(P[20][0]*SK_LOS[13] - P[20][3]*SK_LOS[6] + P[20][1]*SK_LOS[14] + P[20][2]*SK_LOS[15] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[20][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[21] = SK_LOS[1]*(P[21][0]*SK_LOS[13] - P[21][3]*SK_LOS[6] + P[21][1]*SK_LOS[14] + P[21][2]*SK_LOS[15] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[21][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[22] = SK_LOS[1]*(P[22][0]*SK_LOS[13] - P[22][3]*SK_LOS[6] + P[22][1]*SK_LOS[14] + P[22][2]*SK_LOS[15] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[22][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + varInnovLOS[1] = 1.0f/SK_LOS[1]; + innovLOS[1] = losPred[1] - losData[1]; + } + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovLOS[obsIndex]*innovLOS[obsIndex]/varInnovMag[obsIndex]) < 25.0) + { + // correct the state vector + for (uint8_t j = 0; j < n_states; j++) + { + states[j] = states[j] - Kfusion[j] * innovLOS[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 < n_states; i++) + { + for (uint8_t j = 0; j <= 6; j++) + { + KH[i][j] = Kfusion[i] * H_LOS[j]; + } + for (uint8_t j = 7; j <= 8; j++) + { + KH[i][j] = 0.0f; + } + KH[i][9] = Kfusion[i] * H_LOS[9]; + for (uint8_t j = 10; j <= 21; j++) + { + KH[i][j] = 0.0f; + } + KH[i][22] = Kfusion[i] * H_LOS[22]; + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k <= 6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; + KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; + } + } + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + obsIndex = obsIndex + 1; + + ForceSymmetry(); + ConstrainVariances(); +} + void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; @@ -1619,51 +2085,54 @@ void AttPosEKF::ResetStoredStates() } // 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 AttPosEKF::RecallStates(float* statesForFusion, 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; - // } + int64_t bestTimeDelta = 200; + unsigned bestStoreIndex = 0; + for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + { + // Work around a GCC compiler bug - we know 64bit support on ARM is + // sketchy in GCC. + uint64_t 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 (msec > statetimeStamp[storeIndex]) { + timeDelta = msec - statetimeStamp[storeIndex]; + } else { + timeDelta = statetimeStamp[storeIndex] - msec; + } + + if (timeDelta < bestTimeDelta) + { + bestStoreIndex = storeIndex; + bestTimeDelta = timeDelta; + } + } + if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + { + for (unsigned 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 (unsigned i = 0; i < n_states; i++) { if (isfinite(states[i])) { statesForFusion[i] = states[i]; } else { ret++; } } - // } + } return ret; } @@ -1746,7 +2215,7 @@ void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, velNED[2] = gpsVelD; } -void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef) { posNED[0] = earthRadius * (lat - latRef); posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); @@ -1783,23 +2252,25 @@ void AttPosEKF::CovarianceInit() 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[4][4] = sq(0.7f); P[5][5] = P[4][4]; - P[6][6] = sq(0.7); - P[7][7] = sq(15.0); + P[6][6] = sq(0.7f); + P[7][7] = sq(15.0f); P[8][8] = P[7][7]; - P[9][9] = sq(5.0); - P[10][10] = sq(0.1*deg2rad*dtIMU); + P[9][9] = sq(5.0f); + P[10][10] = sq(0.1f*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]; + P[13][13] = sq(0.2f*dtIMU); + P[14][14] = sq(8.0f); + P[15][14] = P[14][14]; + P[16][16] = sq(0.02f); + P[17][17] = P[16][16]; + P[18][18] = P[16][16]; + P[19][19] = sq(0.02f); + P[20][20] = P[19][19]; + P[21][21] = P[19][19]; + P[22][22] = sq(0.5f); } float AttPosEKF::ConstrainFloat(float val, float min, float max) @@ -1818,45 +2289,52 @@ void AttPosEKF::ConstrainVariances() // 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: Delta Velocity bias - m/s (Z) + // 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) + // 22: Terrain offset - m // Constrain quaternion variances - for (unsigned i = 0; i < 4; i++) { + for (unsigned i = 0; i <= 3; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } - // Constrain velocitie variances - for (unsigned i = 4; i < 7; i++) { + // Constrain velocity variances + for (unsigned i = 4; i <= 6; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); } // Constrain position variances - for (unsigned i = 7; i < 10; i++) { + for (unsigned i = 7; i <= 9; 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)); + // Constrain delta angle bias variances + for (unsigned i = 10; i <= 12; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.12f * dtIMU)); } + // Constrain delta velocity bias variance + P[13][13] = ConstrainFloat(P[13][13], 0.0f, sq(1.0f * dtIMU)); + // Wind velocity variances - for (unsigned i = 13; i < 15; i++) { + for (unsigned i = 14; 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++) { + for (unsigned i = 16; 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++) { + for (unsigned i = 19; i <= 21; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } + // Constrain terrain offset variance + P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); } void AttPosEKF::ConstrainStates() @@ -1870,23 +2348,24 @@ void AttPosEKF::ConstrainStates() // 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: Delta Velocity bias - m/s (Z) + // 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) + // 22: Terrain offset - m // Constrain quaternion - for (unsigned i = 0; i < 4; i++) { + for (unsigned i = 0; i <= 3; 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++) { + for (unsigned i = 4; i <= 6; 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++) { + for (unsigned i = 7; i <= 8; i++) { states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); } @@ -1894,26 +2373,32 @@ void AttPosEKF::ConstrainStates() states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); // Angle bias limit - set to 8 degrees / sec - for (unsigned i = 10; i < 13; i++) { + for (unsigned i = 10; i <= 12; i++) { states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); } + // Constrain delta velocity bias + states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); + // Wind velocity limits - assume 120 m/s max velocity - for (unsigned i = 13; i < 15; i++) { + for (unsigned i = 14; 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++) { + for (unsigned i = 16; 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++) { + for (unsigned i = 19; i <= 21; i++) { states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); } + // Constrain terrain offset + states[22] = ConstrainFloat(states[22], -1000.0f, 1000.0f); + } void AttPosEKF::ForceSymmetry() @@ -2001,7 +2486,7 @@ void AttPosEKF::ResetVelocity(void) void AttPosEKF::FillErrorReport(struct ekf_status_report *err) { - for (int i = 0; i < n_states; i++) + for (unsigned i = 0; i < n_states; i++) { err->states[i] = states[i]; } @@ -2173,14 +2658,15 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) // 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 + 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 + states[16] = initMagNED.x; // Magnetic Field North + states[17] = initMagNED.y; // Magnetic Field East + states[18] = initMagNED.z; // Magnetic Field Down + states[19] = magBias.x; // Magnetic Field Bias X + states[20] = magBias.y; // Magnetic Field Bias Y + states[21] = magBias.z; // Magnetic Field Bias Z + states[22] = 0.0f; // terrain height statesInitialised = true; @@ -2239,10 +2725,10 @@ void AttPosEKF::ZeroVariables() void AttPosEKF::GetFilterState(struct ekf_status_report *state) { - memcpy(state, ¤t_ekf_state, sizeof(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)); + memcpy(last_error, &last_ekf_error, sizeof(*last_error)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e62f1a98a5..6effe062d4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -45,14 +45,9 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1); void swap_var(float &d1, float &d2); -const unsigned int n_states = 21; +const unsigned int n_states = 23; const unsigned int data_buffer_size = 50; -const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions -const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions - -// extern bool staticMode; - enum GPS_FIX { GPS_FIX_NOFIX = 0, GPS_FIX_2D = 2, @@ -82,6 +77,79 @@ 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 a1; // optical flow sensor misalgnment angle about X axis (rad) + float a2; // optical flow sensor misalgnment angle about Y axis (rad) + float a3; // optical flow sensor misalgnment angle about Z axis (rad) + + 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. + a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad) + a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad) + a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad) + + 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 @@ -96,6 +164,8 @@ public: 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 + float statesAtRngTime[n_states]; // filter states at the effective measurement time + float statesAtLosMeasTime[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) @@ -104,6 +174,10 @@ 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) + + Mat3f Tbn; // transformation matrix from body to NED coordinates + Mat3f Tnb; // transformation amtrix from NED to body coordinates + Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) Vector3f dVelIMU; Vector3f dAngIMU; @@ -115,26 +189,30 @@ public: float velNED[3]; // North, East, Down velocity obs (m/s) float posNE[2]; // North, East position obs (m) float hgtMea; // measured height (m) + float rngMea; // Ground distance float posNED[3]; // North, East Down position (m) - float innovMag[3]; // innovation output - float varInnovMag[3]; // innovation variance output + float innovMag[3]; // innovation output for magnetometer measurements + float varInnovMag[3]; // innovation variance output for magnetometer measurements + float varInnovLOS[2]; // innovation variance output for optical flow measurements Vector3f magData; // magnetometer flux radings in X,Y,Z body axes - float innovVtas; // innovation output + float innovVtas; // innovation output for true airspeed measurements + float innovRng; ///< Range finder innovation for rnge finder measurements + float innovLOS[2]; // Innovations for optical flow LOS rate measurements + float losData[2]; // Optical flow LOS rate measurements float varInnovVtas; // innovation variance output float VtasMeas; // true airspeed measurement (m/s) - float latRef; // WGS-84 latitude of reference point (rad) - float lonRef; // WGS-84 longitude of reference point (rad) + double latRef; // WGS-84 latitude of reference point (rad) + double 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 - float EAS2TAS; // ratio f true to equivalent airspeed + unsigned 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; + double gpsLat; + double gpsLon; float gpsHgt; uint8_t GPSstatus; @@ -148,11 +226,15 @@ public: 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 fuseRngData; ///< true when range data is fused + bool fuseOptData; // true when optical flow data is 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 + bool useRangeFinder; ///< true when rangefinder is being used + bool useOpticalFlow; // true when optical flow data is being used struct ekf_status_report current_ekf_state; struct ekf_status_report last_ekf_error; @@ -172,6 +254,10 @@ void FuseMagnetometer(); void FuseAirspeed(); +void FuseRangeFinder(); + +void FuseOpticalFlow(); + 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); @@ -192,7 +278,7 @@ void StoreStates(uint64_t timestamp_ms); * time-wise where valid states were updated and invalid remained at the old * value. */ -int RecallStates(float statesForFusion[n_states], uint64_t msec); +int RecallStates(float *statesForFusion, uint64_t msec); void ResetStoredStates(); @@ -206,7 +292,7 @@ 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 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); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 7857a04693..8715ba49a4 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -210,6 +210,7 @@ private: float abias_pnoise; float mage_pnoise; float magb_pnoise; + float eas_noise; } _parameters; /**< local copies of interesting parameters */ struct { @@ -229,6 +230,7 @@ private: param_t abias_pnoise; param_t mage_pnoise; param_t magb_pnoise; + param_t eas_noise; } _parameter_handles; /**< handles for interesting parameters */ AttPosEKF *_ekf; @@ -335,6 +337,7 @@ FixedwingEstimator::FixedwingEstimator() : _parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE"); _parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE"); _parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE"); + _parameter_handles.eas_noise = param_find("PE_EAS_NOISE"); /* fetch initial parameter values */ parameters_update(); @@ -410,6 +413,25 @@ FixedwingEstimator::parameters_update() param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise)); param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise)); param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise)); + param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise)); + + if (_ekf) { + // _ekf->yawVarScale = 1.0f; + // _ekf->windVelSigma = 0.1f; + _ekf->dAngBiasSigma = _parameters.gbias_pnoise; + _ekf->dVelBiasSigma = _parameters.abias_pnoise; + _ekf->magEarthSigma = _parameters.mage_pnoise; + _ekf->magBodySigma = _parameters.magb_pnoise; + // _ekf->gndHgtSigma = 0.02f; + _ekf->vneSigma = _parameters.velne_noise; + _ekf->vdSigma = _parameters.veld_noise; + _ekf->posNeSigma = _parameters.posne_noise; + _ekf->posDSigma = _parameters.posd_noise; + _ekf->magMeasurementSigma = _parameters.mag_noise; + _ekf->gyroProcessNoise = _parameters.gyro_pnoise; + _ekf->accelProcessNoise = _parameters.acc_pnoise; + _ekf->airspeedMeasurementSigma = _parameters.eas_noise; + } return OK; } @@ -473,6 +495,7 @@ FixedwingEstimator::task_main() orb_set_interval(_sensor_combined_sub, 4); #endif + /* sets also parameters in the EKF object */ parameters_update(); /* set initial filter state */ @@ -914,7 +937,7 @@ FixedwingEstimator::task_main() // perform a covariance prediction if the total delta angle has exceeded the limit // or the time limit will be exceeded at the next IMU update - if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) { + if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { _ekf->CovariancePrediction(dt); _ekf->summedDelAng = _ekf->summedDelAng.zero(); _ekf->summedDelVel = _ekf->summedDelVel.zero(); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c index 9d01a095c8..cfcd99858e 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c @@ -116,7 +116,19 @@ PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210); PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f); /** - * Velocity noise in north-east (horizontal) direction. + * Airspeed measurement noise. + * + * Increasing this value will make the filter trust this sensor + * less and trust other sensors more. + * + * @min 0.5 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f); + +/** + * Velocity measurement noise in north-east (horizontal) direction. * * Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5 * From 46a796fb86986c5172ab4d85d1902e7648afd651 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:04:56 +0200 Subject: [PATCH 05/58] Added home position switch on GPS position - gives a more reliable home position setup --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef9..7ac7aff0f2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,9 +1092,9 @@ int commander_thread_main(int argc, char *argv[]) && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; + home.lat = gps_position.lat / (double)1e7; + home.lon = gps_position.lon / (double)1e7; + home.alt = gps_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); From 7cad27a0243a806aa374ffda4ef9e99a854e1c16 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:06:13 +0200 Subject: [PATCH 06/58] Changed home position set to depend on the commander home position switch --- .../fw_att_pos_estimator_main.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 19333accd1..8f39a6330a 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -152,6 +153,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _mission_sub; + int _home_sub; /**< home position as defined by commander / user */ orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ @@ -476,6 +478,7 @@ FixedwingEstimator::task_main() _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _home_sub = orb_subscribe(ORB_ID(home_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -864,20 +867,27 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (_ekf->GPSstatus == 3)) { + bool home_set; + orb_check(_home_sub, &home_set); + + if (home_set && !_gps_initialized && _gps.fix_type > 2) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - double lat = _gps.lat * 1e-7; - double lon = _gps.lon * 1e-7; - float alt = _gps.alt * 1e-3; + struct home_position_s home; + + orb_copy(ORB_ID(home_position), _home_sub, &home); + + double lat = home.lat; + double lon = home.lon; + float alt = home.alt; _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.lon; + _local_pos.ref_lat = home.lat * 1e7; + _local_pos.ref_lon = home.lon * 1e7; _local_pos.ref_alt = alt; _local_pos.ref_timestamp = _gps.timestamp_position; From 9c5dbeef3a8c9024d710e95dcce3d877ba357656 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:39:43 +0200 Subject: [PATCH 07/58] Proper zero init of the filter --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index ebe30cae0e..94fb31457d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2711,6 +2711,8 @@ void AttPosEKF::ZeroVariables() states[i] = 0.0f; // state matrix } + correctedDelAng.zero(); + for (unsigned i = 0; i < data_buffer_size; i++) { for (unsigned j = 0; j < n_states; j++) { From b37d0f8f2e3cb0962155113b07a01830c189d4ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:41:34 +0200 Subject: [PATCH 08/58] Safety checks, prepared to use GPS variance --- .../fw_att_pos_estimator_main.cpp | 47 +++++++++++++++---- 1 file changed, 38 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 19333accd1..9d00f1d17b 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -319,7 +319,7 @@ FixedwingEstimator::FixedwingEstimator() : _ekf(nullptr) { - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + 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"); @@ -461,6 +461,7 @@ float dt = 0.0f; // time lapsed since last covariance prediction void FixedwingEstimator::task_main() { + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); @@ -600,17 +601,23 @@ FixedwingEstimator::task_main() last_run = _gyro.timestamp; /* guard against too large deltaT's */ - if (deltaT > 1.0f) + if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; + warnx("TS fail"); + } // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; - _ekf->angRate.x = _gyro.x; - _ekf->angRate.y = _gyro.y; - _ekf->angRate.z = _gyro.z; + if (isfinite(_gyro.x) && + isfinite(_gyro.y) && + isfinite(_gyro.z)) { + _ekf->angRate.x = _gyro.x; + _ekf->angRate.y = _gyro.y; + _ekf->angRate.z = _gyro.z; + } _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; @@ -643,16 +650,22 @@ FixedwingEstimator::task_main() last_run = _sensor_combined.timestamp; /* guard against too large deltaT's */ - if (deltaT > 1.0f || deltaT < 0.000001f) + if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; + warnx("TS fail"); + } // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; - _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + if (isfinite(_sensor_combined.gyro_rad_s[0]) && + isfinite(_sensor_combined.gyro_rad_s[1]) && + isfinite(_sensor_combined.gyro_rad_s[2])) { + _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; + _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; + _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + } _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; @@ -725,6 +738,21 @@ FixedwingEstimator::task_main() _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; + + // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { + // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); + // } else { + // _ekf->vneSigma = _parameters.velne_noise; + // } + + // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { + // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); + // } else { + // _ekf->posNeSigma = _parameters.posne_noise; + // } + + // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + newDataGps = true; } @@ -851,6 +879,7 @@ FixedwingEstimator::task_main() } else { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); } + } From 906abbcbb6215e9ae30c51efe3b813a71a963615 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 21:04:05 +0200 Subject: [PATCH 09/58] mavlink: Only write to TX buf if space is available. This is working around a NuttX issue where overflowing the TX buf leads to being unable to send any further data --- src/modules/mavlink/mavlink_main.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a9f5f4de76..2d71bdce60 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -189,9 +189,18 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ if (instance->should_transmit()) { - ssize_t ret = write(uart, ch, desired); + + /* check if there is space in the buffer, let it overflow else */ + if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { + + if (desired > buf_free) { + desired = buf_free; + } + } + + ssize_t ret = write(uart, ch, desired); if (ret != desired) { - // XXX do something here, but change to using FIONWRITE and OS buf size for detection + warnx("TX FAIL"); } } From 1e202a228437c1fa8a04185a5476dcb3d4308759 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 01:18:13 +0200 Subject: [PATCH 10/58] Updated estimator, not using optical flow for now until proven on the bench --- .../ekf_att_pos_estimator/estimator.cpp | 334 ++---------------- src/modules/ekf_att_pos_estimator/estimator.h | 23 +- 2 files changed, 26 insertions(+), 331 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 94fb31457d..2a7dab890b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -7,13 +7,11 @@ float Vector3f::length(void) const return sqrt(x*x + y*y + z*z); } -Vector3f Vector3f::zero(void) const +void Vector3f::zero(void) { - Vector3f ret = *this; - ret.x = 0.0; - ret.y = 0.0; - ret.z = 0.0; - return ret; + x = 0.0f; + y = 0.0f; + z = 0.0f; } Mat3f::Mat3f() { @@ -69,25 +67,6 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn) return vecOut; } -// overload * operator to provide a matrix product -Mat3f operator*( Mat3f matIn1, Mat3f matIn2) -{ - Mat3f matOut; - matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x; - matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y; - matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z; - - matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x; - matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y; - matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z; - - matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x; - matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y; - matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z; - - return matOut; -} - // overload % operator to provide a vector cross product Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) { @@ -1685,6 +1664,19 @@ void AttPosEKF::FuseAirspeed() 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 0.866f) - { - // Sequential fusion of XY components to spread processing load across - // three prediction time steps. - - // Calculate observation jacobians and Kalman gains - if (fuseOptData) - { - // Copy required states to local variable names - q0 = statesAtLosMeasTime[0]; - q1 = statesAtLosMeasTime[1]; - q2 = statesAtLosMeasTime[2]; - q3 = statesAtLosMeasTime[3]; - vn = statesAtLosMeasTime[4]; - ve = statesAtLosMeasTime[5]; - vd = statesAtLosMeasTime[6]; - pd = statesAtLosMeasTime[9]; - ptd = statesAtLosMeasTime[2]; - velNED.x = vn; - velNED.y = ve; - velNED.z = vd; - - // rotate predicted earth components into body axes and calculate - // predicted measurments - float q01 = q0 * q1; - float q02 = q0 * q2; - float q03 = q0 * q3; - float q12 = q1 * q2; - float q13 = q1 * q3; - float q23 = q2 * q3; - DCM.x.x = q0 + q1 - q2 - q3; - DCM.y.y = q0 - q1 + q2 - q3; - DCM.z.z = q0 - q1 - q2 + q3; - DCM.x.y = 2*(q12 - q03); - DCM.x.z = 2*(q13 + q02); - DCM.y.x = 2*(q12 + q03); - DCM.y.z = 2*(q23 - q01); - DCM.z.x = 2*(q13 - q02); - DCM.z.y = 2*(q23 + q01); - // calculate transformation from NED to sensor axes - Tns = DCM*Tsb; - Tns = Tns.transpose(); - // calculate range from ground plain to centre of sensor fov assuming flat earth - float range = ((ptd - pd)/Tns.z.z); - // calculate relative velocity in sensor frame - relVelSensor = Tns*velNED; - // divide by range to get predicted angular LOS rates relative to X and Y axes - losPred[0] = relVelSensor.y/range; - losPred[1] = -relVelSensor.x/range; - - // scale optical flow observation error with total angular rate - R_LOS = 0.01f;// + sq(0.05f*dAngIMU.length()/dtIMU); - - // Calculate observation jacobians - SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); - SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); - SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); - SH_LOS[3] = 1/(pd - ptd); - SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; - SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; - SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; - SH_LOS[7] = 1/sq(pd - ptd); - SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; - SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; - SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; - SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; - SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; - - for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; - H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); - H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); - H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); - H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); - H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); - H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); - H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); - H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; - H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; - - // Calculate Kalman gain - SK_LOS[0] = 1/(R_LOS + (SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]))*(P[0][0]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][0]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][0]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][0]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][0]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][0]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]))*(P[0][1]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][1]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][1]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][1]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][1]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][1]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]))*(P[0][2]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][2]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][2]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][2]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][2]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][2]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - (SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]))*(P[0][3]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][3]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][3]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][3]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][3]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][3]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][9]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][9]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][9]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][9]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][9]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][9]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][22]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][22]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][22]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][22]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][22]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][22]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3))*(P[0][4]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][4]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][4]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][4]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][4]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][4]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3))*(P[0][5]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][5]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][5]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][5]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][5]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][5]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))*(P[0][6]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][6]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][6]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][6]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][6]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][6]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)))); - SK_LOS[1] = 1/(R_LOS + (SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]))*(P[0][0]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][0]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][0]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]))*(P[0][1]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][1]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][1]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]))*(P[0][2]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][2]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][2]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][2]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][2]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - (SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]))*(P[0][3]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][3]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][3]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][3]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][3]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][9]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][9]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][9]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][9]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][9]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][22]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][22]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][22]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][22]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][22]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3))*(P[0][4]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][4]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][4]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][4]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][4]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3))*(P[0][5]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][5]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][5]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][5]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][5]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))*(P[0][6]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][6]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][6]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][6]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][6]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)))); - SK_LOS[2] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); - SK_LOS[3] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); - SK_LOS[4] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); - SK_LOS[5] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); - SK_LOS[6] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); - SK_LOS[7] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); - SK_LOS[8] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); - SK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); - SK_LOS[10] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); - SK_LOS[11] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); - SK_LOS[12] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); - SK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); - SK_LOS[14] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); - SK_LOS[15] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); - SK_LOS[16] = SH_LOS[0]; - - Kfusion[0] = -SK_LOS[0]*(P[0][0]*SK_LOS[11] + P[0][1]*SK_LOS[10] - P[0][3]*SK_LOS[9] + P[0][2]*SK_LOS[12] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[0][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[0][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[1] = -SK_LOS[0]*(P[1][0]*SK_LOS[11] + P[1][1]*SK_LOS[10] - P[1][3]*SK_LOS[9] + P[1][2]*SK_LOS[12] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[1][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[1][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[2] = -SK_LOS[0]*(P[2][0]*SK_LOS[11] + P[2][1]*SK_LOS[10] - P[2][3]*SK_LOS[9] + P[2][2]*SK_LOS[12] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[2][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[2][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[3] = -SK_LOS[0]*(P[3][0]*SK_LOS[11] + P[3][1]*SK_LOS[10] - P[3][3]*SK_LOS[9] + P[3][2]*SK_LOS[12] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[3][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[3][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[4] = -SK_LOS[0]*(P[4][0]*SK_LOS[11] + P[4][1]*SK_LOS[10] - P[4][3]*SK_LOS[9] + P[4][2]*SK_LOS[12] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[4][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[4][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[5] = -SK_LOS[0]*(P[5][0]*SK_LOS[11] + P[5][1]*SK_LOS[10] - P[5][3]*SK_LOS[9] + P[5][2]*SK_LOS[12] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[5][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[5][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[6] = -SK_LOS[0]*(P[6][0]*SK_LOS[11] + P[6][1]*SK_LOS[10] - P[6][3]*SK_LOS[9] + P[6][2]*SK_LOS[12] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[6][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[6][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[7] = -SK_LOS[0]*(P[7][0]*SK_LOS[11] + P[7][1]*SK_LOS[10] - P[7][3]*SK_LOS[9] + P[7][2]*SK_LOS[12] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[7][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[7][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[8] = -SK_LOS[0]*(P[8][0]*SK_LOS[11] + P[8][1]*SK_LOS[10] - P[8][3]*SK_LOS[9] + P[8][2]*SK_LOS[12] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[8][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[8][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[9] = -SK_LOS[0]*(P[9][0]*SK_LOS[11] + P[9][1]*SK_LOS[10] - P[9][3]*SK_LOS[9] + P[9][2]*SK_LOS[12] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[9][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[9][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[10] = -SK_LOS[0]*(P[10][0]*SK_LOS[11] + P[10][1]*SK_LOS[10] - P[10][3]*SK_LOS[9] + P[10][2]*SK_LOS[12] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[10][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[10][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[11] = -SK_LOS[0]*(P[11][0]*SK_LOS[11] + P[11][1]*SK_LOS[10] - P[11][3]*SK_LOS[9] + P[11][2]*SK_LOS[12] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[11][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[11][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[12] = -SK_LOS[0]*(P[12][0]*SK_LOS[11] + P[12][1]*SK_LOS[10] - P[12][3]*SK_LOS[9] + P[12][2]*SK_LOS[12] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[12][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[12][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[13] = -SK_LOS[0]*(P[13][0]*SK_LOS[11] + P[13][1]*SK_LOS[10] - P[13][3]*SK_LOS[9] + P[13][2]*SK_LOS[12] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[13][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[13][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[14] = -SK_LOS[0]*(P[14][0]*SK_LOS[11] + P[14][1]*SK_LOS[10] - P[14][3]*SK_LOS[9] + P[14][2]*SK_LOS[12] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[14][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[14][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[15] = -SK_LOS[0]*(P[15][0]*SK_LOS[11] + P[15][1]*SK_LOS[10] - P[15][3]*SK_LOS[9] + P[15][2]*SK_LOS[12] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[15][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[15][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[16] = -SK_LOS[0]*(P[16][0]*SK_LOS[11] + P[16][1]*SK_LOS[10] - P[16][3]*SK_LOS[9] + P[16][2]*SK_LOS[12] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[16][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[16][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[17] = -SK_LOS[0]*(P[17][0]*SK_LOS[11] + P[17][1]*SK_LOS[10] - P[17][3]*SK_LOS[9] + P[17][2]*SK_LOS[12] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[17][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[17][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[18] = -SK_LOS[0]*(P[18][0]*SK_LOS[11] + P[18][1]*SK_LOS[10] - P[18][3]*SK_LOS[9] + P[18][2]*SK_LOS[12] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[18][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[18][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[19] = -SK_LOS[0]*(P[19][0]*SK_LOS[11] + P[19][1]*SK_LOS[10] - P[19][3]*SK_LOS[9] + P[19][2]*SK_LOS[12] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[19][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[19][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[20] = -SK_LOS[0]*(P[20][0]*SK_LOS[11] + P[20][1]*SK_LOS[10] - P[20][3]*SK_LOS[9] + P[20][2]*SK_LOS[12] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[20][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[20][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[21] = -SK_LOS[0]*(P[21][0]*SK_LOS[11] + P[21][1]*SK_LOS[10] - P[21][3]*SK_LOS[9] + P[21][2]*SK_LOS[12] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[21][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[21][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[22] = -SK_LOS[0]*(P[22][0]*SK_LOS[11] + P[22][1]*SK_LOS[10] - P[22][3]*SK_LOS[9] + P[22][2]*SK_LOS[12] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[22][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[22][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - varInnovLOS[0] = 1.0f/SK_LOS[0]; - innovLOS[0] = losPred[0] - losData[0]; - - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; - fuseOptData = false; - } - else if (obsIndex == 1) // we are now fusing the Y measurement - { - // Calculate observation jacobians - for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; - H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); - H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); - H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); - H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); - H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); - H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); - H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); - H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; - H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; - - // Calculate Kalman gains - Kfusion[0] = SK_LOS[1]*(P[0][0]*SK_LOS[13] - P[0][3]*SK_LOS[6] + P[0][1]*SK_LOS[14] + P[0][2]*SK_LOS[15] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[0][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[1] = SK_LOS[1]*(P[1][0]*SK_LOS[13] - P[1][3]*SK_LOS[6] + P[1][1]*SK_LOS[14] + P[1][2]*SK_LOS[15] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[1][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[2] = SK_LOS[1]*(P[2][0]*SK_LOS[13] - P[2][3]*SK_LOS[6] + P[2][1]*SK_LOS[14] + P[2][2]*SK_LOS[15] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[2][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[3] = SK_LOS[1]*(P[3][0]*SK_LOS[13] - P[3][3]*SK_LOS[6] + P[3][1]*SK_LOS[14] + P[3][2]*SK_LOS[15] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[3][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[4] = SK_LOS[1]*(P[4][0]*SK_LOS[13] - P[4][3]*SK_LOS[6] + P[4][1]*SK_LOS[14] + P[4][2]*SK_LOS[15] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[4][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[5] = SK_LOS[1]*(P[5][0]*SK_LOS[13] - P[5][3]*SK_LOS[6] + P[5][1]*SK_LOS[14] + P[5][2]*SK_LOS[15] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[5][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[6] = SK_LOS[1]*(P[6][0]*SK_LOS[13] - P[6][3]*SK_LOS[6] + P[6][1]*SK_LOS[14] + P[6][2]*SK_LOS[15] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[6][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[7] = SK_LOS[1]*(P[7][0]*SK_LOS[13] - P[7][3]*SK_LOS[6] + P[7][1]*SK_LOS[14] + P[7][2]*SK_LOS[15] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[7][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[8] = SK_LOS[1]*(P[8][0]*SK_LOS[13] - P[8][3]*SK_LOS[6] + P[8][1]*SK_LOS[14] + P[8][2]*SK_LOS[15] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[8][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[9] = SK_LOS[1]*(P[9][0]*SK_LOS[13] - P[9][3]*SK_LOS[6] + P[9][1]*SK_LOS[14] + P[9][2]*SK_LOS[15] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[9][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[10] = SK_LOS[1]*(P[10][0]*SK_LOS[13] - P[10][3]*SK_LOS[6] + P[10][1]*SK_LOS[14] + P[10][2]*SK_LOS[15] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[10][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[11] = SK_LOS[1]*(P[11][0]*SK_LOS[13] - P[11][3]*SK_LOS[6] + P[11][1]*SK_LOS[14] + P[11][2]*SK_LOS[15] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[11][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[12] = SK_LOS[1]*(P[12][0]*SK_LOS[13] - P[12][3]*SK_LOS[6] + P[12][1]*SK_LOS[14] + P[12][2]*SK_LOS[15] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[12][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[13] = SK_LOS[1]*(P[13][0]*SK_LOS[13] - P[13][3]*SK_LOS[6] + P[13][1]*SK_LOS[14] + P[13][2]*SK_LOS[15] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[13][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[14] = SK_LOS[1]*(P[14][0]*SK_LOS[13] - P[14][3]*SK_LOS[6] + P[14][1]*SK_LOS[14] + P[14][2]*SK_LOS[15] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[14][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[15] = SK_LOS[1]*(P[15][0]*SK_LOS[13] - P[15][3]*SK_LOS[6] + P[15][1]*SK_LOS[14] + P[15][2]*SK_LOS[15] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[15][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[16] = SK_LOS[1]*(P[16][0]*SK_LOS[13] - P[16][3]*SK_LOS[6] + P[16][1]*SK_LOS[14] + P[16][2]*SK_LOS[15] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[16][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[17] = SK_LOS[1]*(P[17][0]*SK_LOS[13] - P[17][3]*SK_LOS[6] + P[17][1]*SK_LOS[14] + P[17][2]*SK_LOS[15] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[17][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[18] = SK_LOS[1]*(P[18][0]*SK_LOS[13] - P[18][3]*SK_LOS[6] + P[18][1]*SK_LOS[14] + P[18][2]*SK_LOS[15] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[18][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[19] = SK_LOS[1]*(P[19][0]*SK_LOS[13] - P[19][3]*SK_LOS[6] + P[19][1]*SK_LOS[14] + P[19][2]*SK_LOS[15] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[19][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[20] = SK_LOS[1]*(P[20][0]*SK_LOS[13] - P[20][3]*SK_LOS[6] + P[20][1]*SK_LOS[14] + P[20][2]*SK_LOS[15] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[20][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[21] = SK_LOS[1]*(P[21][0]*SK_LOS[13] - P[21][3]*SK_LOS[6] + P[21][1]*SK_LOS[14] + P[21][2]*SK_LOS[15] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[21][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[22] = SK_LOS[1]*(P[22][0]*SK_LOS[13] - P[22][3]*SK_LOS[6] + P[22][1]*SK_LOS[14] + P[22][2]*SK_LOS[15] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[22][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - varInnovLOS[1] = 1.0f/SK_LOS[1]; - innovLOS[1] = losPred[1] - losData[1]; - } - - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovLOS[obsIndex]*innovLOS[obsIndex]/varInnovMag[obsIndex]) < 25.0) - { - // correct the state vector - for (uint8_t j = 0; j < n_states; j++) - { - states[j] = states[j] - Kfusion[j] * innovLOS[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 < n_states; i++) - { - for (uint8_t j = 0; j <= 6; j++) - { - KH[i][j] = Kfusion[i] * H_LOS[j]; - } - for (uint8_t j = 7; j <= 8; j++) - { - KH[i][j] = 0.0f; - } - KH[i][9] = Kfusion[i] * H_LOS[9]; - for (uint8_t j = 10; j <= 21; j++) - { - KH[i][j] = 0.0f; - } - KH[i][22] = Kfusion[i] * H_LOS[22]; - } - for (uint8_t i = 0; i < n_states; i++) - { - for (uint8_t j = 0; j < n_states; j++) - { - KHP[i][j] = 0.0f; - for (uint8_t k = 0; k <= 6; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; - KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; - } - } - } - for (uint8_t i = 0; i < n_states; i++) - { - for (uint8_t j = 0; j < n_states; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - obsIndex = obsIndex + 1; - - 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 Date: Mon, 21 Apr 2014 01:18:34 +0200 Subject: [PATCH 11/58] ekf_att_pos_estimator: Using right app name --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 0dafc4311e..59ce45e16a 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1211,7 +1211,7 @@ FixedwingEstimator::start() ASSERT(_estimator_task == -1); /* start the task */ - _estimator_task = task_spawn_cmd("fw_att_pos_estimator", + _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 6000, From c08544721a0ac4293786ce410dcd0084e1f7cfe6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 01:20:35 +0200 Subject: [PATCH 12/58] att_pos_estimator_ekf: Update filter to new filter API --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 59ce45e16a..8287757196 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -978,8 +978,8 @@ FixedwingEstimator::task_main() // 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 = _ekf->summedDelAng.zero(); - _ekf->summedDelVel = _ekf->summedDelVel.zero(); + _ekf->summedDelAng.zero(); + _ekf->summedDelVel.zero(); dt = 0.0f; } From aa3aafb1e5d060593529af72bcf22d6351374df9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 10:47:15 +0200 Subject: [PATCH 13/58] Added debug macro for EKF. Fixed mag state handling which was only partially stored in correct states and not properly reset on init / dynamic reset --- .../ekf_att_pos_estimator/estimator.cpp | 139 +++++++++++++----- src/modules/ekf_att_pos_estimator/estimator.h | 19 ++- 2 files changed, 123 insertions(+), 35 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 2a7dab890b..18e17d4d9f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1,7 +1,37 @@ #include "estimator.h" -//#include #include +// 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 + +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); @@ -15,6 +45,10 @@ void Vector3f::zero(void) } Mat3f::Mat3f() { + identity(); +} + +void Mat3f::identity() { x.x = 1.0f; x.y = 0.0f; x.z = 0.0f; @@ -122,6 +156,7 @@ AttPosEKF::AttPosEKF() : storeIndex(0) { InitialiseParameters(); + ZeroVariables(); } AttPosEKF::~AttPosEKF() @@ -1219,26 +1254,23 @@ void AttPosEKF::FuseVelposNED() void AttPosEKF::FuseMagnetometer() { - static uint8_t obsIndex; - static float MagPred[3]; - static float SH_MAG[9]; - 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 magXbias = 0.0f; - static float magYbias = 0.0f; - static float magZbias = 0.0f; - float R_MAG = sq(magMeasurementSigma); - float DCM[3][3] = - { - {1.0f,0.0f,0.0f} , - {0.0f,1.0f,0.0f} , - {0.0f,0.0f,1.0f} - }; + + float &q0 = magstate.q0; + float &q1 = magstate.q1; + float &q2 = magstate.q2; + float &q3 = magstate.q3; + float &magN = magstate.magN; + float &magE = magstate.magE; + float &magD = magstate.magD; + float &magXbias = magstate.magXbias; + float &magYbias = magstate.magYbias; + float &magZbias = magstate.magZbias; + unsigned &obsIndex = magstate.obsIndex; + Mat3f &DCM = magstate.DCM; + float *MagPred = &magstate.MagPred[0]; + float &R_MAG = magstate.R_MAG; + float *SH_MAG = &magstate.SH_MAG[0]; + float SK_MX[6]; float SK_MY[5]; float SK_MZ[6]; @@ -1286,18 +1318,18 @@ void AttPosEKF::FuseMagnetometer() // 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; + DCM.x.x = q0*q0 + q1*q1 - q2*q2 - q3*q3; + DCM.x.y = 2*(q1*q2 + q0*q3); + DCM.x.z = 2*(q1*q3-q0*q2); + DCM.y.x = 2*(q1*q2 - q0*q3); + DCM.y.y = q0*q0 - q1*q1 + q2*q2 - q3*q3; + DCM.y.z = 2*(q2*q3 + q0*q1); + DCM.z.x = 2*(q1*q3 + q0*q2); + DCM.z.y = 2*(q2*q3 - q0*q1); + DCM.z.z = q0*q0 - q1*q1 - q2*q2 + q3*q3; + MagPred[0] = DCM.x.x*magN + DCM.x.y*magE + DCM.x.z*magD + magXbias; + MagPred[1] = DCM.y.x*magN + DCM.y.y*magE + DCM.y.z*magD + magYbias; + MagPred[2] = DCM.z.x*magN + DCM.z.y*magE + DCM.z.z*magD + magZbias; // scale magnetometer observation error with total angular rate R_MAG = sq(magMeasurementSigma) + sq(0.05f*dAngIMU.length()/dtIMU); @@ -2206,22 +2238,44 @@ void AttPosEKF::FillErrorReport(struct ekf_status_report *err) bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { bool err = false; + // check all integrators + if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { + err_report->statesNaN = true; + ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", summedDelAng.x, summedDelAng.y, summedDelAng.z); + err = true; + } // delta angles + + if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { + err_report->statesNaN = true; + ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", correctedDelAng.x, correctedDelAng.y, correctedDelAng.z); + err = true; + } // delta angles + + if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { + err_report->statesNaN = true; + ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", summedDelVel.x, summedDelVel.y, summedDelVel.z); + err = true; + } // delta velocities + // 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; + ekf_debug("KH NaN"); err = true; } // intermediate result used for covariance updates if (!isfinite(KHP[i][j])) { err_report->covarianceNaN = true; + ekf_debug("KHP NaN"); err = true; } // intermediate result used for covariance updates if (!isfinite(P[i][j])) { err_report->covarianceNaN = true; + ekf_debug("P NaN"); err = true; } // covariance matrix } @@ -2229,12 +2283,14 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(Kfusion[i])) { err_report->kalmanGainsNaN = true; + ekf_debug("Kfusion NaN"); err = true; } // Kalman gains if (!isfinite(states[i])) { err_report->statesNaN = true; + ekf_debug("states NaN: i: %u val: %f", i, states[i]); err = true; } // state matrix } @@ -2264,6 +2320,7 @@ int AttPosEKF::CheckAndBound() // Reset the filter if the states went NaN if (StatesNaN(&last_ekf_error)) { + ekf_debug("re-initializing dynamic"); InitializeDynamic(velNED); @@ -2362,7 +2419,15 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) 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; - + magstate.q3 = 1.0f; + magstate.magN = 0.4f; + magstate.magE = 0.0f; + magstate.magD = 0.3f; + magstate.magXbias = 0.0f; + magstate.magYbias = 0.0f; + magstate.magZbias = 0.0f; + magstate.R_MAG = sq(magMeasurementSigma); + magstate.DCM.identity(); // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions @@ -2420,6 +2485,8 @@ void AttPosEKF::ZeroVariables() } correctedDelAng.zero(); + summedDelAng.zero(); + summedDelVel.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { @@ -2430,6 +2497,10 @@ void AttPosEKF::ZeroVariables() statetimeStamp[i] = 0; } + memset(&magstate, 0, sizeof(magstate)); + magstate.q0 = 1.0f; + magstate.DCM.identity(); + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 3a8f45d9af..734ad0c05d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -33,6 +33,7 @@ public: Mat3f(); + void identity(); Mat3f transpose(void) const; }; @@ -140,7 +141,23 @@ public: accelProcessNoise = 0.5f; } - + struct { + unsigned obsIndex; + float MagPred[3]; + float SH_MAG[9]; + float q0; + float q1; + float q2; + float q3; + float magN; + float magE; + float magD; + float magXbias; + float magYbias; + float magZbias; + float R_MAG; + Mat3f DCM; + } magstate; // Global variables From 595eb679b30442b52ccc7a2c2ce7ade7b5e5c6c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 11:02:27 +0200 Subject: [PATCH 14/58] ekf_att_pos_estimator: Fixed mag initialization, now starts with initial measurement instead of defaults for faster convergence --- .../ekf_att_pos_estimator/estimator.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 18e17d4d9f..95afbc181d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2419,15 +2419,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) 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; - magstate.q3 = 1.0f; - magstate.magN = 0.4f; - magstate.magE = 0.0f; - magstate.magD = 0.3f; - magstate.magXbias = 0.0f; - magstate.magYbias = 0.0f; - magstate.magZbias = 0.0f; + magstate.q0 = initQuat[0]; + magstate.q1 = initQuat[1]; + magstate.q2 = initQuat[2]; + magstate.q3 = initQuat[3]; + magstate.magN = magData.x; + magstate.magE = magData.y; + magstate.magD = magData.z; + magstate.magXbias = magBias.x; + magstate.magYbias = magBias.y; + magstate.magZbias = magBias.z; magstate.R_MAG = sq(magMeasurementSigma); - magstate.DCM.identity(); + magstate.DCM = DCM; // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions From 8ae50a4ba524f840dc7d96b79c5dbad1b7be15f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:06:13 +0200 Subject: [PATCH 15/58] Changed home position set to depend on the commander home position switch --- .../fw_att_pos_estimator_main.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8ea6118022..e9c036e858 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -152,6 +153,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _mission_sub; + int _home_sub; /**< home position as defined by commander / user */ orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ @@ -410,6 +412,7 @@ FixedwingEstimator::task_main() _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _home_sub = orb_subscribe(ORB_ID(home_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -797,20 +800,27 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (_ekf->GPSstatus == 3)) { + bool home_set; + orb_check(_home_sub, &home_set); + + if (home_set && !_gps_initialized && _gps.fix_type > 2) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - double lat = _gps.lat * 1e-7; - double lon = _gps.lon * 1e-7; - float alt = _gps.alt * 1e-3; + struct home_position_s home; + + orb_copy(ORB_ID(home_position), _home_sub, &home); + + double lat = home.lat; + double lon = home.lon; + float alt = home.alt; _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.lon; + _local_pos.ref_lat = home.lat * 1e7; + _local_pos.ref_lon = home.lon * 1e7; _local_pos.ref_alt = alt; _local_pos.ref_timestamp = _gps.timestamp_position; From 37b133e231e945c978dd66fd5daed0f12caa8073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:04:56 +0200 Subject: [PATCH 16/58] Added home position switch on GPS position - gives a more reliable home position setup --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef9..7ac7aff0f2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,9 +1092,9 @@ int commander_thread_main(int argc, char *argv[]) && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; + home.lat = gps_position.lat / (double)1e7; + home.lon = gps_position.lon / (double)1e7; + home.alt = gps_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); From 706d08055d2bf63d16c72e86cd60f0e33a20bc4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 01:18:30 +0200 Subject: [PATCH 17/58] Better / cleaner initialization of the attitude estimator --- .../ekf_att_pos_estimator/estimator.cpp | 24 ++++++++----- src/modules/ekf_att_pos_estimator/estimator.h | 2 +- .../fw_att_pos_estimator_main.cpp | 34 ++++++++++++------- 3 files changed, 38 insertions(+), 22 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 95afbc181d..5f4bf07293 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -4,7 +4,7 @@ // 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 +#define EKF_DEBUG #ifdef EKF_DEBUG #include @@ -2263,21 +2263,29 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(KH[i][j])) { err_report->covarianceNaN = true; - ekf_debug("KH NaN"); err = true; } // intermediate result used for covariance updates + if (err) { + ekf_debug("KH NaN"); + } + if (!isfinite(KHP[i][j])) { err_report->covarianceNaN = true; - ekf_debug("KHP NaN"); err = true; } // intermediate result used for covariance updates + if (err) { + ekf_debug("KHP NaN"); + } + if (!isfinite(P[i][j])) { err_report->covarianceNaN = true; - ekf_debug("P NaN"); err = true; } // covariance matrix + if (err) { + ekf_debug("P NaN"); + } } if (!isfinite(Kfusion[i])) { @@ -2461,12 +2469,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) summedDelVel.z = 0.0f; } -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3]) +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt) { //store initial lat,long and height - latRef = gpsLat; - lonRef = gpsLon; - hgtRef = gpsHgt; + latRef = referenceLat; + lonRef = referenceLon; + hgtRef = referenceHgt; 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.h index 734ad0c05d..5e60e506f6 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -308,7 +308,7 @@ void OnGroundCheck(); void CovarianceInit(); -void InitialiseFilter(float (&initvelNED)[3]); +void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt); float ConstrainFloat(float val, float min, float max); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8287757196..c8005f3d3f 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -606,7 +606,6 @@ FixedwingEstimator::task_main() /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; - warnx("TS fail"); } @@ -650,14 +649,14 @@ FixedwingEstimator::task_main() IMUmsec = _sensor_combined.timestamp / 1e3f; float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f; - last_run = _sensor_combined.timestamp; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; - warnx("TS fail"); } + last_run = _sensor_combined.timestamp; + // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; @@ -823,6 +822,8 @@ FixedwingEstimator::task_main() */ int check = _ekf->CheckAndBound(); + const char* ekfname = "[ekf] "; + switch (check) { case 0: /* all ok */ @@ -830,27 +831,34 @@ FixedwingEstimator::task_main() case 1: { const char* str = "NaN in states, resetting"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } case 2: { const char* str = "stale IMU data, resetting"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } case 3: { - const char* str = "switching dynamic / static state"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + const char* str = "switching to dynamic state"; + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } + + default: + { + const char* str = "unknown reset condition"; + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + } } - // If non-zero, we got a problem + // If non-zero, we got a filter reset if (check) { struct ekf_status_report ekf_report; @@ -912,7 +920,7 @@ FixedwingEstimator::task_main() double lon = home.lon; float alt = home.alt; - _ekf->InitialiseFilter(_ekf->velNED); + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt); // Initialize projection _local_pos.ref_lat = home.lat * 1e7; @@ -942,7 +950,7 @@ FixedwingEstimator::task_main() _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; - _ekf->InitialiseFilter(_ekf->velNED); + _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f); } } From db15e2811ea01dd023ae930e6e7a73c1a370cecf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 01:36:32 +0200 Subject: [PATCH 18/58] commander: Fix altitude initialisation, do not depend on global pos valid flag. --- src/modules/commander/commander.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7ac7aff0f2..077184bfbd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1088,13 +1088,12 @@ int commander_thread_main(int argc, char *argv[]) if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && - (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.global_valid) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ home.lat = gps_position.lat / (double)1e7; home.lon = gps_position.lon / (double)1e7; - home.alt = gps_position.alt; + home.alt = gps_position.alt / (float)1e3; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); From 904ada124baea8ef744535053a0c3b40871565e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 02:15:33 +0200 Subject: [PATCH 19/58] ekf: Put reset statements after variable zero operation to ensure values get initialized correctly --- src/modules/ekf_att_pos_estimator/estimator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5f4bf07293..99de161aac 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2407,10 +2407,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) ZeroVariables(); - ResetVelocity(); - ResetPosition(); - ResetHeight(); - // Calculate initial filter quaternion states from raw measurements float initQuat[4]; Vector3f initMagXYZ; @@ -2452,6 +2448,10 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) states[21] = magBias.z; // Magnetic Field Bias Z states[22] = 0.0f; // terrain height + ResetVelocity(); + ResetPosition(); + ResetHeight(); + statesInitialised = true; // initialise the covariance matrix From 39a0d4e54db1252678eb8f4ebc872c589da2d9e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 02:24:29 +0200 Subject: [PATCH 20/58] Better error handling / reporting in filter --- .../ekf_att_pos_estimator/estimator.cpp | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 99de161aac..14761831cf 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2243,18 +2243,21 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->statesNaN = true; ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", summedDelAng.x, summedDelAng.y, summedDelAng.z); err = true; + goto out; } // delta angles if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { err_report->statesNaN = true; ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", correctedDelAng.x, correctedDelAng.y, correctedDelAng.z); err = true; + goto out; } // delta angles if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { err_report->statesNaN = true; ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", summedDelVel.x, summedDelVel.y, summedDelVel.z); err = true; + goto out; } // delta velocities // check all states and covariance matrices @@ -2264,28 +2267,24 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->covarianceNaN = true; err = true; - } // intermediate result used for covariance updates - if (err) { ekf_debug("KH NaN"); - } + goto out; + } // intermediate result used for covariance updates if (!isfinite(KHP[i][j])) { err_report->covarianceNaN = true; err = true; - } // intermediate result used for covariance updates - if (err) { ekf_debug("KHP NaN"); - } + goto out; + } // intermediate result used for covariance updates if (!isfinite(P[i][j])) { err_report->covarianceNaN = true; err = true; - } // covariance matrix - if (err) { ekf_debug("P NaN"); - } + } // covariance matrix } if (!isfinite(Kfusion[i])) { @@ -2293,6 +2292,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->kalmanGainsNaN = true; ekf_debug("Kfusion NaN"); err = true; + goto out; } // Kalman gains if (!isfinite(states[i])) { @@ -2300,9 +2300,11 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->statesNaN = true; ekf_debug("states NaN: i: %u val: %f", i, states[i]); err = true; + goto out; } // state matrix } +out: if (err) { FillErrorReport(err_report); } From 125f0b2f88c390bfde92ebe5423a0913e0e1b114 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 02:25:38 +0200 Subject: [PATCH 21/58] Added trap to filter to catch NaN handling --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index c8005f3d3f..ac34af48a4 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -858,6 +858,11 @@ FixedwingEstimator::task_main() } } + // XXX trap for testing + if (check == 1 || check == 2) { + errx(1, "NUMERIC ERROR IN FILTER"); + } + // If non-zero, we got a filter reset if (check) { From 5b7209639d42290dc5b1e3fc53fc81a455d75b51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 09:56:38 +0200 Subject: [PATCH 22/58] Revert "HIL: Increased MAVLink link wait time based on previous experience that this is timing sensitive." This reverts commit 78bf7ed969624eacea35ae2bf402596aecb3c5a6. --- ROMFS/px4fmu_common/init.d/rcS | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ccac0a2631..2ab1e2e96f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -403,8 +403,7 @@ then then if [ $HIL == yes ] then - # Sleep required to ensure the link is up - sleep 5 + sleep 1 set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0" else # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s From bd637697e4880d3efbb79c2c05647564d3582cde Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 10:26:26 +0200 Subject: [PATCH 23/58] Removed verbose print --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ac34af48a4..ad7cb3687a 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -920,6 +920,7 @@ FixedwingEstimator::task_main() struct home_position_s home; orb_copy(ORB_ID(home_position), _home_sub, &home); + warnx("HOME SET"); double lat = home.lat; double lon = home.lon; From 1e80e624916a0eb1b13adccb4f700adeeee66bba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 10:26:44 +0200 Subject: [PATCH 24/58] ekf: Better variable zeroing --- src/modules/ekf_att_pos_estimator/estimator.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 14761831cf..ac9abf5ca3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -153,8 +153,16 @@ AttPosEKF::AttPosEKF() : useCompass(true), useRangeFinder(true), numericalProtection(true), - storeIndex(0) + storeIndex(0), + gpsHgt(0.0f), + baroHgt(0.0f), + GPSstatus(0), + VtasMeas(0.0f), { + velNED[0] = 0.0f; + velNED[1] = 0.0f; + velNED[2] = 0.0f; + InitialiseParameters(); ZeroVariables(); } @@ -1967,7 +1975,7 @@ void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, flo void AttPosEKF::OnGroundCheck() { - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); + onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); if (staticMode) { staticMode = !(GPSstatus > GPS_FIX_2D); } @@ -2515,6 +2523,7 @@ void AttPosEKF::ZeroVariables() magstate.DCM.identity(); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); + } void AttPosEKF::GetFilterState(struct ekf_status_report *state) From 4585df1182083c39f2439bb7b88953dcc3575240 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 11:02:31 +0200 Subject: [PATCH 25/58] Robustified filter init / sequencing --- src/modules/ekf_att_pos_estimator/estimator.cpp | 6 ++++-- src/modules/ekf_att_pos_estimator/estimator.h | 1 + .../fw_att_pos_estimator_main.cpp | 13 +++++++++---- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index ac9abf5ca3..5de22fdaec 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -153,11 +153,12 @@ AttPosEKF::AttPosEKF() : useCompass(true), useRangeFinder(true), numericalProtection(true), + refSet(false), storeIndex(0), gpsHgt(0.0f), baroHgt(0.0f), GPSstatus(0), - VtasMeas(0.0f), + VtasMeas(0.0f) { velNED[0] = 0.0f; velNED[1] = 0.0f; @@ -1977,7 +1978,7 @@ void AttPosEKF::OnGroundCheck() { onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); if (staticMode) { - staticMode = !(GPSstatus > GPS_FIX_2D); + staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } } @@ -2485,6 +2486,7 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do latRef = referenceLat; lonRef = referenceLon; hgtRef = referenceHgt; + refSet = true; 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.h index 5e60e506f6..e118ae5734 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -211,6 +211,7 @@ public: double latRef; // WGS-84 latitude of reference point (rad) double lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) + bool refSet; ///< flag to indicate if the reference position has been set Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ad7cb3687a..19111a9b42 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -192,6 +192,7 @@ private: bool _initialized; bool _gps_initialized; + uint64_t _gps_start_time; int _mavlink_fd; @@ -720,6 +721,9 @@ FixedwingEstimator::task_main() } else { + /* store time of valid GPS measurement */ + _gps_start_time = hrt_absolute_time(); + /* check if we had a GPS outage for a long time */ if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { _ekf->ResetPosition(); @@ -859,7 +863,7 @@ FixedwingEstimator::task_main() } // XXX trap for testing - if (check == 1 || check == 2) { + if (check == 1) { errx(1, "NUMERIC ERROR IN FILTER"); } @@ -907,7 +911,7 @@ FixedwingEstimator::task_main() // XXX we rather want to check all updated - if (hrt_elapsed_time(&start_time) > 100000) { + if (hrt_elapsed_time(&_gps_start_time) > 50000) { bool home_set; orb_check(_home_sub, &home_set); @@ -920,7 +924,6 @@ FixedwingEstimator::task_main() struct home_position_s home; orb_copy(ORB_ID(home_position), _home_sub, &home); - warnx("HOME SET"); double lat = home.lat; double lon = home.lon; @@ -942,7 +945,9 @@ FixedwingEstimator::task_main() // XXX this is not multithreading safe map_projection_init(lat, lon); - mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)alt); + warnx("[ekf] HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)alt, + (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); _gps_initialized = true; From a30411e9f2438018a08c0965261067940f88be10 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 11:02:53 +0200 Subject: [PATCH 26/58] Fixed printing in attitude control --- src/modules/fw_att_control/fw_att_control_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 2f84dc963b..5ded7aa76f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -771,7 +771,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", - pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); + (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body); } float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, @@ -780,16 +780,16 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { - warnx("yaw_u %.4f", yaw_u); + warnx("yaw_u %.4f", (double)yaw_u); } /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { - warnx("throttle_sp %.4f", throttle_sp); + warnx("throttle_sp %.4f", (double)throttle_sp); } } else { - warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } /* From d4785d4b6561e1f381abd0721e1701f4c141e2fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 15:13:50 +0200 Subject: [PATCH 27/58] Use INAV as default to not break existing setups --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index c75281fcdd..268eb9bba0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -5,8 +5,8 @@ # attitude_estimator_ekf start -ekf_att_pos_estimator start -#position_estimator_inav start +#ekf_att_pos_estimator start +position_estimator_inav start mc_att_control start mc_pos_control start From ad1be765bf36a6068fda25e3c62c92e275aebc6b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 15:14:23 +0200 Subject: [PATCH 28/58] Fix warnings, use more efficient atan2f where it can be safely used --- src/modules/ekf_att_pos_estimator/estimator.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5de22fdaec..6eceb21f8e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2250,21 +2250,21 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { // check all integrators if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { err_report->statesNaN = true; - ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", summedDelAng.x, summedDelAng.y, summedDelAng.z); + 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; - ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", correctedDelAng.x, correctedDelAng.y, correctedDelAng.z); + 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; - ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", summedDelVel.x, summedDelVel.y, summedDelVel.z); + ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); err = true; goto out; } // delta velocities @@ -2381,8 +2381,8 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f float magX, magY; float initialHdg, cosHeading, sinHeading; - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); + initialRoll = atan2f(-ay, -az); + initialPitch = atan2f(ax, -az); cosRoll = cosf(initialRoll); sinRoll = sinf(initialRoll); From 9358eb2428275f78ad5b1e06b42840927092ac89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 15:22:03 +0200 Subject: [PATCH 29/58] Fixed string formatting error --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5315c5e387..e260aae45c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -655,7 +655,7 @@ Sensors::parameters_update() } if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { From 0d50b3ea866a9ca0b9271b8c861f1d9e2a61a24a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 16:06:25 +0200 Subject: [PATCH 30/58] Fix struct inits --- .../fw_att_pos_estimator_main.cpp | 67 +++++++++++++++---- 1 file changed, 55 insertions(+), 12 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ad8c1b4c00..1c943137a1 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -103,8 +103,6 @@ uint32_t millis() return IMUmsec; } -static void print_status(); - class FixedwingEstimator { public: @@ -305,6 +303,25 @@ FixedwingEstimator::FixedwingEstimator() : _local_pos_pub(-1), _estimator_status_pub(-1), + _att({}), + _gyro({}), + _accel({}), + _mag({}), + _airspeed({}), + _baro({}), + _vstatus({}), + _global_pos({}), + _local_pos({}), + _gps({}), + + _gyro_offsets({}), + _accel_offsets({}), + _mag_offsets({}), + + #ifdef SENSOR_COMBINED_SUB + _sensor_combined({}), + #endif + _baro_ref(0.0f), _baro_gps_offset(0.0f), @@ -356,6 +373,10 @@ FixedwingEstimator::FixedwingEstimator() : if (fd > 0) { res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets); close(fd); + + if (res) { + warnx("G SCALE FAIL"); + } } fd = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -363,6 +384,10 @@ FixedwingEstimator::FixedwingEstimator() : if (fd > 0) { res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets); close(fd); + + if (res) { + warnx("A SCALE FAIL"); + } } fd = open(MAG_DEVICE_PATH, O_RDONLY); @@ -370,6 +395,10 @@ FixedwingEstimator::FixedwingEstimator() : if (fd > 0) { res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets); close(fd); + + if (res) { + warnx("M SCALE FAIL"); + } } } @@ -538,12 +567,19 @@ FixedwingEstimator::task_main() fds[1].events = POLLIN; #endif - hrt_abstime start_time = hrt_absolute_time(); - bool newDataGps = false; bool newAdsData = false; bool newDataMag = false; + // Reset relevant structs + _gps = {}; + + #ifndef SENSOR_COMBINED_SUB + _gyro = {}; + #else + _sensor_combined = {}; + #endif + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -624,9 +660,11 @@ FixedwingEstimator::task_main() _ekf->angRate.z = _gyro.z; } - _ekf->accel.x = _accel.x; - _ekf->accel.y = _accel.y; - _ekf->accel.z = _accel.z; + if (accel_updated) { + _ekf->accel.x = _accel.x; + _ekf->accel.y = _accel.y; + _ekf->accel.z = _accel.z; + } _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; _ekf->lastAngRate = angRate; @@ -672,9 +710,11 @@ FixedwingEstimator::task_main() _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; } - _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; - _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; - _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + if (accel_updated) { + _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; + _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; + _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + } _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; lastAngRate = _ekf->angRate; @@ -885,7 +925,7 @@ FixedwingEstimator::task_main() rep.kalman_gain_nan = ekf_report.kalmanGainsNaN; // Copy all states or at least all that we can fit - int i = 0; + 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; @@ -1173,6 +1213,8 @@ FixedwingEstimator::task_main() _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) { @@ -1185,6 +1227,7 @@ FixedwingEstimator::task_main() /* local pos alt is negative, change sign and add alt offset */ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); + _global_pos.rel_alt = (-_local_pos.z); if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -1259,7 +1302,7 @@ FixedwingEstimator::print_status() // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, dt, (int)IMUmsec); + printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)dt, (int)IMUmsec); 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]); From 2d978beefbc010d962034a0f0d588cbf46a063f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 16:16:44 +0200 Subject: [PATCH 31/58] Compile fixes --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 +- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 6eceb21f8e..ffebcd4776 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2307,7 +2307,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(states[i])) { err_report->statesNaN = true; - ekf_debug("states NaN: i: %u val: %f", i, states[i]); + ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); err = true; goto out; } // state matrix diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 1c943137a1..fdb5dd5883 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1213,8 +1213,8 @@ FixedwingEstimator::task_main() _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; + _global_pos.eph = _gps.eph_m; + _global_pos.epv = _gps.epv_m; } if (_local_pos.v_xy_valid) { @@ -1227,7 +1227,6 @@ FixedwingEstimator::task_main() /* local pos alt is negative, change sign and add alt offset */ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); - _global_pos.rel_alt = (-_local_pos.z); if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; From 18a932fe948b06b87d65f5d13102274e5683fab6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:35:22 +0200 Subject: [PATCH 32/58] Better fake / simulation values --- src/drivers/gps/gps.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a902bdf2f5..01be80dcec 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -276,14 +276,14 @@ GPS::task_main() _report.timestamp_position = hrt_absolute_time(); _report.lat = (int32_t)47.378301e7f; _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)400e3f; + _report.alt = (int32_t)1200e3f; _report.timestamp_variance = hrt_absolute_time(); _report.s_variance_m_s = 10.0f; _report.p_variance_m = 10.0f; _report.c_variance_rad = 0.1f; _report.fix_type = 3; - _report.eph_m = 3.0f; - _report.epv_m = 7.0f; + _report.eph_m = 0.9f; + _report.epv_m = 1.8f; _report.timestamp_velocity = hrt_absolute_time(); _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; From 6612cdab856674c4cbec53863c470cb4e96a9f7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:36:09 +0200 Subject: [PATCH 33/58] Let commander be less pedantic about positioning data --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3527117341..148a0aafa2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */ +#define POSITION_TIMEOUT 50000 /**< consider the local or global position estimate invalid after 50ms */ #define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 From 925430796ed56e97bf2310423361a8e6e29e350b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:37:26 +0200 Subject: [PATCH 34/58] Reworked how we deal with altitudes --- .../fw_att_pos_estimator_main.cpp | 79 ++++++++++++------- .../fw_att_pos_estimator_params.c | 11 +++ 2 files changed, 62 insertions(+), 28 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index fdb5dd5883..a02b2c34fb 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -191,6 +191,7 @@ private: perf_counter_t _perf_airspeed; ///yawVarScale = 1.0f; @@ -568,18 +574,10 @@ FixedwingEstimator::task_main() #endif bool newDataGps = false; + bool newHgtData = false; bool newAdsData = false; bool newDataMag = false; - // Reset relevant structs - _gps = {}; - - #ifndef SENSOR_COMBINED_SUB - _gyro = {}; - #else - _sensor_combined = {}; - #endif - while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -611,6 +609,7 @@ FixedwingEstimator::task_main() if (fds[1].revents & POLLIN) { /* check vehicle status for changes to publication state */ + bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON); vehicle_status_poll(); bool accel_updated; @@ -618,6 +617,12 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); + /* Reset baro reference if switching to HIL */ + if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + _baro_init = false; + _gps_initialized = false; + } + /** * PART ONE: COLLECT ALL DATA **/ @@ -813,10 +818,17 @@ FixedwingEstimator::task_main() if (baro_updated) { orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _ekf->baroHgt = _baro.altitude - _baro_ref; + _ekf->baroHgt = _baro.altitude; - // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; + if (!_baro_init) { + _baro_ref = _baro.altitude; + _baro_init = true; + warnx("ALT REF INIT"); + } + + newHgtData = true; + } else { + newHgtData = false; } #ifndef SENSOR_COMBINED_SUB @@ -904,9 +916,9 @@ FixedwingEstimator::task_main() } } - // XXX trap for testing + // warn on fatal resets if (check == 1) { - errx(1, "NUMERIC ERROR IN FILTER"); + warnx("NUMERIC ERROR IN FILTER"); } // If non-zero, we got a filter reset @@ -960,7 +972,7 @@ FixedwingEstimator::task_main() // struct home_position_s home; // orb_copy(ORB_ID(home_position), _home_sub, &home); - if (!_gps_initialized && _gps.fix_type > 2) { + if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; @@ -968,26 +980,37 @@ FixedwingEstimator::task_main() // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; double lon = _gps.lon / 1.0e7; - float alt = _gps.alt / 1e3f; + float gps_alt = _gps.alt / 1e3f; - _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt); + // Set up height correctly + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + _baro_gps_offset = gps_alt - _baro.altitude; + _ekf->baroHgt = _baro.altitude; + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + + // Set up position variables correctly + _ekf->GPSstatus = _gps.fix_type; + _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->gpsLat = math::radians(_gps.lat / (double)1e7); + _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; + _ekf->gpsHgt = _gps.alt / 1e3f; + + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt); // Initialize projection _local_pos.ref_lat = _gps.lat; _local_pos.ref_lon = _gps.alt; - _local_pos.ref_alt = alt; + _local_pos.ref_alt = _baro_ref + _baro_gps_offset; _local_pos.ref_timestamp = _gps.timestamp_position; - // Store - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_ref = _baro.altitude; - _ekf->baroHgt = _baro.altitude - _baro_ref; - _baro_gps_offset = _baro_ref - _local_pos.ref_alt; - map_projection_init(&_pos_ref, lat, lon); - mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)alt); - warnx("[ekf] HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)alt, + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + 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("GPS: eph: %8.4f, epv: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m); _gps_initialized = true; @@ -1089,9 +1112,9 @@ FixedwingEstimator::task_main() _ekf->fusePosData = false; } - if (newAdsData && _ekf->statesInitialised) { + if (newHgtData && _ekf->statesInitialised) { // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; + _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)); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c index cfcd99858e..d2c6e1f15e 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c @@ -258,3 +258,14 @@ PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f); */ PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f); +/** + * Threshold for filter initialization. + * + * If the standard deviation of the GPS position estimate is below this threshold + * in meters, the filter will initialize. + * + * @min 0.3 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f); From 6ab878c0218f26e5fa71053b75d7075b594b937e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:38:37 +0200 Subject: [PATCH 35/58] Emit the local position against the GPS reference - this means it can jump. --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index a02b2c34fb..a82e8d7044 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1203,7 +1203,7 @@ FixedwingEstimator::task_main() _local_pos.timestamp = last_sensor_timestamp; _local_pos.x = _ekf->states[7]; _local_pos.y = _ekf->states[8]; - _local_pos.z = _ekf->states[9]; + _local_pos.z = _ekf->states[9] + _baro_gps_offset; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; From 0745334ac46c40ed407dc11ebbb7c252e37bcb43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 11:41:40 +0200 Subject: [PATCH 36/58] Reset init flags as well --- src/modules/ekf_att_pos_estimator/estimator.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 14761831cf..6eccc4c4d2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2407,6 +2407,19 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) // Clear the init flag statesInitialised = false; + // Clear other flags, waiting for new data + fusionModeGPS = 0; + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; + // onGround(true), + // staticMode(true), + useAirspeed = true; + useCompass = true; + useRangeFinder = true; + ZeroVariables(); // Calculate initial filter quaternion states from raw measurements From 90569d22bcee65b102c9f55be71806d301829cee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 18:34:33 +0200 Subject: [PATCH 37/58] Added support for automatic mag declination setup --- .../ekf_att_pos_estimator/estimator.cpp | 33 +++++++++++++------ src/modules/ekf_att_pos_estimator/estimator.h | 7 ++-- .../fw_att_pos_estimator_main.cpp | 13 +++++--- 3 files changed, 35 insertions(+), 18 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 8ac2d61541..1ca34ec307 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -158,7 +158,8 @@ AttPosEKF::AttPosEKF() : gpsHgt(0.0f), baroHgt(0.0f), GPSstatus(0), - VtasMeas(0.0f) + VtasMeas(0.0f), + magDeclination(0.0f) { velNED[0] = 0.0f; velNED[1] = 0.0f; @@ -2341,7 +2342,7 @@ int AttPosEKF::CheckAndBound() if (StatesNaN(&last_ekf_error)) { ekf_debug("re-initializing dynamic"); - InitializeDynamic(velNED); + InitializeDynamic(velNED, magDeclination); return 1; } @@ -2374,7 +2375,7 @@ int AttPosEKF::CheckAndBound() return 0; } -void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) +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; @@ -2394,6 +2395,8 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f 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); @@ -2408,9 +2411,17 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; + + /* normalize */ + float norm = sqrtf(initQuat[0]*initQuat[0] + initQuat[1]*initQuat[1] + initQuat[2]*initQuat[2] + initQuat[3]*initQuat[3]); + + initQuat[0] /= norm; + initQuat[1] /= norm; + initQuat[2] /= norm; + initQuat[3] /= norm; } -void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) +void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { // Clear the init flag @@ -2431,11 +2442,13 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) ZeroVariables(); + magDeclination = declination; + // 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, initQuat); + AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, declination, initQuat); // Calculate initial Tbn matrix and rotate Mag measurements into NED // to set initial NED magnetic field states @@ -2451,9 +2464,9 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) magstate.q1 = initQuat[1]; magstate.q2 = initQuat[2]; magstate.q3 = initQuat[3]; - magstate.magN = magData.x; - magstate.magE = magData.y; - magstate.magD = magData.z; + magstate.magN = initMagNED.x; + magstate.magE = initMagNED.y; + magstate.magD = initMagNED.z; magstate.magXbias = magBias.x; magstate.magYbias = magBias.y; magstate.magZbias = magBias.z; @@ -2493,7 +2506,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) summedDelVel.z = 0.0f; } -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt) +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) { //store initial lat,long and height latRef = referenceLat; @@ -2503,7 +2516,7 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do memset(&last_ekf_error, 0, sizeof(last_ekf_error)); - InitializeDynamic(initvelNED); + InitializeDynamic(initvelNED, declination); } void AttPosEKF::ZeroVariables() diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e118ae5734..378107b698 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -208,6 +208,7 @@ public: float innovRng; ///< Range finder innovation float varInnovVtas; // innovation variance output float VtasMeas; // true airspeed measurement (m/s) + float magDeclination; ///< magnetic declination double latRef; // WGS-84 latitude of reference point (rad) double lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) @@ -309,7 +310,7 @@ void OnGroundCheck(); void CovarianceInit(); -void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt); +void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); float ConstrainFloat(float val, float min, float max); @@ -334,7 +335,7 @@ 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]); +void InitializeDynamic(float (&initvelNED)[3], float declination); protected: @@ -342,7 +343,7 @@ bool FilterHealthy(); void ResetHeight(void); -void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat); +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/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index a82e8d7044..23f9e80bd8 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -994,11 +994,14 @@ FixedwingEstimator::task_main() _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); - _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; - _ekf->gpsHgt = _gps.alt / 1e3f; + _ekf->gpsLat = math::radians(lat); + _ekf->gpsLon = math::radians(lon) - M_PI; + _ekf->gpsHgt = gps_alt; - _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt); + // Look up mag declination based on current position + float declination = math::radians(get_mag_declination(lat, lon)); + + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection _local_pos.ref_lat = _gps.lat; @@ -1024,7 +1027,7 @@ FixedwingEstimator::task_main() _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; - _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f); + _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f, 0.0f); } } From 11a1053b73787bb2afbaff360f720de430447455 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Apr 2014 19:15:58 +0200 Subject: [PATCH 38/58] ekf_att_pos_estimator: local position reference fixed --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 23f9e80bd8..fb019a354e 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1004,8 +1004,8 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.alt; + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; _local_pos.ref_alt = _baro_ref + _baro_gps_offset; _local_pos.ref_timestamp = _gps.timestamp_position; From 848c1c2552996281685d2691c1b394f25a4391b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Apr 2014 21:42:58 +0200 Subject: [PATCH 39/58] ekf: More complete re-initialization --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 1ca34ec307..e1ffc15d3b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2536,6 +2536,8 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); + magBias.zero(); + magState.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { From d54f9973f26e5503f33ad53fd927696089a742ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 08:13:42 +0200 Subject: [PATCH 40/58] Fixed typos in declination table lookup --- src/lib/geo/geo_mag_declination.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c index b96f877214..cfee6b423d 100644 --- a/src/lib/geo/geo_mag_declination.c +++ b/src/lib/geo/geo_mag_declination.c @@ -115,8 +115,8 @@ __EXPORT float get_mag_declination(float lat, float lon) } /* find index of nearest low sampling point */ - unsigned min_lat_index = (90 + min_lat) / SAMPLING_RES; - unsigned min_lon_index = (180 + min_lat) / SAMPLING_RES; + unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES; + unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES; float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index); float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1); From e2af77123fb14e9e19d1aa9e02f04b62a342c724 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 08:25:36 +0200 Subject: [PATCH 41/58] ekf: Cleanup init --- src/modules/ekf_att_pos_estimator/estimator.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index e1ffc15d3b..a294210459 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2455,7 +2455,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) 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; @@ -2497,13 +2496,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) //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) @@ -2537,7 +2529,7 @@ void AttPosEKF::ZeroVariables() summedDelAng.zero(); summedDelVel.zero(); magBias.zero(); - magState.zero(); + magData.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { From be1f39e3851036e43be1bea0fa09096691559a0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 08:52:37 +0200 Subject: [PATCH 42/58] ekf Print declination on init --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index fb019a354e..b87338282a 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1013,7 +1013,7 @@ FixedwingEstimator::task_main() mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); 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("GPS: eph: %8.4f, epv: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m); + warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination)); _gps_initialized = true; From 7a4049b12a8be77bb67906c666cddf93286d39fc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 16:55:08 +0200 Subject: [PATCH 43/58] Fix use of declination in estimator, remove bogus measurement value reset on reinit --- src/modules/ekf_att_pos_estimator/estimator.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index a294210459..86b7efafb3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2396,7 +2396,7 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f initialHdg = atan2f(-magY, magX); /* true heading is the mag heading minus declination */ - initialHdg -= declination; + initialHdg += declination; cosRoll = cosf(initialRoll * 0.5f); sinRoll = sinf(initialRoll * 0.5f); @@ -2528,8 +2528,6 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); - magBias.zero(); - magData.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { From 6cb96d074d282af92540b6481d843c2295426773 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 17:01:32 +0200 Subject: [PATCH 44/58] EKF: Introduce proper check flags for sensor init states --- .../fw_att_pos_estimator_main.cpp | 40 ++++++++++++++++--- 1 file changed, 34 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index b87338282a..40729e94b1 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -194,6 +194,9 @@ private: bool _baro_init; bool _gps_initialized; uint64_t _gps_start_time; + bool _gyro_valid; + bool _accel_valid; + bool _mag_valid; int _mavlink_fd; @@ -341,6 +344,9 @@ FixedwingEstimator::FixedwingEstimator() : _initialized(false), _baro_init(false), _gps_initialized(false), + _gyro_valid(false), + _accel_valid(false), + _mag_valid(false), _mavlink_fd(-1), _ekf(nullptr) { @@ -617,10 +623,30 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); - /* Reset baro reference if switching to HIL */ + /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + /* system is in HIL now, we got plenty of time - make sure real sensors are off */ + usleep(250000); + +#ifndef SENSOR_COMBINED_SUB + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); +#else + /* now read all sensor publications to ensure all real sensor data is purged */ + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); +#endif + + /* set sensors to de-initialized state */ + _gyro_valid = false; + _accel_valid = false; + _mag_valid = false; + _baro_init = false; _gps_initialized = false; + + /* now skip this loop and get data on the next one */ + continue; } /** @@ -663,12 +689,14 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _gyro.x; _ekf->angRate.y = _gyro.y; _ekf->angRate.z = _gyro.z; + _gyro_valid = true; } if (accel_updated) { _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; _ekf->accel.z = _accel.z; + _accel_valid = true; } _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; @@ -713,12 +741,14 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + _gyro_valid = true; } if (accel_updated) { _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + _accel_valid = true; } _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; @@ -837,6 +867,8 @@ FixedwingEstimator::task_main() if (mag_updated) { + _mag_valid = true; + perf_count(_perf_mag); #ifndef SENSOR_COMBINED_SUB @@ -961,11 +993,7 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - // Wait long enough to ensure all sensors updated once - // XXX we rather want to check all updated - - - if (hrt_elapsed_time(&_gps_start_time) > 50000) { + if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { // bool home_set; // orb_check(_home_sub, &home_set); From 319ce3de1005d7199a992b227c7e75acb9db376b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 08:40:23 +0200 Subject: [PATCH 45/58] Minor cleanups in EKF estimator --- .../fw_att_pos_estimator_main.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 40729e94b1..e563b4b87c 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -625,8 +625,8 @@ FixedwingEstimator::task_main() /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { - /* system is in HIL now, we got plenty of time - make sure real sensors are off */ - usleep(250000); + /* system is in HIL now, wait for measurements to come in one last round */ + usleep(65000); #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -1399,13 +1399,13 @@ int FixedwingEstimator::trip_nan() { _ekf->states[5] = nan_val; usleep(100000); - // warnx("tripping covariance #1 with NaN values"); - // KH[2][2] = nan_val; // intermediate result used for covariance updates - // usleep(100000); + warnx("tripping covariance #1 with NaN values"); + _ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates + usleep(100000); - // warnx("tripping covariance #2 with NaN values"); - // KHP[5][5] = nan_val; // intermediate result used for covariance updates - // usleep(100000); + warnx("tripping covariance #2 with NaN values"); + _ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates + usleep(100000); warnx("tripping covariance #3 with NaN values"); _ekf->P[3][3] = nan_val; // covariance matrix From 21edf72779286bbfb3c4c6bb4acad5c353300024 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 14:15:11 +0200 Subject: [PATCH 46/58] Do not send a critical message when switching to dynamic state --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index e563b4b87c..05cc39f11b 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -936,7 +936,7 @@ FixedwingEstimator::task_main() { const char* str = "switching to dynamic state"; warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); break; } From 23937150bce38463612ac170803a06a3424d480d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 18:07:57 +0200 Subject: [PATCH 47/58] Fixed re-initialization of estimator, re-initializes in air now reliably. Does give useful HIL results. --- .../ekf_att_pos_estimator/estimator.cpp | 85 ++++++++-------- .../fw_att_pos_estimator_main.cpp | 96 ++++++++++++------- 2 files changed, 98 insertions(+), 83 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 86b7efafb3..a6d6a9db55 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -138,35 +138,15 @@ void swap_var(float &d1, float &d2) d2 = tmp; } -AttPosEKF::AttPosEKF() : - 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) -{ - velNED[0] = 0.0f; - velNED[1] = 0.0f; - velNED[2] = 0.0f; +AttPosEKF::AttPosEKF() + + /* NOTE: DO NOT initialize class members here. Use ZeroVariables() + * instead to allow clean in-air re-initialization. + */ +{ - InitialiseParameters(); ZeroVariables(); + InitialiseParameters(); } AttPosEKF::~AttPosEKF() @@ -2348,7 +2328,7 @@ int AttPosEKF::CheckAndBound() } // Reset the filter if the IMU data is too old - if (dtIMU > 0.2f) { + if (dtIMU > 0.3f) { ResetVelocity(); ResetPosition(); @@ -2424,24 +2404,10 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { - // Clear the init flag - statesInitialised = false; - - // Clear other flags, waiting for new data - fusionModeGPS = 0; - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - fuseMagData = false; - fuseVtasData = false; - // onGround(true), - // staticMode(true), - useAirspeed = true; - useCompass = true; - useRangeFinder = true; - - ZeroVariables(); - + // Fill variables with valid data + velNED[0] = initvelNED[0]; + velNED[1] = initvelNED[1]; + velNED[2] = initvelNED[2]; magDeclination = declination; // Calculate initial filter quaternion states from raw measurements @@ -2513,6 +2479,30 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do 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++) { for (unsigned j = 0; j < n_states; j++) { @@ -2529,6 +2519,9 @@ void AttPosEKF::ZeroVariables() summedDelAng.zero(); summedDelVel.zero(); + dAngIMU.zero(); + dVelIMU.zero(); + for (unsigned i = 0; i < data_buffer_size; i++) { for (unsigned j = 0; j < n_states; j++) { diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 05cc39f11b..07dfdbd682 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -546,24 +546,8 @@ FixedwingEstimator::task_main() /* sets also parameters in the EKF object */ parameters_update(); - /* set initial filter state */ - _ekf->fuseVelData = false; - _ekf->fusePosData = false; - _ekf->fuseHgtData = false; - _ekf->fuseMagData = false; - _ekf->fuseVtasData = false; - _ekf->statesInitialised = false; - - /* initialize measurement data */ - _ekf->VtasMeas = 0.0f; Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; - Vector3f lastAccel = {0.0f, 0.0f, -9.81f}; - _ekf->dVelIMU.x = 0.0f; - _ekf->dVelIMU.y = 0.0f; - _ekf->dVelIMU.z = 0.0f; - _ekf->dAngIMU.x = 0.0f; - _ekf->dAngIMU.y = 0.0f; - _ekf->dAngIMU.z = 0.0f; + Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; /* wakeup source(s) */ struct pollfd fds[2]; @@ -621,12 +605,14 @@ 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 */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { /* system is in HIL now, wait for measurements to come in one last round */ - usleep(65000); + usleep(60000); #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -644,8 +630,13 @@ FixedwingEstimator::task_main() _baro_init = false; _gps_initialized = false; + last_sensor_timestamp = hrt_absolute_time(); + last_run = last_sensor_timestamp; - /* now skip this loop and get data on the next one */ + _ekf->ZeroVariables(); + _ekf->dtIMU = 0.01f; + + /* now skip this loop and get data on the next one, which will also re-init the filter */ continue; } @@ -653,8 +644,6 @@ FixedwingEstimator::task_main() * PART ONE: COLLECT ALL DATA **/ - hrt_abstime last_sensor_timestamp; - /* load local copies */ #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -668,7 +657,7 @@ FixedwingEstimator::task_main() } last_sensor_timestamp = _gyro.timestamp; - _ekf.IMUmsec = _gyro.timestamp / 1e3f; + IMUmsec = _gyro.timestamp / 1e3f; float deltaT = (_gyro.timestamp - last_run) / 1e6f; last_run = _gyro.timestamp; @@ -689,6 +678,11 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _gyro.x; _ekf->angRate.y = _gyro.y; _ekf->angRate.z = _gyro.z; + + if (!_gyro_valid) { + lastAngRate = _ekf->angRate; + } + _gyro_valid = true; } @@ -696,6 +690,11 @@ FixedwingEstimator::task_main() _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; _ekf->accel.z = _accel.z; + + if (!_accel_valid) { + lastAccel = _ekf->accel; + } + _accel_valid = true; } @@ -741,6 +740,11 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + + if (!_gyro_valid) { + lastAngRate = _ekf->angRate; + } + _gyro_valid = true; } @@ -748,6 +752,11 @@ FixedwingEstimator::task_main() _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + + if (!_accel_valid) { + lastAccel = _ekf->accel; + } + _accel_valid = true; } @@ -986,6 +995,22 @@ FixedwingEstimator::task_main() _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 + continue; + } @@ -995,15 +1020,13 @@ FixedwingEstimator::task_main() if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { - // bool home_set; - // orb_check(_home_sub, &home_set); - // struct home_position_s home; - // orb_copy(ORB_ID(home_position), _home_sub, &home); + float initVelNED[3]; if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) { - _ekf->velNED[0] = _gps.vel_n_m_s; - _ekf->velNED[1] = _gps.vel_e_m_s; - _ekf->velNED[2] = _gps.vel_d_m_s; + + 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; @@ -1018,9 +1041,6 @@ FixedwingEstimator::task_main() // Set up position variables correctly _ekf->GPSstatus = _gps.fix_type; - _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->gpsLat = math::radians(lat); _ekf->gpsLon = math::radians(lon) - M_PI; @@ -1029,7 +1049,7 @@ FixedwingEstimator::task_main() // Look up mag declination based on current position float declination = math::radians(get_mag_declination(lat, lon)); - _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); + _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection _local_pos.ref_lat = lat; @@ -1041,21 +1061,23 @@ FixedwingEstimator::task_main() mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); 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", _ekf->baroHgt, _ekf->hgtMea); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination)); _gps_initialized = true; } else if (!_ekf->statesInitialised) { - _ekf->velNED[0] = 0.0f; - _ekf->velNED[1] = 0.0f; - _ekf->velNED[2] = 0.0f; + + 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->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f, 0.0f); + _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); } } From da67b2b241ca8eff0d25bfe10b0213f3d570dd50 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 May 2014 17:17:25 +0200 Subject: [PATCH 48/58] Disable time compensation for further testing --- .../ekf_att_pos_estimator/estimator.cpp | 66 +++++++++---------- 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index a6d6a9db55..5a6f879b8c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1815,41 +1815,41 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) { int ret = 0; - int64_t bestTimeDelta = 200; - unsigned bestStoreIndex = 0; - for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) - { - // Work around a GCC compiler bug - we know 64bit support on ARM is - // sketchy in GCC. - uint64_t timeDelta; + // int64_t bestTimeDelta = 200; + // unsigned bestStoreIndex = 0; + // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + // { + // // 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]; - } else { - timeDelta = statetimeStamp[storeIndex] - msec; - } + // if (msec > statetimeStamp[storeIndex]) { + // timeDelta = msec - statetimeStamp[storeIndex]; + // } else { + // timeDelta = statetimeStamp[storeIndex] - msec; + // } - if (timeDelta < bestTimeDelta) - { - bestStoreIndex = storeIndex; - bestTimeDelta = timeDelta; - } - } - if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - { - for (unsigned 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 + // if (timeDelta < bestTimeDelta) + // { + // bestStoreIndex = storeIndex; + // bestTimeDelta = timeDelta; + // } + // } + // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + // { + // for (unsigned 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 (unsigned i = 0; i < n_states; i++) { if (isfinite(states[i])) { From e098591a58771c4f49776a7b061939055c7cfc1e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 13:57:32 +0200 Subject: [PATCH 49/58] Addressed linter concerns --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 4aba65b4aa..ba8a58f650 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -302,6 +302,8 @@ FixedwingEstimator::FixedwingEstimator() : _vstatus_sub(-1), _params_sub(-1), _manual_control_sub(-1), + _mission_sub(-1), + _home_sub(-1), /* publications */ _att_pub(-1), @@ -712,6 +714,8 @@ FixedwingEstimator::task_main() if (last_accel != _sensor_combined.accelerometer_timestamp) { accel_updated = true; + } else { + accel_updated = false; } last_accel = _sensor_combined.accelerometer_timestamp; @@ -802,7 +806,6 @@ FixedwingEstimator::task_main() perf_count(_perf_gps); if (_gps.fix_type < 3) { - gps_updated = false; newDataGps = false; } else { From 5f392c80ad9bbc0e1f424125b70f277c2c677be1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 14:42:33 +0200 Subject: [PATCH 50/58] More debug in filter --- .../ekf_att_pos_estimator/estimator.cpp | 35 +++++++++++++------ 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5a6f879b8c..068d665a57 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1268,7 +1268,7 @@ void AttPosEKF::FuseMagnetometer() for (uint8_t i = 0; i < n_states; i++) { H_MAG[i] = 0.0f; } - uint8_t indexLimit; + unsigned indexLimit; // Perform sequential fusion of Magnetometer measurements. // This assumes that the errors in the different components are @@ -1281,11 +1281,11 @@ void AttPosEKF::FuseMagnetometer() // Limit range of states modified when on ground if(!onGround) { - indexLimit = 22; + indexLimit = n_states; } else { - indexLimit = 13; + indexLimit = 13 + 1; } // Sequential fusion of XYZ components to spread processing load across @@ -1483,7 +1483,7 @@ void AttPosEKF::FuseMagnetometer() if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) { // correct the state vector - for (uint8_t j= 0; j<=indexLimit; j++) + for (uint8_t j= 0; j < indexLimit; j++) { states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; } @@ -1500,7 +1500,7 @@ void AttPosEKF::FuseMagnetometer() // 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 i = 0; i < indexLimit; i++) { for (uint8_t j = 0; j <= 3; j++) { @@ -1522,9 +1522,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i<=indexLimit; i++) + for (uint8_t i = 0; i < indexLimit; i++) { - for (uint8_t j = 0; j<=indexLimit; j++) + for (uint8_t j = 0; j < indexLimit; j++) { KHP[i][j] = 0.0f; for (uint8_t k = 0; k <= 3; k++) @@ -1541,9 +1541,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i <= indexLimit; i++) + for (uint8_t i = 0; i < indexLimit; i++) { - for (uint8_t j = 0; j <= indexLimit; j++) + for (uint8_t j = 0; j < indexLimit; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -2001,7 +2001,22 @@ void AttPosEKF::CovarianceInit() float AttPosEKF::ConstrainFloat(float val, float min, float max) { - return (val > max) ? max : ((val < min) ? min : val); + float ret; + if (val > max) { + ret = max; + ekf_debug("> max: %8.4f, val: %8.4f", min); + } else if (val < min) { + ret = min; + ekf_debug("< min: %8.4f, val: %8.4f", max); + } else { + ret = val; + } + + if (!isfinite) { + ekf_debug("constrain: non-finite!"); + } + + return ret; } void AttPosEKF::ConstrainVariances() From 7ec8fe8d61ea56b632d8b02bae5d847a3f031771 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 18:42:46 +0200 Subject: [PATCH 51/58] Experimental init delay --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ba8a58f650..d96ae56373 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1021,7 +1021,7 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { + if (hrt_absolute_time() > 2 * 1000 * 1000 && _baro_init && _gyro_valid && _accel_valid && _mag_valid) { float initVelNED[3]; From 5581802f0f2f4bb34ac8c43b8aa9c1c555013758 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 18:45:55 +0200 Subject: [PATCH 52/58] ekf: Move dt inside class --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index d96ae56373..997ea17598 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -504,14 +504,13 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) estimator::g_estimator->task_main(); } -float dt = 0.0f; // time lapsed since last covariance prediction - void FixedwingEstimator::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); + float dt = 0.0f; // time lapsed since last covariance prediction if (!_ekf) { errx(1, "failed allocating EKF filter - out of RAM!"); From eeba000b8728e9b7a8daae0c0ad129257ec39403 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 19:29:43 +0200 Subject: [PATCH 53/58] stupid fix --- src/modules/ekf_att_pos_estimator/estimator.cpp | 8 +++++--- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 068d665a57..4239db6691 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2004,15 +2004,15 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max) float ret; if (val > max) { ret = max; - ekf_debug("> max: %8.4f, val: %8.4f", min); + ekf_debug("> max: %8.4f, val: %8.4f", max, val); } else if (val < min) { ret = min; - ekf_debug("< min: %8.4f, val: %8.4f", max); + ekf_debug("< min: %8.4f, val: %8.4f", min, val); } else { ret = val; } - if (!isfinite) { + if (!isfinite(val)) { ekf_debug("constrain: non-finite!"); } @@ -2119,7 +2119,9 @@ void AttPosEKF::ConstrainStates() } // Constrain delta velocity bias + ekf_debug("pre delta vel"); states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); + ekf_debug("post delta vel"); // Wind velocity limits - assume 120 m/s max velocity for (unsigned i = 14; i <= 15; i++) { diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 997ea17598..9065063ce1 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1379,7 +1379,7 @@ FixedwingEstimator::print_status() // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)dt, (int)IMUmsec); + printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); 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]); From 077de5eb0b0093e131f94f063a107f290d6c293b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 20:16:04 +0200 Subject: [PATCH 54/58] Clean implementation of filter startup delay implementation --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 9065063ce1..0921de8690 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -97,6 +97,7 @@ __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; uint32_t millis() { @@ -194,6 +195,7 @@ private: bool _baro_init; bool _gps_initialized; uint64_t _gps_start_time; + uint64_t _filter_start_time; bool _gyro_valid; bool _accel_valid; bool _mag_valid; @@ -511,6 +513,7 @@ FixedwingEstimator::task_main() _ekf = new AttPosEKF(); float dt = 0.0f; // time lapsed since last covariance prediction + _filter_start_time = hrt_absolute_time(); if (!_ekf) { errx(1, "failed allocating EKF filter - out of RAM!"); @@ -636,6 +639,7 @@ FixedwingEstimator::task_main() _ekf->ZeroVariables(); _ekf->dtIMU = 0.01f; + _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; @@ -1020,7 +1024,7 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - if (hrt_absolute_time() > 2 * 1000 * 1000 && _baro_init && _gyro_valid && _accel_valid && _mag_valid) { + if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) { float initVelNED[3]; From 1b3007aa813829e401849f53552ac5917da71f5b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 20:18:09 +0200 Subject: [PATCH 55/58] Re-enabled time compensation --- .../ekf_att_pos_estimator/estimator.cpp | 66 +++++++++---------- 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 4239db6691..9d60f5c0e5 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1815,41 +1815,41 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) { int ret = 0; - // int64_t bestTimeDelta = 200; - // unsigned bestStoreIndex = 0; - // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) - // { - // // Work around a GCC compiler bug - we know 64bit support on ARM is - // // sketchy in GCC. - // uint64_t timeDelta; + int64_t bestTimeDelta = 200; + unsigned bestStoreIndex = 0; + for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + { + // 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]; - // } else { - // timeDelta = statetimeStamp[storeIndex] - msec; - // } + if (msec > statetimeStamp[storeIndex]) { + timeDelta = msec - statetimeStamp[storeIndex]; + } else { + timeDelta = statetimeStamp[storeIndex] - msec; + } - // if (timeDelta < bestTimeDelta) - // { - // bestStoreIndex = storeIndex; - // bestTimeDelta = timeDelta; - // } - // } - // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - // { - // for (unsigned 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 + if (timeDelta < bestTimeDelta) + { + bestStoreIndex = storeIndex; + bestTimeDelta = timeDelta; + } + } + if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + { + for (unsigned 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 (unsigned i = 0; i < n_states; i++) { if (isfinite(states[i])) { From f975f9837e03cdddad20f934dfce113a6172c646 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 20:48:21 +0200 Subject: [PATCH 56/58] ekf: less console spam --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 9d60f5c0e5..ada5a541bf 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2119,9 +2119,7 @@ void AttPosEKF::ConstrainStates() } // Constrain delta velocity bias - ekf_debug("pre delta vel"); states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); - ekf_debug("post delta vel"); // Wind velocity limits - assume 120 m/s max velocity for (unsigned i = 14; i <= 15; i++) { From 6906dc4edac85113c3217b250c84e89ed1fe26a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 May 2014 11:23:48 +0200 Subject: [PATCH 57/58] Minor improvements to estimator --- src/modules/ekf_att_pos_estimator/estimator.cpp | 11 ++++++++--- src/modules/ekf_att_pos_estimator/estimator.h | 1 + 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 9d60f5c0e5..d05506396f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1,5 +1,6 @@ #include "estimator.h" #include +#include // Define EKF_DEBUG here to enable the debug print calls // if the macro is not set, these will be completely @@ -2119,9 +2120,7 @@ void AttPosEKF::ConstrainStates() } // Constrain delta velocity bias - ekf_debug("pre delta vel"); states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); - ekf_debug("post delta vel"); // Wind velocity limits - assume 120 m/s max velocity for (unsigned i = 14; i <= 15; i++) { @@ -2483,12 +2482,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) { - //store initial lat,long and height + // store initial lat,long and height latRef = referenceLat; lonRef = referenceLon; hgtRef = referenceHgt; refSet = true; + // we are at reference altitude, so measurement must be zero + hgtMea = 0.0f; + + // the baro offset must be this difference now + baroHgtOffset = baroHgt - referenceHgt; + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); InitializeDynamic(initvelNED, declination); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 378107b698..e821089f23 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -198,6 +198,7 @@ public: float velNED[3]; // North, East, Down velocity obs (m/s) 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) From ec409a1337a0f9c73cbe54776b254464e3dcd1d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 May 2014 08:53:09 +0200 Subject: [PATCH 58/58] EKF / Paul Riseborough: Added guards for mag and airspeed innovations --- .../ekf_att_pos_estimator/estimator.cpp | 45 +++++++++++++++++-- 1 file changed, 41 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index d05506396f..23ecd89ac3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1347,7 +1347,16 @@ void AttPosEKF::FuseMagnetometer() H_MAG[19] = 1.0f; // Calculate Kalman gain - SK_MX[0] = 1/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + float temp = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + if (temp >= R_MAG) { + SK_MX[0] = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the state variances and try again next time + P[19][19] += 0.1f*R_MAG; + obsIndex = 1; + return; + } 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; @@ -1399,7 +1408,16 @@ void AttPosEKF::FuseMagnetometer() H_MAG[20] = 1; // Calculate Kalman gain - SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + float temp = (P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + if (temp >= R_MAG) { + SK_MY[0] = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the state variances and try again next time + P[20][20] += 0.1f*R_MAG; + obsIndex = 2; + return; + } 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; @@ -1445,7 +1463,16 @@ void AttPosEKF::FuseMagnetometer() H_MAG[21] = 1; // Calculate Kalman gain - SK_MZ[0] = 1/(P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + float temp = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + if (temp >= R_MAG) { + SK_MZ[0] = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the state variances and try again next time + P[21][21] += 0.1f*R_MAG; + obsIndex = 3; + return; + } SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; @@ -1565,6 +1592,7 @@ void AttPosEKF::FuseAirspeed() float vwe; float R_TAS = sq(airspeedMeasurementSigma); float SH_TAS[3]; + float SK_TAS; float VtasPred; // Copy required states to local variable names @@ -1594,7 +1622,16 @@ void AttPosEKF::FuseAirspeed() H_TAS[15] = -SH_TAS[1]; // Calculate Kalman gains - float SK_TAS = 1/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + float temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + if (temp >= R_TAS) { + SK_TAS = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the wind state variances and try again next time + P[14][14] += 0.05f*R_TAS; + P[15][15] += 0.05f*R_TAS; + return; + } Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][14]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][15]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][14]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][15]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][14]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][15]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]);