diff --git a/flight/libraries/alarms.c b/flight/libraries/alarms.c index 7cc7f39aa..b9a977a8b 100644 --- a/flight/libraries/alarms.c +++ b/flight/libraries/alarms.c @@ -307,6 +307,7 @@ static const char *const systemalarms_alarm_names[] = { [SYSTEMALARMS_ALARM_MAGNETOMETER] = "MAG", [SYSTEMALARMS_ALARM_AIRSPEED] = "AIRSPD", [SYSTEMALARMS_ALARM_STABILIZATION] = "STAB", + [SYSTEMALARMS_ALARM_NAV] = "NAV", [SYSTEMALARMS_ALARM_GUIDANCE] = "GUIDANCE", [SYSTEMALARMS_ALARM_PATHPLAN] = "PLAN", [SYSTEMALARMS_ALARM_BATTERY] = "BATT", diff --git a/flight/libraries/inc/insgps.h b/flight/libraries/inc/insgps.h index c77d44510..fabf77b48 100644 --- a/flight/libraries/inc/insgps.h +++ b/flight/libraries/inc/insgps.h @@ -32,7 +32,8 @@ #ifndef INSGPS_H_ #define INSGPS_H_ -#include "stdint.h" +#include +#include /** * @addtogroup Constants @@ -54,22 +55,24 @@ // Exposed Function Prototypes void INSGPSInit(); -void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT); +void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT); void INSCovariancePrediction(float dT); -void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed); +void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3], + float BaroAlt, uint16_t SensorsUsed); +void INSResetP(const float PDiag[13]); +void INSGetVariance(float PDiag[13]); +void INSSetState(const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], const float accel_bias[3]); +void INSSetPosVelVar(const float PosVar[3], const float VelVar[3]); +void INSSetGyroBias(const float gyro_bias[3]); +void INSSetAccelVar(const float accel_var[3]); +void INSSetGyroVar(const float gyro_var[3]); +void INSSetGyroBiasVar(const float gyro_bias_var[3]); +void INSSetMagNorth(const float B[3]); +void INSSetMagVar(const float scaled_mag_var[3]); +void INSSetBaroVar(const float baro_var); +void INSSetArmed(bool armed); +void INSPosVelReset(const float pos[3], const float vel[3]); -void INSResetP(float PDiag[13]); -void INSGetP(float PDiag[13]); -void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]); -void INSSetPosVelVar(float PosVar[3], float VelVar[3]); -void INSSetGyroBias(float gyro_bias[3]); -void INSSetAccelVar(float accel_var[3]); -void INSSetGyroVar(float gyro_var[3]); -void INSSetGyroBiasVar(float gyro_bias_var[3]); -void INSSetMagNorth(float B[3]); -void INSSetMagVar(float scaled_mag_var[3]); -void INSSetBaroVar(float baro_var); -void INSPosVelReset(float pos[3], float vel[3]); void MagCorrection(float mag_data[3]); void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt); diff --git a/flight/libraries/insgps13state.c b/flight/libraries/insgps13state.c index c311f05d3..f6aa54bee 100644 --- a/flight/libraries/insgps13state.c +++ b/flight/libraries/insgps13state.c @@ -163,7 +163,22 @@ void INSGPSInit() // pretty much just a place holder for now ekf.R[9] = .25f; // High freq altimeter noise variance (m^2) } -void INSResetP(float PDiag[NUMX]) +// ! Set the current flight state +void INSSetArmed(bool armed) +{ + return; + + // Speed up convergence of accel and gyro bias when not armed + if (armed) { + ekf.Q[9] = 1e-4f; + ekf.Q[8] = 2e-9f; + } else { + ekf.Q[9] = 1e-2f; + ekf.Q[8] = 2e-8f; + } +} + +void INSResetP(const float PDiag[NUMX]) { uint8_t i, j; @@ -178,7 +193,7 @@ void INSResetP(float PDiag[NUMX]) } } -void INSGetP(float PDiag[NUMX]) +void INSGetVariance(float PDiag[NUMX]) { uint8_t i; @@ -189,8 +204,7 @@ void INSGetP(float PDiag[NUMX]) } } } - -void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], __attribute__((unused)) float accel_bias[3]) +void INSSetState(const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], __attribute__((unused)) const float accel_bias[3]) { /* Note: accel_bias not used in 13 state INS */ ekf.X[0] = pos[0]; @@ -208,7 +222,7 @@ void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], __a ekf.X[12] = gyro_bias[2]; } -void INSPosVelReset(float pos[3], float vel[3]) +void INSPosVelReset(const float pos[3], const float vel[3]) { for (int i = 0; i < 6; i++) { for (int j = i; j < NUMX; j++) { @@ -228,7 +242,7 @@ void INSPosVelReset(float pos[3], float vel[3]) ekf.X[5] = vel[2]; } -void INSSetPosVelVar(float PosVar[3], float VelVar[3]) +void INSSetPosVelVar(const float PosVar[3], const float VelVar[3]) { ekf.R[0] = PosVar[0]; ekf.R[1] = PosVar[1]; @@ -238,35 +252,35 @@ void INSSetPosVelVar(float PosVar[3], float VelVar[3]) ekf.R[5] = VelVar[2]; } -void INSSetGyroBias(float gyro_bias[3]) +void INSSetGyroBias(const float gyro_bias[3]) { ekf.X[10] = gyro_bias[0]; ekf.X[11] = gyro_bias[1]; ekf.X[12] = gyro_bias[2]; } -void INSSetAccelVar(float accel_var[3]) +void INSSetAccelVar(const float accel_var[3]) { ekf.Q[3] = accel_var[0]; ekf.Q[4] = accel_var[1]; ekf.Q[5] = accel_var[2]; } -void INSSetGyroVar(float gyro_var[3]) +void INSSetGyroVar(const float gyro_var[3]) { ekf.Q[0] = gyro_var[0]; ekf.Q[1] = gyro_var[1]; ekf.Q[2] = gyro_var[2]; } -void INSSetGyroBiasVar(float gyro_bias_var[3]) +void INSSetGyroBiasVar(const float gyro_bias_var[3]) { ekf.Q[6] = gyro_bias_var[0]; ekf.Q[7] = gyro_bias_var[1]; ekf.Q[8] = gyro_bias_var[2]; } -void INSSetMagVar(float scaled_mag_var[3]) +void INSSetMagVar(const float scaled_mag_var[3]) { ekf.R[6] = scaled_mag_var[0]; ekf.R[7] = scaled_mag_var[1]; @@ -278,16 +292,14 @@ void INSSetBaroVar(float baro_var) ekf.R[9] = baro_var; } -void INSSetMagNorth(float B[3]) +void INSSetMagNorth(const float B[3]) { - float invmag = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]); - - ekf.Be[0] = B[0] * invmag; - ekf.Be[1] = B[1] * invmag; - ekf.Be[2] = B[2] * invmag; + ekf.Be[0] = B[0]; + ekf.Be[1] = B[1]; + ekf.Be[2] = B[2]; } -void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT) +void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT) { float U[6]; float invqmag; @@ -371,8 +383,8 @@ void VelBaroCorrection(float Vel[3], float BaroAlt) HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR); } -void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], - float BaroAlt, uint16_t SensorsUsed) +void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3], + const float BaroAlt, uint16_t SensorsUsed) { float Z[10] = { 0 }; float Y[10] = { 0 }; @@ -390,10 +402,28 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], if (SensorsUsed & MAG_SENSORS) { - float invBmag = invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]); - Z[6] = mag_data[0] * invBmag; - Z[7] = mag_data[1] * invBmag; - Z[8] = mag_data[2] * invBmag; + // magnetometer data in any units (use unit vector) and in body frame + float Rbe_a[3][3]; + float q0 = ekf.X[6]; + float q1 = ekf.X[7]; + float q2 = ekf.X[8]; + float q3 = ekf.X[9]; + float k1 = 1.0f / sqrtf(powf(q0 * q1 * 2.0f + q2 * q3 * 2.0f, 2.0f) + powf(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3, 2.0f)); + float k2 = sqrtf(-powf(q0 * q2 * 2.0f - q1 * q3 * 2.0f, 2.0f) + 1.0f); + + Rbe_a[0][0] = k2; + Rbe_a[0][1] = 0.0f; + Rbe_a[0][2] = q0 * q2 * -2.0f + q1 * q3 * 2.0f; + Rbe_a[1][0] = k1 * (q0 * q1 * 2.0f + q2 * q3 * 2.0f) * (q0 * q2 * 2.0f - q1 * q3 * 2.0f); + Rbe_a[1][1] = k1 * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + Rbe_a[1][2] = k1 * sqrtf(-powf(q0 * q2 * 2.0f - q1 * q3 * 2.0f, 2.0f) + 1.0f) * (q0 * q1 * 2.0f + q2 * q3 * 2.0f); + Rbe_a[2][0] = k1 * (q0 * q2 * 2.0f - q1 * q3 * 2.0f) * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + Rbe_a[2][1] = -k1 * (q0 * q1 * 2.0f + q2 * q3 * 2.0f); + Rbe_a[2][2] = k1 * k2 * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + + Z[6] = Rbe_a[0][0] * mag_data[0] + Rbe_a[1][0] * mag_data[1] + Rbe_a[2][0] * mag_data[2]; + Z[7] = Rbe_a[0][1] * mag_data[0] + Rbe_a[1][1] * mag_data[1] + Rbe_a[2][1] * mag_data[2]; + Z[8] = Rbe_a[0][2] * mag_data[0] + Rbe_a[1][2] * mag_data[1] + Rbe_a[2][2] * mag_data[2]; } // barometric altimeter in meters and in local NED frame diff --git a/flight/libraries/insgps14state.c b/flight/libraries/insgps14state.c new file mode 100644 index 000000000..79fe79a97 --- /dev/null +++ b/flight/libraries/insgps14state.c @@ -0,0 +1,985 @@ +/** + ****************************************************************************** + * @addtogroup Math + * @{ + * @addtogroup INSGPS + * @{ + * @brief INSGPS is a joint attitude and position estimation EKF + * + * @file insgps.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Tau Labs, http://github.com/TauLabs Copyright (C) 2012-2013. + * The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * @brief An INS/GPS algorithm implemented with an EKF. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "insgps.h" +#include +#include +#include +#include +#include +#include + +// constants/macros/typdefs +#define NUMX 14 // number of states, X is the state vector +#define NUMW 10 // number of plant noise inputs, w is disturbance noise vector +#define NUMV 10 // number of measurements, v is the measurement noise vector +#define NUMU 6 // number of deterministic inputs, U is the input vector + +#if defined(GENERAL_COV) +// This might trick people so I have a note here. There is a slower but bigger version of the +// code here but won't fit when debugging disabled (requires -Os) +#define COVARIANCE_PREDICTION_GENERAL +#endif + +// Private functions +void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], + float Q[NUMW], float dT, float P[NUMX][NUMX]); +void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], + float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], + uint16_t SensorsUsed); +void RungeKutta(float X[NUMX], float U[NUMU], float dT); +void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]); +void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX], + float G[NUMX][NUMW]); +void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]); +void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]); + + +// Private variables +static struct EKFData { + float F[NUMX][NUMX]; + float G[NUMX][NUMW]; + float H[NUMV][NUMX]; // linearized system matrices + // global to init to zero and maintain zero elements + float Be[3]; // local magnetic unit vector in NED frame + float P[NUMX][NUMX]; + float X[NUMX]; // covariance matrix and state vector + float Q[NUMW]; + float R[NUMV]; // input noise and measurement noise variances + float K[NUMX][NUMV]; // feedback gain matrix +} ekf; + +// Global variables +struct NavStruct Nav; + +// ************* Exposed Functions **************** +// ************************************************* + +uint16_t ins_get_num_states() +{ + return NUMX; +} + +void INSGPSInit() +{ + ekf.Be[0] = 1.0f; + ekf.Be[1] = 0; + ekf.Be[2] = 0; // local magnetic unit vector + + for (int i = 0; i < NUMX; i++) { + for (int j = 0; j < NUMX; j++) { + ekf.P[i][j] = 0.0f; // zero all terms + ekf.F[i][j] = 0.0f; + } + for (int j = 0; j < NUMW; j++) { + ekf.G[i][j] = 0.0f; + } + + for (int j = 0; j < NUMV; j++) { + ekf.H[j][i] = 0.0f; + ekf.K[i][j] = 0.0f; + } + + ekf.X[i] = 0.0f; + } + for (int i = 0; i < NUMW; i++) { + ekf.Q[i] = 0.0f; + } + for (int i = 0; i < NUMV; i++) { + ekf.R[i] = 0.0f; + } + + ekf.P[0][0] = ekf.P[1][1] = ekf.P[2][2] = 25.0f; // initial position variance (m^2) + ekf.P[3][3] = ekf.P[4][4] = ekf.P[5][5] = 5.0f; // initial velocity variance (m/s)^2 + ekf.P[6][6] = ekf.P[7][7] = ekf.P[8][8] = ekf.P[9][9] = 1e-5f; // initial quaternion variance + ekf.P[10][10] = ekf.P[11][11] = ekf.P[12][12] = 1e-6f; // initial gyro bias variance (rad/s)^2 + ekf.P[13][13] = 1e-5f; // initial accel bias variance (deg/s)^2 + + ekf.X[0] = ekf.X[1] = ekf.X[2] = ekf.X[3] = ekf.X[4] = ekf.X[5] = 0.0f; // initial pos and vel (m) + ekf.X[6] = 1.0f; + ekf.X[7] = ekf.X[8] = ekf.X[9] = 0.0f; // initial quaternion (level and North) (m/s) + ekf.X[10] = ekf.X[11] = ekf.X[12] = 0.0f; // initial gyro bias (rad/s) + ekf.X[13] = 0.0f; // initial accel bias + + ekf.Q[0] = ekf.Q[1] = ekf.Q[2] = 1e-5f; // gyro noise variance (rad/s)^2 + ekf.Q[3] = ekf.Q[4] = ekf.Q[5] = 1e-5f; // accelerometer noise variance (m/s^2)^2 + ekf.Q[6] = ekf.Q[7] = 1e-6f; // gyro x and y bias random walk variance (rad/s^2)^2 + ekf.Q[8] = 1e-6f; // gyro z bias random walk variance (rad/s^2)^2 + ekf.Q[9] = 5e-4f; // accel bias random walk variance (m/s^3)^2 + + ekf.R[0] = ekf.R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2) + ekf.R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2) + ekf.R[3] = ekf.R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2 + ekf.R[5] = 0.004f; // High freq GPS vertical velocity noise variance (m/s)^2 + ekf.R[6] = ekf.R[7] = ekf.R[8] = 0.005f; // magnetometer unit vector noise variance + ekf.R[9] = .05f; // High freq altimeter noise variance (m^2) +} + +// ! Set the current flight state +void INSSetArmed(bool armed) +{ + return; + + // Speed up convergence of accel and gyro bias when not armed + if (armed) { + ekf.Q[9] = 1e-4f; + ekf.Q[8] = 2e-9f; + } else { + ekf.Q[9] = 1e-2f; + ekf.Q[8] = 2e-8f; + } +} + +/** + * Get the current state estimate (null input skips that get) + * @param[out] pos The position in NED space (m) + * @param[out] vel The velocity in NED (m/s) + * @param[out] attitude Quaternion representation of attitude + * @param[out] gyros_bias Estimate of gyro bias (rad/s) + * @param[out] accel_bias Estiamte of the accel bias (m/s^2) + */ +void INSGetState(float *pos, float *vel, float *attitude, float *gyro_bias, float *accel_bias) +{ + if (pos) { + pos[0] = ekf.X[0]; + pos[1] = ekf.X[1]; + pos[2] = ekf.X[2]; + } + + if (vel) { + vel[0] = ekf.X[3]; + vel[1] = ekf.X[4]; + vel[2] = ekf.X[5]; + } + + if (attitude) { + attitude[0] = ekf.X[6]; + attitude[1] = ekf.X[7]; + attitude[2] = ekf.X[8]; + attitude[3] = ekf.X[9]; + } + + if (gyro_bias) { + gyro_bias[0] = ekf.X[10]; + gyro_bias[1] = ekf.X[11]; + gyro_bias[2] = ekf.X[12]; + } + + if (accel_bias) { + accel_bias[0] = 0.0f; + accel_bias[1] = 0.0f; + accel_bias[2] = ekf.X[13]; + } +} + +/** + * Get the variance, for visualizing the filter performance + * @param[out var_out The variances + */ +void INSGetVariance(float *var_out) +{ + for (uint32_t i = 0; i < NUMX; i++) { + var_out[i] = ekf.P[i][i]; + } +} + +void INSResetP(const float *PDiag) +{ + uint8_t i, j; + + // if PDiag[i] nonzero then clear row and column and set diagonal element + for (i = 0; i < NUMX; i++) { + if (PDiag != 0) { + for (j = 0; j < NUMX; j++) { + ekf.P[i][j] = ekf.P[j][i] = 0.0f; + } + ekf.P[i][i] = PDiag[i]; + } + } +} + +void INSSetState(const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], const float accel_bias[3]) +{ + ekf.X[0] = pos[0]; + ekf.X[1] = pos[1]; + ekf.X[2] = pos[2]; + ekf.X[3] = vel[0]; + ekf.X[4] = vel[1]; + ekf.X[5] = vel[2]; + ekf.X[6] = q[0]; + ekf.X[7] = q[1]; + ekf.X[8] = q[2]; + ekf.X[9] = q[3]; + ekf.X[10] = gyro_bias[0]; + ekf.X[11] = gyro_bias[1]; + ekf.X[12] = gyro_bias[2]; + ekf.X[13] = accel_bias[2]; +} + +void INSPosVelReset(const float pos[3], const float vel[3]) +{ + for (int i = 0; i < 6; i++) { + for (int j = i; j < NUMX; j++) { + ekf.P[i][j] = 0.0f; // zero the first 6 rows and columns + ekf.P[j][i] = 0.0f; + } + } + + ekf.P[0][0] = ekf.P[1][1] = ekf.P[2][2] = 25.0f; // initial position variance (m^2) + ekf.P[3][3] = ekf.P[4][4] = ekf.P[5][5] = 5.0f; // initial velocity variance (m/s)^2 + + ekf.X[0] = pos[0]; + ekf.X[1] = pos[1]; + ekf.X[2] = pos[2]; + ekf.X[3] = vel[0]; + ekf.X[4] = vel[1]; + ekf.X[5] = vel[2]; +} +void INSSetPosVelVar(const float PosVar[3], const float VelVar[3]) +{ + ekf.R[0] = PosVar[0]; + ekf.R[1] = PosVar[1]; + ekf.R[2] = PosVar[2]; + ekf.R[3] = VelVar[0]; + ekf.R[4] = VelVar[1]; + ekf.R[5] = VelVar[2]; // Don't change vertical velocity, not measured +} + +void INSSetGyroBias(const float gyro_bias[3]) +{ + ekf.X[10] = gyro_bias[0]; + ekf.X[11] = gyro_bias[1]; + ekf.X[12] = gyro_bias[2]; +} + +void INSSetAccelBias(const float accel_bias[3]) +{ + ekf.X[13] = accel_bias[2]; +} + +void INSSetAccelVar(const float accel_var[3]) +{ + ekf.Q[3] = accel_var[0]; + ekf.Q[4] = accel_var[1]; + ekf.Q[5] = accel_var[2]; +} + +void INSSetGyroVar(const float gyro_var[3]) +{ + ekf.Q[0] = gyro_var[0]; + ekf.Q[1] = gyro_var[1]; + ekf.Q[2] = gyro_var[2]; +} + +void INSSetGyroBiasVar(const float gyro_bias_var[3]) +{ + ekf.Q[6] = gyro_bias_var[0]; + ekf.Q[7] = gyro_bias_var[1]; + ekf.Q[8] = gyro_bias_var[2]; +} + +void INSSetMagVar(const float scaled_mag_var[3]) +{ + ekf.R[6] = scaled_mag_var[0]; + ekf.R[7] = scaled_mag_var[1]; + ekf.R[8] = scaled_mag_var[2]; +} + +void INSSetBaroVar(const float baro_var) +{ + ekf.R[9] = baro_var; +} + +void INSSetMagNorth(const float B[3]) +{ + ekf.Be[0] = B[0]; + ekf.Be[1] = B[1]; + ekf.Be[2] = B[2]; +} + +void INSLimitBias() +{ + // The Z accel bias should never wander too much. This helps ensure the filter + // remains stable. + if (ekf.X[13] > 0.1f) { + ekf.X[13] = 0.1f; + } else if (ekf.X[13] < -0.1f) { + ekf.X[13] = -0.1f; + } + + // Make sure no gyro bias gets to more than 10 deg / s. This should be more than + // enough for well behaving sensors. + const float GYRO_BIAS_LIMIT = DEG2RAD(10); + for (int i = 10; i < 13; i++) { + if (ekf.X[i] < -GYRO_BIAS_LIMIT) { + ekf.X[i] = -GYRO_BIAS_LIMIT; + } else if (ekf.X[i] > GYRO_BIAS_LIMIT) { + ekf.X[i] = GYRO_BIAS_LIMIT; + } + } +} + +void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT) +{ + float U[6]; + float qmag; + + // rate gyro inputs in units of rad/s + U[0] = gyro_data[0]; + U[1] = gyro_data[1]; + U[2] = gyro_data[2]; + + // accelerometer inputs in units of m/s + U[3] = accel_data[0]; + U[4] = accel_data[1]; + U[5] = accel_data[2]; + + // EKF prediction step + LinearizeFG(ekf.X, U, ekf.F, ekf.G); + RungeKutta(ekf.X, U, dT); + qmag = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); + ekf.X[6] /= qmag; + ekf.X[7] /= qmag; + ekf.X[8] /= qmag; + ekf.X[9] /= qmag; + + // Update Nav solution structure + Nav.Pos[0] = ekf.X[0]; + Nav.Pos[1] = ekf.X[1]; + Nav.Pos[2] = ekf.X[2]; + Nav.Vel[0] = ekf.X[3]; + Nav.Vel[1] = ekf.X[4]; + Nav.Vel[2] = ekf.X[5]; + Nav.q[0] = ekf.X[6]; + Nav.q[1] = ekf.X[7]; + Nav.q[2] = ekf.X[8]; + Nav.q[3] = ekf.X[9]; + Nav.gyro_bias[0] = ekf.X[10]; + Nav.gyro_bias[1] = ekf.X[11]; + Nav.gyro_bias[2] = ekf.X[12]; + Nav.gyro_bias[0] = 0.0f; + Nav.gyro_bias[1] = 0.0f; + Nav.gyro_bias[2] = ekf.X[13]; +} + +void INSCovariancePrediction(float dT) +{ + CovariancePrediction(ekf.F, ekf.G, ekf.Q, dT, ekf.P); +} + +void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3], + float BaroAlt, uint16_t SensorsUsed) +{ + float Z[10], Y[10]; + float qmag; + + // GPS Position in meters and in local NED frame + Z[0] = Pos[0]; + Z[1] = Pos[1]; + Z[2] = Pos[2]; + + // GPS Velocity in meters and in local NED frame + Z[3] = Vel[0]; + Z[4] = Vel[1]; + Z[5] = Vel[2]; + + if (SensorsUsed & MAG_SENSORS) { + // magnetometer data in any units (use unit vector) and in body frame + float Rbe_a[3][3]; + float q0 = ekf.X[6]; + float q1 = ekf.X[7]; + float q2 = ekf.X[8]; + float q3 = ekf.X[9]; + float k1 = 1.0f / sqrtf(powf(q0 * q1 * 2.0f + q2 * q3 * 2.0f, 2.0f) + powf(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3, 2.0f)); + float k2 = sqrtf(-powf(q0 * q2 * 2.0f - q1 * q3 * 2.0f, 2.0f) + 1.0f); + + Rbe_a[0][0] = k2; + Rbe_a[0][1] = 0.0f; + Rbe_a[0][2] = q0 * q2 * -2.0f + q1 * q3 * 2.0f; + Rbe_a[1][0] = k1 * (q0 * q1 * 2.0f + q2 * q3 * 2.0f) * (q0 * q2 * 2.0f - q1 * q3 * 2.0f); + Rbe_a[1][1] = k1 * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + Rbe_a[1][2] = k1 * sqrtf(-powf(q0 * q2 * 2.0f - q1 * q3 * 2.0f, 2.0f) + 1.0f) * (q0 * q1 * 2.0f + q2 * q3 * 2.0f); + Rbe_a[2][0] = k1 * (q0 * q2 * 2.0f - q1 * q3 * 2.0f) * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + Rbe_a[2][1] = -k1 * (q0 * q1 * 2.0f + q2 * q3 * 2.0f); + Rbe_a[2][2] = k1 * k2 * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + + Z[6] = Rbe_a[0][0] * mag_data[0] + Rbe_a[1][0] * mag_data[1] + Rbe_a[2][0] * mag_data[2]; + Z[7] = Rbe_a[0][1] * mag_data[0] + Rbe_a[1][1] * mag_data[1] + Rbe_a[2][1] * mag_data[2]; + Z[8] = Rbe_a[0][2] * mag_data[0] + Rbe_a[1][2] * mag_data[1] + Rbe_a[2][2] * mag_data[2]; + } + + // barometric altimeter in meters and in local NED frame + Z[9] = BaroAlt; + + // EKF correction step + LinearizeH(ekf.X, ekf.Be, ekf.H); + MeasurementEq(ekf.X, ekf.Be, Y); + SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed); + qmag = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); + ekf.X[6] /= qmag; + ekf.X[7] /= qmag; + ekf.X[8] /= qmag; + ekf.X[9] /= qmag; + + INSLimitBias(); +} + +// ************* CovariancePrediction ************* +// Does the prediction step of the Kalman filter for the covariance matrix +// Output, Pnew, overwrites P, the input covariance +// Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' +// Q is the discrete time covariance of process noise +// Q is vector of the diagonal for a square matrix with +// dimensions equal to the number of disturbance noise variables +// The General Method is very inefficient,not taking advantage of the sparse F and G +// The first Method is very specific to this implementation +// ************************************************ + +#ifdef COVARIANCE_PREDICTION_GENERAL + +void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], + float Q[NUMW], float dT, float P[NUMX][NUMX]) +{ + float Dummy[NUMX][NUMX], dTsq; + uint8_t i, j, k; + + // Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')] + + dTsq = dT * dT; + + for (i = 0; i < NUMX; i++) { // Calculate Dummy = (P/T +F*P) + for (j = 0; j < NUMX; j++) { + Dummy[i][j] = P[i][j] / dT; + for (k = 0; k < NUMX; k++) { + Dummy[i][j] += F[i][k] * P[k][j]; + } + } + } + for (i = 0; i < NUMX; i++) { // Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G' + for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular + P[i][j] = Dummy[i][j] / dT; + for (k = 0; k < NUMX; k++) { + P[i][j] += Dummy[i][k] * F[j][k]; // P = Dummy/T + Dummy*F' + } + for (k = 0; k < NUMW; k++) { + P[i][j] += Q[k] * G[i][k] * G[j][k]; // P = Dummy/T + Dummy*F' + G*Q*G' + } + P[j][i] = P[i][j] = P[i][j] * dTsq; // Pnew = T^2*P and fill in lower triangular; + } + } +} + +#else /* ifdef COVARIANCE_PREDICTION_GENERAL */ + +void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW], + float Q[NUMW], float dT, float P[NUMX][NUMX]) +{ + float D[NUMX][NUMX], T, Tsq; + uint8_t i, j; + + // Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator + + T = dT; + Tsq = dT * dT; + + for (i = 0; i < NUMX; i++) { // Create a copy of the upper triangular of P + for (j = i; j < NUMX; j++) { + D[i][j] = P[i][j]; + } + } + + // Brute force calculation of the elements of P + P[0][0] = D[3][3] * Tsq + (2 * D[0][3]) * T + D[0][0]; + P[0][1] = P[1][0] = D[3][4] * Tsq + (D[0][4] + D[1][3]) * T + D[0][1]; + P[0][2] = P[2][0] = D[3][5] * Tsq + (D[0][5] + D[2][3]) * T + D[0][2]; + P[0][3] = P[3][0] = (F[3][6] * D[3][6] + F[3][7] * D[3][7] + F[3][8] * D[3][8] + F[3][9] * D[3][9] + F[3][13] * D[3][13]) * Tsq + (D[3][3] + F[3][6] * D[0][6] + F[3][7] * D[0][7] + F[3][8] * D[0][8] + F[3][9] * D[0][9] + F[3][13] * D[0][13]) * T + D[0][3]; + P[0][4] = P[4][0] = (F[4][6] * D[3][6] + F[4][7] * D[3][7] + F[4][8] * D[3][8] + F[4][9] * D[3][9] + F[4][13] * D[3][13]) * Tsq + (D[3][4] + F[4][6] * D[0][6] + F[4][7] * D[0][7] + F[4][8] * D[0][8] + F[4][9] * D[0][9] + F[4][13] * D[0][13]) * T + D[0][4]; + P[0][5] = P[5][0] = (F[5][6] * D[3][6] + F[5][7] * D[3][7] + F[5][8] * D[3][8] + F[5][9] * D[3][9] + F[5][13] * D[3][13]) * Tsq + (D[3][5] + F[5][6] * D[0][6] + F[5][7] * D[0][7] + F[5][8] * D[0][8] + F[5][9] * D[0][9] + F[5][13] * D[0][13]) * T + D[0][5]; + P[0][6] = P[6][0] = (F[6][7] * D[3][7] + F[6][8] * D[3][8] + F[6][9] * D[3][9] + F[6][10] * D[3][10] + F[6][11] * D[3][11] + F[6][12] * D[3][12]) * Tsq + (D[3][6] + F[6][7] * D[0][7] + F[6][8] * D[0][8] + F[6][9] * D[0][9] + F[6][10] * D[0][10] + F[6][11] * D[0][11] + F[6][12] * D[0][12]) * T + D[0][6]; + P[0][7] = P[7][0] = (F[7][6] * D[3][6] + F[7][8] * D[3][8] + F[7][9] * D[3][9] + F[7][10] * D[3][10] + F[7][11] * D[3][11] + F[7][12] * D[3][12]) * Tsq + (D[3][7] + F[7][6] * D[0][6] + F[7][8] * D[0][8] + F[7][9] * D[0][9] + F[7][10] * D[0][10] + F[7][11] * D[0][11] + F[7][12] * D[0][12]) * T + D[0][7]; + P[0][8] = P[8][0] = (F[8][6] * D[3][6] + F[8][7] * D[3][7] + F[8][9] * D[3][9] + F[8][10] * D[3][10] + F[8][11] * D[3][11] + F[8][12] * D[3][12]) * Tsq + (D[3][8] + F[8][6] * D[0][6] + F[8][7] * D[0][7] + F[8][9] * D[0][9] + F[8][10] * D[0][10] + F[8][11] * D[0][11] + F[8][12] * D[0][12]) * T + D[0][8]; + P[0][9] = P[9][0] = (F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] + F[9][10] * D[3][10] + F[9][11] * D[3][11] + F[9][12] * D[3][12]) * Tsq + (D[3][9] + F[9][6] * D[0][6] + F[9][7] * D[0][7] + F[9][8] * D[0][8] + F[9][10] * D[0][10] + F[9][11] * D[0][11] + F[9][12] * D[0][12]) * T + D[0][9]; + P[0][10] = P[10][0] = D[3][10] * T + D[0][10]; + P[0][11] = P[11][0] = D[3][11] * T + D[0][11]; + P[0][12] = P[12][0] = D[3][12] * T + D[0][12]; + P[0][13] = P[13][0] = D[3][13] * T + D[0][13]; + P[1][1] = D[4][4] * Tsq + (2 * D[1][4]) * T + D[1][1]; + P[1][2] = P[2][1] = D[4][5] * Tsq + (D[1][5] + D[2][4]) * T + D[1][2]; + P[1][3] = P[3][1] = (F[3][6] * D[4][6] + F[3][7] * D[4][7] + F[3][8] * D[4][8] + F[3][9] * D[4][9] + F[3][13] * D[4][13]) * Tsq + (D[3][4] + F[3][6] * D[1][6] + F[3][7] * D[1][7] + F[3][8] * D[1][8] + F[3][9] * D[1][9] + F[3][13] * D[1][13]) * T + D[1][3]; + P[1][4] = P[4][1] = (F[4][6] * D[4][6] + F[4][7] * D[4][7] + F[4][8] * D[4][8] + F[4][9] * D[4][9] + F[4][13] * D[4][13]) * Tsq + (D[4][4] + F[4][6] * D[1][6] + F[4][7] * D[1][7] + F[4][8] * D[1][8] + F[4][9] * D[1][9] + F[4][13] * D[1][13]) * T + D[1][4]; + P[1][5] = P[5][1] = (F[5][6] * D[4][6] + F[5][7] * D[4][7] + F[5][8] * D[4][8] + F[5][9] * D[4][9] + F[5][13] * D[4][13]) * Tsq + (D[4][5] + F[5][6] * D[1][6] + F[5][7] * D[1][7] + F[5][8] * D[1][8] + F[5][9] * D[1][9] + F[5][13] * D[1][13]) * T + D[1][5]; + P[1][6] = P[6][1] = (F[6][7] * D[4][7] + F[6][8] * D[4][8] + F[6][9] * D[4][9] + F[6][10] * D[4][10] + F[6][11] * D[4][11] + F[6][12] * D[4][12]) * Tsq + (D[4][6] + F[6][7] * D[1][7] + F[6][8] * D[1][8] + F[6][9] * D[1][9] + F[6][10] * D[1][10] + F[6][11] * D[1][11] + F[6][12] * D[1][12]) * T + D[1][6]; + P[1][7] = P[7][1] = (F[7][6] * D[4][6] + F[7][8] * D[4][8] + F[7][9] * D[4][9] + F[7][10] * D[4][10] + F[7][11] * D[4][11] + F[7][12] * D[4][12]) * Tsq + (D[4][7] + F[7][6] * D[1][6] + F[7][8] * D[1][8] + F[7][9] * D[1][9] + F[7][10] * D[1][10] + F[7][11] * D[1][11] + F[7][12] * D[1][12]) * T + D[1][7]; + P[1][8] = P[8][1] = (F[8][6] * D[4][6] + F[8][7] * D[4][7] + F[8][9] * D[4][9] + F[8][10] * D[4][10] + F[8][11] * D[4][11] + F[8][12] * D[4][12]) * Tsq + (D[4][8] + F[8][6] * D[1][6] + F[8][7] * D[1][7] + F[8][9] * D[1][9] + F[8][10] * D[1][10] + F[8][11] * D[1][11] + F[8][12] * D[1][12]) * T + D[1][8]; + P[1][9] = P[9][1] = (F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] + F[9][10] * D[4][10] + F[9][11] * D[4][11] + F[9][12] * D[4][12]) * Tsq + (D[4][9] + F[9][6] * D[1][6] + F[9][7] * D[1][7] + F[9][8] * D[1][8] + F[9][10] * D[1][10] + F[9][11] * D[1][11] + F[9][12] * D[1][12]) * T + D[1][9]; + P[1][10] = P[10][1] = D[4][10] * T + D[1][10]; + P[1][11] = P[11][1] = D[4][11] * T + D[1][11]; + P[1][12] = P[12][1] = D[4][12] * T + D[1][12]; + P[1][13] = P[13][1] = D[4][13] * T + D[1][13]; + P[2][2] = D[5][5] * Tsq + (2 * D[2][5]) * T + D[2][2]; + P[2][3] = P[3][2] = (F[3][6] * D[5][6] + F[3][7] * D[5][7] + F[3][8] * D[5][8] + F[3][9] * D[5][9] + F[3][13] * D[5][13]) * Tsq + (D[3][5] + F[3][6] * D[2][6] + F[3][7] * D[2][7] + F[3][8] * D[2][8] + F[3][9] * D[2][9] + F[3][13] * D[2][13]) * T + D[2][3]; + P[2][4] = P[4][2] = (F[4][6] * D[5][6] + F[4][7] * D[5][7] + F[4][8] * D[5][8] + F[4][9] * D[5][9] + F[4][13] * D[5][13]) * Tsq + (D[4][5] + F[4][6] * D[2][6] + F[4][7] * D[2][7] + F[4][8] * D[2][8] + F[4][9] * D[2][9] + F[4][13] * D[2][13]) * T + D[2][4]; + P[2][5] = P[5][2] = (F[5][6] * D[5][6] + F[5][7] * D[5][7] + F[5][8] * D[5][8] + F[5][9] * D[5][9] + F[5][13] * D[5][13]) * Tsq + (D[5][5] + F[5][6] * D[2][6] + F[5][7] * D[2][7] + F[5][8] * D[2][8] + F[5][9] * D[2][9] + F[5][13] * D[2][13]) * T + D[2][5]; + P[2][6] = P[6][2] = (F[6][7] * D[5][7] + F[6][8] * D[5][8] + F[6][9] * D[5][9] + F[6][10] * D[5][10] + F[6][11] * D[5][11] + F[6][12] * D[5][12]) * Tsq + (D[5][6] + F[6][7] * D[2][7] + F[6][8] * D[2][8] + F[6][9] * D[2][9] + F[6][10] * D[2][10] + F[6][11] * D[2][11] + F[6][12] * D[2][12]) * T + D[2][6]; + P[2][7] = P[7][2] = (F[7][6] * D[5][6] + F[7][8] * D[5][8] + F[7][9] * D[5][9] + F[7][10] * D[5][10] + F[7][11] * D[5][11] + F[7][12] * D[5][12]) * Tsq + (D[5][7] + F[7][6] * D[2][6] + F[7][8] * D[2][8] + F[7][9] * D[2][9] + F[7][10] * D[2][10] + F[7][11] * D[2][11] + F[7][12] * D[2][12]) * T + D[2][7]; + P[2][8] = P[8][2] = (F[8][6] * D[5][6] + F[8][7] * D[5][7] + F[8][9] * D[5][9] + F[8][10] * D[5][10] + F[8][11] * D[5][11] + F[8][12] * D[5][12]) * Tsq + (D[5][8] + F[8][6] * D[2][6] + F[8][7] * D[2][7] + F[8][9] * D[2][9] + F[8][10] * D[2][10] + F[8][11] * D[2][11] + F[8][12] * D[2][12]) * T + D[2][8]; + P[2][9] = P[9][2] = (F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] + F[9][10] * D[5][10] + F[9][11] * D[5][11] + F[9][12] * D[5][12]) * Tsq + (D[5][9] + F[9][6] * D[2][6] + F[9][7] * D[2][7] + F[9][8] * D[2][8] + F[9][10] * D[2][10] + F[9][11] * D[2][11] + F[9][12] * D[2][12]) * T + D[2][9]; + P[2][10] = P[10][2] = D[5][10] * T + D[2][10]; + P[2][11] = P[11][2] = D[5][11] * T + D[2][11]; + P[2][12] = P[12][2] = D[5][12] * T + D[2][12]; + P[2][13] = P[13][2] = D[5][13] * T + D[2][13]; + P[3][3] = (Q[3] * G[3][3] * G[3][3] + Q[4] * G[3][4] * G[3][4] + Q[5] * G[3][5] * G[3][5] + F[3][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[3][8] * D[6][8] + F[3][9] * D[6][9] + F[3][13] * D[6][13]) + F[3][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] + F[3][8] * D[7][8] + F[3][9] * D[7][9] + F[3][13] * D[7][13]) + F[3][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[3][8] * D[8][8] + F[3][9] * D[8][9] + F[3][13] * D[8][13]) + F[3][9] * (F[3][6] * D[6][9] + F[3][7] * D[7][9] + F[3][8] * D[8][9] + F[3][9] * D[9][9] + F[3][13] * D[9][13]) + F[3][13] * (F[3][6] * D[6][13] + F[3][7] * D[7][13] + F[3][8] * D[8][13] + F[3][9] * D[9][13] + F[3][13] * D[13][13])) * Tsq + (2 * F[3][6] * D[3][6] + 2 * F[3][7] * D[3][7] + 2 * F[3][8] * D[3][8] + 2 * F[3][9] * D[3][9] + 2 * F[3][13] * D[3][13]) * T + D[3][3]; + P[3][4] = P[4][3] = (F[4][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[3][8] * D[6][8] + F[3][9] * D[6][9] + F[3][13] * D[6][13]) + F[4][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] + F[3][8] * D[7][8] + F[3][9] * D[7][9] + F[3][13] * D[7][13]) + F[4][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[3][8] * D[8][8] + F[3][9] * D[8][9] + F[3][13] * D[8][13]) + F[4][9] * (F[3][6] * D[6][9] + F[3][7] * D[7][9] + F[3][8] * D[8][9] + F[3][9] * D[9][9] + F[3][13] * D[9][13]) + F[4][13] * (F[3][6] * D[6][13] + F[3][7] * D[7][13] + F[3][8] * D[8][13] + F[3][9] * D[9][13] + F[3][13] * D[13][13]) + G[3][3] * G[4][3] * Q[3] + G[3][4] * G[4][4] * Q[4] + G[3][5] * G[4][5] * Q[5]) * Tsq + (F[3][6] * D[4][6] + F[4][6] * D[3][6] + F[3][7] * D[4][7] + F[4][7] * D[3][7] + F[3][8] * D[4][8] + F[4][8] * D[3][8] + F[3][9] * D[4][9] + F[4][9] * D[3][9] + F[3][13] * D[4][13] + F[4][13] * D[3][13]) * T + D[3][4]; + P[3][5] = P[5][3] = (F[5][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[3][8] * D[6][8] + F[3][9] * D[6][9] + F[3][13] * D[6][13]) + F[5][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] + F[3][8] * D[7][8] + F[3][9] * D[7][9] + F[3][13] * D[7][13]) + F[5][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[3][8] * D[8][8] + F[3][9] * D[8][9] + F[3][13] * D[8][13]) + F[5][9] * (F[3][6] * D[6][9] + F[3][7] * D[7][9] + F[3][8] * D[8][9] + F[3][9] * D[9][9] + F[3][13] * D[9][13]) + F[5][13] * (F[3][6] * D[6][13] + F[3][7] * D[7][13] + F[3][8] * D[8][13] + F[3][9] * D[9][13] + F[3][13] * D[13][13]) + G[3][3] * G[5][3] * Q[3] + G[3][4] * G[5][4] * Q[4] + G[3][5] * G[5][5] * Q[5]) * Tsq + (F[3][6] * D[5][6] + F[5][6] * D[3][6] + F[3][7] * D[5][7] + F[5][7] * D[3][7] + F[3][8] * D[5][8] + F[5][8] * D[3][8] + F[3][9] * D[5][9] + F[5][9] * D[3][9] + F[3][13] * D[5][13] + F[5][13] * D[3][13]) * T + D[3][5]; + P[3][6] = P[6][3] = (F[6][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] + F[3][8] * D[7][8] + F[3][9] * D[7][9] + F[3][13] * D[7][13]) + F[6][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[3][8] * D[8][8] + F[3][9] * D[8][9] + F[3][13] * D[8][13]) + F[6][9] * (F[3][6] * D[6][9] + F[3][7] * D[7][9] + F[3][8] * D[8][9] + F[3][9] * D[9][9] + F[3][13] * D[9][13]) + F[6][10] * (F[3][6] * D[6][10] + F[3][7] * D[7][10] + F[3][8] * D[8][10] + F[3][9] * D[9][10] + F[3][13] * D[10][13]) + F[6][11] * (F[3][6] * D[6][11] + F[3][7] * D[7][11] + F[3][8] * D[8][11] + F[3][9] * D[9][11] + F[3][13] * D[11][13]) + F[6][12] * (F[3][6] * D[6][12] + F[3][7] * D[7][12] + F[3][8] * D[8][12] + F[3][9] * D[9][12] + F[3][13] * D[12][13])) * Tsq + (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[6][7] * D[3][7] + F[3][8] * D[6][8] + F[6][8] * D[3][8] + F[3][9] * D[6][9] + F[6][9] * D[3][9] + F[6][10] * D[3][10] + F[6][11] * D[3][11] + F[6][12] * D[3][12] + F[3][13] * D[6][13]) * T + D[3][6]; + P[3][7] = P[7][3] = (F[7][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[3][8] * D[6][8] + F[3][9] * D[6][9] + F[3][13] * D[6][13]) + F[7][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[3][8] * D[8][8] + F[3][9] * D[8][9] + F[3][13] * D[8][13]) + F[7][9] * (F[3][6] * D[6][9] + F[3][7] * D[7][9] + F[3][8] * D[8][9] + F[3][9] * D[9][9] + F[3][13] * D[9][13]) + F[7][10] * (F[3][6] * D[6][10] + F[3][7] * D[7][10] + F[3][8] * D[8][10] + F[3][9] * D[9][10] + F[3][13] * D[10][13]) + F[7][11] * (F[3][6] * D[6][11] + F[3][7] * D[7][11] + F[3][8] * D[8][11] + F[3][9] * D[9][11] + F[3][13] * D[11][13]) + F[7][12] * (F[3][6] * D[6][12] + F[3][7] * D[7][12] + F[3][8] * D[8][12] + F[3][9] * D[9][12] + F[3][13] * D[12][13])) * Tsq + (F[3][6] * D[6][7] + F[7][6] * D[3][6] + F[3][7] * D[7][7] + F[3][8] * D[7][8] + F[7][8] * D[3][8] + F[3][9] * D[7][9] + F[7][9] * D[3][9] + F[7][10] * D[3][10] + F[7][11] * D[3][11] + F[7][12] * D[3][12] + F[3][13] * D[7][13]) * T + D[3][7]; + P[3][8] = P[8][3] = (F[8][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[3][8] * D[6][8] + F[3][9] * D[6][9] + F[3][13] * D[6][13]) + F[8][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] + F[3][8] * D[7][8] + F[3][9] * D[7][9] + F[3][13] * D[7][13]) + F[8][9] * (F[3][6] * D[6][9] + F[3][7] * D[7][9] + F[3][8] * D[8][9] + F[3][9] * D[9][9] + F[3][13] * D[9][13]) + F[8][10] * (F[3][6] * D[6][10] + F[3][7] * D[7][10] + F[3][8] * D[8][10] + F[3][9] * D[9][10] + F[3][13] * D[10][13]) + F[8][11] * (F[3][6] * D[6][11] + F[3][7] * D[7][11] + F[3][8] * D[8][11] + F[3][9] * D[9][11] + F[3][13] * D[11][13]) + F[8][12] * (F[3][6] * D[6][12] + F[3][7] * D[7][12] + F[3][8] * D[8][12] + F[3][9] * D[9][12] + F[3][13] * D[12][13])) * Tsq + (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[8][6] * D[3][6] + F[8][7] * D[3][7] + F[3][8] * D[8][8] + F[3][9] * D[8][9] + F[8][9] * D[3][9] + F[8][10] * D[3][10] + F[8][11] * D[3][11] + F[8][12] * D[3][12] + F[3][13] * D[8][13]) * T + D[3][8]; + P[3][9] = P[9][3] = (F[9][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[3][8] * D[6][8] + F[3][9] * D[6][9] + F[3][13] * D[6][13]) + F[9][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] + F[3][8] * D[7][8] + F[3][9] * D[7][9] + F[3][13] * D[7][13]) + F[9][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[3][8] * D[8][8] + F[3][9] * D[8][9] + F[3][13] * D[8][13]) + F[9][10] * (F[3][6] * D[6][10] + F[3][7] * D[7][10] + F[3][8] * D[8][10] + F[3][9] * D[9][10] + F[3][13] * D[10][13]) + F[9][11] * (F[3][6] * D[6][11] + F[3][7] * D[7][11] + F[3][8] * D[8][11] + F[3][9] * D[9][11] + F[3][13] * D[11][13]) + F[9][12] * (F[3][6] * D[6][12] + F[3][7] * D[7][12] + F[3][8] * D[8][12] + F[3][9] * D[9][12] + F[3][13] * D[12][13])) * Tsq + (F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] + F[3][6] * D[6][9] + F[3][7] * D[7][9] + F[3][8] * D[8][9] + F[3][9] * D[9][9] + F[9][10] * D[3][10] + F[9][11] * D[3][11] + F[9][12] * D[3][12] + F[3][13] * D[9][13]) * T + D[3][9]; + P[3][10] = P[10][3] = (F[3][6] * D[6][10] + F[3][7] * D[7][10] + F[3][8] * D[8][10] + F[3][9] * D[9][10] + F[3][13] * D[10][13]) * T + D[3][10]; + P[3][11] = P[11][3] = (F[3][6] * D[6][11] + F[3][7] * D[7][11] + F[3][8] * D[8][11] + F[3][9] * D[9][11] + F[3][13] * D[11][13]) * T + D[3][11]; + P[3][12] = P[12][3] = (F[3][6] * D[6][12] + F[3][7] * D[7][12] + F[3][8] * D[8][12] + F[3][9] * D[9][12] + F[3][13] * D[12][13]) * T + D[3][12]; + P[3][13] = P[13][3] = (F[3][6] * D[6][13] + F[3][7] * D[7][13] + F[3][8] * D[8][13] + F[3][9] * D[9][13] + F[3][13] * D[13][13]) * T + D[3][13]; + P[4][4] = (Q[3] * G[4][3] * G[4][3] + Q[4] * G[4][4] * G[4][4] + Q[5] * G[4][5] * G[4][5] + F[4][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[4][8] * D[6][8] + F[4][9] * D[6][9] + F[4][13] * D[6][13]) + F[4][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] + F[4][8] * D[7][8] + F[4][9] * D[7][9] + F[4][13] * D[7][13]) + F[4][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[4][8] * D[8][8] + F[4][9] * D[8][9] + F[4][13] * D[8][13]) + F[4][9] * (F[4][6] * D[6][9] + F[4][7] * D[7][9] + F[4][8] * D[8][9] + F[4][9] * D[9][9] + F[4][13] * D[9][13]) + F[4][13] * (F[4][6] * D[6][13] + F[4][7] * D[7][13] + F[4][8] * D[8][13] + F[4][9] * D[9][13] + F[4][13] * D[13][13])) * Tsq + (2 * F[4][6] * D[4][6] + 2 * F[4][7] * D[4][7] + 2 * F[4][8] * D[4][8] + 2 * F[4][9] * D[4][9] + 2 * F[4][13] * D[4][13]) * T + D[4][4]; + P[4][5] = P[5][4] = (F[5][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[4][8] * D[6][8] + F[4][9] * D[6][9] + F[4][13] * D[6][13]) + F[5][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] + F[4][8] * D[7][8] + F[4][9] * D[7][9] + F[4][13] * D[7][13]) + F[5][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[4][8] * D[8][8] + F[4][9] * D[8][9] + F[4][13] * D[8][13]) + F[5][9] * (F[4][6] * D[6][9] + F[4][7] * D[7][9] + F[4][8] * D[8][9] + F[4][9] * D[9][9] + F[4][13] * D[9][13]) + F[5][13] * (F[4][6] * D[6][13] + F[4][7] * D[7][13] + F[4][8] * D[8][13] + F[4][9] * D[9][13] + F[4][13] * D[13][13]) + G[4][3] * G[5][3] * Q[3] + G[4][4] * G[5][4] * Q[4] + G[4][5] * G[5][5] * Q[5]) * Tsq + (F[4][6] * D[5][6] + F[5][6] * D[4][6] + F[4][7] * D[5][7] + F[5][7] * D[4][7] + F[4][8] * D[5][8] + F[5][8] * D[4][8] + F[4][9] * D[5][9] + F[5][9] * D[4][9] + F[4][13] * D[5][13] + F[5][13] * D[4][13]) * T + D[4][5]; + P[4][6] = P[6][4] = (F[6][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] + F[4][8] * D[7][8] + F[4][9] * D[7][9] + F[4][13] * D[7][13]) + F[6][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[4][8] * D[8][8] + F[4][9] * D[8][9] + F[4][13] * D[8][13]) + F[6][9] * (F[4][6] * D[6][9] + F[4][7] * D[7][9] + F[4][8] * D[8][9] + F[4][9] * D[9][9] + F[4][13] * D[9][13]) + F[6][10] * (F[4][6] * D[6][10] + F[4][7] * D[7][10] + F[4][8] * D[8][10] + F[4][9] * D[9][10] + F[4][13] * D[10][13]) + F[6][11] * (F[4][6] * D[6][11] + F[4][7] * D[7][11] + F[4][8] * D[8][11] + F[4][9] * D[9][11] + F[4][13] * D[11][13]) + F[6][12] * (F[4][6] * D[6][12] + F[4][7] * D[7][12] + F[4][8] * D[8][12] + F[4][9] * D[9][12] + F[4][13] * D[12][13])) * Tsq + (F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[6][7] * D[4][7] + F[4][8] * D[6][8] + F[6][8] * D[4][8] + F[4][9] * D[6][9] + F[6][9] * D[4][9] + F[6][10] * D[4][10] + F[6][11] * D[4][11] + F[6][12] * D[4][12] + F[4][13] * D[6][13]) * T + D[4][6]; + P[4][7] = P[7][4] = (F[7][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[4][8] * D[6][8] + F[4][9] * D[6][9] + F[4][13] * D[6][13]) + F[7][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[4][8] * D[8][8] + F[4][9] * D[8][9] + F[4][13] * D[8][13]) + F[7][9] * (F[4][6] * D[6][9] + F[4][7] * D[7][9] + F[4][8] * D[8][9] + F[4][9] * D[9][9] + F[4][13] * D[9][13]) + F[7][10] * (F[4][6] * D[6][10] + F[4][7] * D[7][10] + F[4][8] * D[8][10] + F[4][9] * D[9][10] + F[4][13] * D[10][13]) + F[7][11] * (F[4][6] * D[6][11] + F[4][7] * D[7][11] + F[4][8] * D[8][11] + F[4][9] * D[9][11] + F[4][13] * D[11][13]) + F[7][12] * (F[4][6] * D[6][12] + F[4][7] * D[7][12] + F[4][8] * D[8][12] + F[4][9] * D[9][12] + F[4][13] * D[12][13])) * Tsq + (F[4][6] * D[6][7] + F[7][6] * D[4][6] + F[4][7] * D[7][7] + F[4][8] * D[7][8] + F[7][8] * D[4][8] + F[4][9] * D[7][9] + F[7][9] * D[4][9] + F[7][10] * D[4][10] + F[7][11] * D[4][11] + F[7][12] * D[4][12] + F[4][13] * D[7][13]) * T + D[4][7]; + P[4][8] = P[8][4] = (F[8][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[4][8] * D[6][8] + F[4][9] * D[6][9] + F[4][13] * D[6][13]) + F[8][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] + F[4][8] * D[7][8] + F[4][9] * D[7][9] + F[4][13] * D[7][13]) + F[8][9] * (F[4][6] * D[6][9] + F[4][7] * D[7][9] + F[4][8] * D[8][9] + F[4][9] * D[9][9] + F[4][13] * D[9][13]) + F[8][10] * (F[4][6] * D[6][10] + F[4][7] * D[7][10] + F[4][8] * D[8][10] + F[4][9] * D[9][10] + F[4][13] * D[10][13]) + F[8][11] * (F[4][6] * D[6][11] + F[4][7] * D[7][11] + F[4][8] * D[8][11] + F[4][9] * D[9][11] + F[4][13] * D[11][13]) + F[8][12] * (F[4][6] * D[6][12] + F[4][7] * D[7][12] + F[4][8] * D[8][12] + F[4][9] * D[9][12] + F[4][13] * D[12][13])) * Tsq + (F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[8][6] * D[4][6] + F[8][7] * D[4][7] + F[4][8] * D[8][8] + F[4][9] * D[8][9] + F[8][9] * D[4][9] + F[8][10] * D[4][10] + F[8][11] * D[4][11] + F[8][12] * D[4][12] + F[4][13] * D[8][13]) * T + D[4][8]; + P[4][9] = P[9][4] = (F[9][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[4][8] * D[6][8] + F[4][9] * D[6][9] + F[4][13] * D[6][13]) + F[9][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] + F[4][8] * D[7][8] + F[4][9] * D[7][9] + F[4][13] * D[7][13]) + F[9][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[4][8] * D[8][8] + F[4][9] * D[8][9] + F[4][13] * D[8][13]) + F[9][10] * (F[4][6] * D[6][10] + F[4][7] * D[7][10] + F[4][8] * D[8][10] + F[4][9] * D[9][10] + F[4][13] * D[10][13]) + F[9][11] * (F[4][6] * D[6][11] + F[4][7] * D[7][11] + F[4][8] * D[8][11] + F[4][9] * D[9][11] + F[4][13] * D[11][13]) + F[9][12] * (F[4][6] * D[6][12] + F[4][7] * D[7][12] + F[4][8] * D[8][12] + F[4][9] * D[9][12] + F[4][13] * D[12][13])) * Tsq + (F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] + F[4][6] * D[6][9] + F[4][7] * D[7][9] + F[4][8] * D[8][9] + F[4][9] * D[9][9] + F[9][10] * D[4][10] + F[9][11] * D[4][11] + F[9][12] * D[4][12] + F[4][13] * D[9][13]) * T + D[4][9]; + P[4][10] = P[10][4] = (F[4][6] * D[6][10] + F[4][7] * D[7][10] + F[4][8] * D[8][10] + F[4][9] * D[9][10] + F[4][13] * D[10][13]) * T + D[4][10]; + P[4][11] = P[11][4] = (F[4][6] * D[6][11] + F[4][7] * D[7][11] + F[4][8] * D[8][11] + F[4][9] * D[9][11] + F[4][13] * D[11][13]) * T + D[4][11]; + P[4][12] = P[12][4] = (F[4][6] * D[6][12] + F[4][7] * D[7][12] + F[4][8] * D[8][12] + F[4][9] * D[9][12] + F[4][13] * D[12][13]) * T + D[4][12]; + P[4][13] = P[13][4] = (F[4][6] * D[6][13] + F[4][7] * D[7][13] + F[4][8] * D[8][13] + F[4][9] * D[9][13] + F[4][13] * D[13][13]) * T + D[4][13]; + P[5][5] = (Q[3] * G[5][3] * G[5][3] + Q[4] * G[5][4] * G[5][4] + Q[5] * G[5][5] * G[5][5] + F[5][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[5][8] * D[6][8] + F[5][9] * D[6][9] + F[5][13] * D[6][13]) + F[5][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] + F[5][8] * D[7][8] + F[5][9] * D[7][9] + F[5][13] * D[7][13]) + F[5][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[5][8] * D[8][8] + F[5][9] * D[8][9] + F[5][13] * D[8][13]) + F[5][9] * (F[5][6] * D[6][9] + F[5][7] * D[7][9] + F[5][8] * D[8][9] + F[5][9] * D[9][9] + F[5][13] * D[9][13]) + F[5][13] * (F[5][6] * D[6][13] + F[5][7] * D[7][13] + F[5][8] * D[8][13] + F[5][9] * D[9][13] + F[5][13] * D[13][13])) * Tsq + (2 * F[5][6] * D[5][6] + 2 * F[5][7] * D[5][7] + 2 * F[5][8] * D[5][8] + 2 * F[5][9] * D[5][9] + 2 * F[5][13] * D[5][13]) * T + D[5][5]; + P[5][6] = P[6][5] = (F[6][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] + F[5][8] * D[7][8] + F[5][9] * D[7][9] + F[5][13] * D[7][13]) + F[6][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[5][8] * D[8][8] + F[5][9] * D[8][9] + F[5][13] * D[8][13]) + F[6][9] * (F[5][6] * D[6][9] + F[5][7] * D[7][9] + F[5][8] * D[8][9] + F[5][9] * D[9][9] + F[5][13] * D[9][13]) + F[6][10] * (F[5][6] * D[6][10] + F[5][7] * D[7][10] + F[5][8] * D[8][10] + F[5][9] * D[9][10] + F[5][13] * D[10][13]) + F[6][11] * (F[5][6] * D[6][11] + F[5][7] * D[7][11] + F[5][8] * D[8][11] + F[5][9] * D[9][11] + F[5][13] * D[11][13]) + F[6][12] * (F[5][6] * D[6][12] + F[5][7] * D[7][12] + F[5][8] * D[8][12] + F[5][9] * D[9][12] + F[5][13] * D[12][13])) * Tsq + (F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[6][7] * D[5][7] + F[5][8] * D[6][8] + F[6][8] * D[5][8] + F[5][9] * D[6][9] + F[6][9] * D[5][9] + F[6][10] * D[5][10] + F[6][11] * D[5][11] + F[6][12] * D[5][12] + F[5][13] * D[6][13]) * T + D[5][6]; + P[5][7] = P[7][5] = (F[7][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[5][8] * D[6][8] + F[5][9] * D[6][9] + F[5][13] * D[6][13]) + F[7][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[5][8] * D[8][8] + F[5][9] * D[8][9] + F[5][13] * D[8][13]) + F[7][9] * (F[5][6] * D[6][9] + F[5][7] * D[7][9] + F[5][8] * D[8][9] + F[5][9] * D[9][9] + F[5][13] * D[9][13]) + F[7][10] * (F[5][6] * D[6][10] + F[5][7] * D[7][10] + F[5][8] * D[8][10] + F[5][9] * D[9][10] + F[5][13] * D[10][13]) + F[7][11] * (F[5][6] * D[6][11] + F[5][7] * D[7][11] + F[5][8] * D[8][11] + F[5][9] * D[9][11] + F[5][13] * D[11][13]) + F[7][12] * (F[5][6] * D[6][12] + F[5][7] * D[7][12] + F[5][8] * D[8][12] + F[5][9] * D[9][12] + F[5][13] * D[12][13])) * Tsq + (F[5][6] * D[6][7] + F[7][6] * D[5][6] + F[5][7] * D[7][7] + F[5][8] * D[7][8] + F[7][8] * D[5][8] + F[5][9] * D[7][9] + F[7][9] * D[5][9] + F[7][10] * D[5][10] + F[7][11] * D[5][11] + F[7][12] * D[5][12] + F[5][13] * D[7][13]) * T + D[5][7]; + P[5][8] = P[8][5] = (F[8][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[5][8] * D[6][8] + F[5][9] * D[6][9] + F[5][13] * D[6][13]) + F[8][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] + F[5][8] * D[7][8] + F[5][9] * D[7][9] + F[5][13] * D[7][13]) + F[8][9] * (F[5][6] * D[6][9] + F[5][7] * D[7][9] + F[5][8] * D[8][9] + F[5][9] * D[9][9] + F[5][13] * D[9][13]) + F[8][10] * (F[5][6] * D[6][10] + F[5][7] * D[7][10] + F[5][8] * D[8][10] + F[5][9] * D[9][10] + F[5][13] * D[10][13]) + F[8][11] * (F[5][6] * D[6][11] + F[5][7] * D[7][11] + F[5][8] * D[8][11] + F[5][9] * D[9][11] + F[5][13] * D[11][13]) + F[8][12] * (F[5][6] * D[6][12] + F[5][7] * D[7][12] + F[5][8] * D[8][12] + F[5][9] * D[9][12] + F[5][13] * D[12][13])) * Tsq + (F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[8][6] * D[5][6] + F[8][7] * D[5][7] + F[5][8] * D[8][8] + F[5][9] * D[8][9] + F[8][9] * D[5][9] + F[8][10] * D[5][10] + F[8][11] * D[5][11] + F[8][12] * D[5][12] + F[5][13] * D[8][13]) * T + D[5][8]; + P[5][9] = P[9][5] = (F[9][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[5][8] * D[6][8] + F[5][9] * D[6][9] + F[5][13] * D[6][13]) + F[9][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] + F[5][8] * D[7][8] + F[5][9] * D[7][9] + F[5][13] * D[7][13]) + F[9][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[5][8] * D[8][8] + F[5][9] * D[8][9] + F[5][13] * D[8][13]) + F[9][10] * (F[5][6] * D[6][10] + F[5][7] * D[7][10] + F[5][8] * D[8][10] + F[5][9] * D[9][10] + F[5][13] * D[10][13]) + F[9][11] * (F[5][6] * D[6][11] + F[5][7] * D[7][11] + F[5][8] * D[8][11] + F[5][9] * D[9][11] + F[5][13] * D[11][13]) + F[9][12] * (F[5][6] * D[6][12] + F[5][7] * D[7][12] + F[5][8] * D[8][12] + F[5][9] * D[9][12] + F[5][13] * D[12][13])) * Tsq + (F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] + F[5][6] * D[6][9] + F[5][7] * D[7][9] + F[5][8] * D[8][9] + F[5][9] * D[9][9] + F[9][10] * D[5][10] + F[9][11] * D[5][11] + F[9][12] * D[5][12] + F[5][13] * D[9][13]) * T + D[5][9]; + P[5][10] = P[10][5] = (F[5][6] * D[6][10] + F[5][7] * D[7][10] + F[5][8] * D[8][10] + F[5][9] * D[9][10] + F[5][13] * D[10][13]) * T + D[5][10]; + P[5][11] = P[11][5] = (F[5][6] * D[6][11] + F[5][7] * D[7][11] + F[5][8] * D[8][11] + F[5][9] * D[9][11] + F[5][13] * D[11][13]) * T + D[5][11]; + P[5][12] = P[12][5] = (F[5][6] * D[6][12] + F[5][7] * D[7][12] + F[5][8] * D[8][12] + F[5][9] * D[9][12] + F[5][13] * D[12][13]) * T + D[5][12]; + P[5][13] = P[13][5] = (F[5][6] * D[6][13] + F[5][7] * D[7][13] + F[5][8] * D[8][13] + F[5][9] * D[9][13] + F[5][13] * D[13][13]) * T + D[5][13]; + P[6][6] = (Q[0] * G[6][0] * G[6][0] + Q[1] * G[6][1] * G[6][1] + Q[2] * G[6][2] * G[6][2] + F[6][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] + F[6][9] * D[7][9] + F[6][10] * D[7][10] + F[6][11] * D[7][11] + F[6][12] * D[7][12]) + F[6][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] + F[6][9] * D[8][9] + F[6][10] * D[8][10] + F[6][11] * D[8][11] + F[6][12] * D[8][12]) + F[6][9] * (F[6][7] * D[7][9] + F[6][8] * D[8][9] + F[6][9] * D[9][9] + F[6][10] * D[9][10] + F[6][11] * D[9][11] + F[6][12] * D[9][12]) + F[6][10] * (F[6][7] * D[7][10] + F[6][8] * D[8][10] + F[6][9] * D[9][10] + F[6][10] * D[10][10] + F[6][11] * D[10][11] + F[6][12] * D[10][12]) + F[6][11] * (F[6][7] * D[7][11] + F[6][8] * D[8][11] + F[6][9] * D[9][11] + F[6][10] * D[10][11] + F[6][11] * D[11][11] + F[6][12] * D[11][12]) + F[6][12] * (F[6][7] * D[7][12] + F[6][8] * D[8][12] + F[6][9] * D[9][12] + F[6][10] * D[10][12] + F[6][11] * D[11][12] + F[6][12] * D[12][12])) * Tsq + (2 * F[6][7] * D[6][7] + 2 * F[6][8] * D[6][8] + 2 * F[6][9] * D[6][9] + 2 * F[6][10] * D[6][10] + 2 * F[6][11] * D[6][11] + 2 * F[6][12] * D[6][12]) * T + D[6][6]; + P[6][7] = P[7][6] = (F[7][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] + F[6][9] * D[6][9] + F[6][10] * D[6][10] + F[6][11] * D[6][11] + F[6][12] * D[6][12]) + F[7][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] + F[6][9] * D[8][9] + F[6][10] * D[8][10] + F[6][11] * D[8][11] + F[6][12] * D[8][12]) + F[7][9] * (F[6][7] * D[7][9] + F[6][8] * D[8][9] + F[6][9] * D[9][9] + F[6][10] * D[9][10] + F[6][11] * D[9][11] + F[6][12] * D[9][12]) + F[7][10] * (F[6][7] * D[7][10] + F[6][8] * D[8][10] + F[6][9] * D[9][10] + F[6][10] * D[10][10] + F[6][11] * D[10][11] + F[6][12] * D[10][12]) + F[7][11] * (F[6][7] * D[7][11] + F[6][8] * D[8][11] + F[6][9] * D[9][11] + F[6][10] * D[10][11] + F[6][11] * D[11][11] + F[6][12] * D[11][12]) + F[7][12] * (F[6][7] * D[7][12] + F[6][8] * D[8][12] + F[6][9] * D[9][12] + F[6][10] * D[10][12] + F[6][11] * D[11][12] + F[6][12] * D[12][12]) + G[6][0] * G[7][0] * Q[0] + G[6][1] * G[7][1] * Q[1] + G[6][2] * G[7][2] * Q[2]) * Tsq + (F[7][6] * D[6][6] + F[6][7] * D[7][7] + F[6][8] * D[7][8] + F[7][8] * D[6][8] + F[6][9] * D[7][9] + F[7][9] * D[6][9] + F[6][10] * D[7][10] + F[7][10] * D[6][10] + F[6][11] * D[7][11] + F[7][11] * D[6][11] + F[6][12] * D[7][12] + F[7][12] * D[6][12]) * T + D[6][7]; + P[6][8] = P[8][6] = (F[8][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] + F[6][9] * D[6][9] + F[6][10] * D[6][10] + F[6][11] * D[6][11] + F[6][12] * D[6][12]) + F[8][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] + F[6][9] * D[7][9] + F[6][10] * D[7][10] + F[6][11] * D[7][11] + F[6][12] * D[7][12]) + F[8][9] * (F[6][7] * D[7][9] + F[6][8] * D[8][9] + F[6][9] * D[9][9] + F[6][10] * D[9][10] + F[6][11] * D[9][11] + F[6][12] * D[9][12]) + F[8][10] * (F[6][7] * D[7][10] + F[6][8] * D[8][10] + F[6][9] * D[9][10] + F[6][10] * D[10][10] + F[6][11] * D[10][11] + F[6][12] * D[10][12]) + F[8][11] * (F[6][7] * D[7][11] + F[6][8] * D[8][11] + F[6][9] * D[9][11] + F[6][10] * D[10][11] + F[6][11] * D[11][11] + F[6][12] * D[11][12]) + F[8][12] * (F[6][7] * D[7][12] + F[6][8] * D[8][12] + F[6][9] * D[9][12] + F[6][10] * D[10][12] + F[6][11] * D[11][12] + F[6][12] * D[12][12]) + G[6][0] * G[8][0] * Q[0] + G[6][1] * G[8][1] * Q[1] + G[6][2] * G[8][2] * Q[2]) * Tsq + (F[6][7] * D[7][8] + F[8][6] * D[6][6] + F[8][7] * D[6][7] + F[6][8] * D[8][8] + F[6][9] * D[8][9] + F[8][9] * D[6][9] + F[6][10] * D[8][10] + F[8][10] * D[6][10] + F[6][11] * D[8][11] + F[8][11] * D[6][11] + F[6][12] * D[8][12] + F[8][12] * D[6][12]) * T + D[6][8]; + P[6][9] = P[9][6] = (F[9][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] + F[6][9] * D[6][9] + F[6][10] * D[6][10] + F[6][11] * D[6][11] + F[6][12] * D[6][12]) + F[9][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] + F[6][9] * D[7][9] + F[6][10] * D[7][10] + F[6][11] * D[7][11] + F[6][12] * D[7][12]) + F[9][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] + F[6][9] * D[8][9] + F[6][10] * D[8][10] + F[6][11] * D[8][11] + F[6][12] * D[8][12]) + F[9][10] * (F[6][7] * D[7][10] + F[6][8] * D[8][10] + F[6][9] * D[9][10] + F[6][10] * D[10][10] + F[6][11] * D[10][11] + F[6][12] * D[10][12]) + F[9][11] * (F[6][7] * D[7][11] + F[6][8] * D[8][11] + F[6][9] * D[9][11] + F[6][10] * D[10][11] + F[6][11] * D[11][11] + F[6][12] * D[11][12]) + F[9][12] * (F[6][7] * D[7][12] + F[6][8] * D[8][12] + F[6][9] * D[9][12] + F[6][10] * D[10][12] + F[6][11] * D[11][12] + F[6][12] * D[12][12]) + G[6][0] * G[9][0] * Q[0] + G[6][1] * G[9][1] * Q[1] + G[6][2] * G[9][2] * Q[2]) * Tsq + (F[9][6] * D[6][6] + F[9][7] * D[6][7] + F[9][8] * D[6][8] + F[6][7] * D[7][9] + F[6][8] * D[8][9] + F[6][9] * D[9][9] + F[6][10] * D[9][10] + F[9][10] * D[6][10] + F[6][11] * D[9][11] + F[9][11] * D[6][11] + F[6][12] * D[9][12] + F[9][12] * D[6][12]) * T + D[6][9]; + P[6][10] = P[10][6] = (F[6][7] * D[7][10] + F[6][8] * D[8][10] + F[6][9] * D[9][10] + F[6][10] * D[10][10] + F[6][11] * D[10][11] + F[6][12] * D[10][12]) * T + D[6][10]; + P[6][11] = P[11][6] = (F[6][7] * D[7][11] + F[6][8] * D[8][11] + F[6][9] * D[9][11] + F[6][10] * D[10][11] + F[6][11] * D[11][11] + F[6][12] * D[11][12]) * T + D[6][11]; + P[6][12] = P[12][6] = (F[6][7] * D[7][12] + F[6][8] * D[8][12] + F[6][9] * D[9][12] + F[6][10] * D[10][12] + F[6][11] * D[11][12] + F[6][12] * D[12][12]) * T + D[6][12]; + P[6][13] = P[13][6] = (F[6][7] * D[7][13] + F[6][8] * D[8][13] + F[6][9] * D[9][13] + F[6][10] * D[10][13] + F[6][11] * D[11][13] + F[6][12] * D[12][13]) * T + D[6][13]; + P[7][7] = (Q[0] * G[7][0] * G[7][0] + Q[1] * G[7][1] * G[7][1] + Q[2] * G[7][2] * G[7][2] + F[7][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] + F[7][9] * D[6][9] + F[7][10] * D[6][10] + F[7][11] * D[6][11] + F[7][12] * D[6][12]) + F[7][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] + F[7][9] * D[8][9] + F[7][10] * D[8][10] + F[7][11] * D[8][11] + F[7][12] * D[8][12]) + F[7][9] * (F[7][6] * D[6][9] + F[7][8] * D[8][9] + F[7][9] * D[9][9] + F[7][10] * D[9][10] + F[7][11] * D[9][11] + F[7][12] * D[9][12]) + F[7][10] * (F[7][6] * D[6][10] + F[7][8] * D[8][10] + F[7][9] * D[9][10] + F[7][10] * D[10][10] + F[7][11] * D[10][11] + F[7][12] * D[10][12]) + F[7][11] * (F[7][6] * D[6][11] + F[7][8] * D[8][11] + F[7][9] * D[9][11] + F[7][10] * D[10][11] + F[7][11] * D[11][11] + F[7][12] * D[11][12]) + F[7][12] * (F[7][6] * D[6][12] + F[7][8] * D[8][12] + F[7][9] * D[9][12] + F[7][10] * D[10][12] + F[7][11] * D[11][12] + F[7][12] * D[12][12])) * Tsq + (2 * F[7][6] * D[6][7] + 2 * F[7][8] * D[7][8] + 2 * F[7][9] * D[7][9] + 2 * F[7][10] * D[7][10] + 2 * F[7][11] * D[7][11] + 2 * F[7][12] * D[7][12]) * T + D[7][7]; + P[7][8] = P[8][7] = (F[8][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] + F[7][9] * D[6][9] + F[7][10] * D[6][10] + F[7][11] * D[6][11] + F[7][12] * D[6][12]) + F[8][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] + F[7][9] * D[7][9] + F[7][10] * D[7][10] + F[7][11] * D[7][11] + F[7][12] * D[7][12]) + F[8][9] * (F[7][6] * D[6][9] + F[7][8] * D[8][9] + F[7][9] * D[9][9] + F[7][10] * D[9][10] + F[7][11] * D[9][11] + F[7][12] * D[9][12]) + F[8][10] * (F[7][6] * D[6][10] + F[7][8] * D[8][10] + F[7][9] * D[9][10] + F[7][10] * D[10][10] + F[7][11] * D[10][11] + F[7][12] * D[10][12]) + F[8][11] * (F[7][6] * D[6][11] + F[7][8] * D[8][11] + F[7][9] * D[9][11] + F[7][10] * D[10][11] + F[7][11] * D[11][11] + F[7][12] * D[11][12]) + F[8][12] * (F[7][6] * D[6][12] + F[7][8] * D[8][12] + F[7][9] * D[9][12] + F[7][10] * D[10][12] + F[7][11] * D[11][12] + F[7][12] * D[12][12]) + G[7][0] * G[8][0] * Q[0] + G[7][1] * G[8][1] * Q[1] + G[7][2] * G[8][2] * Q[2]) * Tsq + (F[7][6] * D[6][8] + F[8][6] * D[6][7] + F[8][7] * D[7][7] + F[7][8] * D[8][8] + F[7][9] * D[8][9] + F[8][9] * D[7][9] + F[7][10] * D[8][10] + F[8][10] * D[7][10] + F[7][11] * D[8][11] + F[8][11] * D[7][11] + F[7][12] * D[8][12] + F[8][12] * D[7][12]) * T + D[7][8]; + P[7][9] = P[9][7] = (F[9][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] + F[7][9] * D[6][9] + F[7][10] * D[6][10] + F[7][11] * D[6][11] + F[7][12] * D[6][12]) + F[9][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] + F[7][9] * D[7][9] + F[7][10] * D[7][10] + F[7][11] * D[7][11] + F[7][12] * D[7][12]) + F[9][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] + F[7][9] * D[8][9] + F[7][10] * D[8][10] + F[7][11] * D[8][11] + F[7][12] * D[8][12]) + F[9][10] * (F[7][6] * D[6][10] + F[7][8] * D[8][10] + F[7][9] * D[9][10] + F[7][10] * D[10][10] + F[7][11] * D[10][11] + F[7][12] * D[10][12]) + F[9][11] * (F[7][6] * D[6][11] + F[7][8] * D[8][11] + F[7][9] * D[9][11] + F[7][10] * D[10][11] + F[7][11] * D[11][11] + F[7][12] * D[11][12]) + F[9][12] * (F[7][6] * D[6][12] + F[7][8] * D[8][12] + F[7][9] * D[9][12] + F[7][10] * D[10][12] + F[7][11] * D[11][12] + F[7][12] * D[12][12]) + G[7][0] * G[9][0] * Q[0] + G[7][1] * G[9][1] * Q[1] + G[7][2] * G[9][2] * Q[2]) * Tsq + (F[9][6] * D[6][7] + F[9][7] * D[7][7] + F[9][8] * D[7][8] + F[7][6] * D[6][9] + F[7][8] * D[8][9] + F[7][9] * D[9][9] + F[7][10] * D[9][10] + F[9][10] * D[7][10] + F[7][11] * D[9][11] + F[9][11] * D[7][11] + F[7][12] * D[9][12] + F[9][12] * D[7][12]) * T + D[7][9]; + P[7][10] = P[10][7] = (F[7][6] * D[6][10] + F[7][8] * D[8][10] + F[7][9] * D[9][10] + F[7][10] * D[10][10] + F[7][11] * D[10][11] + F[7][12] * D[10][12]) * T + D[7][10]; + P[7][11] = P[11][7] = (F[7][6] * D[6][11] + F[7][8] * D[8][11] + F[7][9] * D[9][11] + F[7][10] * D[10][11] + F[7][11] * D[11][11] + F[7][12] * D[11][12]) * T + D[7][11]; + P[7][12] = P[12][7] = (F[7][6] * D[6][12] + F[7][8] * D[8][12] + F[7][9] * D[9][12] + F[7][10] * D[10][12] + F[7][11] * D[11][12] + F[7][12] * D[12][12]) * T + D[7][12]; + P[7][13] = P[13][7] = (F[7][6] * D[6][13] + F[7][8] * D[8][13] + F[7][9] * D[9][13] + F[7][10] * D[10][13] + F[7][11] * D[11][13] + F[7][12] * D[12][13]) * T + D[7][13]; + P[8][8] = (Q[0] * G[8][0] * G[8][0] + Q[1] * G[8][1] * G[8][1] + Q[2] * G[8][2] * G[8][2] + F[8][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] + F[8][9] * D[6][9] + F[8][10] * D[6][10] + F[8][11] * D[6][11] + F[8][12] * D[6][12]) + F[8][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] + F[8][9] * D[7][9] + F[8][10] * D[7][10] + F[8][11] * D[7][11] + F[8][12] * D[7][12]) + F[8][9] * (F[8][6] * D[6][9] + F[8][7] * D[7][9] + F[8][9] * D[9][9] + F[8][10] * D[9][10] + F[8][11] * D[9][11] + F[8][12] * D[9][12]) + F[8][10] * (F[8][6] * D[6][10] + F[8][7] * D[7][10] + F[8][9] * D[9][10] + F[8][10] * D[10][10] + F[8][11] * D[10][11] + F[8][12] * D[10][12]) + F[8][11] * (F[8][6] * D[6][11] + F[8][7] * D[7][11] + F[8][9] * D[9][11] + F[8][10] * D[10][11] + F[8][11] * D[11][11] + F[8][12] * D[11][12]) + F[8][12] * (F[8][6] * D[6][12] + F[8][7] * D[7][12] + F[8][9] * D[9][12] + F[8][10] * D[10][12] + F[8][11] * D[11][12] + F[8][12] * D[12][12])) * Tsq + (2 * F[8][6] * D[6][8] + 2 * F[8][7] * D[7][8] + 2 * F[8][9] * D[8][9] + 2 * F[8][10] * D[8][10] + 2 * F[8][11] * D[8][11] + 2 * F[8][12] * D[8][12]) * T + D[8][8]; + P[8][9] = P[9][8] = (F[9][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] + F[8][9] * D[6][9] + F[8][10] * D[6][10] + F[8][11] * D[6][11] + F[8][12] * D[6][12]) + F[9][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] + F[8][9] * D[7][9] + F[8][10] * D[7][10] + F[8][11] * D[7][11] + F[8][12] * D[7][12]) + F[9][8] * (F[8][6] * D[6][8] + F[8][7] * D[7][8] + F[8][9] * D[8][9] + F[8][10] * D[8][10] + F[8][11] * D[8][11] + F[8][12] * D[8][12]) + F[9][10] * (F[8][6] * D[6][10] + F[8][7] * D[7][10] + F[8][9] * D[9][10] + F[8][10] * D[10][10] + F[8][11] * D[10][11] + F[8][12] * D[10][12]) + F[9][11] * (F[8][6] * D[6][11] + F[8][7] * D[7][11] + F[8][9] * D[9][11] + F[8][10] * D[10][11] + F[8][11] * D[11][11] + F[8][12] * D[11][12]) + F[9][12] * (F[8][6] * D[6][12] + F[8][7] * D[7][12] + F[8][9] * D[9][12] + F[8][10] * D[10][12] + F[8][11] * D[11][12] + F[8][12] * D[12][12]) + G[8][0] * G[9][0] * Q[0] + G[8][1] * G[9][1] * Q[1] + G[8][2] * G[9][2] * Q[2]) * Tsq + (F[9][6] * D[6][8] + F[9][7] * D[7][8] + F[9][8] * D[8][8] + F[8][6] * D[6][9] + F[8][7] * D[7][9] + F[8][9] * D[9][9] + F[8][10] * D[9][10] + F[9][10] * D[8][10] + F[8][11] * D[9][11] + F[9][11] * D[8][11] + F[8][12] * D[9][12] + F[9][12] * D[8][12]) * T + D[8][9]; + P[8][10] = P[10][8] = (F[8][6] * D[6][10] + F[8][7] * D[7][10] + F[8][9] * D[9][10] + F[8][10] * D[10][10] + F[8][11] * D[10][11] + F[8][12] * D[10][12]) * T + D[8][10]; + P[8][11] = P[11][8] = (F[8][6] * D[6][11] + F[8][7] * D[7][11] + F[8][9] * D[9][11] + F[8][10] * D[10][11] + F[8][11] * D[11][11] + F[8][12] * D[11][12]) * T + D[8][11]; + P[8][12] = P[12][8] = (F[8][6] * D[6][12] + F[8][7] * D[7][12] + F[8][9] * D[9][12] + F[8][10] * D[10][12] + F[8][11] * D[11][12] + F[8][12] * D[12][12]) * T + D[8][12]; + P[8][13] = P[13][8] = (F[8][6] * D[6][13] + F[8][7] * D[7][13] + F[8][9] * D[9][13] + F[8][10] * D[10][13] + F[8][11] * D[11][13] + F[8][12] * D[12][13]) * T + D[8][13]; + P[9][9] = (Q[0] * G[9][0] * G[9][0] + Q[1] * G[9][1] * G[9][1] + Q[2] * G[9][2] * G[9][2] + F[9][6] * (F[9][6] * D[6][6] + F[9][7] * D[6][7] + F[9][8] * D[6][8] + F[9][10] * D[6][10] + F[9][11] * D[6][11] + F[9][12] * D[6][12]) + F[9][7] * (F[9][6] * D[6][7] + F[9][7] * D[7][7] + F[9][8] * D[7][8] + F[9][10] * D[7][10] + F[9][11] * D[7][11] + F[9][12] * D[7][12]) + F[9][8] * (F[9][6] * D[6][8] + F[9][7] * D[7][8] + F[9][8] * D[8][8] + F[9][10] * D[8][10] + F[9][11] * D[8][11] + F[9][12] * D[8][12]) + F[9][10] * (F[9][6] * D[6][10] + F[9][7] * D[7][10] + F[9][8] * D[8][10] + F[9][10] * D[10][10] + F[9][11] * D[10][11] + F[9][12] * D[10][12]) + F[9][11] * (F[9][6] * D[6][11] + F[9][7] * D[7][11] + F[9][8] * D[8][11] + F[9][10] * D[10][11] + F[9][11] * D[11][11] + F[9][12] * D[11][12]) + F[9][12] * (F[9][6] * D[6][12] + F[9][7] * D[7][12] + F[9][8] * D[8][12] + F[9][10] * D[10][12] + F[9][11] * D[11][12] + F[9][12] * D[12][12])) * Tsq + (2 * F[9][6] * D[6][9] + 2 * F[9][7] * D[7][9] + 2 * F[9][8] * D[8][9] + 2 * F[9][10] * D[9][10] + 2 * F[9][11] * D[9][11] + 2 * F[9][12] * D[9][12]) * T + D[9][9]; + P[9][10] = P[10][9] = (F[9][6] * D[6][10] + F[9][7] * D[7][10] + F[9][8] * D[8][10] + F[9][10] * D[10][10] + F[9][11] * D[10][11] + F[9][12] * D[10][12]) * T + D[9][10]; + P[9][11] = P[11][9] = (F[9][6] * D[6][11] + F[9][7] * D[7][11] + F[9][8] * D[8][11] + F[9][10] * D[10][11] + F[9][11] * D[11][11] + F[9][12] * D[11][12]) * T + D[9][11]; + P[9][12] = P[12][9] = (F[9][6] * D[6][12] + F[9][7] * D[7][12] + F[9][8] * D[8][12] + F[9][10] * D[10][12] + F[9][11] * D[11][12] + F[9][12] * D[12][12]) * T + D[9][12]; + P[9][13] = P[13][9] = (F[9][6] * D[6][13] + F[9][7] * D[7][13] + F[9][8] * D[8][13] + F[9][10] * D[10][13] + F[9][11] * D[11][13] + F[9][12] * D[12][13]) * T + D[9][13]; + P[10][10] = Q[6] * Tsq + D[10][10]; + P[10][11] = P[11][10] = D[10][11]; + P[10][12] = P[12][10] = D[10][12]; + P[10][13] = P[13][10] = D[10][13]; + P[11][11] = Q[7] * Tsq + D[11][11]; + P[11][12] = P[12][11] = D[11][12]; + P[11][13] = P[13][11] = D[11][13]; + P[12][12] = Q[8] * Tsq + D[12][12]; + P[12][13] = P[13][12] = D[12][13]; + P[13][13] = Q[9] * Tsq + D[13][13]; +} +#endif /* ifdef COVARIANCE_PREDICTION_GENERAL */ + +// ************* SerialUpdate ******************* +// Does the update step of the Kalman filter for the covariance and estimate +// Outputs are Xnew & Pnew, and are written over P and X +// Z is actual measurement, Y is predicted measurement +// Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P, +// where K=P*H'*inv[H*P*H'+R] +// NOTE the algorithm assumes R (measurement covariance matrix) is diagonal +// i.e. the measurment noises are uncorrelated. +// It therefore uses a serial update that requires no matrix inversion by +// processing the measurements one at a time. +// Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253 +// - or see Simon, "Optimal State Estimation," 1st Ed, p.150 +// The SensorsUsed variable is a bitwise mask indicating which sensors +// should be used in the update. +// ************************************************ + +void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV], + float Y[NUMV], float P[NUMX][NUMX], float X[NUMX], + uint16_t SensorsUsed) +{ + float HP[NUMX], HPHR, Error; + uint8_t i, j, k, m; + + // Iterate through all the possible measurements and apply the + // appropriate corrections + for (m = 0; m < NUMV; m++) { + if (SensorsUsed & (0x01 << m)) { // use this sensor for update + for (j = 0; j < NUMX; j++) { // Find Hp = H*P + HP[j] = 0.0f; + for (k = 0; k < NUMX; k++) { + HP[j] += H[m][k] * P[k][j]; + } + } + HPHR = R[m]; // Find HPHR = H*P*H' + R + for (k = 0; k < NUMX; k++) { + HPHR += HP[k] * H[m][k]; + } + + for (k = 0; k < NUMX; k++) { + ekf.K[k][m] = HP[k] / HPHR; // find K = HP/HPHR + } + for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP + for (j = i; j < NUMX; j++) { + P[i][j] = P[j][i] = + P[i][j] - ekf.K[i][m] * HP[j]; + } + } + + Error = Z[m] - Y[m]; + for (i = 0; i < NUMX; i++) { // Find X(m)= X(m-1) + K*Error + X[i] = X[i] + ekf.K[i][m] * Error; + } + } + } + + INSLimitBias(); +} + +// ************* RungeKutta ********************** +// Does a 4th order Runge Kutta numerical integration step +// Output, Xnew, is written over X +// NOTE the algorithm assumes time invariant state equations and +// constant inputs over integration step +// ************************************************ + +void RungeKutta(float X[NUMX], float U[NUMU], float dT) +{ + float dT2 = + dT / 2.0f, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX]; + uint8_t i; + + for (i = 0; i < NUMX; i++) { + Xlast[i] = X[i]; // make a working copy + } + StateEq(X, U, K1); // k1 = f(x,u) + for (i = 0; i < NUMX; i++) { + X[i] = Xlast[i] + dT2 * K1[i]; + } + StateEq(X, U, K2); // k2 = f(x+0.5*dT*k1,u) + for (i = 0; i < NUMX; i++) { + X[i] = Xlast[i] + dT2 * K2[i]; + } + StateEq(X, U, K3); // k3 = f(x+0.5*dT*k2,u) + for (i = 0; i < NUMX; i++) { + X[i] = Xlast[i] + dT * K3[i]; + } + StateEq(X, U, K4); // k4 = f(x+dT*k3,u) + + // Xnew = X + dT*(k1+2*k2+2*k3+k4)/6 + for (i = 0; i < NUMX; i++) { + X[i] = + Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] + + K4[i]) / 6.0f; + } +} + +// ************* Model Specific Stuff *************************** +// *** StateEq, MeasurementEq, LinerizeFG, and LinearizeH ******** +// +// State Variables = [Pos Vel Quaternion GyroBias AccelBias] +// Deterministic Inputs = [AngularVel Accel] +// Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise AccelRandomWalkNoise] +// +// Measurement Variables = [Pos Vel BodyFrameMagField Altimeter] +// Inputs to Measurement = [EarthFrameMagField] +// +// Notes: Pos and Vel in earth frame +// AngularVel and Accel in body frame +// MagFields are unit vectors +// Xdot is output of StateEq() +// F and G are outputs of LinearizeFG(), all elements not set should be zero +// y is output of OutputEq() +// H is output of LinearizeH(), all elements not set should be zero +// ************************************************ + +void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]) +{ + const float wx = U[0] - X[10]; + const float wy = U[1] - X[11]; + const float wz = U[2] - X[12]; // subtract the biases on gyros + const float ax = U[3]; + const float ay = U[4]; + const float az = U[5] - X[13]; // subtract the biases on accels + const float q0 = X[6]; + const float q1 = X[7]; + const float q2 = X[8]; + const float q3 = X[9]; + + // Pdot = V + Xdot[0] = X[3]; + Xdot[1] = X[4]; + Xdot[2] = X[5]; + + // Vdot = Reb*a + Xdot[3] = + (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2.0f * (q1 * q2 - + q0 * q3) * + ay + 2.0f * (q1 * q3 + q0 * q2) * az; + Xdot[4] = + 2.0f * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 - + q3 * q3) * ay + 2.0f * (q2 * q3 - + q0 * q1) * + az; + Xdot[5] = + 2.0f * (q1 * q3 - q0 * q2) * ax + 2.0f * (q2 * q3 + q0 * q1) * ay + + (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + PIOS_CONST_MKS_GRAV_ACCEL_F; + + // qdot = Q*w + Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2.0f; + Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2.0f; + Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2.0f; + Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2.0f; + + // best guess is that bias stays constant + Xdot[10] = Xdot[11] = Xdot[12] = 0; + + // For accels to make sure things stay stable, assume bias always walks weakly + // towards zero for the horizontal axis. This prevents drifting around an + // unobservable manifold of possible attitudes and gyro biases. The z-axis + // we assume no drift because this is the one we want to estimate most accurately. + Xdot[13] = 0.0f; +} + +/** + * Linearize the state equations around the current state estimate. + * @param[in] X the current state estimate + * @param[in] U the control inputs + * @param[out] F the linearized natural dynamics + * @param[out] G the linearized influence of disturbance model + * + * so the prediction of the next state is + * Xdot = F * X + G * U + * where X is the current state and U is the current input + * + * For reference the state order (in F) is pos, vel, attitude, gyro bias, accel bias + * and the input order is gyro, bias + */ +void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX], + float G[NUMX][NUMW]) +{ + const float wx = U[0] - X[10]; + const float wy = U[1] - X[11]; + const float wz = U[2] - X[12]; // subtract the biases on gyros + const float ax = U[3]; + const float ay = U[4]; + const float az = U[5] - X[13]; // subtract the biases on accels + const float q0 = X[6]; + const float q1 = X[7]; + const float q2 = X[8]; + const float q3 = X[9]; + + // Pdot = V + F[0][3] = F[1][4] = F[2][5] = 1.0f; + + // dVdot/dq + F[3][6] = 2.0f * (q0 * ax - q3 * ay + q2 * az); + F[3][7] = 2.0f * (q1 * ax + q2 * ay + q3 * az); + F[3][8] = 2.0f * (-q2 * ax + q1 * ay + q0 * az); + F[3][9] = 2.0f * (-q3 * ax - q0 * ay + q1 * az); + F[4][6] = 2.0f * (q3 * ax + q0 * ay - q1 * az); + F[4][7] = 2.0f * (q2 * ax - q1 * ay - q0 * az); + F[4][8] = 2.0f * (q1 * ax + q2 * ay + q3 * az); + F[4][9] = 2.0f * (q0 * ax - q3 * ay + q2 * az); + F[5][6] = 2.0f * (-q2 * ax + q1 * ay + q0 * az); + F[5][7] = 2.0f * (q3 * ax + q0 * ay - q1 * az); + F[5][8] = 2.0f * (-q0 * ax + q3 * ay - q2 * az); + F[5][9] = 2.0f * (q1 * ax + q2 * ay + q3 * az); + + // dVdot/dabias & dVdot/dna - the equations for how the accel input and accel bias influence velocity are the same + F[3][13] = G[3][5] = -2.0f * (q1 * q3 + q0 * q2); + F[4][13] = G[4][5] = 2.0f * (-q2 * q3 + q0 * q1); + F[5][13] = G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3; + + // dqdot/dq + F[6][6] = 0; + F[6][7] = -wx / 2.0f; + F[6][8] = -wy / 2.0f; + F[6][9] = -wz / 2.0f; + F[7][6] = wx / 2.0f; + F[7][7] = 0; + F[7][8] = wz / 2.0f; + F[7][9] = -wy / 2.0f; + F[8][6] = wy / 2.0f; + F[8][7] = -wz / 2.0f; + F[8][8] = 0; + F[8][9] = wx / 2.0f; + F[9][6] = wz / 2.0f; + F[9][7] = wy / 2.0f; + F[9][8] = -wx / 2.0f; + F[9][9] = 0; + + // dqdot/dwbias + F[6][10] = q1 / 2.0f; + F[6][11] = q2 / 2.0f; + F[6][12] = q3 / 2.0f; + F[7][10] = -q0 / 2.0f; + F[7][11] = q3 / 2.0f; + F[7][12] = -q2 / 2.0f; + F[8][10] = -q3 / 2.0f; + F[8][11] = -q0 / 2.0f; + F[8][12] = q1 / 2.0f; + F[9][10] = q2 / 2.0f; + F[9][11] = -q1 / 2.0f; + F[9][12] = -q0 / 2.0f; + + // dVdot/dna + G[3][3] = -q0 * q0 - q1 * q1 + q2 * q2 + q3 * q3; G[3][4] = 2 * (-q1 * q2 + q0 * q3); G[3][5] = -2 * (q1 * q3 + q0 * q2); + G[4][3] = -2 * (q1 * q2 + q0 * q3); G[4][4] = -q0 * q0 + q1 * q1 - q2 * q2 + q3 * q3; G[4][5] = 2 * (-q2 * q3 + q0 * q1); + G[5][3] = 2 * (-q1 * q3 + q0 * q2); G[5][4] = -2 * (q2 * q3 + q0 * q1); G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3; + + // dqdot/dnw + G[6][0] = q1 / 2.0f; + G[6][1] = q2 / 2.0f; + G[6][2] = q3 / 2.0f; + G[7][0] = -q0 / 2.0f; + G[7][1] = q3 / 2.0f; + G[7][2] = -q2 / 2.0f; + G[8][0] = -q3 / 2.0f; + G[8][1] = -q0 / 2.0f; + G[8][2] = q1 / 2.0f; + G[9][0] = q2 / 2.0f; + G[9][1] = -q1 / 2.0f; + G[9][2] = -q0 / 2.0f; +} + +/** + * Predicts the measurements from the current state. Note + * that this is very similar to @ref LinearizeH except this + * directly computes the outputs instead of a matrix that + * you transform the state by + */ +void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]) +{ + const float q0 = X[6]; + const float q1 = X[7]; + const float q2 = X[8]; + const float q3 = X[9]; + + // first six outputs are P and V + Y[0] = X[0]; + Y[1] = X[1]; + Y[2] = X[2]; + Y[3] = X[3]; + Y[4] = X[4]; + Y[5] = X[5]; + + // Rotate Be by only the yaw heading + const float a1 = 2 * q0 * q3 + 2 * q1 * q2; + const float a2 = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3; + const float r = sqrtf(a1 * a1 + a2 * a2); + const float cP = a2 / r; + const float sP = a1 / r; + Y[6] = Be[0] * cP + Be[1] * sP; + Y[7] = -Be[0] * sP + Be[1] * cP; + Y[8] = 0; // don't care + + // Alt = -Pz + Y[9] = X[2] * -1.0f; +} + +/** + * Linearize the measurement around the current state estiamte + * so the predicted measurements are + * Z = H * X + */ +void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]) +{ + const float q0 = X[6]; + const float q1 = X[7]; + const float q2 = X[8]; + const float q3 = X[9]; + + // dP/dP=I; (expect position to measure the position) + H[0][0] = H[1][1] = H[2][2] = 1.0f; + // dV/dV=I; (expect velocity to measure the velocity) + H[3][3] = H[4][4] = H[5][5] = 1.0f; + + // dBb/dq (expected magnetometer readings in the horizontal plane) + // these equations were generated by Rhb(q)*Be which is the matrix that + // rotates the earth magnetic field into the horizontal plane, and then + // taking the partial derivative wrt each term in q. Maniuplated in + // matlab symbolic toolbox + const float Be_0 = Be[0]; + const float Be_1 = Be[1]; + const float a1 = q0 * q3 * 2.0f + q1 * q2 * 2.0f; + const float a1s = a1 * a1; + const float a2 = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3; + const float a2s = a2 * a2; + const float a3 = 1.0f / powf(a1s + a2s, 3.0f / 2.0f) * (1.0f / 2.0f); + + const float k1 = 1.0f / sqrtf(a1s + a2s); + const float k3 = a3 * a2; + const float k4 = a2 * 4.0f; + const float k5 = a1 * 4.0f; + const float k6 = a3 * a1; + + H[6][6] = Be_0 * q0 * k1 * 2.0f + Be_1 * q3 * k1 * 2.0f - Be_0 * (q0 * k4 + q3 * k5) * k3 - Be_1 * (q0 * k4 + q3 * k5) * k6; + H[6][7] = Be_0 * q1 * k1 * 2.0f + Be_1 * q2 * k1 * 2.0f - Be_0 * (q1 * k4 + q2 * k5) * k3 - Be_1 * (q1 * k4 + q2 * k5) * k6; + H[6][8] = Be_0 * q2 * k1 * -2.0f + Be_1 * q1 * k1 * 2.0f + Be_0 * (q2 * k4 - q1 * k5) * k3 + Be_1 * (q2 * k4 - q1 * k5) * k6; + H[6][9] = Be_1 * q0 * k1 * 2.0f - Be_0 * q3 * k1 * 2.0f + Be_0 * (q3 * k4 - q0 * k5) * k3 + Be_1 * (q3 * k4 - q0 * k5) * k6; + H[7][6] = Be_1 * q0 * k1 * 2.0f - Be_0 * q3 * k1 * 2.0f - Be_1 * (q0 * k4 + q3 * k5) * k3 + Be_0 * (q0 * k4 + q3 * k5) * k6; + H[7][7] = Be_0 * q2 * k1 * -2.0f + Be_1 * q1 * k1 * 2.0f - Be_1 * (q1 * k4 + q2 * k5) * k3 + Be_0 * (q1 * k4 + q2 * k5) * k6; + H[7][8] = Be_0 * q1 * k1 * -2.0f - Be_1 * q2 * k1 * 2.0f + Be_1 * (q2 * k4 - q1 * k5) * k3 - Be_0 * (q2 * k4 - q1 * k5) * k6; + H[7][9] = Be_0 * q0 * k1 * -2.0f - Be_1 * q3 * k1 * 2.0f + Be_1 * (q3 * k4 - q0 * k5) * k3 - Be_0 * (q3 * k4 - q0 * k5) * k6; + H[8][6] = 0.0f; + H[8][7] = 0.0f; + H[8][9] = 0.0f; + + // dAlt/dPz = -1 (expected baro readings) + H[9][2] = -1.0f; +} + +/** + * @} + * @} + */ diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 8c019e38e..dff5572fb 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -81,6 +81,7 @@ int32_t configuration_check() switch (revoFusion) { case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR: case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13: + case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF: navCapableFusion = true; break; default: @@ -93,7 +94,7 @@ int32_t configuration_check() } } } -#else +#else /* ifdef REVOLUTION */ const bool navCapableFusion = false; #endif /* ifdef REVOLUTION */ diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 1fd036afd..b0cb714b3 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -71,87 +71,68 @@ struct data { bool inited; PiOSDeltatimeConfig dtconfig; + bool navOnly; }; -// Private variables -static bool initialized = 0; - // Private functions -static int32_t init13i(stateFilter *self); -static int32_t init13(stateFilter *self); -static int32_t maininit(stateFilter *self); +static int32_t init(stateFilter *self); static filterResult filter(stateFilter *self, stateEstimation *state); static inline bool invalid_var(float data); -static void globalInit(void); +static int32_t globalInit(stateFilter *handle, bool usePos, bool navOnly); -static void globalInit(void) +static int32_t globalInit(stateFilter *handle, bool usePos, bool navOnly) { - if (!initialized) { - initialized = 1; - EKFConfigurationInitialize(); - EKFStateVarianceInitialize(); - HomeLocationInitialize(); - } + handle->init = &init; + handle->filter = &filter; + handle->localdata = pios_malloc(sizeof(struct data)); + struct data *this = (struct data *)handle->localdata; + this->usePos = usePos; + this->navOnly = navOnly; + EKFConfigurationInitialize(); + EKFStateVarianceInitialize(); + HomeLocationInitialize(); + return STACK_REQUIRED; } int32_t filterEKF13iInitialize(stateFilter *handle) { - globalInit(); - handle->init = &init13i; - handle->filter = &filter; - handle->localdata = pios_malloc(sizeof(struct data)); - return STACK_REQUIRED; + return globalInit(handle, false, false); } + int32_t filterEKF13Initialize(stateFilter *handle) { - globalInit(); - handle->init = &init13; - handle->filter = &filter; - handle->localdata = pios_malloc(sizeof(struct data)); - return STACK_REQUIRED; + return globalInit(handle, true, false); } + +int32_t filterEKF13iNavOnlyInitialize(stateFilter *handle) +{ + return globalInit(handle, false, true); +} + +int32_t filterEKF13NavOnlyInitialize(stateFilter *handle) +{ + return globalInit(handle, true, true); +} + +#ifdef FALSE // XXX // TODO: Until the 16 state EKF is implemented, run 13 state, so compilation runs through // XXX int32_t filterEKF16iInitialize(stateFilter *handle) { - globalInit(); - handle->init = &init13i; - handle->filter = &filter; - handle->localdata = pios_malloc(sizeof(struct data)); - return STACK_REQUIRED; + return filterEKFi13Initialize(handle); } int32_t filterEKF16Initialize(stateFilter *handle) { - globalInit(); - handle->init = &init13; - handle->filter = &filter; - handle->localdata = pios_malloc(sizeof(struct data)); - return STACK_REQUIRED; + return filterEKF13Initialize(handle); } +#endif - -static int32_t init13i(stateFilter *self) -{ - struct data *this = (struct data *)self->localdata; - - this->usePos = 0; - return maininit(self); -} - -static int32_t init13(stateFilter *self) -{ - struct data *this = (struct data *)self->localdata; - - this->usePos = 1; - return maininit(self); -} - -static int32_t maininit(stateFilter *self) +static int32_t init(stateFilter *self) { struct data *this = (struct data *)self->localdata; @@ -202,8 +183,9 @@ static filterResult filter(stateFilter *self, stateEstimation *state) float dT; uint16_t sensors = 0; + INSSetArmed(state->armed); + state->navUsed = (this->usePos || this->navOnly); this->work.updated |= state->updated; - // check magnetometer alarm, discard any magnetometer readings if not OK // during initialization phase (but let them through afterwards) SystemAlarmsAlarmData alarms; @@ -241,10 +223,9 @@ static filterResult filter(stateFilter *self, stateEstimation *state) // Reset the INS algorithm INSGPSInit(); // variance is measured in mGaus, but internally the EKF works with a normalized vector. Scale down by Be^2 - float Be2 = this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2]; - INSSetMagVar((float[3]) { this->ekfConfiguration.R.MagX / Be2, - this->ekfConfiguration.R.MagY / Be2, - this->ekfConfiguration.R.MagZ / Be2 } + INSSetMagVar((float[3]) { this->ekfConfiguration.R.MagX, + this->ekfConfiguration.R.MagY, + this->ekfConfiguration.R.MagZ } ); INSSetAccelVar((float[3]) { this->ekfConfiguration.Q.AccelX, this->ekfConfiguration.Q.AccelY, @@ -303,13 +284,16 @@ static filterResult filter(stateFilter *self, stateEstimation *state) // Copy the attitude into the state // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true - state->attitude[0] = Nav.q[0]; - state->attitude[1] = Nav.q[1]; - state->attitude[2] = Nav.q[2]; - state->attitude[3] = Nav.q[3]; - state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]); - state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]); - state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]); + if (!this->navOnly) { + state->attitude[0] = Nav.q[0]; + state->attitude[1] = Nav.q[1]; + state->attitude[2] = Nav.q[2]; + state->attitude[3] = Nav.q[3]; + + state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]); + state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]); + state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]); + } state->pos[0] = Nav.Pos[0]; state->pos[1] = Nav.Pos[1]; state->pos[2] = Nav.Pos[2]; @@ -321,6 +305,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state) this->init_stage++; if (this->init_stage > 10) { + state->navOk = true; this->inited = true; } @@ -328,7 +313,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state) } if (!this->inited) { - return FILTERRESULT_CRITICAL; + return this->navOnly ? FILTERRESULT_OK : FILTERRESULT_CRITICAL; } float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) }; @@ -338,13 +323,20 @@ static filterResult filter(stateFilter *self, stateEstimation *state) // Copy the attitude into the state // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true - state->attitude[0] = Nav.q[0]; - state->attitude[1] = Nav.q[1]; - state->attitude[2] = Nav.q[2]; - state->attitude[3] = Nav.q[3]; - state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]); - state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]); - state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]); + if (!this->navOnly) { + state->attitude[0] = Nav.q[0]; + state->attitude[1] = Nav.q[1]; + state->attitude[2] = Nav.q[2]; + state->attitude[3] = Nav.q[3]; + state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]); + state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]); + state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]); + } + { + float tmp[3]; + Quaternion2RPY(Nav.q, tmp); + state->debugNavYaw = tmp[2]; + } state->pos[0] = Nav.Pos[0]; state->pos[1] = Nav.Pos[1]; state->pos[2] = Nav.Pos[2]; @@ -421,7 +413,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state) EKFStateVarianceData vardata; EKFStateVarianceGet(&vardata); - INSGetP(EKFStateVariancePToArray(vardata.P)); + INSGetVariance(EKFStateVariancePToArray(vardata.P)); EKFStateVarianceSet(&vardata); int t; for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) { @@ -436,7 +428,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state) this->work.updated = 0; if (this->init_stage < 0) { - return FILTERRESULT_WARNING; + return this->navOnly ? FILTERRESULT_OK : FILTERRESULT_WARNING; } else { return FILTERRESULT_OK; } diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h index 38d21e599..ad71c7c18 100644 --- a/flight/modules/StateEstimation/inc/stateestimation.h +++ b/flight/modules/StateEstimation/inc/stateestimation.h @@ -70,8 +70,12 @@ typedef struct { float airspeed[2]; float baro[1]; float auxMag[3]; - uint8_t magStatus; float boardMag[3]; + float debugNavYaw; + uint8_t magStatus; + bool navOk; + bool navUsed; + bool armed; sensorUpdates updated; } stateEstimation; @@ -94,6 +98,8 @@ int32_t filterCFInitialize(stateFilter *handle); int32_t filterCFMInitialize(stateFilter *handle); int32_t filterEKF13iInitialize(stateFilter *handle); int32_t filterEKF13Initialize(stateFilter *handle); +int32_t filterEKF13NavOnlyInitialize(stateFilter *handle); +int32_t filterEKF13iNavOnlyInitialize(stateFilter *handle); int32_t filterEKF16iInitialize(stateFilter *handle); int32_t filterEKF16Initialize(stateFilter *handle); diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 72d28fb00..ad06cba0e 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -157,6 +157,8 @@ static stateFilter cfFilter; static stateFilter cfmFilter; static stateFilter ekf13iFilter; static stateFilter ekf13Filter; +static stateFilter ekf13iNavFilter; +static stateFilter ekf13NavFilter; // this is a hack to provide a computational shortcut for faster gyro state progression static float gyroRaw[3]; @@ -251,6 +253,52 @@ static const filterPipeline *ekf13Queue = &(filterPipeline) { } }; +static const filterPipeline *ekf13NavCFAttQueue = &(filterPipeline) { + .filter = &magFilter, + .next = &(filterPipeline) { + .filter = &airFilter, + .next = &(filterPipeline) { + .filter = &llaFilter, + .next = &(filterPipeline) { + .filter = &baroFilter, + .next = &(filterPipeline) { + .filter = &ekf13NavFilter, + .next = &(filterPipeline) { + .filter = &velocityFilter, + .next = &(filterPipeline) { + .filter = &cfmFilter, + .next = NULL, + } + } + } + } + } + } +}; + +static const filterPipeline *ekf13iNavCFAttQueue = &(filterPipeline) { + .filter = &magFilter, + .next = &(filterPipeline) { + .filter = &airFilter, + .next = &(filterPipeline) { + .filter = &baroiFilter, + .next = &(filterPipeline) { + .filter = &stationaryFilter, + .next = &(filterPipeline) { + .filter = &ekf13iNavFilter, + .next = &(filterPipeline) { + .filter = &velocityFilter, + .next = &(filterPipeline) { + .filter = &cfmFilter, + .next = NULL, + } + } + } + } + } + } +}; + // Private functions static void settingsUpdatedCb(UAVObjEvent *objEv); @@ -320,6 +368,8 @@ int32_t StateEstimationInitialize(void) stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter)); stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter)); stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter)); + stack_required = maxint32_t(stack_required, filterEKF13NavOnlyInitialize(&ekf13NavFilter)); + stack_required = maxint32_t(stack_required, filterEKF13iNavOnlyInitialize(&ekf13iNavFilter)); stateEstimationCallback = PIOS_CALLBACKSCHEDULER_Create(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_STATEESTIMATION, stack_required); @@ -348,9 +398,11 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart); */ static void StateEstimationCb(void) { - static filterResult alarm = FILTERRESULT_OK; - static filterResult lastAlarm = FILTERRESULT_UNINITIALISED; - static uint16_t alarmcounter = 0; + static filterResult alarm = FILTERRESULT_OK; + static filterResult lastAlarm = FILTERRESULT_UNINITIALISED; + static bool lastNavStatus = false; + static uint16_t alarmcounter = 0; + static uint16_t navstatuscounter = 0; static const filterPipeline *current; static stateEstimation states; static uint32_t last_time; @@ -373,12 +425,13 @@ static void StateEstimationCb(void) } else { last_time = PIOS_DELAY_GetRaw(); } + FlightStatusArmedOptions fsarmed; + FlightStatusArmedGet(&fsarmed); + states.armed = fsarmed != FLIGHTSTATUS_ARMED_DISARMED; // check if a new filter chain should be initialized if (fusionAlgorithm != revoSettings.FusionAlgorithm) { - FlightStatusData fs; - FlightStatusGet(&fs); - if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) { + if (fsarmed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) { const filterPipeline *newFilterChain; switch ((RevoSettingsFusionAlgorithmOptions)revoSettings.FusionAlgorithm) { case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY: @@ -398,12 +451,21 @@ static void StateEstimationCb(void) case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13: newFilterChain = ekf13Queue; break; + case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF: + newFilterChain = ekf13NavCFAttQueue; + break; + case REVOSETTINGS_FUSIONALGORITHM_TESTINGINSINDOORCF: + newFilterChain = ekf13iNavCFAttQueue; + break; default: newFilterChain = NULL; } // initialize filters in chain current = newFilterChain; bool error = 0; + states.debugNavYaw = 0; + states.navOk = false; + states.navUsed = false; while (current != NULL) { int32_t result = current->filter->init((stateFilter *)current->filter); if (result != 0) { @@ -493,11 +555,12 @@ static void StateEstimationCb(void) if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \ AttitudeStateData s; AttitudeStateGet(&s); - s.q1 = states.attitude[0]; - s.q2 = states.attitude[1]; - s.q3 = states.attitude[2]; - s.q4 = states.attitude[3]; + s.q1 = states.attitude[0]; + s.q2 = states.attitude[1]; + s.q3 = states.attitude[2]; + s.q4 = states.attitude[3]; Quaternion2RPY(&s.q1, &s.Roll); + s.NavYaw = states.debugNavYaw; AttitudeStateSet(&s); } // throttle alarms, raise alarm flags immediately @@ -515,6 +578,18 @@ static void StateEstimationCb(void) } } + if (lastNavStatus < states.navOk) { + lastNavStatus = states.navOk; + navstatuscounter = 0; + } else { + if (navstatuscounter < 100) { + navstatuscounter++; + } else { + lastNavStatus = states.navOk; + navstatuscounter = 0; + } + } + // clear alarms if everything is alright, then schedule callback execution after timeout if (lastAlarm == FILTERRESULT_WARNING) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); @@ -526,6 +601,16 @@ static void StateEstimationCb(void) AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); } + if (!states.navUsed) { + AlarmsSet(SYSTEMALARMS_ALARM_NAV, SYSTEMALARMS_ALARM_UNINITIALISED); + } else { + if (states.navOk) { + AlarmsClear(SYSTEMALARMS_ALARM_NAV); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_NAV, SYSTEMALARMS_ALARM_CRITICAL); + } + } + if (updatedSensors) { PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback); } else { diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h index 8cc57cad5..8c89b13c5 100644 --- a/flight/pios/inc/pios_math.h +++ b/flight/pios/inc/pios_math.h @@ -79,7 +79,7 @@ #define MAX(a, b) ((a) > (b) ? (a) : (b)) #define MIN(a, b) ((a) < (b) ? (a) : (b)) -#define IS_REAL(f) (!isnan(f) && !isinf(f)) +#define IS_REAL(f) (isfinite(f)) // Bitfield access #define IS_SET(field, mask) (((field) & (mask)) == (mask)) diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index d9e9c67f4..41f1837d0 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -97,7 +97,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/paths.c SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c - SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/insgps14state.c SRC += $(FLIGHTLIB)/auxmagsupport.c SRC += $(FLIGHTLIB)/lednotification.c diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 5ee49d1a2..62e0dd156 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -95,7 +95,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/paths.c SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c - SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/insgps14state.c SRC += $(FLIGHTLIB)/auxmagsupport.c SRC += $(FLIGHTLIB)/lednotification.c SRC += $(FLIGHTLIB)/sha1.c diff --git a/flight/targets/boards/revonano/firmware/Makefile b/flight/targets/boards/revonano/firmware/Makefile index 1abf5b82a..4c473dbf9 100644 --- a/flight/targets/boards/revonano/firmware/Makefile +++ b/flight/targets/boards/revonano/firmware/Makefile @@ -94,7 +94,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/paths.c SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c - SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/insgps14state.c SRC += $(FLIGHTLIB)/lednotification.c SRC += $(FLIGHTLIB)/auxmagsupport.c ## UAVObjects diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index d8d5124c0..f40e1a308 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -92,7 +92,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/paths.c SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c - SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/insgps14state.c SRC += $(FLIGHTLIB)/auxmagsupport.c ## UAVObjects diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 1849b9740..033993773 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -106,7 +106,7 @@ SRC += $(FLIGHT_UAVOBJ_DIR)/uavobjectsinit.c SRC += $(FLIGHTLIB)/CoordinateConversions.c SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/WorldMagModel.c -SRC += $(FLIGHTLIB)/insgps13state.c +SRC += $(FLIGHTLIB)/insgps14state.c SRC += $(FLIGHTLIB)/paths.c SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/sanitycheck.c diff --git a/flight/targets/boards/sparky2/firmware/Makefile b/flight/targets/boards/sparky2/firmware/Makefile index 45c7f2891..942944f6d 100644 --- a/flight/targets/boards/sparky2/firmware/Makefile +++ b/flight/targets/boards/sparky2/firmware/Makefile @@ -95,7 +95,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/paths.c SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c - SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/insgps14state.c SRC += $(FLIGHTLIB)/auxmagsupport.c SRC += $(FLIGHTLIB)/lednotification.c SRC += $(FLIGHTLIB)/sha1.c diff --git a/ground/gcs/src/share/qml/js/uav.js b/ground/gcs/src/share/qml/js/uav.js index b92ed926f..967c5f8c7 100644 --- a/ground/gcs/src/share/qml/js/uav.js +++ b/ground/gcs/src/share/qml/js/uav.js @@ -278,7 +278,7 @@ function gpsStatus() { } function fusionAlgorithm() { - var fusionAlgorithmText = ["None", "Basic (No Nav)", "CompMag", "Comp+Mag+GPS", "EKFIndoor", "GPSNav (INS13)"]; + var fusionAlgorithmText = ["None", "Basic (No Nav)", "CompMag", "Comp+Mag+GPS", "EKFIndoor", "GPSNav (INS)", "GPSNav (INS+CF)", "Testing (INS Indoor+CF)"]; if (fusionAlgorithmText.length != RevoSettings.RevoSettingsConstants.FusionAlgorithmCount) { console.log("uav.js: fusionAlgorithm() do not match revoSettings.fusionAlgorithm uavo"); diff --git a/shared/uavobjectdefinition/attitudestate.xml b/shared/uavobjectdefinition/attitudestate.xml index 0d5dc0c08..045f8bc22 100644 --- a/shared/uavobjectdefinition/attitudestate.xml +++ b/shared/uavobjectdefinition/attitudestate.xml @@ -8,6 +8,7 @@ + diff --git a/shared/uavobjectdefinition/ekfconfiguration.xml b/shared/uavobjectdefinition/ekfconfiguration.xml index fee4f68e7..bf248a84b 100644 --- a/shared/uavobjectdefinition/ekfconfiguration.xml +++ b/shared/uavobjectdefinition/ekfconfiguration.xml @@ -2,9 +2,9 @@ Extended Kalman Filter initialisation PositionNorth @@ -23,8 +23,8 @@ GyroX @@ -39,9 +39,9 @@ GPSPosNorth diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 116b842cf..2cddb0f9d 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -2,7 +2,7 @@ Settings for the revo to control the algorithm and what is updated