diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index f52cb8e15d..6d8ae81132 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -72,6 +72,9 @@ void Ekf::fuseAirspeed() // Perform fusion of True Airspeed measurement if (v_tas_pred > 1.0f) { + // determine if we need the sideslip fusion to correct states other than wind + bool update_wind_only = !_is_dead_reckoning; + // Calculate the observation jacobian // intermediate variable from algebraic optimisation SH_TAS[0] = 1.0f/v_tas_pred; @@ -95,14 +98,25 @@ void Ekf::fuseAirspeed() } else { // Reset the estimator covarinace matrix _fault_status.flags.bad_airspeed = true; - initialiseCovariance(); - ECL_ERR("EKF airspeed fusion numerical error - covariance reset"); + + // if we are getting aiding from other sources, warn and reset the wind states and covariances only + if (update_wind_only) { + resetWindStates(); + resetWindCovariance(); + ECL_ERR("EKF airspeed fusion badly conditioned - wind covariance reset"); + + } else { + initialiseCovariance(); + _state.wind_vel.setZero(); + ECL_ERR("EKF airspeed fusion badly conditioned - full covariance reset"); + } + return; } SK_TAS[1] = SH_TAS[1]; - if (((_time_last_imu - _time_last_gps) < 1e6) || ((_time_last_imu - _time_last_ext_vision) < 1e6) || ((_time_last_imu - _time_last_optflow) < 1e6)) { + if (update_wind_only) { // If we are getting aiding from other sources, then don't allow the airspeed measurements to affect the non-windspeed states for (unsigned row = 0; row <= 21; row++) { Kfusion[row] = 0.0f; diff --git a/EKF/common.h b/EKF/common.h index 70cb67cf49..a998987e1d 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -416,6 +416,7 @@ union filter_control_status_u { uint32_t update_mag_states_only : 1; ///< 16 - true when only the magnetometer states are updated by the magnetometer uint32_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle uint32_t mag_fault : 1; ///< 18 - true when the magnetomer has been declared faulty and is no longer being used + uint32_t fuse_aspd : 1; ///< 19 - true when airspedd measurements are being fused } flags; uint32_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index d27421b831..5daa3e92ca 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -131,7 +131,7 @@ void Ekf::controlFusionModes() // Additional data from an external vision sensor can also be fused. controlExternalVisionFusion(); - // report dead reckoning if we are no longer fusing measurements that constrain velocity drift + // report dead reckoning if we are no longer fusing measurements that directly constrain velocity drift _is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > _params.no_aid_timeout_max) && (_time_last_imu - _time_last_vel_fuse > _params.no_aid_timeout_max) && (_time_last_imu - _time_last_of_fuse > _params.no_aid_timeout_max); @@ -261,20 +261,11 @@ void Ekf::controlExternalVisionFusion() fuseHeading(); } - } + } else if (_control_status.flags.ev_pos && (_time_last_imu - _time_last_ext_vision > 5e6)) { + // Turn off EV fusion mode if no data has been received + _control_status.flags.ev_pos = false; + ECL_INFO("EKF External Vision Data Stopped"); - // handle the case when we are relying on ev data and lose it - if (_control_status.flags.ev_pos && !_control_status.flags.gps && !_control_status.flags.opt_flow) { - // We are relying on ev aiding to constrain drift so after 5s without aiding we need to do something - if ((_time_last_imu - _time_last_pos_fuse > 5e6)) { - // Switch to the non-aiding mode, zero the velocity states - // and set the synthetic position to the current estimate - _control_status.flags.ev_pos = false; - _last_known_posNE(0) = _state.pos(0); - _last_known_posNE(1) = _state.pos(1); - _state.vel.setZero(); - - } } } @@ -381,20 +372,10 @@ void Ekf::controlOpticalFlowFusion() _last_known_posNE(1) = _state.pos(1); } - } + } else if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow > 5e6)) { + ECL_INFO("EKF Optical Flow Data Stopped"); + _control_status.flags.opt_flow = false; - // handle the case when we are relying on optical flow fusion and lose it - if (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos) { - // We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something - if ((_time_last_imu - _time_last_of_fuse > 5e6)) { - // Switch to the non-aiding mode, zero the velocity states - // and set the synthetic position to the current estimate - _control_status.flags.opt_flow = false; - _last_known_posNE(0) = _state.pos(0); - _last_known_posNE(1) = _state.pos(1); - _state.vel.setZero(); - ECL_WARN("EKF Stopping Optical Flow Use"); - } } } @@ -490,19 +471,10 @@ void Ekf::controlGpsFusion() } - } else { - // handle the case where we do not have GPS and have not been using it for an extended period, but are still relying on it - if ((_time_last_imu - _time_last_gps > 10e6) && (_time_last_imu - _time_last_airspeed > 1e6) && (_time_last_imu - _time_last_optflow > 1e6) && _control_status.flags.gps) { - // if we don't have a source of aiding to constrain attitude drift, - // then we need to switch to the non-aiding mode, zero the velocity states - // and set the synthetic GPS position to the current estimate + } else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > 10e6)) { _control_status.flags.gps = false; - _last_known_posNE(0) = _state.pos(0); - _last_known_posNE(1) = _state.pos(1); - _state.vel.setZero(); - ECL_WARN("EKF measurement timeout - stopping navigation"); + ECL_WARN("EKF GPS data stopped"); - } } } @@ -1005,9 +977,18 @@ void Ekf::controlAirDataFusion() } - // Always try to fuse airspeed data if available and we are in flight and the filter is operating in a normal aiding mode - bool is_aiding = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos; - if (_tas_data_ready && _control_status.flags.in_air && is_aiding) { + if (_control_status.flags.fuse_aspd && airspeed_timed_out) { + _control_status.flags.fuse_aspd = false; + + } + + // Always try to fuse airspeed data if available and we are in flight + if (_tas_data_ready && _control_status.flags.in_air) { + // always fuse airsped data if we are flying and data is present + if (!_control_status.flags.fuse_aspd) { + _control_status.flags.fuse_aspd = true; + } + // If starting wind state estimation, reset the wind states and covariances before fusing any data if (!_control_status.flags.wind) { // activate the wind states @@ -1042,10 +1023,7 @@ void Ekf::controlBetaFusion() // Suffient time has lapsed sice the last fusion bool beta_fusion_time_triggered = _time_last_imu - _time_last_beta_fuse > _params.beta_avg_ft_us; - // The filter is operating in a mode where velocity states can be used - bool vel_states_active = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos; - - if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air && vel_states_active) { + if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) { // If starting wind state estimation, reset the wind states and covariances before fusing any data if (!_control_status.flags.wind) { // activate the wind states @@ -1262,8 +1240,21 @@ void Ekf::controlVelPosFusion() { // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz - if (!_control_status.flags.gps && !_control_status.flags.opt_flow && !_control_status.flags.ev_pos - && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { + if (!_control_status.flags.gps && + !_control_status.flags.opt_flow && + !_control_status.flags.ev_pos && + !(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta) && + ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) + { + // Reset position and velocity states if we re-commence this aiding method + if ((_time_last_imu - _time_last_fake_gps) > 4E5) { + _last_known_posNE(0) = _state.pos(0); + _last_known_posNE(1) = _state.pos(1); + _state.vel.setZero(); + ECL_WARN("EKF stopping navigation"); + + } + _fuse_pos = true; _time_last_fake_gps = _time_last_imu; diff --git a/EKF/ekf.h b/EKF/ekf.h index acbb43f7c8..080b296b73 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -138,6 +138,13 @@ public: // get the 1-sigma horizontal and vertical velocity uncertainty void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning); + /* + Returns the following vehicle control limits required by the estimator. + vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required. + tilt_rate_max : maximum allowed tilt rate against the direction of travel (rad/sec). NaN when no limiting required. + */ + void get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl); + void get_vel_var(Vector3f &vel_var); void get_pos_var(Vector3f &pos_var); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 521e00e5c4..de27c0db40 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -902,7 +902,10 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko // TODO - allow for baro drift in vertical position error float hpos_err; float vpos_err; - bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos); + bool vel_pos_aiding = (_control_status.flags.gps || + _control_status.flags.opt_flow || + _control_status.flags.ev_pos || + (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)); if (vel_pos_aiding && _NED_origin_initialised) { hpos_err = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph)); @@ -933,7 +936,10 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko // TODO - allow for baro drift in vertical position error float hpos_err; float vpos_err; - bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos); + bool vel_pos_aiding = (_control_status.flags.gps || + _control_status.flags.opt_flow || + _control_status.flags.ev_pos || + (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)); if (vel_pos_aiding && _NED_origin_initialised) { hpos_err = sqrtf(P[7][7] + P[8][8]); @@ -963,7 +969,10 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon { float hvel_err; float vvel_err; - bool vel_pos_aiding = (_control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos); + bool vel_pos_aiding = (_control_status.flags.gps || + _control_status.flags.opt_flow || + _control_status.flags.ev_pos || + (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)); if (vel_pos_aiding && _NED_origin_initialised) { hvel_err = sqrtf(P[4][4] + P[5][5]); @@ -1000,6 +1009,36 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckon memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool)); } +/* +Returns the following vehicle control limits required by the estimator. +vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required. +limit_hagl : Boolean true when height above ground needs to be controlled to remain between optical flow focus and rang efinder max range limits. +*/ +void Ekf::get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl) +{ + float flow_gnd_spd_max; + bool flow_limit_hagl; + + // If relying on optical flow for navigation we need to keep within flow and range sensor limits + bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos); + if (relying_on_optical_flow) { + // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion + flow_gnd_spd_max = 0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2)); + flow_gnd_spd_max = fmaxf(flow_gnd_spd_max , 0.0f); + + flow_limit_hagl = true; + + } else { + flow_gnd_spd_max = NAN; + flow_limit_hagl = false; + + } + + memcpy(vxy_max, &flow_gnd_spd_max, sizeof(float)); + memcpy(limit_hagl, &flow_limit_hagl, sizeof(bool)); + +} + // get EKF innovation consistency check status information comprising of: // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. @@ -1028,9 +1067,9 @@ void Ekf::get_ekf_soln_status(uint16_t *status) { ekf_solution_status soln_status{}; soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0); - soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); + soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0); - soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); + soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow ) && (_fault_status.value == 0); soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert; soln_status.flags.pos_vert_agl = get_terrain_valid(); @@ -1105,18 +1144,17 @@ void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first bool Ekf::global_position_is_valid() { - // return true if the position estimate is valid - // TODO implement proper check based on published GPS accuracy, innovation consistency checks and timeout status - return (_NED_origin_initialised && ((_time_last_imu - _time_last_gps) < 5e6) && _control_status.flags.gps); + // return true if we are not doing unconstrained free inertial navigation and the origin is set + return (_NED_origin_initialised && !inertial_dead_reckoning()); } // return true if we are totally reliant on inertial dead-reckoning for position bool Ekf::inertial_dead_reckoning() { bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos) - && ((_time_last_imu - _time_last_pos_fuse <= 1E6) || (_time_last_imu - _time_last_vel_fuse <= 1E6)); - bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= 1E6); - bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= 1E6); + && ((_time_last_imu - _time_last_pos_fuse <= _params.no_aid_timeout_max) || (_time_last_imu - _time_last_vel_fuse <= _params.no_aid_timeout_max)); + bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= _params.no_aid_timeout_max); + bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= _params.no_aid_timeout_max) && (_time_last_imu - _time_last_beta_fuse <= _params.no_aid_timeout_max); return !velPosAiding && !optFlowAiding && !airDataAiding; } diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 3eedddf3a0..ddee0c0e30 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -464,8 +464,6 @@ void EstimatorInterface::unallocate_buffers() bool EstimatorInterface::local_position_is_valid() { - // return true if the position estimate is valid - return (((_time_last_imu - _time_last_optflow) < 5e6) && _control_status.flags.opt_flow) || - (((_time_last_imu - _time_last_ext_vision) < 5e6) && _control_status.flags.ev_pos) || - global_position_is_valid(); + // return true if we are not doing unconstrained free inertial navigation + return !inertial_dead_reckoning(); } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 80fd3f2c1f..f337da9685 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -148,6 +148,13 @@ public: // get the 1-sigma horizontal and vertical velocity uncertainty virtual void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning) = 0; + /* + Returns the following vehicle control limits required by the estimator. + vxy_max : Maximum ground relative horizontal speed (metres/sec). NaN when no limiting required. + limit_hagl : Boolean true when height above ground needs to be controlled to remain between optical flow focus and rang efinder max range limits. + */ + virtual void get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl) = 0; + // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; } @@ -192,7 +199,7 @@ public: void set_is_fixed_wing(bool is_fixed_wing) {_control_status.flags.fixed_wing = is_fixed_wing;} // set flag if synthetic sideslip measurement should be fused - void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = fuse_beta;} + void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air);} // set flag if only only mag states should be updated by the magnetometer void set_update_mag_states_only_flag(bool update_mag_states_only) {_control_status.flags.update_mag_states_only = update_mag_states_only;} diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index b3f102b0fc..a3eed52e35 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -79,7 +79,7 @@ void Ekf::fuseSideslip() rel_wind = earth_to_body * rel_wind; // perform fusion of assumed sideslip = 0 - if (rel_wind(0) > 7.0f) { + if (rel_wind.norm() > 7.0f) { // Calculate the observation jacobians // intermediate variable from algebraic optimisation @@ -117,7 +117,7 @@ void Ekf::fuseSideslip() } // determine if we need the sideslip fusion to correct states other than wind - bool update_wind_only = ((_time_last_imu - _time_last_gps) < 1e6) || ((_time_last_imu - _time_last_ext_vision) < 1e6) || ((_time_last_imu - _time_last_optflow) < 1e6); + bool update_wind_only = !_is_dead_reckoning; // intermediate variables - note SK_BETA[0] is 1/(innovation variance) _beta_innov_var = (R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][4]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][4]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][4]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][4]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][4]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][4]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][4]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][22]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][22]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][22]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][22]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][22]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][22]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][22]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2))*(P[22][5]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][5]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][5]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][5]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][5]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][5]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][5]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2))*(P[22][23]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][23]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][23]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][23]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][23]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][23]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][23]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P[22][0]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][0]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][0]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][0]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][0]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][0]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][0]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P[22][1]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][1]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][1]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][1]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][1]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][1]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][1]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P[22][2]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][2]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][2]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][2]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][2]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][2]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][2]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][3]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][3]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][3]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][3]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][3]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][3]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][3]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))*(P[22][6]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][6]*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P[23][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P[0][6]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][6]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][6]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][6]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][6]*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))));