aboutsummaryrefslogtreecommitdiff
path: root/src/modules/ekf_att_pos_estimator
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/ekf_att_pos_estimator')
-rw-r--r--src/modules/ekf_att_pos_estimator/estimator.cpp2539
-rw-r--r--src/modules/ekf_att_pos_estimator/estimator.h350
-rw-r--r--src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp1393
-rw-r--r--src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c260
-rw-r--r--src/modules/ekf_att_pos_estimator/module.mk42
5 files changed, 4584 insertions, 0 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp
new file mode 100644
index 000000000..5de22fdae
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/estimator.cpp
@@ -0,0 +1,2539 @@
+#include "estimator.h"
+#include <string.h>
+
+// Define EKF_DEBUG here to enable the debug print calls
+// if the macro is not set, these will be completely
+// optimized out by the compiler.
+#define EKF_DEBUG
+
+#ifdef EKF_DEBUG
+#include <stdio.h>
+
+static void
+ekf_debug_print(const char *fmt, va_list args)
+{
+ fprintf(stderr, "%s: ", "[ekf]");
+ vfprintf(stderr, fmt, args);
+
+ fprintf(stderr, "\n");
+}
+
+static void
+ekf_debug(const char *fmt, ...)
+{
+ va_list args;
+
+ va_start(args, fmt);
+ ekf_debug_print(fmt, args);
+}
+
+#else
+
+static void ekf_debug(const char *fmt, ...) { while(0){} }
+#endif
+
+float Vector3f::length(void) const
+{
+ return sqrt(x*x + y*y + z*z);
+}
+
+void Vector3f::zero(void)
+{
+ x = 0.0f;
+ y = 0.0f;
+ z = 0.0f;
+}
+
+Mat3f::Mat3f() {
+ identity();
+}
+
+void Mat3f::identity() {
+ x.x = 1.0f;
+ x.y = 0.0f;
+ x.z = 0.0f;
+
+ y.x = 0.0f;
+ y.y = 1.0f;
+ y.z = 0.0f;
+
+ z.x = 0.0f;
+ z.y = 0.0f;
+ z.z = 1.0f;
+}
+
+Mat3f Mat3f::transpose(void) const
+{
+ Mat3f ret = *this;
+ swap_var(ret.x.y, ret.y.x);
+ swap_var(ret.x.z, ret.z.x);
+ swap_var(ret.y.z, ret.z.y);
+ return ret;
+}
+
+// overload + operator to provide a vector addition
+Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
+{
+ Vector3f vecOut;
+ vecOut.x = vecIn1.x + vecIn2.x;
+ vecOut.y = vecIn1.y + vecIn2.y;
+ vecOut.z = vecIn1.z + vecIn2.z;
+ return vecOut;
+}
+
+// overload - operator to provide a vector subtraction
+Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
+{
+ Vector3f vecOut;
+ vecOut.x = vecIn1.x - vecIn2.x;
+ vecOut.y = vecIn1.y - vecIn2.y;
+ vecOut.z = vecIn1.z - vecIn2.z;
+ return vecOut;
+}
+
+// overload * operator to provide a matrix vector product
+Vector3f operator*( Mat3f matIn, Vector3f vecIn)
+{
+ Vector3f vecOut;
+ vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
+ vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
+ vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
+ return vecOut;
+}
+
+// overload % operator to provide a vector cross product
+Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
+{
+ Vector3f vecOut;
+ vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
+ vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
+ vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
+ return vecOut;
+}
+
+// overload * operator to provide a vector scaler product
+Vector3f operator*(Vector3f vecIn1, float sclIn1)
+{
+ Vector3f vecOut;
+ vecOut.x = vecIn1.x * sclIn1;
+ vecOut.y = vecIn1.y * sclIn1;
+ vecOut.z = vecIn1.z * sclIn1;
+ return vecOut;
+}
+
+// overload * operator to provide a vector scaler product
+Vector3f operator*(float sclIn1, Vector3f vecIn1)
+{
+ Vector3f vecOut;
+ vecOut.x = vecIn1.x * sclIn1;
+ vecOut.y = vecIn1.y * sclIn1;
+ vecOut.z = vecIn1.z * sclIn1;
+ return vecOut;
+}
+
+void swap_var(float &d1, float &d2)
+{
+ float tmp = d1;
+ d1 = d2;
+ d2 = tmp;
+}
+
+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)
+{
+ velNED[0] = 0.0f;
+ velNED[1] = 0.0f;
+ velNED[2] = 0.0f;
+
+ InitialiseParameters();
+ ZeroVariables();
+}
+
+AttPosEKF::~AttPosEKF()
+{
+}
+
+void AttPosEKF::UpdateStrapdownEquationsNED()
+{
+ Vector3f delVelNav;
+ float q00;
+ float q11;
+ float q22;
+ float q33;
+ float q01;
+ float q02;
+ float q03;
+ float q12;
+ float q13;
+ float q23;
+ float rotationMag;
+ float qUpdated[4];
+ float quatMag;
+ float deltaQuat[4];
+ const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS};
+
+// Remove sensor bias errors
+ correctedDelAng.x = dAngIMU.x - states[10];
+ correctedDelAng.y = dAngIMU.y - states[11];
+ correctedDelAng.z = dAngIMU.z - states[12];
+ dVelIMU.x = dVelIMU.x;
+ dVelIMU.y = dVelIMU.y;
+ dVelIMU.z = dVelIMU.z - states[13];
+
+// Save current measurements
+ Vector3f prevDelAng = correctedDelAng;
+
+// Apply corrections for earths rotation rate and coning errors
+// * and + operators have been overloaded
+ correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
+
+// Convert the rotation vector to its equivalent quaternion
+ rotationMag = correctedDelAng.length();
+ if (rotationMag < 1e-12f)
+ {
+ deltaQuat[0] = 1.0;
+ deltaQuat[1] = 0.0;
+ deltaQuat[2] = 0.0;
+ deltaQuat[3] = 0.0;
+ }
+ else
+ {
+ deltaQuat[0] = 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;
+ }
+
+// Update the quaternions by rotating from the previous attitude through
+// the delta angle rotation quaternion
+ qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3];
+ qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2];
+ qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3];
+ qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
+
+// Normalise the quaternions and update the quaternion states
+ quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
+ if (quatMag > 1e-16f)
+ {
+ float quatMagInv = 1.0f/quatMag;
+ states[0] = quatMagInv*qUpdated[0];
+ states[1] = quatMagInv*qUpdated[1];
+ states[2] = quatMagInv*qUpdated[2];
+ states[3] = quatMagInv*qUpdated[3];
+ }
+
+// Calculate the body to nav cosine matrix
+ q00 = sq(states[0]);
+ q11 = sq(states[1]);
+ q22 = sq(states[2]);
+ q33 = sq(states[3]);
+ q01 = states[0]*states[1];
+ q02 = states[0]*states[2];
+ q03 = states[0]*states[3];
+ q12 = states[1]*states[2];
+ q13 = states[1]*states[3];
+ q23 = states[2]*states[3];
+
+ Tbn.x.x = q00 + q11 - q22 - q33;
+ Tbn.y.y = q00 - q11 + q22 - q33;
+ Tbn.z.z = q00 - q11 - q22 + q33;
+ Tbn.x.y = 2*(q12 - q03);
+ Tbn.x.z = 2*(q13 + q02);
+ Tbn.y.x = 2*(q12 + q03);
+ Tbn.y.z = 2*(q23 - q01);
+ Tbn.z.x = 2*(q13 - q02);
+ Tbn.z.y = 2*(q23 + q01);
+
+ Tnb = Tbn.transpose();
+
+// transform body delta velocities to delta velocities in the nav frame
+// * and + operators have been overloaded
+ //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
+ delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU;
+ delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU;
+ delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU;
+
+// calculate the magnitude of the nav acceleration (required for GPS
+// variance estimation)
+ accNavMag = delVelNav.length()/dtIMU;
+
+// If calculating position save previous velocity
+ float lastVelocity[3];
+ lastVelocity[0] = states[4];
+ lastVelocity[1] = states[5];
+ lastVelocity[2] = states[6];
+
+// Sum delta velocities to get velocity
+ states[4] = states[4] + delVelNav.x;
+ states[5] = states[5] + delVelNav.y;
+ states[6] = states[6] + delVelNav.z;
+
+// If calculating postions, do a trapezoidal integration for position
+ states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU;
+ states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU;
+ states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU;
+
+ // Constrain states (to protect against filter divergence)
+ ConstrainStates();
+}
+
+void AttPosEKF::CovariancePrediction(float dt)
+{
+ // scalars
+ float daxCov;
+ float dayCov;
+ float dazCov;
+ float dvxCov;
+ float dvyCov;
+ float dvzCov;
+ float dvx;
+ float dvy;
+ float dvz;
+ float dax;
+ float day;
+ float daz;
+ float q0;
+ float q1;
+ float q2;
+ float q3;
+ float dax_b;
+ float day_b;
+ float daz_b;
+ float dvz_b;
+
+ // arrays
+ float processNoise[n_states];
+ float SF[15];
+ float SG[8];
+ float SQ[11];
+ float SPP[8] = {0};
+ float nextP[n_states][n_states];
+
+ // calculate covariance prediction process noise
+ for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f;
+ for (uint8_t i=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;
+ dvy = summedDelVel.y;
+ dvz = summedDelVel.z;
+ dax = summedDelAng.x;
+ day = summedDelAng.y;
+ daz = summedDelAng.z;
+ q0 = states[0];
+ q1 = states[1];
+ q2 = states[2];
+ q3 = states[3];
+ dax_b = states[10];
+ day_b = states[11];
+ daz_b = states[12];
+ 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);
+ accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f);
+ dvxCov = sq(dt*accelProcessNoise);
+ dvyCov = sq(dt*accelProcessNoise);
+ dvzCov = sq(dt*accelProcessNoise);
+
+ // Predicted covariance calculation
+ SF[0] = dvz - dvz_b;
+ SF[1] = 2*q3*SF[0] + 2*dvx*q1 + 2*dvy*q2;
+ SF[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0;
+ SF[3] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3;
+ SF[4] = day/2 - day_b/2;
+ SF[5] = daz/2 - daz_b/2;
+ SF[6] = dax/2 - dax_b/2;
+ SF[7] = dax_b/2 - dax/2;
+ SF[8] = daz_b/2 - daz/2;
+ SF[9] = day_b/2 - day/2;
+ SF[10] = 2*q0*SF[0];
+ SF[11] = q1/2;
+ SF[12] = q2/2;
+ SF[13] = q3/2;
+ SF[14] = 2*dvy*q1;
+
+ SG[0] = q0/2;
+ SG[1] = sq(q3);
+ SG[2] = sq(q2);
+ SG[3] = sq(q1);
+ SG[4] = sq(q0);
+ SG[5] = 2*q2*q3;
+ SG[6] = 2*q1*q3;
+ SG[7] = 2*q1*q2;
+
+ SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3);
+ SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3);
+ SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
+ SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4;
+ SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4;
+ SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4;
+ SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4;
+ SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2;
+ SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4;
+ SQ[9] = sq(SG[0]);
+ SQ[10] = sq(q1);
+
+ SPP[0] = SF[10] + SF[14] - 2*dvx*q2;
+ SPP[1] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3;
+ SPP[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0;
+ SPP[3] = 2*q0*q1 - 2*q2*q3;
+ SPP[4] = 2*q0*q2 + 2*q1*q3;
+ SPP[5] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
+ SPP[6] = SF[13];
+ SPP[7] = SF[12];
+
+ nextP[0][0] = P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6] + (daxCov*SQ[10])/4 + SF[7]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[9]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[8]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) + SPP[7]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[6]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4;
+ nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6] + SF[6]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[5]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[9]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[6]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) - SPP[7]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - (q0*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]))/2;
+ nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6] + SF[4]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[8]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[6]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - SPP[6]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]))/2;
+ nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6] + SF[5]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[4]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[7]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SF[11]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[7]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]))/2;
+ nextP[0][4] = P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6] + SF[3]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[0]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SPP[2]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[4]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
+ nextP[0][5] = P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6] + SF[2]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[3]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[0]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[3]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
+ nextP[0][6] = P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6] + SF[2]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[1]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[0]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) - SPP[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
+ nextP[0][7] = P[0][7] + P[1][7]*SF[7] + P[2][7]*SF[9] + P[3][7]*SF[8] + P[10][7]*SF[11] + P[11][7]*SPP[7] + P[12][7]*SPP[6] + dt*(P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6]);
+ nextP[0][8] = P[0][8] + P[1][8]*SF[7] + P[2][8]*SF[9] + P[3][8]*SF[8] + P[10][8]*SF[11] + P[11][8]*SPP[7] + P[12][8]*SPP[6] + dt*(P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6]);
+ nextP[0][9] = P[0][9] + P[1][9]*SF[7] + P[2][9]*SF[9] + P[3][9]*SF[8] + P[10][9]*SF[11] + P[11][9]*SPP[7] + P[12][9]*SPP[6] + dt*(P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6]);
+ nextP[0][10] = P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6];
+ nextP[0][11] = P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6];
+ nextP[0][12] = P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6];
+ nextP[0][13] = P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6];
+ nextP[0][14] = P[0][14] + P[1][14]*SF[7] + P[2][14]*SF[9] + P[3][14]*SF[8] + P[10][14]*SF[11] + P[11][14]*SPP[7] + P[12][14]*SPP[6];
+ nextP[0][15] = P[0][15] + P[1][15]*SF[7] + P[2][15]*SF[9] + P[3][15]*SF[8] + P[10][15]*SF[11] + P[11][15]*SPP[7] + P[12][15]*SPP[6];
+ nextP[0][16] = P[0][16] + P[1][16]*SF[7] + P[2][16]*SF[9] + P[3][16]*SF[8] + P[10][16]*SF[11] + P[11][16]*SPP[7] + P[12][16]*SPP[6];
+ nextP[0][17] = P[0][17] + P[1][17]*SF[7] + P[2][17]*SF[9] + P[3][17]*SF[8] + P[10][17]*SF[11] + P[11][17]*SPP[7] + P[12][17]*SPP[6];
+ nextP[0][18] = P[0][18] + P[1][18]*SF[7] + P[2][18]*SF[9] + P[3][18]*SF[8] + P[10][18]*SF[11] + P[11][18]*SPP[7] + P[12][18]*SPP[6];
+ nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6];
+ nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6];
+ nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6];
+ nextP[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);
+ nextP[7][10] = P[7][10] + P[4][10]*dt;
+ nextP[7][11] = P[7][11] + P[4][11]*dt;
+ nextP[7][12] = P[7][12] + P[4][12]*dt;
+ nextP[7][13] = P[7][13] + P[4][13]*dt;
+ nextP[7][14] = P[7][14] + P[4][14]*dt;
+ nextP[7][15] = P[7][15] + P[4][15]*dt;
+ nextP[7][16] = P[7][16] + P[4][16]*dt;
+ nextP[7][17] = P[7][17] + P[4][17]*dt;
+ nextP[7][18] = P[7][18] + P[4][18]*dt;
+ nextP[7][19] = P[7][19] + P[4][19]*dt;
+ nextP[7][20] = P[7][20] + P[4][20]*dt;
+ nextP[7][21] = P[7][21] + P[4][21]*dt;
+ nextP[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);
+ nextP[8][10] = P[8][10] + P[5][10]*dt;
+ nextP[8][11] = P[8][11] + P[5][11]*dt;
+ nextP[8][12] = P[8][12] + P[5][12]*dt;
+ nextP[8][13] = P[8][13] + P[5][13]*dt;
+ nextP[8][14] = P[8][14] + P[5][14]*dt;
+ nextP[8][15] = P[8][15] + P[5][15]*dt;
+ nextP[8][16] = P[8][16] + P[5][16]*dt;
+ nextP[8][17] = P[8][17] + P[5][17]*dt;
+ nextP[8][18] = P[8][18] + P[5][18]*dt;
+ nextP[8][19] = P[8][19] + P[5][19]*dt;
+ nextP[8][20] = P[8][20] + P[5][20]*dt;
+ nextP[8][21] = P[8][21] + P[5][21]*dt;
+ nextP[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);
+ nextP[9][10] = P[9][10] + P[6][10]*dt;
+ nextP[9][11] = P[9][11] + P[6][11]*dt;
+ nextP[9][12] = P[9][12] + P[6][12]*dt;
+ nextP[9][13] = P[9][13] + P[6][13]*dt;
+ nextP[9][14] = P[9][14] + P[6][14]*dt;
+ nextP[9][15] = P[9][15] + P[6][15]*dt;
+ nextP[9][16] = P[9][16] + P[6][16]*dt;
+ nextP[9][17] = P[9][17] + P[6][17]*dt;
+ nextP[9][18] = P[9][18] + P[6][18]*dt;
+ nextP[9][19] = P[9][19] + P[6][19]*dt;
+ nextP[9][20] = P[9][20] + P[6][20]*dt;
+ nextP[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;
+ nextP[10][10] = P[10][10];
+ nextP[10][11] = P[10][11];
+ nextP[10][12] = P[10][12];
+ nextP[10][13] = P[10][13];
+ nextP[10][14] = P[10][14];
+ nextP[10][15] = P[10][15];
+ nextP[10][16] = P[10][16];
+ nextP[10][17] = P[10][17];
+ nextP[10][18] = P[10][18];
+ nextP[10][19] = P[10][19];
+ nextP[10][20] = P[10][20];
+ nextP[10][21] = P[10][21];
+ nextP[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;
+ nextP[11][10] = P[11][10];
+ nextP[11][11] = P[11][11];
+ nextP[11][12] = P[11][12];
+ nextP[11][13] = P[11][13];
+ nextP[11][14] = P[11][14];
+ nextP[11][15] = P[11][15];
+ nextP[11][16] = P[11][16];
+ nextP[11][17] = P[11][17];
+ nextP[11][18] = P[11][18];
+ nextP[11][19] = P[11][19];
+ nextP[11][20] = P[11][20];
+ nextP[11][21] = P[11][21];
+ nextP[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;
+ nextP[12][10] = P[12][10];
+ nextP[12][11] = P[12][11];
+ nextP[12][12] = P[12][12];
+ nextP[12][13] = P[12][13];
+ nextP[12][14] = P[12][14];
+ nextP[12][15] = P[12][15];
+ nextP[12][16] = P[12][16];
+ nextP[12][17] = P[12][17];
+ nextP[12][18] = P[12][18];
+ nextP[12][19] = P[12][19];
+ nextP[12][20] = P[12][20];
+ nextP[12][21] = P[12][21];
+ nextP[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;
+ nextP[13][10] = P[13][10];
+ nextP[13][11] = P[13][11];
+ nextP[13][12] = P[13][12];
+ nextP[13][13] = P[13][13];
+ nextP[13][14] = P[13][14];
+ nextP[13][15] = P[13][15];
+ nextP[13][16] = P[13][16];
+ nextP[13][17] = P[13][17];
+ nextP[13][18] = P[13][18];
+ nextP[13][19] = P[13][19];
+ nextP[13][20] = P[13][20];
+ nextP[13][21] = P[13][21];
+ nextP[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;
+ nextP[14][10] = P[14][10];
+ nextP[14][11] = P[14][11];
+ nextP[14][12] = P[14][12];
+ nextP[14][13] = P[14][13];
+ nextP[14][14] = P[14][14];
+ nextP[14][15] = P[14][15];
+ nextP[14][16] = P[14][16];
+ nextP[14][17] = P[14][17];
+ nextP[14][18] = P[14][18];
+ nextP[14][19] = P[14][19];
+ nextP[14][20] = P[14][20];
+ nextP[14][21] = P[14][21];
+ nextP[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;
+ nextP[15][10] = P[15][10];
+ nextP[15][11] = P[15][11];
+ nextP[15][12] = P[15][12];
+ nextP[15][13] = P[15][13];
+ nextP[15][14] = P[15][14];
+ nextP[15][15] = P[15][15];
+ nextP[15][16] = P[15][16];
+ nextP[15][17] = P[15][17];
+ nextP[15][18] = P[15][18];
+ nextP[15][19] = P[15][19];
+ nextP[15][20] = P[15][20];
+ nextP[15][21] = P[15][21];
+ nextP[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;
+ nextP[16][10] = P[16][10];
+ nextP[16][11] = P[16][11];
+ nextP[16][12] = P[16][12];
+ nextP[16][13] = P[16][13];
+ nextP[16][14] = P[16][14];
+ nextP[16][15] = P[16][15];
+ nextP[16][16] = P[16][16];
+ nextP[16][17] = P[16][17];
+ nextP[16][18] = P[16][18];
+ nextP[16][19] = P[16][19];
+ nextP[16][20] = P[16][20];
+ nextP[16][21] = P[16][21];
+ nextP[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;
+ nextP[17][10] = P[17][10];
+ nextP[17][11] = P[17][11];
+ nextP[17][12] = P[17][12];
+ nextP[17][13] = P[17][13];
+ nextP[17][14] = P[17][14];
+ nextP[17][15] = P[17][15];
+ nextP[17][16] = P[17][16];
+ nextP[17][17] = P[17][17];
+ nextP[17][18] = P[17][18];
+ nextP[17][19] = P[17][19];
+ nextP[17][20] = P[17][20];
+ nextP[17][21] = P[17][21];
+ nextP[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;
+ nextP[18][10] = P[18][10];
+ nextP[18][11] = P[18][11];
+ nextP[18][12] = P[18][12];
+ nextP[18][13] = P[18][13];
+ nextP[18][14] = P[18][14];
+ nextP[18][15] = P[18][15];
+ nextP[18][16] = P[18][16];
+ nextP[18][17] = P[18][17];
+ nextP[18][18] = P[18][18];
+ nextP[18][19] = P[18][19];
+ nextP[18][20] = P[18][20];
+ nextP[18][21] = P[18][21];
+ nextP[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;
+ nextP[19][10] = P[19][10];
+ nextP[19][11] = P[19][11];
+ nextP[19][12] = P[19][12];
+ nextP[19][13] = P[19][13];
+ nextP[19][14] = P[19][14];
+ nextP[19][15] = P[19][15];
+ nextP[19][16] = P[19][16];
+ nextP[19][17] = P[19][17];
+ nextP[19][18] = P[19][18];
+ nextP[19][19] = P[19][19];
+ nextP[19][20] = P[19][20];
+ nextP[19][21] = P[19][21];
+ nextP[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;
+ nextP[20][10] = P[20][10];
+ nextP[20][11] = P[20][11];
+ nextP[20][12] = P[20][12];
+ nextP[20][13] = P[20][13];
+ nextP[20][14] = P[20][14];
+ nextP[20][15] = P[20][15];
+ nextP[20][16] = P[20][16];
+ nextP[20][17] = P[20][17];
+ nextP[20][18] = P[20][18];
+ nextP[20][19] = P[20][19];
+ nextP[20][20] = P[20][20];
+ nextP[20][21] = P[20][21];
+ nextP[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++)
+ {
+ nextP[i][i] = nextP[i][i] + processNoise[i];
+ }
+
+ // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by
+ // setting the coresponding covariance terms to zero.
+ if (onGround || !useCompass)
+ {
+ zeroRows(nextP,16,21);
+ zeroCols(nextP,16,21);
+ }
+
+ // If on ground or not using airspeed sensing, inhibit wind velocity
+ // covariance growth.
+ if (onGround || !useAirspeed)
+ {
+ 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
+ // growth by setting the predicted to the previous values
+ // This prevent an ill conditioned matrix from occurring for long periods
+ // without GPS
+ if ((P[7][7] + P[8][8]) > 1E6f)
+ {
+ for (uint8_t i=7; i<=8; i++)
+ {
+ for (unsigned j = 0; j < n_states; j++)
+ {
+ nextP[i][j] = P[i][j];
+ nextP[j][i] = P[j][i];
+ }
+ }
+ }
+
+ if (onGround || staticMode) {
+ // copy the portion of the variances we want to
+ // propagate
+ for (unsigned i = 0; i <= 13; i++) {
+ P[i][i] = nextP[i][i];
+
+ // force symmetry for observable states
+ // force zero for non-observable states
+ for (unsigned i = 1; i < n_states; i++)
+ {
+ for (uint8_t j = 0; j < i; j++)
+ {
+ if ((i > 13) || (j > 13)) {
+ P[i][j] = 0.0f;
+ } else {
+ P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
+ }
+ P[j][i] = P[i][j];
+ }
+ }
+ }
+
+ } else {
+
+ // Copy covariance
+ for (unsigned i = 0; i < n_states; i++) {
+ P[i][i] = nextP[i][i];
+ }
+
+ // force symmetry for observable states
+ for (unsigned i = 1; i < n_states; i++)
+ {
+ for (uint8_t j = 0; j < i; j++)
+ {
+ P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
+ P[j][i] = P[i][j];
+ }
+ }
+
+ }
+
+ ConstrainVariances();
+}
+
+void AttPosEKF::FuseVelposNED()
+{
+
+// declare variables used by fault isolation logic
+ uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure
+ uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available
+ uint32_t hgtRetryTime = 5000; // height measurement retry time
+ uint32_t horizRetryTime;
+
+// declare variables used to check measurement errors
+ float velInnov[3] = {0.0f,0.0f,0.0f};
+ float posInnov[2] = {0.0f,0.0f};
+ float hgtInnov = 0.0f;
+
+// declare variables used to control access to arrays
+ bool fuseData[6] = {false,false,false,false,false,false};
+ uint8_t stateIndex;
+ uint8_t obsIndex;
+ uint8_t indexLimit;
+
+// declare variables used by state and covariance update calculations
+ float velErr;
+ float posErr;
+ float R_OBS[6];
+ float observation[6];
+ float SK;
+ float quatMag;
+
+// Perform sequential fusion of GPS measurements. This assumes that the
+// errors in the different velocity and position components are
+// uncorrelated which is not true, however in the absence of covariance
+// data from the GPS receiver it is the only assumption we can make
+// so we might as well take advantage of the computational efficiencies
+// associated with sequential fusion
+ if (fuseVelData || fusePosData || fuseHgtData)
+ {
+ // set the GPS data timeout depending on whether airspeed data is present
+ if (useAirspeed) horizRetryTime = gpsRetryTime;
+ else horizRetryTime = gpsRetryTimeNoTAS;
+
+ // Form the observation vector
+ for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i];
+ for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3];
+ observation[5] = -(hgtMea);
+
+ // Estimate the GPS Velocity, GPS horiz position and height measurement variances.
+ velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
+ posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
+ R_OBS[0] = sq(vneSigma) + sq(velErr);
+ R_OBS[1] = R_OBS[0];
+ 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++)
+ {
+ varInnovVelPos[i] = 0.0f;
+ }
+ // calculate innovations and check GPS data validity using an innovation consistency check
+ if (fuseVelData)
+ {
+ // test velocity measurements
+ uint8_t imax = 2;
+ if (fusionModeGPS == 1) imax = 1;
+ for (uint8_t i = 0; i<=imax; i++)
+ {
+ velInnov[i] = statesAtVelTime[i+4] - velNED[i];
+ stateIndex = 4 + i;
+ varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
+ }
+ // apply a 5-sigma threshold
+ current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
+ current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
+ if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
+ {
+ current_ekf_state.velHealth = true;
+ current_ekf_state.velFailTime = millis();
+ }
+ else
+ {
+ current_ekf_state.velHealth = false;
+ }
+ }
+ if (fusePosData)
+ {
+ // test horizontal position measurements
+ posInnov[0] = statesAtPosTime[7] - posNE[0];
+ posInnov[1] = statesAtPosTime[8] - posNE[1];
+ varInnovVelPos[3] = P[7][7] + R_OBS[3];
+ varInnovVelPos[4] = P[8][8] + R_OBS[4];
+ // apply a 10-sigma threshold
+ current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]);
+ current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime;
+ if (current_ekf_state.posHealth || current_ekf_state.posTimeout)
+ {
+ current_ekf_state.posHealth = true;
+ current_ekf_state.posFailTime = millis();
+ }
+ else
+ {
+ current_ekf_state.posHealth = false;
+ }
+ }
+ // test height measurements
+ if (fuseHgtData)
+ {
+ hgtInnov = statesAtHgtTime[9] + hgtMea;
+ varInnovVelPos[5] = P[9][9] + R_OBS[5];
+ // apply a 10-sigma threshold
+ current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
+ current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
+ if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout)
+ {
+ current_ekf_state.hgtHealth = true;
+ current_ekf_state.hgtFailTime = millis();
+ }
+ else
+ {
+ current_ekf_state.hgtHealth = false;
+ }
+ }
+ // Set range for sequential fusion of velocity and position measurements depending
+ // on which data is available and its health
+ if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth)
+ {
+ fuseData[0] = true;
+ fuseData[1] = true;
+ fuseData[2] = true;
+ }
+ if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth)
+ {
+ fuseData[0] = true;
+ fuseData[1] = true;
+ }
+ if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth)
+ {
+ fuseData[3] = true;
+ fuseData[4] = true;
+ }
+ if (fuseHgtData && current_ekf_state.hgtHealth)
+ {
+ fuseData[5] = true;
+ }
+ // Limit range of states modified when on ground
+ if(!onGround)
+ {
+ indexLimit = 22;
+ }
+ else
+ {
+ indexLimit = 13;
+ }
+ // Fuse measurements sequentially
+ for (obsIndex=0; obsIndex<=5; obsIndex++)
+ {
+ if (fuseData[obsIndex])
+ {
+ stateIndex = 4 + obsIndex;
+ // Calculate the measurement innovation, using states from a
+ // different time coordinate if fusing height data
+ if (obsIndex >= 0 && obsIndex <= 2)
+ {
+ innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex];
+ }
+ else if (obsIndex == 3 || obsIndex == 4)
+ {
+ innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex];
+ }
+ else if (obsIndex == 5)
+ {
+ innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex];
+ }
+ // Calculate the Kalman Gain
+ // Calculate innovation variances - also used for data logging
+ varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
+ SK = 1.0/varInnovVelPos[obsIndex];
+ for (uint8_t i= 0; i<=indexLimit; i++)
+ {
+ Kfusion[i] = P[i][stateIndex]*SK;
+ }
+
+ // 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++)
+ {
+ states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
+ }
+ quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+ if (quatMag > 1e-12f) // divide by 0 protection
+ {
+ for (uint8_t i = 0; i<=3; i++)
+ {
+ states[i] = states[i] / quatMag;
+ }
+ }
+ // Update the covariance - take advantage of direct observation of a
+ // single state at index = stateIndex to reduce computations
+ // Optimised implementation of standard equation P = (I - K*H)*P;
+ for (uint8_t i= 0; i<=indexLimit; i++)
+ {
+ for (uint8_t j= 0; j<=indexLimit; j++)
+ {
+ KHP[i][j] = Kfusion[i] * P[stateIndex][j];
+ }
+ }
+ for (uint8_t i= 0; i<=indexLimit; i++)
+ {
+ for (uint8_t j= 0; j<=indexLimit; j++)
+ {
+ P[i][j] = P[i][j] - KHP[i][j];
+ }
+ }
+ }
+ }
+ }
+
+ ForceSymmetry();
+ ConstrainVariances();
+
+}
+
+void AttPosEKF::FuseMagnetometer()
+{
+
+ 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];
+ 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
+// uncorrelated which is not true, however in the absence of covariance
+// data fit is the only assumption we can make
+// so we might as well take advantage of the computational efficiencies
+// associated with sequential fusion
+ if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2))
+ {
+ // Limit range of states modified when on ground
+ if(!onGround)
+ {
+ indexLimit = 22;
+ }
+ else
+ {
+ indexLimit = 13;
+ }
+
+ // Sequential fusion of XYZ components to spread processing load across
+ // three prediction time steps.
+
+ // Calculate observation jacobians and Kalman gains
+ if (fuseMagData)
+ {
+ // Copy required states to local variable names
+ q0 = statesAtMagMeasTime[0];
+ q1 = statesAtMagMeasTime[1];
+ q2 = statesAtMagMeasTime[2];
+ q3 = statesAtMagMeasTime[3];
+ 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
+ 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);
+
+ // Calculate observation jacobians
+ SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
+ SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
+ SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
+ SH_MAG[3] = sq(q3);
+ SH_MAG[4] = sq(q2);
+ SH_MAG[5] = sq(q1);
+ SH_MAG[6] = sq(q0);
+ SH_MAG[7] = 2*magN*q0;
+ SH_MAG[8] = 2*magE*q3;
+
+ for (uint8_t i = 0; i < 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[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[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][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 < n_states; i++) H_MAG[i] = 0;
+ H_MAG[0] = SH_MAG[2];
+ H_MAG[1] = SH_MAG[1];
+ H_MAG[2] = SH_MAG[0];
+ H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7];
+ H_MAG[16] = 2*q1*q2 - 2*q0*q3;
+ H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
+ H_MAG[18] = 2*q0*q1 + 2*q2*q3;
+ H_MAG[20] = 1;
+
+ // 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));
+ SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
+ SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+ SK_MY[3] = 2*q0*q3 - 2*q1*q2;
+ SK_MY[4] = 2*q0*q1 + 2*q2*q3;
+ Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]);
+ Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]);
+ Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]);
+ Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]);
+ Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]);
+ Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]);
+ Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]);
+ Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]);
+ Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]);
+ Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]);
+ Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]);
+ Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]);
+ Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]);
+ // 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_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]);
+ Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]);
+ Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]);
+ Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]);
+ Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]);
+ Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]);
+ Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]);
+ Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]);
+ Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
+ Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
+ varInnovMag[1] = 1.0f/SK_MY[0];
+ innovMag[1] = MagPred[1] - magData.y;
+ }
+ else if (obsIndex == 2) // we are now fusing the Z measurement
+ {
+ // Calculate observation jacobians
+ for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0;
+ H_MAG[0] = SH_MAG[1];
+ H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1;
+ H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+ H_MAG[3] = SH_MAG[0];
+ H_MAG[16] = 2*q0*q2 + 2*q1*q3;
+ H_MAG[17] = 2*q2*q3 - 2*q0*q1;
+ H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
+ H_MAG[21] = 1;
+
+ // 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));
+ SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
+ SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
+ SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+ SK_MZ[4] = 2*q0*q1 - 2*q2*q3;
+ SK_MZ[5] = 2*q0*q2 + 2*q1*q3;
+ Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[5] - P[0][17]*SK_MZ[4]);
+ Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[5] - P[1][17]*SK_MZ[4]);
+ Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[5] - P[2][17]*SK_MZ[4]);
+ Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[5] - P[3][17]*SK_MZ[4]);
+ Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[5] - P[4][17]*SK_MZ[4]);
+ Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[5] - P[5][17]*SK_MZ[4]);
+ Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[5] - P[6][17]*SK_MZ[4]);
+ Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[5] - P[7][17]*SK_MZ[4]);
+ Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[5] - P[8][17]*SK_MZ[4]);
+ Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[5] - P[9][17]*SK_MZ[4]);
+ Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[5] - P[10][17]*SK_MZ[4]);
+ Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[5] - P[11][17]*SK_MZ[4]);
+ Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]);
+ // 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_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]);
+ Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]);
+ Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]);
+ Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]);
+ Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]);
+ Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]);
+ Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]);
+ Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]);
+ Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]);
+ Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]);
+ varInnovMag[2] = 1.0f/SK_MZ[0];
+ innovMag[2] = MagPred[2] - magData.z;
+
+ }
+
+ // Check the innovation for consistency and don't fuse if > 5Sigma
+ if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0)
+ {
+ // correct the state vector
+ for (uint8_t j= 0; j<=indexLimit; j++)
+ {
+ states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
+ }
+ // normalise the quaternion states
+ float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+ if (quatMag > 1e-12)
+ {
+ for (uint8_t j= 0; j<=3; j++)
+ {
+ float quatMagInv = 1.0f/quatMag;
+ states[j] = states[j] * quatMagInv;
+ }
+ }
+ // correct the covariance P = (I - K*H)*P
+ // take advantage of the empty columns in KH to reduce the
+ // number of operations
+ for (uint8_t i = 0; i<=indexLimit; i++)
+ {
+ for (uint8_t j = 0; j <= 3; j++)
+ {
+ KH[i][j] = Kfusion[i] * H_MAG[j];
+ }
+ for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f;
+ if (!onGround)
+ {
+ for (uint8_t j = 16; j <= 21; j++)
+ {
+ KH[i][j] = Kfusion[i] * H_MAG[j];
+ }
+ }
+ else
+ {
+ for (uint8_t j = 16; j <= 21; j++)
+ {
+ KH[i][j] = 0.0f;
+ }
+ }
+ }
+ for (uint8_t i = 0; i<=indexLimit; i++)
+ {
+ for (uint8_t j = 0; j<=indexLimit; j++)
+ {
+ KHP[i][j] = 0.0f;
+ for (uint8_t k = 0; k <= 3; k++)
+ {
+ KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+ }
+ if (!onGround)
+ {
+ for (uint8_t k = 16; k<=21; k++)
+ {
+ KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+ }
+ }
+ }
+ }
+ }
+ for (uint8_t i = 0; i <= indexLimit; i++)
+ {
+ for (uint8_t j = 0; j <= indexLimit; j++)
+ {
+ P[i][j] = P[i][j] - KHP[i][j];
+ }
+ }
+ }
+ obsIndex = obsIndex + 1;
+
+ ForceSymmetry();
+ ConstrainVariances();
+}
+
+void AttPosEKF::FuseAirspeed()
+{
+ float vn;
+ float ve;
+ float vd;
+ float vwn;
+ float vwe;
+ float R_TAS = sq(airspeedMeasurementSigma);
+ float SH_TAS[3];
+ float VtasPred;
+
+ // Copy required states to local variable names
+ vn = statesAtVtasMeasTime[4];
+ ve = statesAtVtasMeasTime[5];
+ vd = statesAtVtasMeasTime[6];
+ vwn = statesAtVtasMeasTime[14];
+ vwe = statesAtVtasMeasTime[15];
+
+ // Need to check that it is flying before fusing airspeed data
+ // Calculate the predicted airspeed
+ VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
+ // Perform fusion of True Airspeed measurement
+ if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
+ {
+ // Calculate observation jacobians
+ SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
+ SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
+ SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
+
+ float H_TAS[n_states];
+ for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f;
+ H_TAS[4] = SH_TAS[2];
+ H_TAS[5] = SH_TAS[1];
+ H_TAS[6] = vd*SH_TAS[0];
+ H_TAS[14] = -SH_TAS[2];
+ H_TAS[15] = -SH_TAS[1];
+
+ // 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]));
+ 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]);
+ Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][14]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][15]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]);
+ Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][14]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][15]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]);
+ Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][14]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][15]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]);
+ Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][14]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][15]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]);
+ Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][14]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][15]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]);
+ Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][14]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][15]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]);
+ Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][14]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][15]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]);
+ Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][14]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][15]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]);
+ Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][14]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][15]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]);
+ Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]);
+ // 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_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]);
+ Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]);
+ Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]);
+ Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]);
+ Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]);
+ Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]);
+ Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]);
+ Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
+ Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]);
+ Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]);
+ varInnovVtas = 1.0f/SK_TAS;
+
+ // Calculate the measurement innovation
+ innovVtas = VtasPred - VtasMeas;
+ // Check the innovation for consistency and don't fuse if > 5Sigma
+ if ((innovVtas*innovVtas*SK_TAS) < 25.0)
+ {
+ // correct the state vector
+ for (uint8_t j=0; j <= 22; j++)
+ {
+ states[j] = states[j] - Kfusion[j] * innovVtas;
+ }
+ // normalise the quaternion states
+ float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+ if (quatMag > 1e-12f)
+ {
+ for (uint8_t j= 0; j <= 3; j++)
+ {
+ float quatMagInv = 1.0f/quatMag;
+ states[j] = states[j] * quatMagInv;
+ }
+ }
+ // correct the covariance P = (I - K*H)*P
+ // take advantage of the empty columns in H to reduce the
+ // number of operations
+ for (uint8_t i = 0; i <= 22; i++)
+ {
+ for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0;
+ for (uint8_t j = 4; j <= 6; j++)
+ {
+ KH[i][j] = Kfusion[i] * H_TAS[j];
+ }
+ for (uint8_t j = 7; j <= 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 = 16; j <= 22; j++) KH[i][j] = 0.0;
+ }
+ for (uint8_t i = 0; i <= 22; i++)
+ {
+ for (uint8_t j = 0; j <= 22; j++)
+ {
+ KHP[i][j] = 0.0;
+ for (uint8_t k = 4; k <= 6; k++)
+ {
+ KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+ }
+ for (uint8_t k = 14; k <= 15; k++)
+ {
+ KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+ }
+ }
+ }
+ for (uint8_t i = 0; i <= 22; i++)
+ {
+ for (uint8_t j = 0; j <= 22; j++)
+ {
+ P[i][j] = P[i][j] - KHP[i][j];
+ }
+ }
+ }
+ }
+
+ ForceSymmetry();
+ ConstrainVariances();
+}
+
+void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
+{
+ uint8_t row;
+ uint8_t col;
+ for (row=first; row<=last; row++)
+ {
+ for (col=0; col<n_states; col++)
+ {
+ covMat[row][col] = 0.0;
+ }
+ }
+}
+
+void AttPosEKF::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::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
+{
+ uint8_t row;
+ uint8_t col;
+ for (col=first; col<=last; col++)
+ {
+ for (row=0; row < n_states; row++)
+ {
+ covMat[row][col] = 0.0;
+ }
+ }
+}
+
+float AttPosEKF::sq(float valIn)
+{
+ return valIn*valIn;
+}
+
+// Store states in a history array along with time stamp
+void AttPosEKF::StoreStates(uint64_t timestamp_ms)
+{
+ for (unsigned i=0; i<n_states; i++)
+ storedStates[i][storeIndex] = states[i];
+ statetimeStamp[storeIndex] = timestamp_ms;
+ storeIndex++;
+ if (storeIndex == data_buffer_size)
+ storeIndex = 0;
+}
+
+void AttPosEKF::ResetStoredStates()
+{
+ // reset all stored states
+ memset(&storedStates[0][0], 0, sizeof(storedStates));
+ memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
+
+ // reset store index to first
+ storeIndex = 0;
+
+ // overwrite all existing states
+ for (unsigned i = 0; i < n_states; i++) {
+ storedStates[i][storeIndex] = states[i];
+ }
+
+ statetimeStamp[storeIndex] = millis();
+
+ // increment to next storage index
+ storeIndex++;
+}
+
+// Output the state vector stored at the time that best matches that specified by msec
+int AttPosEKF::RecallStates(float* statesForFusion, 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;
+
+ 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;
+}
+
+void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
+{
+ // Calculate the nav to body cosine matrix
+ float q00 = sq(quat[0]);
+ float q11 = sq(quat[1]);
+ float q22 = sq(quat[2]);
+ float q33 = sq(quat[3]);
+ float q01 = quat[0]*quat[1];
+ float q02 = quat[0]*quat[2];
+ float q03 = quat[0]*quat[3];
+ float q12 = quat[1]*quat[2];
+ float q13 = quat[1]*quat[3];
+ float q23 = quat[2]*quat[3];
+
+ Tnb.x.x = q00 + q11 - q22 - q33;
+ Tnb.y.y = q00 - q11 + q22 - q33;
+ Tnb.z.z = q00 - q11 - q22 + q33;
+ Tnb.y.x = 2*(q12 - q03);
+ Tnb.z.x = 2*(q13 + q02);
+ Tnb.x.y = 2*(q12 + q03);
+ Tnb.z.y = 2*(q23 - q01);
+ Tnb.x.z = 2*(q13 - q02);
+ Tnb.y.z = 2*(q23 + q01);
+}
+
+void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
+{
+ // Calculate the body to nav cosine matrix
+ float q00 = sq(quat[0]);
+ float q11 = sq(quat[1]);
+ float q22 = sq(quat[2]);
+ float q33 = sq(quat[3]);
+ float q01 = quat[0]*quat[1];
+ float q02 = quat[0]*quat[2];
+ float q03 = quat[0]*quat[3];
+ float q12 = quat[1]*quat[2];
+ float q13 = quat[1]*quat[3];
+ float q23 = quat[2]*quat[3];
+
+ Tbn.x.x = q00 + q11 - q22 - q33;
+ Tbn.y.y = q00 - q11 + q22 - q33;
+ Tbn.z.z = q00 - q11 - q22 + q33;
+ Tbn.x.y = 2*(q12 - q03);
+ Tbn.x.z = 2*(q13 + q02);
+ Tbn.y.x = 2*(q12 + q03);
+ Tbn.y.z = 2*(q23 - q01);
+ Tbn.z.x = 2*(q13 - q02);
+ Tbn.z.y = 2*(q23 + q01);
+}
+
+void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
+{
+ float u1 = cos(0.5f*eul[0]);
+ float u2 = cos(0.5f*eul[1]);
+ float u3 = cos(0.5f*eul[2]);
+ float u4 = sin(0.5f*eul[0]);
+ float u5 = sin(0.5f*eul[1]);
+ float u6 = sin(0.5f*eul[2]);
+ quat[0] = u1*u2*u3+u4*u5*u6;
+ quat[1] = u4*u2*u3-u1*u5*u6;
+ quat[2] = u1*u5*u3+u4*u2*u6;
+ quat[3] = u1*u2*u6-u4*u5*u3;
+}
+
+void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4])
+{
+ y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
+ y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2]));
+ y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
+}
+
+void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
+{
+ velNED[0] = gpsGndSpd*cosf(gpsCourse);
+ velNED[1] = gpsGndSpd*sinf(gpsCourse);
+ velNED[2] = gpsVelD;
+}
+
+void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef)
+{
+ posNED[0] = earthRadius * (lat - latRef);
+ posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
+ posNED[2] = -(hgt - hgtRef);
+}
+
+void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
+{
+ lat = latRef + posNED[0] * earthRadiusInv;
+ lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
+ hgt = hgtRef - posNED[2];
+}
+
+void AttPosEKF::OnGroundCheck()
+{
+ onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f));
+ if (staticMode) {
+ staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
+ }
+}
+
+void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
+{
+ //Define Earth rotation vector in the NED navigation frame
+ omega.x = earthRate*cosf(latitude);
+ omega.y = 0.0f;
+ omega.z = -earthRate*sinf(latitude);
+}
+
+void AttPosEKF::CovarianceInit()
+{
+ // Calculate the initial covariance matrix P
+ P[0][0] = 0.25f * sq(1.0f*deg2rad);
+ P[1][1] = 0.25f * sq(1.0f*deg2rad);
+ P[2][2] = 0.25f * sq(1.0f*deg2rad);
+ P[3][3] = 0.25f * sq(10.0f*deg2rad);
+ P[4][4] = sq(0.7f);
+ P[5][5] = P[4][4];
+ P[6][6] = sq(0.7f);
+ P[7][7] = sq(15.0f);
+ P[8][8] = P[7][7];
+ 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(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)
+{
+ return (val > max) ? max : ((val < min) ? min : val);
+}
+
+void AttPosEKF::ConstrainVariances()
+{
+ if (!numericalProtection) {
+ return;
+ }
+
+ // State vector:
+ // 0-3: quaternions (q0, q1, q2, q3)
+ // 4-6: Velocity - m/sec (North, East, Down)
+ // 7-9: Position - m (North, East, Down)
+ // 10-12: Delta Angle bias - rad (X,Y,Z)
+ // 13: 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 <= 3; i++) {
+ P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+ }
+
+ // 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 <= 9; i++) {
+ P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f);
+ }
+
+ // 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 = 14; i <= 15; i++) {
+ P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
+ }
+
+ // Earth magnetic field variances
+ 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 = 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()
+{
+ if (!numericalProtection) {
+ return;
+ }
+
+ // State vector:
+ // 0-3: quaternions (q0, q1, q2, q3)
+ // 4-6: Velocity - m/sec (North, East, Down)
+ // 7-9: Position - m (North, East, Down)
+ // 10-12: Delta Angle bias - rad (X,Y,Z)
+ // 13: 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 <= 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 <= 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 <= 8; i++) {
+ states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f);
+ }
+
+ // Constrain altitude
+ states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f);
+
+ // Angle bias limit - set to 8 degrees / sec
+ for (unsigned i = 10; i <= 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 = 14; i <= 15; i++) {
+ states[i] = ConstrainFloat(states[i], -120.0f, 120.0f);
+ }
+
+ // Earth magnetic field limits (in Gauss)
+ 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 = 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()
+{
+ if (!numericalProtection) {
+ return;
+ }
+
+ // Force symmetry on the covariance matrix to prevent ill-conditioning
+ // of the matrix which would cause the filter to blow-up
+ for (unsigned i = 1; i < n_states; i++)
+ {
+ for (uint8_t j = 0; j < i; j++)
+ {
+ P[i][j] = 0.5f * (P[i][j] + P[j][i]);
+ P[j][i] = P[i][j];
+ }
+ }
+}
+
+bool AttPosEKF::FilterHealthy()
+{
+ if (!statesInitialised) {
+ return false;
+ }
+
+ // XXX Check state vector for NaNs and ill-conditioning
+
+ // Check if any of the major inputs timed out
+ if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) {
+ return false;
+ }
+
+ // Nothing fired, return ok.
+ return true;
+}
+
+/**
+ * Reset the filter position states
+ *
+ * This resets the position to the last GPS measurement
+ * or to zero in case of static position.
+ */
+void AttPosEKF::ResetPosition(void)
+{
+ if (staticMode) {
+ states[7] = 0;
+ states[8] = 0;
+ } else if (GPSstatus >= GPS_FIX_3D) {
+
+ // reset the states from the GPS measurements
+ states[7] = posNE[0];
+ states[8] = posNE[1];
+ }
+}
+
+/**
+ * Reset the height state.
+ *
+ * This resets the height state with the last altitude measurements
+ */
+void AttPosEKF::ResetHeight(void)
+{
+ // write to the state vector
+ states[9] = -hgtMea;
+}
+
+/**
+ * Reset the velocity state.
+ */
+void AttPosEKF::ResetVelocity(void)
+{
+ if (staticMode) {
+ states[4] = 0.0f;
+ states[5] = 0.0f;
+ states[6] = 0.0f;
+ } else if (GPSstatus >= GPS_FIX_3D) {
+
+ states[4] = velNED[0]; // north velocity from last reading
+ states[5] = velNED[1]; // east velocity from last reading
+ states[6] = velNED[2]; // down velocity from last reading
+ }
+}
+
+
+void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
+{
+ for (unsigned i = 0; i < n_states; i++)
+ {
+ err->states[i] = states[i];
+ }
+
+ err->velHealth = current_ekf_state.velHealth;
+ err->posHealth = current_ekf_state.posHealth;
+ err->hgtHealth = current_ekf_state.hgtHealth;
+ err->velTimeout = current_ekf_state.velTimeout;
+ err->posTimeout = current_ekf_state.posTimeout;
+ err->hgtTimeout = current_ekf_state.hgtTimeout;
+}
+
+bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
+ bool 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;
+ 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
+ for (unsigned i = 0; i < n_states; i++) {
+ for (unsigned j = 0; j < n_states; j++) {
+ if (!isfinite(KH[i][j])) {
+
+ err_report->covarianceNaN = true;
+ err = true;
+ ekf_debug("KH NaN");
+ goto out;
+ } // intermediate result used for covariance updates
+
+ if (!isfinite(KHP[i][j])) {
+
+ err_report->covarianceNaN = true;
+ err = true;
+ ekf_debug("KHP NaN");
+ goto out;
+ } // intermediate result used for covariance updates
+
+ if (!isfinite(P[i][j])) {
+
+ err_report->covarianceNaN = true;
+ err = true;
+ ekf_debug("P NaN");
+ } // covariance matrix
+ }
+
+ if (!isfinite(Kfusion[i])) {
+
+ err_report->kalmanGainsNaN = true;
+ ekf_debug("Kfusion NaN");
+ err = true;
+ goto out;
+ } // Kalman gains
+
+ if (!isfinite(states[i])) {
+
+ 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);
+ }
+
+ return err;
+
+}
+
+/**
+ * Check the filter inputs and bound its operational state
+ *
+ * This check will reset the filter states if required
+ * due to a failure of consistency or timeout checks.
+ * it should be run after the measurement data has been
+ * updated, but before any of the fusion steps are
+ * executed.
+ */
+int AttPosEKF::CheckAndBound()
+{
+
+ // Store the old filter state
+ bool currStaticMode = staticMode;
+
+ // Reset the filter if the states went NaN
+ if (StatesNaN(&last_ekf_error)) {
+ ekf_debug("re-initializing dynamic");
+
+ InitializeDynamic(velNED);
+
+ return 1;
+ }
+
+ // Reset the filter if the IMU data is too old
+ if (dtIMU > 0.2f) {
+
+ ResetVelocity();
+ ResetPosition();
+ ResetHeight();
+ ResetStoredStates();
+
+ // that's all we can do here, return
+ return 2;
+ }
+
+ // Check if we're on ground - this also sets static mode.
+ OnGroundCheck();
+
+ // Check if we switched between states
+ if (currStaticMode != staticMode) {
+ ResetVelocity();
+ ResetPosition();
+ ResetHeight();
+ ResetStoredStates();
+
+ return 3;
+ }
+
+ return 0;
+}
+
+void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
+{
+ float initialRoll, initialPitch;
+ float cosRoll, sinRoll, cosPitch, sinPitch;
+ float magX, magY;
+ float initialHdg, cosHeading, sinHeading;
+
+ initialRoll = atan2(-ay, -az);
+ initialPitch = atan2(ax, -az);
+
+ cosRoll = cosf(initialRoll);
+ sinRoll = sinf(initialRoll);
+ cosPitch = cosf(initialPitch);
+ sinPitch = sinf(initialPitch);
+
+ magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
+
+ magY = my * cosRoll - mz * sinRoll;
+
+ initialHdg = atan2f(-magY, magX);
+
+ cosRoll = cosf(initialRoll * 0.5f);
+ sinRoll = sinf(initialRoll * 0.5f);
+
+ cosPitch = cosf(initialPitch * 0.5f);
+ sinPitch = sinf(initialPitch * 0.5f);
+
+ cosHeading = cosf(initialHdg * 0.5f);
+ sinHeading = sinf(initialHdg * 0.5f);
+
+ initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
+ initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
+ initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
+ initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
+}
+
+void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
+{
+
+ // Clear the init flag
+ statesInitialised = false;
+
+ ZeroVariables();
+
+ // Calculate initial filter quaternion states from raw measurements
+ float initQuat[4];
+ Vector3f initMagXYZ;
+ initMagXYZ = magData - magBias;
+ AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, initQuat);
+
+ // Calculate initial Tbn matrix and rotate Mag measurements into NED
+ // to set initial NED magnetic field states
+ Mat3f DCM;
+ quat2Tbn(DCM, initQuat);
+ Vector3f initMagNED;
+ initMagXYZ = magData - magBias;
+ initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z;
+ initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
+ initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
+
+ 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 = DCM;
+
+ // write to state vector
+ for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
+ for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities
+ for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel
+ 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
+
+ ResetVelocity();
+ ResetPosition();
+ ResetHeight();
+
+ statesInitialised = true;
+
+ // initialise the covariance matrix
+ CovarianceInit();
+
+ //Define Earth rotation vector in the NED navigation frame
+ calcEarthRateNED(earthRateNED, latRef);
+
+ //Initialise summed variables used by covariance prediction
+ summedDelAng.x = 0.0f;
+ summedDelAng.y = 0.0f;
+ summedDelAng.z = 0.0f;
+ summedDelVel.x = 0.0f;
+ summedDelVel.y = 0.0f;
+ summedDelVel.z = 0.0f;
+}
+
+void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt)
+{
+ //store initial lat,long and height
+ latRef = referenceLat;
+ lonRef = referenceLon;
+ hgtRef = referenceHgt;
+ refSet = true;
+
+ memset(&last_ekf_error, 0, sizeof(last_ekf_error));
+
+ InitializeDynamic(initvelNED);
+}
+
+void AttPosEKF::ZeroVariables()
+{
+ // Do the data structure init
+ for (unsigned i = 0; i < n_states; i++) {
+ for (unsigned j = 0; j < n_states; j++) {
+ KH[i][j] = 0.0f; // intermediate result used for covariance updates
+ KHP[i][j] = 0.0f; // intermediate result used for covariance updates
+ P[i][j] = 0.0f; // covariance matrix
+ }
+
+ Kfusion[i] = 0.0f; // Kalman gains
+ states[i] = 0.0f; // state matrix
+ }
+
+ correctedDelAng.zero();
+ summedDelAng.zero();
+ summedDelVel.zero();
+
+ for (unsigned i = 0; i < data_buffer_size; i++) {
+
+ for (unsigned j = 0; j < n_states; j++) {
+ storedStates[j][i] = 0.0f;
+ }
+
+ statetimeStamp[i] = 0;
+ }
+
+ memset(&magstate, 0, sizeof(magstate));
+ magstate.q0 = 1.0f;
+ magstate.DCM.identity();
+
+ memset(&current_ekf_state, 0, sizeof(current_ekf_state));
+
+}
+
+void AttPosEKF::GetFilterState(struct ekf_status_report *state)
+{
+ memcpy(state, &current_ekf_state, sizeof(*state));
+}
+
+void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
+{
+ memcpy(last_error, &last_ekf_error, sizeof(*last_error));
+}
diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h
new file mode 100644
index 000000000..e118ae573
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/estimator.h
@@ -0,0 +1,350 @@
+#include <math.h>
+#include <stdint.h>
+
+#pragma once
+
+#define GRAVITY_MSS 9.80665f
+#define deg2rad 0.017453292f
+#define rad2deg 57.295780f
+#define pi 3.141592657f
+#define earthRate 0.000072921f
+#define earthRadius 6378145.0f
+#define earthRadiusInv 1.5678540e-7f
+
+class Vector3f
+{
+private:
+public:
+ float x;
+ float y;
+ float z;
+
+ float length(void) const;
+ void zero(void);
+};
+
+class Mat3f
+{
+private:
+public:
+ Vector3f x;
+ Vector3f y;
+ Vector3f z;
+
+ Mat3f();
+
+ void identity();
+ Mat3f transpose(void) const;
+};
+
+Vector3f operator*(float sclIn1, Vector3f vecIn1);
+Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator*( Mat3f matIn, Vector3f vecIn);
+Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator*(Vector3f vecIn1, float sclIn1);
+
+void swap_var(float &d1, float &d2);
+
+const unsigned int n_states = 23;
+const unsigned int data_buffer_size = 50;
+
+enum GPS_FIX {
+ GPS_FIX_NOFIX = 0,
+ GPS_FIX_2D = 2,
+ GPS_FIX_3D = 3
+};
+
+struct ekf_status_report {
+ bool velHealth;
+ bool posHealth;
+ bool hgtHealth;
+ bool velTimeout;
+ bool posTimeout;
+ bool hgtTimeout;
+ uint32_t velFailTime;
+ uint32_t posFailTime;
+ uint32_t hgtFailTime;
+ float states[n_states];
+ bool statesNaN;
+ bool covarianceNaN;
+ bool kalmanGainsNaN;
+};
+
+class AttPosEKF {
+
+public:
+
+ AttPosEKF();
+ ~AttPosEKF();
+
+
+
+ /* ##############################################
+ *
+ * M A I N F I L T E R P A R A M E T E R S
+ *
+ * ########################################### */
+
+ /*
+ * parameters are defined here and initialised in
+ * the InitialiseParameters() (which is just 20 lines down)
+ */
+
+ float covTimeStepMax; // maximum time allowed between covariance predictions
+ float covDelAngMax; // maximum delta angle between covariance predictions
+ float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+
+ float yawVarScale;
+ float windVelSigma;
+ float dAngBiasSigma;
+ float dVelBiasSigma;
+ float magEarthSigma;
+ float magBodySigma;
+ float gndHgtSigma;
+
+ float vneSigma;
+ float vdSigma;
+ float posNeSigma;
+ float posDSigma;
+ float magMeasurementSigma;
+ float airspeedMeasurementSigma;
+
+ float gyroProcessNoise;
+ float accelProcessNoise;
+
+ float EAS2TAS; // ratio f true to equivalent airspeed
+
+ void InitialiseParameters()
+ {
+ covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
+ covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
+ rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+ EAS2TAS = 1.0f;
+
+ yawVarScale = 1.0f;
+ windVelSigma = 0.1f;
+ dAngBiasSigma = 5.0e-7f;
+ dVelBiasSigma = 1e-4f;
+ magEarthSigma = 3.0e-4f;
+ magBodySigma = 3.0e-4f;
+ gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma
+
+ vneSigma = 0.2f;
+ vdSigma = 0.3f;
+ posNeSigma = 2.0f;
+ posDSigma = 2.0f;
+
+ magMeasurementSigma = 0.05;
+ airspeedMeasurementSigma = 1.4f;
+ gyroProcessNoise = 1.4544411e-2f;
+ accelProcessNoise = 0.5f;
+ }
+
+ 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
+ float KH[n_states][n_states]; // intermediate result used for covariance updates
+ float KHP[n_states][n_states]; // intermediate result used for covariance updates
+ float P[n_states][n_states]; // covariance matrix
+ float Kfusion[n_states]; // Kalman gains
+ float states[n_states]; // state matrix
+ float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
+ uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
+
+ float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
+ float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
+ float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
+ float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
+ float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
+ float statesAtRngTime[n_states]; // filter states at the effective measurement time
+
+ Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
+ Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
+ Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
+ Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
+ float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
+ Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
+ Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
+
+ 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;
+ float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
+ uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
+ float innovVelPos[6]; // innovation output
+ float varInnovVelPos[6]; // innovation variance output
+
+ float velNED[3]; // North, East, Down velocity obs (m/s)
+ float posNE[2]; // North, East position obs (m)
+ float hgtMea; // measured height (m)
+ float rngMea; // Ground distance
+ float posNED[3]; // North, East Down position (m)
+
+ float innovMag[3]; // innovation output
+ float varInnovMag[3]; // innovation variance output
+ Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
+ float innovVtas; // innovation output
+ float innovRng; ///< Range finder innovation
+ float varInnovVtas; // innovation variance output
+ float VtasMeas; // true airspeed measurement (m/s)
+ 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
+
+ // GPS input data variables
+ float gpsCourse;
+ float gpsVelD;
+ double gpsLat;
+ double gpsLon;
+ float gpsHgt;
+ uint8_t GPSstatus;
+
+ // Baro input
+ float baroHgt;
+
+ bool statesInitialised;
+
+ bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
+ bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
+ bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
+ bool fuseMagData; // boolean true when magnetometer data is to be fused
+ bool fuseVtasData; // boolean true when airspeed data is to be fused
+ bool fuseRngData; ///< true when range 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
+
+ struct ekf_status_report current_ekf_state;
+ struct ekf_status_report last_ekf_error;
+
+ bool numericalProtection;
+
+ unsigned storeIndex;
+
+
+void UpdateStrapdownEquationsNED();
+
+void CovariancePrediction(float dt);
+
+void FuseVelposNED();
+
+void FuseMagnetometer();
+
+void FuseAirspeed();
+
+void 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);
+
+void quatNorm(float (&quatOut)[4], const float quatIn[4]);
+
+// store staes along with system time stamp in msces
+void StoreStates(uint64_t timestamp_ms);
+
+/**
+ * Recall the state vector.
+ *
+ * Recalls the vector stored at closest time to the one specified by msec
+ *
+ * @return zero on success, integer indicating the number of invalid states on failure.
+ * Does only copy valid states, if the statesForFusion vector was initialized
+ * correctly by the caller, the result can be safely used, but is a mixture
+ * time-wise where valid states were updated and invalid remained at the old
+ * value.
+ */
+int RecallStates(float *statesForFusion, uint64_t msec);
+
+void ResetStoredStates();
+
+void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
+
+void calcEarthRateNED(Vector3f &omega, float latitude);
+
+static void eul2quat(float (&quat)[4], const float (&eul)[3]);
+
+static void quat2eul(float (&eul)[3], const float (&quat)[4]);
+
+static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
+
+static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
+
+static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+
+static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
+
+static float sq(float valIn);
+
+void OnGroundCheck();
+
+void CovarianceInit();
+
+void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt);
+
+float ConstrainFloat(float val, float min, float max);
+
+void ConstrainVariances();
+
+void ConstrainStates();
+
+void ForceSymmetry();
+
+int CheckAndBound();
+
+void ResetPosition();
+
+void ResetVelocity();
+
+void ZeroVariables();
+
+void GetFilterState(struct ekf_status_report *state);
+
+void GetLastErrorState(struct ekf_status_report *last_error);
+
+bool StatesNaN(struct ekf_status_report *err_report);
+void FillErrorReport(struct ekf_status_report *err);
+
+void InitializeDynamic(float (&initvelNED)[3]);
+
+protected:
+
+bool FilterHealthy();
+
+void ResetHeight(void);
+
+void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat);
+
+};
+
+uint32_t millis();
+
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
new file mode 100644
index 000000000..8031a311e
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -0,0 +1,1393 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fw_att_pos_estimator_main.cpp
+ * Implementation of the attitude and position estimator.
+ *
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <drivers/drv_hrt.h>
+
+#define SENSOR_COMBINED_SUB
+
+
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
+#ifdef SENSOR_COMBINED_SUB
+#include <uORB/topics/sensor_combined.h>
+#endif
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/home_position.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+#include <mavlink/mavlink_log.h>
+
+#include "estimator.h"
+
+
+
+/**
+ * estimator app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
+
+__EXPORT uint32_t millis();
+
+static uint64_t last_run = 0;
+static uint64_t IMUmsec = 0;
+
+uint32_t millis()
+{
+ return IMUmsec;
+}
+
+static void print_status();
+
+class FixedwingEstimator
+{
+public:
+ /**
+ * Constructor
+ */
+ FixedwingEstimator();
+
+ /**
+ * Destructor, also kills the sensors task.
+ */
+ ~FixedwingEstimator();
+
+ /**
+ * Start the sensors task.
+ *
+ * @return OK on success.
+ */
+ int start();
+
+ /**
+ * Print the current status.
+ */
+ void print_status();
+
+ /**
+ * Trip the filter by feeding it NaN values.
+ */
+ int trip_nan();
+
+private:
+
+ bool _task_should_exit; /**< if true, sensor task should exit */
+ int _estimator_task; /**< task handle for sensor task */
+#ifndef SENSOR_COMBINED_SUB
+ int _gyro_sub; /**< gyro sensor subscription */
+ int _accel_sub; /**< accel sensor subscription */
+ int _mag_sub; /**< mag sensor subscription */
+#else
+ int _sensor_combined_sub;
+#endif
+ int _airspeed_sub; /**< airspeed subscription */
+ int _baro_sub; /**< barometer subscription */
+ int _gps_sub; /**< GPS subscription */
+ int _vstatus_sub; /**< vehicle status subscription */
+ 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 */
+ orb_advert_t _local_pos_pub; /**< position in local frame */
+ orb_advert_t _estimator_status_pub; /**< status of the estimator */
+
+ struct vehicle_attitude_s _att; /**< vehicle attitude */
+ struct gyro_report _gyro;
+ struct accel_report _accel;
+ struct mag_report _mag;
+ struct airspeed_s _airspeed; /**< airspeed */
+ struct baro_report _baro; /**< baro readings */
+ struct vehicle_status_s _vstatus; /**< vehicle status */
+ struct vehicle_global_position_s _global_pos; /**< global vehicle position */
+ struct vehicle_local_position_s _local_pos; /**< local vehicle position */
+ struct vehicle_gps_position_s _gps; /**< GPS position */
+
+ struct gyro_scale _gyro_offsets;
+ struct accel_scale _accel_offsets;
+ struct mag_scale _mag_offsets;
+
+#ifdef SENSOR_COMBINED_SUB
+ struct sensor_combined_s _sensor_combined;
+#endif
+
+ struct map_projection_reference_s _pos_ref;
+
+ float _baro_ref; /**< barometer reference altitude */
+ float _baro_gps_offset; /**< offset between GPS and baro */
+
+ perf_counter_t _loop_perf; /**< loop performance counter */
+ perf_counter_t _perf_gyro; ///<local performance counter for gyro updates
+ perf_counter_t _perf_accel; ///<local performance counter for accel updates
+ perf_counter_t _perf_mag; ///<local performance counter for mag updates
+ perf_counter_t _perf_gps; ///<local performance counter for gps updates
+ perf_counter_t _perf_baro; ///<local performance counter for baro updates
+ perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates
+
+ bool _initialized;
+ bool _gps_initialized;
+ uint64_t _gps_start_time;
+
+ int _mavlink_fd;
+
+ struct {
+ int32_t vel_delay_ms;
+ int32_t pos_delay_ms;
+ 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;
+ float eas_noise;
+ } _parameters; /**< local copies of interesting parameters */
+
+ struct {
+ param_t vel_delay_ms;
+ param_t pos_delay_ms;
+ 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;
+ param_t eas_noise;
+ } _parameter_handles; /**< handles for interesting parameters */
+
+ AttPosEKF *_ekf;
+
+ /**
+ * Update our local parameter cache.
+ */
+ int parameters_update();
+
+ /**
+ * Update control outputs
+ *
+ */
+ void control_update();
+
+ /**
+ * Check for changes in vehicle status.
+ */
+ void vehicle_status_poll();
+
+ /**
+ * Shim for calling task_main from task_create.
+ */
+ static void task_main_trampoline(int argc, char *argv[]);
+
+ /**
+ * Main sensor collection task.
+ */
+ void task_main() __attribute__((noreturn));
+};
+
+namespace estimator
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+FixedwingEstimator *g_estimator;
+}
+
+FixedwingEstimator::FixedwingEstimator() :
+
+ _task_should_exit(false),
+ _estimator_task(-1),
+
+/* subscriptions */
+#ifndef SENSOR_COMBINED_SUB
+ _gyro_sub(-1),
+ _accel_sub(-1),
+ _mag_sub(-1),
+#else
+ _sensor_combined_sub(-1),
+#endif
+ _airspeed_sub(-1),
+ _baro_sub(-1),
+ _gps_sub(-1),
+ _vstatus_sub(-1),
+ _params_sub(-1),
+ _manual_control_sub(-1),
+
+/* publications */
+ _att_pub(-1),
+ _global_pos_pub(-1),
+ _local_pos_pub(-1),
+ _estimator_status_pub(-1),
+
+ _baro_ref(0.0f),
+ _baro_gps_offset(0.0f),
+
+/* performance counters */
+ _loop_perf(perf_alloc(PC_COUNT, "fw_att_pos_estimator")),
+ _perf_gyro(perf_alloc(PC_COUNT, "fw_ekf_gyro_upd")),
+ _perf_accel(perf_alloc(PC_COUNT, "fw_ekf_accel_upd")),
+ _perf_mag(perf_alloc(PC_COUNT, "fw_ekf_mag_upd")),
+ _perf_gps(perf_alloc(PC_COUNT, "fw_ekf_gps_upd")),
+ _perf_baro(perf_alloc(PC_COUNT, "fw_ekf_baro_upd")),
+ _perf_airspeed(perf_alloc(PC_COUNT, "fw_ekf_aspd_upd")),
+
+/* states */
+ _initialized(false),
+ _gps_initialized(false),
+ _mavlink_fd(-1),
+ _ekf(nullptr)
+{
+
+ 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");
+ _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");
+ _parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
+
+ /* fetch initial parameter values */
+ parameters_update();
+
+ /* get offsets */
+
+ int fd, res;
+
+ fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+
+ if (fd > 0) {
+ res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets);
+ close(fd);
+ }
+
+ fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+ if (fd > 0) {
+ res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets);
+ close(fd);
+ }
+
+ fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+ if (fd > 0) {
+ res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets);
+ close(fd);
+ }
+}
+
+FixedwingEstimator::~FixedwingEstimator()
+{
+ if (_estimator_task != -1) {
+
+ /* task wakes up every 100ms or so at the longest */
+ _task_should_exit = true;
+
+ /* wait for a second for the task to quit at our request */
+ unsigned i = 0;
+
+ do {
+ /* wait 20ms */
+ usleep(20000);
+
+ /* if we have given up, kill it */
+ if (++i > 50) {
+ task_delete(_estimator_task);
+ break;
+ }
+ } while (_estimator_task != -1);
+ }
+
+ estimator::g_estimator = nullptr;
+}
+
+int
+FixedwingEstimator::parameters_update()
+{
+
+ param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms));
+ param_get(_parameter_handles.pos_delay_ms, &(_parameters.pos_delay_ms));
+ 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));
+ 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;
+}
+
+void
+FixedwingEstimator::vehicle_status_poll()
+{
+ bool vstatus_updated;
+
+ /* Check HIL state if vehicle status has changed */
+ orb_check(_vstatus_sub, &vstatus_updated);
+
+ if (vstatus_updated) {
+
+ orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
+ }
+}
+
+void
+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();
+
+ if (!_ekf) {
+ errx(1, "failed allocating EKF filter - out of RAM!");
+ }
+
+ /*
+ * do subscriptions
+ */
+ _baro_sub = orb_subscribe(ORB_ID(sensor_baro));
+ _airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+ _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);
+
+#ifndef SENSOR_COMBINED_SUB
+
+ _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
+ _accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+ _mag_sub = orb_subscribe(ORB_ID(sensor_mag));
+
+ /* rate limit gyro updates to 50 Hz */
+ /* XXX remove this!, BUT increase the data buffer size! */
+ orb_set_interval(_gyro_sub, 4);
+#else
+ _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+ /* XXX remove this!, BUT increase the data buffer size! */
+ orb_set_interval(_sensor_combined_sub, 4);
+#endif
+
+ /* 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;
+
+ /* wakeup source(s) */
+ struct pollfd fds[2];
+
+ /* Setup of loop */
+ fds[0].fd = _params_sub;
+ fds[0].events = POLLIN;
+#ifndef SENSOR_COMBINED_SUB
+ fds[1].fd = _gyro_sub;
+ fds[1].events = POLLIN;
+#else
+ fds[1].fd = _sensor_combined_sub;
+ fds[1].events = POLLIN;
+#endif
+
+ hrt_abstime start_time = hrt_absolute_time();
+
+ bool newDataGps = false;
+ bool newAdsData = false;
+ bool newDataMag = false;
+
+ while (!_task_should_exit) {
+
+ /* wait for up to 500ms for data */
+ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+ /* timed out - periodic check for _task_should_exit, etc. */
+ if (pret == 0)
+ continue;
+
+ /* this is undesirable but not much we can do - might want to flag unhappy status */
+ if (pret < 0) {
+ warn("poll error %d, %d", pret, errno);
+ continue;
+ }
+
+ perf_begin(_loop_perf);
+
+ /* only update parameters if they changed */
+ if (fds[0].revents & POLLIN) {
+ /* read from param to clear updated flag */
+ struct parameter_update_s update;
+ orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+
+ /* update parameters from storage */
+ parameters_update();
+ }
+
+ /* only run estimator if gyro updated */
+ if (fds[1].revents & POLLIN) {
+
+ /* check vehicle status for changes to publication state */
+ vehicle_status_poll();
+
+ bool accel_updated;
+ bool mag_updated;
+
+ perf_count(_perf_gyro);
+
+ /**
+ * 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);
+
+
+ orb_check(_accel_sub, &accel_updated);
+
+ if (accel_updated) {
+ perf_count(_perf_accel);
+ orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+ }
+
+ last_sensor_timestamp = _gyro.timestamp;
+ _ekf.IMUmsec = _gyro.timestamp / 1e3f;
+
+ float deltaT = (_gyro.timestamp - last_run) / 1e6f;
+ last_run = _gyro.timestamp;
+
+ /* guard against too large deltaT's */
+ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
+ deltaT = 0.01f;
+ }
+
+
+ // Always store data, independent of init status
+ /* fill in last data set */
+ _ekf->dtIMU = deltaT;
+
+ 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;
+ _ekf->accel.z = _accel.z;
+
+ _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
+ _ekf->lastAngRate = angRate;
+ _ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
+ _ekf->lastAccel = accel;
+
+
+#else
+ orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
+
+ static hrt_abstime last_accel = 0;
+ static hrt_abstime last_mag = 0;
+
+ if (last_accel != _sensor_combined.accelerometer_timestamp) {
+ accel_updated = true;
+ }
+
+ last_accel = _sensor_combined.accelerometer_timestamp;
+
+
+ // Copy gyro and accel
+ last_sensor_timestamp = _sensor_combined.timestamp;
+ IMUmsec = _sensor_combined.timestamp / 1e3f;
+
+ float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f;
+
+ /* guard against too large deltaT's */
+ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
+ deltaT = 0.01f;
+ }
+
+ last_run = _sensor_combined.timestamp;
+
+ // Always store data, independent of init status
+ /* fill in last data set */
+ _ekf->dtIMU = deltaT;
+
+ 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];
+ _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
+
+ _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
+ lastAngRate = _ekf->angRate;
+ _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
+ lastAccel = _ekf->accel;
+
+ if (last_mag != _sensor_combined.magnetometer_timestamp) {
+ mag_updated = true;
+ newDataMag = true;
+
+ } else {
+ newDataMag = false;
+ }
+
+ last_mag = _sensor_combined.magnetometer_timestamp;
+
+#endif
+
+ bool airspeed_updated;
+ orb_check(_airspeed_sub, &airspeed_updated);
+
+ if (airspeed_updated) {
+ orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+ perf_count(_perf_airspeed);
+
+ _ekf->VtasMeas = _airspeed.true_airspeed_m_s;
+ newAdsData = true;
+
+ } else {
+ newAdsData = false;
+ }
+
+ bool gps_updated;
+ orb_check(_gps_sub, &gps_updated);
+
+ if (gps_updated) {
+
+ uint64_t last_gps = _gps.timestamp_position;
+
+ orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
+ perf_count(_perf_gps);
+
+ if (_gps.fix_type < 3) {
+ gps_updated = false;
+ newDataGps = false;
+
+ } 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();
+ _ekf->ResetVelocity();
+ _ekf->ResetStoredStates();
+ }
+
+ /* fuse GPS updates */
+
+ //_gps.timestamp / 1e3;
+ _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;
+
+ // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
+
+ _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;
+
+ }
+
+ }
+
+ bool baro_updated;
+ orb_check(_baro_sub, &baro_updated);
+
+ if (baro_updated) {
+ orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+
+ _ekf->baroHgt = _baro.altitude - _baro_ref;
+
+ // Could use a blend of GPS and baro alt data if desired
+ _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
+ }
+
+#ifndef SENSOR_COMBINED_SUB
+ orb_check(_mag_sub, &mag_updated);
+#endif
+
+ if (mag_updated) {
+
+ perf_count(_perf_mag);
+
+#ifndef SENSOR_COMBINED_SUB
+ orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
+
+ // XXX we compensate the offsets upfront - should be close to zero.
+ // 0.001f
+ _ekf->magData.x = _mag.x;
+ _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
+
+ _ekf->magData.y = _mag.y;
+ _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+
+ _ekf->magData.z = _mag.z;
+ _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
+
+#else
+
+ // XXX we compensate the offsets upfront - should be close to zero.
+ // 0.001f
+ _ekf->magData.x = _sensor_combined.magnetometer_ga[0];
+ _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
+
+ _ekf->magData.y = _sensor_combined.magnetometer_ga[1];
+ _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+
+ _ekf->magData.z = _sensor_combined.magnetometer_ga[2];
+ _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
+
+#endif
+
+ newDataMag = true;
+
+ } else {
+ newDataMag = false;
+ }
+
+
+ /**
+ * CHECK IF THE INPUT DATA IS SANE
+ */
+ int check = _ekf->CheckAndBound();
+
+ const char* ekfname = "[ekf] ";
+
+ switch (check) {
+ case 0:
+ /* all ok */
+ break;
+ case 1:
+ {
+ const char* str = "NaN in states, resetting";
+ warnx("%s%s", ekfname, str);
+ mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
+ break;
+ }
+ case 2:
+ {
+ const char* str = "stale IMU data, resetting";
+ warnx("%s%s", ekfname, str);
+ mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
+ break;
+ }
+ case 3:
+ {
+ 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);
+ }
+ }
+
+ // XXX trap for testing
+ if (check == 1) {
+ errx(1, "NUMERIC ERROR IN FILTER");
+ }
+
+ // If non-zero, we got a filter reset
+ if (check) {
+
+ struct ekf_status_report ekf_report;
+
+ _ekf->GetLastErrorState(&ekf_report);
+
+ struct estimator_status_report rep;
+ memset(&rep, 0, sizeof(rep));
+ rep.timestamp = hrt_absolute_time();
+
+ rep.states_nan = ekf_report.statesNaN;
+ rep.covariance_nan = ekf_report.covarianceNaN;
+ rep.kalman_gain_nan = ekf_report.kalmanGainsNaN;
+
+ // Copy all states or at least all that we can fit
+ int i = 0;
+ unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0]));
+ unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
+ rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
+
+ while ((i < ekf_n_states) && (i < max_states)) {
+
+ rep.states[i] = ekf_report.states[i];
+ i++;
+ }
+
+ if (_estimator_status_pub > 0) {
+ orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep);
+ } else {
+ _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
+ }
+
+ }
+
+
+ /**
+ * 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) {
+
+ // bool home_set;
+ // orb_check(_home_sub, &home_set);
+ // struct home_position_s home;
+ // orb_copy(ORB_ID(home_position), _home_sub, &home);
+
+ if (!_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;
+
+ // GPS is in scaled integers, convert
+ double lat = _gps.lat / 1.0e7;
+ double lon = _gps.lon / 1.0e7;
+ float alt = _gps.alt / 1e3f;
+
+ _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt);
+
+ // Initialize projection
+ _local_pos.ref_lat = _gps.lat;
+ _local_pos.ref_lon = _gps.alt;
+ _local_pos.ref_alt = alt;
+ _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,
+ (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
+
+ _gps_initialized = true;
+
+ } else if (!_ekf->statesInitialised) {
+ _ekf->velNED[0] = 0.0f;
+ _ekf->velNED[1] = 0.0f;
+ _ekf->velNED[2] = 0.0f;
+ _ekf->posNED[0] = 0.0f;
+ _ekf->posNED[1] = 0.0f;
+ _ekf->posNED[2] = 0.0f;
+
+ _ekf->posNE[0] = _ekf->posNED[0];
+ _ekf->posNE[1] = _ekf->posNED[1];
+ _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f);
+ }
+ }
+
+ // If valid IMU data and states initialised, predict states and covariances
+ if (_ekf->statesInitialised) {
+ // Run the strapdown INS equations every IMU update
+ _ekf->UpdateStrapdownEquationsNED();
+#if 0
+ // debug code - could be tunred into a filter mnitoring/watchdog function
+ float tempQuat[4];
+
+ for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
+
+ quat2eul(eulerEst, tempQuat);
+
+ for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
+
+ if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
+
+ if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
+
+#endif
+ // store the predicted states for subsequent use by measurement fusion
+ _ekf->StoreStates(IMUmsec);
+ // Check if on ground - status is used by covariance prediction
+ _ekf->OnGroundCheck();
+ // sum delta angles and time used by covariance prediction
+ _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
+ _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
+ dt += _ekf->dtIMU;
+
+ // perform a covariance prediction if the total delta angle has exceeded the limit
+ // or the time limit will be exceeded at the next IMU update
+ if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
+ _ekf->CovariancePrediction(dt);
+ _ekf->summedDelAng.zero();
+ _ekf->summedDelVel.zero();
+ dt = 0.0f;
+ }
+
+ _initialized = true;
+ }
+
+ // Fuse GPS Measurements
+ if (newDataGps && _gps_initialized) {
+ // Convert GPS measurements to Pos NE, hgt and Vel NED
+ _ekf->velNED[0] = _gps.vel_n_m_s;
+ _ekf->velNED[1] = _gps.vel_e_m_s;
+ _ekf->velNED[2] = _gps.vel_d_m_s;
+ _ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
+
+ _ekf->posNE[0] = _ekf->posNED[0];
+ _ekf->posNE[1] = _ekf->posNED[1];
+ // set fusion flags
+ _ekf->fuseVelData = true;
+ _ekf->fusePosData = true;
+ // recall states stored at time of measurement after adjusting for delays
+ _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+ _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+ // run the fusion step
+ _ekf->FuseVelposNED();
+
+ } else if (_ekf->statesInitialised) {
+ // Convert GPS measurements to Pos NE, hgt and Vel NED
+ _ekf->velNED[0] = 0.0f;
+ _ekf->velNED[1] = 0.0f;
+ _ekf->velNED[2] = 0.0f;
+ _ekf->posNED[0] = 0.0f;
+ _ekf->posNED[1] = 0.0f;
+ _ekf->posNED[2] = 0.0f;
+
+ _ekf->posNE[0] = _ekf->posNED[0];
+ _ekf->posNE[1] = _ekf->posNED[1];
+ // set fusion flags
+ _ekf->fuseVelData = true;
+ _ekf->fusePosData = true;
+ // recall states stored at time of measurement after adjusting for delays
+ _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+ _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+ // run the fusion step
+ _ekf->FuseVelposNED();
+
+ } else {
+ _ekf->fuseVelData = false;
+ _ekf->fusePosData = false;
+ }
+
+ if (newAdsData && _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->fuseHgtData = true;
+ // recall states stored at time of measurement after adjusting for delays
+ _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
+ // run the fusion step
+ _ekf->FuseVelposNED();
+
+ } else {
+ _ekf->fuseHgtData = false;
+ }
+
+ // Fuse Magnetometer Measurements
+ if (newDataMag && _ekf->statesInitialised) {
+ _ekf->fuseMagData = true;
+ _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
+
+ } else {
+ _ekf->fuseMagData = false;
+ }
+
+ if (_ekf->statesInitialised) _ekf->FuseMagnetometer();
+
+ // Fuse Airspeed Measurements
+ if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
+ _ekf->fuseVtasData = true;
+ _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
+ _ekf->FuseAirspeed();
+
+ } else {
+ _ekf->fuseVtasData = false;
+ }
+
+ // Publish results
+ if (_initialized && (check == OK)) {
+
+
+
+ // State vector:
+ // 0-3: quaternions (q0, q1, q2, q3)
+ // 4-6: Velocity - m/sec (North, East, Down)
+ // 7-9: Position - m (North, East, Down)
+ // 10-12: Delta Angle bias - rad (X,Y,Z)
+ // 13-14: Wind Vector - m/sec (North,East)
+ // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
+ // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
+
+ math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
+ math::Matrix<3, 3> R = q.to_dcm();
+ math::Vector<3> euler = R.to_euler();
+
+ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+ _att.R[i][j] = R(i, j);
+
+ _att.timestamp = last_sensor_timestamp;
+ _att.q[0] = _ekf->states[0];
+ _att.q[1] = _ekf->states[1];
+ _att.q[2] = _ekf->states[2];
+ _att.q[3] = _ekf->states[3];
+ _att.q_valid = true;
+ _att.R_valid = true;
+
+ _att.timestamp = last_sensor_timestamp;
+ _att.roll = euler(0);
+ _att.pitch = euler(1);
+ _att.yaw = euler(2);
+
+ _att.rollspeed = _ekf->angRate.x - _ekf->states[10];
+ _att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
+ _att.yawspeed = _ekf->angRate.z - _ekf->states[12];
+ // gyro offsets
+ _att.rate_offsets[0] = _ekf->states[10];
+ _att.rate_offsets[1] = _ekf->states[11];
+ _att.rate_offsets[2] = _ekf->states[12];
+
+ /* lazily publish the attitude only once available */
+ if (_att_pub > 0) {
+ /* publish the attitude setpoint */
+ orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
+
+ } else {
+ /* advertise and publish */
+ _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
+ }
+ }
+
+ if (_gps_initialized) {
+ _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.vx = _ekf->states[4];
+ _local_pos.vy = _ekf->states[5];
+ _local_pos.vz = _ekf->states[6];
+
+ _local_pos.xy_valid = _gps_initialized;
+ _local_pos.z_valid = true;
+ _local_pos.v_xy_valid = _gps_initialized;
+ _local_pos.v_z_valid = true;
+ _local_pos.xy_global = true;
+
+ _local_pos.z_global = false;
+ _local_pos.yaw = _att.yaw;
+
+ /* lazily publish the local position only once available */
+ if (_local_pos_pub > 0) {
+ /* publish the attitude setpoint */
+ orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
+
+ } else {
+ /* advertise and publish */
+ _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
+ }
+
+ _global_pos.timestamp = _local_pos.timestamp;
+
+ if (_local_pos.xy_global) {
+ double est_lat, est_lon;
+ map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
+ _global_pos.lat = est_lat;
+ _global_pos.lon = est_lon;
+ _global_pos.time_gps_usec = _gps.time_gps_usec;
+ }
+
+ if (_local_pos.v_xy_valid) {
+ _global_pos.vel_n = _local_pos.vx;
+ _global_pos.vel_e = _local_pos.vy;
+ } else {
+ _global_pos.vel_n = 0.0f;
+ _global_pos.vel_e = 0.0f;
+ }
+
+ /* local pos alt is negative, change sign and add alt offset */
+ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z);
+
+ if (_local_pos.v_z_valid) {
+ _global_pos.vel_d = _local_pos.vz;
+ }
+
+ _global_pos.yaw = _local_pos.yaw;
+
+ _global_pos.eph = _gps.eph_m;
+ _global_pos.epv = _gps.epv_m;
+
+ _global_pos.timestamp = _local_pos.timestamp;
+
+ /* lazily publish the global position only once available */
+ if (_global_pos_pub > 0) {
+ /* publish the attitude setpoint */
+ orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
+
+ } else {
+ /* advertise and publish */
+ _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
+ }
+ }
+
+ }
+
+ perf_end(_loop_perf);
+ }
+
+ warnx("exiting.\n");
+
+ _estimator_task = -1;
+ _exit(0);
+}
+
+int
+FixedwingEstimator::start()
+{
+ ASSERT(_estimator_task == -1);
+
+ /* start the task */
+ _estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_MAX - 40,
+ 6000,
+ (main_t)&FixedwingEstimator::task_main_trampoline,
+ nullptr);
+
+ if (_estimator_task < 0) {
+ warn("task start failed");
+ return -errno;
+ }
+
+ return OK;
+}
+
+void
+FixedwingEstimator::print_status()
+{
+ math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
+ math::Matrix<3, 3> R = q.to_dcm();
+ math::Vector<3> euler = R.to_euler();
+
+ printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n",
+ (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));
+
+ // State vector:
+ // 0-3: quaternions (q0, q1, q2, q3)
+ // 4-6: Velocity - m/sec (North, East, Down)
+ // 7-9: Position - m (North, East, Down)
+ // 10-12: Delta Angle bias - rad (X,Y,Z)
+ // 13-14: Wind Vector - m/sec (North,East)
+ // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
+ // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
+
+ printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, 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]);
+ printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
+ printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
+ printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
+ printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
+ printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
+ printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
+ printf("states: %s %s %s %s %s %s %s %s %s %s\n",
+ (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
+ (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
+ (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
+ (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
+ (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
+ (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
+ (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
+ (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
+ (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
+ (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
+}
+
+int FixedwingEstimator::trip_nan() {
+
+ int ret = 0;
+
+ // If system is not armed, inject a NaN value into the filter
+ int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+ struct actuator_armed_s armed;
+ orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+
+ if (armed.armed) {
+ warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM");
+ ret = 1;
+ } else {
+
+ float nan_val = 0.0f / 0.0f;
+
+ warnx("system not armed, tripping state vector with NaN values");
+ _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 #2 with NaN values");
+ // 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
+ usleep(100000);
+
+ warnx("tripping Kalman gains with NaN values");
+ _ekf->Kfusion[0] = nan_val; // Kalman gains
+ usleep(100000);
+
+ warnx("tripping stored states[0] with NaN values");
+ _ekf->storedStates[0][0] = nan_val;
+ usleep(100000);
+
+ warnx("\nDONE - FILTER STATE:");
+ print_status();
+ }
+
+ close(armed_sub);
+ return ret;
+}
+
+int ekf_att_pos_estimator_main(int argc, char *argv[])
+{
+ if (argc < 1)
+ errx(1, "usage: ekf_att_pos_estimator {start|stop|status}");
+
+ if (!strcmp(argv[1], "start")) {
+
+ if (estimator::g_estimator != nullptr)
+ errx(1, "already running");
+
+ estimator::g_estimator = new FixedwingEstimator;
+
+ if (estimator::g_estimator == nullptr)
+ errx(1, "alloc failed");
+
+ if (OK != estimator::g_estimator->start()) {
+ delete estimator::g_estimator;
+ estimator::g_estimator = nullptr;
+ err(1, "start failed");
+ }
+
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "stop")) {
+ if (estimator::g_estimator == nullptr)
+ errx(1, "not running");
+
+ delete estimator::g_estimator;
+ estimator::g_estimator = nullptr;
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "status")) {
+ if (estimator::g_estimator) {
+ warnx("running");
+
+ estimator::g_estimator->print_status();
+
+ exit(0);
+
+ } else {
+ errx(1, "not running");
+ }
+ }
+
+ if (!strcmp(argv[1], "trip")) {
+ if (estimator::g_estimator) {
+ int ret = estimator::g_estimator->trip_nan();
+
+ exit(ret);
+
+ } else {
+ errx(1, "not running");
+ }
+ }
+
+ warnx("unrecognized command");
+ return 1;
+}
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
new file mode 100644
index 000000000..cfcd99858
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c
@@ -0,0 +1,260 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fw_att_pos_estimator_params.c
+ *
+ * Parameters defined by the attitude and position estimator task
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Estimator parameters, accessible via MAVLink
+ *
+ */
+
+/**
+ * Velocity estimate delay
+ *
+ * The delay in milliseconds of the velocity estimate from GPS.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230);
+
+/**
+ * Position estimate delay
+ *
+ * The delay in milliseconds of the position estimate from GPS.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210);
+
+/**
+ * Height estimate delay
+ *
+ * The delay in milliseconds of the height estimate from the barometer.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350);
+
+/**
+ * Mag estimate delay
+ *
+ * The delay in milliseconds of the magnetic field estimate from
+ * the magnetometer.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30);
+
+/**
+ * True airspeeed estimate delay
+ *
+ * The delay in milliseconds of the airspeed estimate.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
+
+/**
+ * GPS vs. barometric altitude update weight
+ *
+ * RE-CHECK this.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
+
+/**
+ * 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
+ *
+ * @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);
+
diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk
new file mode 100644
index 000000000..30955d0dd
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Main Attitude and Position Estimator for Fixed Wing Aircraft
+#
+
+MODULE_COMMAND = ekf_att_pos_estimator
+
+SRCS = fw_att_pos_estimator_main.cpp \
+ fw_att_pos_estimator_params.c \
+ estimator.cpp
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-15 23:20:31 +0000