zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF3: rework parameter handling 

and fixed indentation
c415-sdk
Andrew Tridgell 4 years ago
parent
1c1c067dee
commit
be4d10a95e
4 changed files with 254 additions and 261 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 79
      libraries/AP_NavEKF3/AP_NavEKF3.cpp
  2. 6
      libraries/AP_NavEKF3/AP_NavEKF3.h
  3. 421
      libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
  4. 9
      libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

79
libraries/AP_NavEKF3/AP_NavEKF3.cpp
Unescape View File

@ -188,25 +188,9 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: m // @Units: m
AP_GROUPINFO("GLITCH_RAD", 7, NavEKF3, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT), AP_GROUPINFO("GLITCH_RAD", 7, NavEKF3, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
// bluff body drag fusion // 8 previously used for EKF3_GPS_DELAY parameter that has been deprecated.
// The EKF now takes its GPS delay form the GPS library with the default delays
// @Param: BCOEF_X // specified by the GPS_DELAY and GPS_DELAY2 parameters.
// @DisplayName: Ballistic coefficient measured in X direction
// @Description: Ratio of mass to drag coefficient measured along the X body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the frontal area. The predicted drag from the rotors is specified separately by the EK3_MCOEF parameter.
// @Range: 0.0 1000.0
// @User: Advanced
// @Param: BCOEF_Y
// @DisplayName: Ballistic coefficient measured in Y direction
// @Description: Ratio of mass to drag coefficient measured along the Y body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the side area. The predicted drag from the rotors is specified separately by the EK3_MCOEF parameter.
// @Range: 50.0 1000.0
// @User: Advanced
// @Param: BCOEF_Z
// @DisplayName: unused
// @Description: unused
// @User: Advanced
AP_GROUPINFO("BCOEF", 8, NavEKF3, _ballisticCoef, 0.0f),
// Height measurement parameters // Height measurement parameters
@ -372,14 +356,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: rad/s/s // @Units: rad/s/s
AP_GROUPINFO("GBIAS_P_NSE", 26, NavEKF3, _gyroBiasProcessNoise, GBIAS_P_NSE_DEFAULT), AP_GROUPINFO("GBIAS_P_NSE", 26, NavEKF3, _gyroBiasProcessNoise, GBIAS_P_NSE_DEFAULT),
// @Param: DRAG_M_NSE // 27 previously used for EK2_GSCL_P_NSE parameter that has been removed
// @DisplayName: Observation noise for drag acceleration
// @Description: This sets the amount of noise used when fusing X and Y acceleration as an observation that enables esitmation of wind velocity for multi-rotor vehicles. This feature is enabled by the EK3_BCOEF_X and EK3_BCOEF_Y parameters
// @Range: 0.1 2.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s/s
AP_GROUPINFO("DRAG_M_NSE", 27, NavEKF3, _dragObsNoise, 0.5f),
// @Param: ABIAS_P_NSE // @Param: ABIAS_P_NSE
// @DisplayName: Accelerometer bias stability (m/s^3) // @DisplayName: Accelerometer bias stability (m/s^3)
@ -389,13 +366,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Units: m/s/s/s // @Units: m/s/s/s
AP_GROUPINFO("ABIAS_P_NSE", 28, NavEKF3, _accelBiasProcessNoise, ABIAS_P_NSE_DEFAULT), AP_GROUPINFO("ABIAS_P_NSE", 28, NavEKF3, _accelBiasProcessNoise, ABIAS_P_NSE_DEFAULT),
// @Param: MCOEF // 29 previously used for EK2_MAG_P_NSE parameter that has been replaced with EK3_MAGE_P_NSE and EK3_MAGB_P_NSE
// @DisplayName: Momentum drag coefficient
// @Description: This parameter is used to predict the drag produced by the rotors when flying a multi-copter, enabling estimation of wind drift. It represents the ratio of rotor drag induced acceleration to airspeed. The drag produced by this effect scales with speed not speed squared and is produced because some of the air velocity normal to the rotors axis of rotation is lost when passing through the rotor disc which changes the momentum of the airflow causing drag. For unducted rotors the effect is roughly proportional to the area of the propeller blades when viewed side on an will change with different propellers. For example if flying at 15 m/s at sea level conditions produces a rotor induced drag acceleration of 1.5 m/s/s, then EK3_MCOEF would be set to 0.1 = (1.5/15.0). Set EK3_MCOEFto a postive value to enable wind estimation using this drag effect. To account for the drag produced by the body which scales with speed squared, see documentation for the EK3_BCOEF_X and EK3_BCOEF_Y parameters.
// @Range: 0.0 1.0
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("MCOEF", 29, NavEKF3, _momentumDragCoef, 0.0f),
// @Param: WIND_P_NSE // @Param: WIND_P_NSE
// @DisplayName: Wind velocity process noise (m/s^2) // @DisplayName: Wind velocity process noise (m/s^2)
@ -685,6 +656,37 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = {
// @Path: ../AP_NavEKF/AP_NavEKF_Source.cpp // @Path: ../AP_NavEKF/AP_NavEKF_Source.cpp
AP_SUBGROUPINFO(sources, "SRC", 1, NavEKF3, AP_NavEKF_Source), AP_SUBGROUPINFO(sources, "SRC", 1, NavEKF3, AP_NavEKF_Source),
// @Param: BCOEF_X
// @DisplayName: Ballistic coefficient measured in X direction
// @Description: Ratio of mass to drag coefficient measured along the X body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the frontal area. The predicted drag from the rotors is specified separately by the EK3_MCOEF parameter.
// @Range: 0.0 1000.0
// @User: Advanced
AP_GROUPINFO("BCOEF_X", 2, NavEKF3, _ballisticCoef_x, 0.0f),
// @Param: BCOEF_Y
// @DisplayName: Ballistic coefficient measured in Y direction
// @Description: Ratio of mass to drag coefficient measured along the Y body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the side area. The predicted drag from the rotors is specified separately by the EK3_MCOEF parameter.
// @Range: 50.0 1000.0
// @User: Advanced
AP_GROUPINFO("BCOEF_Y", 3, NavEKF3, _ballisticCoef_y, 0.0f),
// @Param: DRAG_M_NSE
// @DisplayName: Observation noise for drag acceleration
// @Description: This sets the amount of noise used when fusing X and Y acceleration as an observation that enables esitmation of wind velocity for multi-rotor vehicles. This feature is enabled by the EK3_BCOEF_X and EK3_BCOEF_Y parameters
// @Range: 0.1 2.0
// @Increment: 0.1
// @User: Advanced
// @Units: m/s/s
AP_GROUPINFO("DRAG_M_NSE", 4, NavEKF3, _dragObsNoise, 0.5f),
// @Param: MCOEF
// @DisplayName: Momentum drag coefficient
// @Description: This parameter is used to predict the drag produced by the rotors when flying a multi-copter, enabling estimation of wind drift. It represents the ratio of rotor drag induced acceleration to airspeed. The drag produced by this effect scales with speed not speed squared and is produced because some of the air velocity normal to the rotors axis of rotation is lost when passing through the rotor disc which changes the momentum of the airflow causing drag. For unducted rotors the effect is roughly proportional to the area of the propeller blades when viewed side on an will change with different propellers. For example if flying at 15 m/s at sea level conditions produces a rotor induced drag acceleration of 1.5 m/s/s, then EK3_MCOEF would be set to 0.1 = (1.5/15.0). Set EK3_MCOEFto a postive value to enable wind estimation using this drag effect. To account for the drag produced by the body which scales with speed squared, see documentation for the EK3_BCOEF_X and EK3_BCOEF_Y parameters.
// @Range: 0.0 1.0
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("MCOEF", 5, NavEKF3, _momentumDragCoef, 0.0f),
AP_GROUPEND AP_GROUPEND
}; };
@ -1408,15 +1410,6 @@ void NavEKF3::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posI
} }
} }
// publish output observer angular, velocity and position tracking error
void NavEKF3::getOutputTrackingError(int8_t instance, Vector3f &error) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
core[instance].getOutputTrackingError(error);
}
}
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{ {

6
libraries/AP_NavEKF3/AP_NavEKF3.h
Unescape View File

@ -181,9 +181,6 @@ public:
// An out of range instance (eg -1) returns data for the primary instance // An out of range instance (eg -1) returns data for the primary instance
void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const; void getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
// publish output observer angular, velocity and position tracking error
void getOutputTrackingError(int8_t instance, Vector3f &error) const;
// return the innovation consistency test ratios for the specified instance // return the innovation consistency test ratios for the specified instance
// An out of range instance (eg -1) returns data for the primary instance // An out of range instance (eg -1) returns data for the primary instance
void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const; void getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
@ -462,7 +459,8 @@ private:
AP_Float _err_thresh; // lanes have to be consistently better than the primary by at least this threshold to reduce their overall relativeCoreError AP_Float _err_thresh; // lanes have to be consistently better than the primary by at least this threshold to reduce their overall relativeCoreError
AP_Int32 _affinity; // bitmask of sensor affinity options AP_Int32 _affinity; // bitmask of sensor affinity options
AP_Float _dragObsNoise; // drag specific force observatoin noise (m/s/s)**2 AP_Float _dragObsNoise; // drag specific force observatoin noise (m/s/s)**2
AP_Vector3f _ballisticCoef; // ballistic coefficient measured for flow in X and Y body frame directions (Z is unused) AP_Float _ballisticCoef_x; // ballistic coefficient measured for flow in X body frame directions
AP_Float _ballisticCoef_y; // ballistic coefficient measured for flow in Y body frame directions
AP_Float _momentumDragCoef; // lift rotor momentum drag coefficient AP_Float _momentumDragCoef; // lift rotor momentum drag coefficient
// Possible values for _flowUse // Possible values for _flowUse

421
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
Unescape View File

@ -456,41 +456,42 @@ void NavEKF3_core::FuseSideslip()
*/ */
void NavEKF3_core::FuseDragForces() void NavEKF3_core::FuseDragForces()
{ {
// drag model parameters // drag model parameters
const Vector3f bcoef = frontend->_ballisticCoef.get(); const float bcoef_x = frontend->_ballisticCoef_x;
const float bcoef_y = frontend->_ballisticCoef_x;
const float mcoef = frontend->_momentumDragCoef.get(); const float mcoef = frontend->_momentumDragCoef.get();
const bool using_bcoef_x = bcoef.x > 1.0f; const bool using_bcoef_x = bcoef_x > 1.0f;
const bool using_bcoef_y = bcoef.y > 1.0f; const bool using_bcoef_y = bcoef_y > 1.0f;
const bool using_mcoef = mcoef > 0.001f; const bool using_mcoef = mcoef > 0.001f;
memset (&Kfusion, 0, sizeof(Kfusion)); memset (&Kfusion, 0, sizeof(Kfusion));
Vector24 Hfusion; // Observation Jacobians Vector24 Hfusion; // Observation Jacobians
const float R_ACC = sq(fmaxf(frontend->_dragObsNoise, 0.5f)); const float R_ACC = sq(fmaxf(frontend->_dragObsNoise, 0.5f));
const float density_ratio = sqrtf(dal.get_EAS2TAS()); const float density_ratio = sqrtf(dal.get_EAS2TAS());
const float rho = fmaxf(1.225f * density_ratio, 0.1f); // air density const float rho = fmaxf(1.225f * density_ratio, 0.1f); // air density
// get latest estimated orientation // get latest estimated orientation
const float &q0 = stateStruct.quat[0]; const float &q0 = stateStruct.quat[0];
const float &q1 = stateStruct.quat[1]; const float &q1 = stateStruct.quat[1];
const float &q2 = stateStruct.quat[2]; const float &q2 = stateStruct.quat[2];
const float &q3 = stateStruct.quat[3]; const float &q3 = stateStruct.quat[3];
// get latest velocity in earth frame // get latest velocity in earth frame
const float &vn = stateStruct.velocity.x; const float &vn = stateStruct.velocity.x;
const float &ve = stateStruct.velocity.y; const float &ve = stateStruct.velocity.y;
const float &vd = stateStruct.velocity.z; const float &vd = stateStruct.velocity.z;
// get latest wind velocity in earth frame // get latest wind velocity in earth frame
const float &vwn = stateStruct.wind_vel.x; const float &vwn = stateStruct.wind_vel.x;
const float &vwe = stateStruct.wind_vel.y; const float &vwe = stateStruct.wind_vel.y;
// predicted specific forces // predicted specific forces
// calculate relative wind velocity in earth frame and rotate into body frame // calculate relative wind velocity in earth frame and rotate into body frame
const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd); const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd);
const Vector3f rel_wind_body = prevTnb * rel_wind_earth; const Vector3f rel_wind_body = prevTnb * rel_wind_earth;
// perform sequential fusion of XY specific forces // perform sequential fusion of XY specific forces
for (uint8_t axis_index = 0; axis_index < 2; axis_index++) { for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
// correct accel data for bias // correct accel data for bias
const float mea_acc = dragSampleDelayed.accelXY[axis_index] - stateStruct.accel_bias[axis_index] / dtEkfAvg; const float mea_acc = dragSampleDelayed.accelXY[axis_index] - stateStruct.accel_bias[axis_index] / dtEkfAvg;
@ -498,224 +499,224 @@ void NavEKF3_core::FuseDragForces()
// Initialised to measured value and updated later using available drag model // Initialised to measured value and updated later using available drag model
float predAccel = mea_acc; float predAccel = mea_acc;
// predicted sign of drag force // predicted sign of drag force
const float dragForceSign = is_positive(rel_wind_body[axis_index]) ? -1.0f : 1.0f; const float dragForceSign = is_positive(rel_wind_body[axis_index]) ? -1.0f : 1.0f;
if (axis_index == 0) { if (axis_index == 0) {
// drag can be modelled as an arbitrary combination of bluff body drag that proportional to // drag can be modelled as an arbitrary combination of bluff body drag that proportional to
// speed squared, and rotor momentum drag that is proportional to speed. // speed squared, and rotor momentum drag that is proportional to speed.
float Kacc; // Derivative of specific force wrt airspeed float Kacc; // Derivative of specific force wrt airspeed
if (using_mcoef && using_bcoef_x) { if (using_mcoef && using_bcoef_x) {
// mixed bluff body and propeller momentum drag // mixed bluff body and propeller momentum drag
const float airSpd = (bcoef.x / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef.x) * fabsf(mea_acc))); const float airSpd = (bcoef_x / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef_x) * fabsf(mea_acc)));
Kacc = fmaxf(1e-1f, (rho / bcoef.x) * airSpd + mcoef * density_ratio); Kacc = fmaxf(1e-1f, (rho / bcoef_x) * airSpd + mcoef * density_ratio);
predAccel = (0.5f / bcoef[0]) * rho * sq(rel_wind_body[0]) * dragForceSign - rel_wind_body[0] * mcoef * density_ratio; predAccel = (0.5f / bcoef_x) * rho * sq(rel_wind_body[0]) * dragForceSign - rel_wind_body[0] * mcoef * density_ratio;
} else if (using_mcoef) { } else if (using_mcoef) {
// propeller momentum drag only // propeller momentum drag only
Kacc = fmaxf(1e-1f, mcoef * density_ratio); Kacc = fmaxf(1e-1f, mcoef * density_ratio);
predAccel = - rel_wind_body[0] * mcoef * density_ratio; predAccel = - rel_wind_body[0] * mcoef * density_ratio;
} else if (using_bcoef_x) { } else if (using_bcoef_x) {
// bluff body drag only // bluff body drag only
const float airSpd = sqrtf((2.0f * bcoef.x * fabsf(mea_acc)) / rho); const float airSpd = sqrtf((2.0f * bcoef_x * fabsf(mea_acc)) / rho);
Kacc = fmaxf(1e-1f, (rho / bcoef.x) * airSpd); Kacc = fmaxf(1e-1f, (rho / bcoef_x) * airSpd);
predAccel = (0.5f / bcoef[0]) * rho * sq(rel_wind_body[0]) * dragForceSign; predAccel = (0.5f / bcoef_x) * rho * sq(rel_wind_body[0]) * dragForceSign;
} else { } else {
// skip this axis // skip this axis
continue; continue;
} }
// intermediate variables // intermediate variables
const float HK0 = vn - vwn; const float HK0 = vn - vwn;
const float HK1 = ve - vwe; const float HK1 = ve - vwe;
const float HK2 = HK0*q0 + HK1*q3 - q2*vd; const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
const float HK3 = 2*Kacc; const float HK3 = 2*Kacc;
const float HK4 = HK0*q1 + HK1*q2 + q3*vd; const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
const float HK5 = HK0*q2 - HK1*q1 + q0*vd; const float HK5 = HK0*q2 - HK1*q1 + q0*vd;
const float HK6 = -HK0*q3 + HK1*q0 + q1*vd; const float HK6 = -HK0*q3 + HK1*q0 + q1*vd;
const float HK7 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2); const float HK7 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
const float HK8 = HK7*Kacc; const float HK8 = HK7*Kacc;
const float HK9 = q0*q3 + q1*q2; const float HK9 = q0*q3 + q1*q2;
const float HK10 = HK3*HK9; const float HK10 = HK3*HK9;
const float HK11 = q0*q2 - q1*q3; const float HK11 = q0*q2 - q1*q3;
const float HK12 = 2*HK9; const float HK12 = 2*HK9;
const float HK13 = 2*HK11; const float HK13 = 2*HK11;
const float HK14 = 2*HK4; const float HK14 = 2*HK4;
const float HK15 = 2*HK2; const float HK15 = 2*HK2;
const float HK16 = 2*HK5; const float HK16 = 2*HK5;
const float HK17 = 2*HK6; const float HK17 = 2*HK6;
const float HK18 = -HK12*P[0][23] + HK12*P[0][5] - HK13*P[0][6] + HK14*P[0][1] + HK15*P[0][0] - HK16*P[0][2] + HK17*P[0][3] - HK7*P[0][22] + HK7*P[0][4]; const float HK18 = -HK12*P[0][23] + HK12*P[0][5] - HK13*P[0][6] + HK14*P[0][1] + HK15*P[0][0] - HK16*P[0][2] + HK17*P[0][3] - HK7*P[0][22] + HK7*P[0][4];
const float HK19 = HK12*P[5][23]; const float HK19 = HK12*P[5][23];
const float HK20 = -HK12*P[23][23] - HK13*P[6][23] + HK14*P[1][23] + HK15*P[0][23] - HK16*P[2][23] + HK17*P[3][23] + HK19 - HK7*P[22][23] + HK7*P[4][23]; const float HK20 = -HK12*P[23][23] - HK13*P[6][23] + HK14*P[1][23] + HK15*P[0][23] - HK16*P[2][23] + HK17*P[3][23] + HK19 - HK7*P[22][23] + HK7*P[4][23];
const float HK21 = powf(Kacc, 2); const float HK21 = powf(Kacc, 2);
const float HK22 = HK12*HK21; const float HK22 = HK12*HK21;
const float HK23 = HK12*P[5][5] - HK13*P[5][6] + HK14*P[1][5] + HK15*P[0][5] - HK16*P[2][5] + HK17*P[3][5] - HK19 + HK7*P[4][5] - HK7*P[5][22]; const float HK23 = HK12*P[5][5] - HK13*P[5][6] + HK14*P[1][5] + HK15*P[0][5] - HK16*P[2][5] + HK17*P[3][5] - HK19 + HK7*P[4][5] - HK7*P[5][22];
const float HK24 = HK12*P[5][6] - HK12*P[6][23] - HK13*P[6][6] + HK14*P[1][6] + HK15*P[0][6] - HK16*P[2][6] + HK17*P[3][6] + HK7*P[4][6] - HK7*P[6][22]; const float HK24 = HK12*P[5][6] - HK12*P[6][23] - HK13*P[6][6] + HK14*P[1][6] + HK15*P[0][6] - HK16*P[2][6] + HK17*P[3][6] + HK7*P[4][6] - HK7*P[6][22];
const float HK25 = HK7*P[4][22]; const float HK25 = HK7*P[4][22];
const float HK26 = -HK12*P[4][23] + HK12*P[4][5] - HK13*P[4][6] + HK14*P[1][4] + HK15*P[0][4] - HK16*P[2][4] + HK17*P[3][4] - HK25 + HK7*P[4][4]; const float HK26 = -HK12*P[4][23] + HK12*P[4][5] - HK13*P[4][6] + HK14*P[1][4] + HK15*P[0][4] - HK16*P[2][4] + HK17*P[3][4] - HK25 + HK7*P[4][4];
const float HK27 = HK21*HK7; const float HK27 = HK21*HK7;
const float HK28 = -HK12*P[22][23] + HK12*P[5][22] - HK13*P[6][22] + HK14*P[1][22] + HK15*P[0][22] - HK16*P[2][22] + HK17*P[3][22] + HK25 - HK7*P[22][22]; const float HK28 = -HK12*P[22][23] + HK12*P[5][22] - HK13*P[6][22] + HK14*P[1][22] + HK15*P[0][22] - HK16*P[2][22] + HK17*P[3][22] + HK25 - HK7*P[22][22];
const float HK29 = -HK12*P[1][23] + HK12*P[1][5] - HK13*P[1][6] + HK14*P[1][1] + HK15*P[0][1] - HK16*P[1][2] + HK17*P[1][3] - HK7*P[1][22] + HK7*P[1][4]; const float HK29 = -HK12*P[1][23] + HK12*P[1][5] - HK13*P[1][6] + HK14*P[1][1] + HK15*P[0][1] - HK16*P[1][2] + HK17*P[1][3] - HK7*P[1][22] + HK7*P[1][4];
const float HK30 = -HK12*P[2][23] + HK12*P[2][5] - HK13*P[2][6] + HK14*P[1][2] + HK15*P[0][2] - HK16*P[2][2] + HK17*P[2][3] - HK7*P[2][22] + HK7*P[2][4]; const float HK30 = -HK12*P[2][23] + HK12*P[2][5] - HK13*P[2][6] + HK14*P[1][2] + HK15*P[0][2] - HK16*P[2][2] + HK17*P[2][3] - HK7*P[2][22] + HK7*P[2][4];
const float HK31 = -HK12*P[3][23] + HK12*P[3][5] - HK13*P[3][6] + HK14*P[1][3] + HK15*P[0][3] - HK16*P[2][3] + HK17*P[3][3] - HK7*P[3][22] + HK7*P[3][4]; const float HK31 = -HK12*P[3][23] + HK12*P[3][5] - HK13*P[3][6] + HK14*P[1][3] + HK15*P[0][3] - HK16*P[2][3] + HK17*P[3][3] - HK7*P[3][22] + HK7*P[3][4];
// const float HK32 = Kacc/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC); // const float HK32 = Kacc/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC);
// calculate innovation variance and exit if badly conditioned // calculate innovation variance and exit if badly conditioned
innovDragVar.x = (-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC); innovDragVar.x = (-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC);
if (innovDragVar.x < R_ACC) { if (innovDragVar.x < R_ACC) {
return; return;
} }
const float HK32 = Kacc / innovDragVar.x; const float HK32 = Kacc / innovDragVar.x;
// Observation Jacobians // Observation Jacobians
Hfusion[0] = -HK2*HK3; Hfusion[0] = -HK2*HK3;
Hfusion[1] = -HK3*HK4; Hfusion[1] = -HK3*HK4;
Hfusion[2] = HK3*HK5; Hfusion[2] = HK3*HK5;
Hfusion[3] = -HK3*HK6; Hfusion[3] = -HK3*HK6;
Hfusion[4] = -HK8; Hfusion[4] = -HK8;
Hfusion[5] = -HK10; Hfusion[5] = -HK10;
Hfusion[6] = HK11*HK3; Hfusion[6] = HK11*HK3;
Hfusion[22] = HK8; Hfusion[22] = HK8;
Hfusion[23] = HK10; Hfusion[23] = HK10;
// Kalman gains // Kalman gains
// Don't allow modification of any states other than wind velocity - we only need a wind estimate. // Don't allow modification of any states other than wind velocity - we only need a wind estimate.
// Kfusion[0] = -HK18*HK32; // Kfusion[0] = -HK18*HK32;
// Kfusion[1] = -HK29*HK32; // Kfusion[1] = -HK29*HK32;
// Kfusion[2] = -HK30*HK32; // Kfusion[2] = -HK30*HK32;
// Kfusion[3] = -HK31*HK32; // Kfusion[3] = -HK31*HK32;
// Kfusion[4] = -HK26*HK32; // Kfusion[4] = -HK26*HK32;
// Kfusion[5] = -HK23*HK32; // Kfusion[5] = -HK23*HK32;
// Kfusion[6] = -HK24*HK32; // Kfusion[6] = -HK24*HK32;
// Kfusion[7] = -HK32*(HK12*P[5][7] - HK12*P[7][23] - HK13*P[6][7] + HK14*P[1][7] + HK15*P[0][7] - HK16*P[2][7] + HK17*P[3][7] + HK7*P[4][7] - HK7*P[7][22]); // Kfusion[7] = -HK32*(HK12*P[5][7] - HK12*P[7][23] - HK13*P[6][7] + HK14*P[1][7] + HK15*P[0][7] - HK16*P[2][7] + HK17*P[3][7] + HK7*P[4][7] - HK7*P[7][22]);
// Kfusion[8] = -HK32*(HK12*P[5][8] - HK12*P[8][23] - HK13*P[6][8] + HK14*P[1][8] + HK15*P[0][8] - HK16*P[2][8] + HK17*P[3][8] + HK7*P[4][8] - HK7*P[8][22]); // Kfusion[8] = -HK32*(HK12*P[5][8] - HK12*P[8][23] - HK13*P[6][8] + HK14*P[1][8] + HK15*P[0][8] - HK16*P[2][8] + HK17*P[3][8] + HK7*P[4][8] - HK7*P[8][22]);
// Kfusion[9] = -HK32*(HK12*P[5][9] - HK12*P[9][23] - HK13*P[6][9] + HK14*P[1][9] + HK15*P[0][9] - HK16*P[2][9] + HK17*P[3][9] + HK7*P[4][9] - HK7*P[9][22]); // Kfusion[9] = -HK32*(HK12*P[5][9] - HK12*P[9][23] - HK13*P[6][9] + HK14*P[1][9] + HK15*P[0][9] - HK16*P[2][9] + HK17*P[3][9] + HK7*P[4][9] - HK7*P[9][22]);
// Kfusion[10] = -HK32*(-HK12*P[10][23] + HK12*P[5][10] - HK13*P[6][10] + HK14*P[1][10] + HK15*P[0][10] - HK16*P[2][10] + HK17*P[3][10] - HK7*P[10][22] + HK7*P[4][10]); // Kfusion[10] = -HK32*(-HK12*P[10][23] + HK12*P[5][10] - HK13*P[6][10] + HK14*P[1][10] + HK15*P[0][10] - HK16*P[2][10] + HK17*P[3][10] - HK7*P[10][22] + HK7*P[4][10]);
// Kfusion[11] = -HK32*(-HK12*P[11][23] + HK12*P[5][11] - HK13*P[6][11] + HK14*P[1][11] + HK15*P[0][11] - HK16*P[2][11] + HK17*P[3][11] - HK7*P[11][22] + HK7*P[4][11]); // Kfusion[11] = -HK32*(-HK12*P[11][23] + HK12*P[5][11] - HK13*P[6][11] + HK14*P[1][11] + HK15*P[0][11] - HK16*P[2][11] + HK17*P[3][11] - HK7*P[11][22] + HK7*P[4][11]);
// Kfusion[12] = -HK32*(-HK12*P[12][23] + HK12*P[5][12] - HK13*P[6][12] + HK14*P[1][12] + HK15*P[0][12] - HK16*P[2][12] + HK17*P[3][12] - HK7*P[12][22] + HK7*P[4][12]); // Kfusion[12] = -HK32*(-HK12*P[12][23] + HK12*P[5][12] - HK13*P[6][12] + HK14*P[1][12] + HK15*P[0][12] - HK16*P[2][12] + HK17*P[3][12] - HK7*P[12][22] + HK7*P[4][12]);
// Kfusion[13] = -HK32*(-HK12*P[13][23] + HK12*P[5][13] - HK13*P[6][13] + HK14*P[1][13] + HK15*P[0][13] - HK16*P[2][13] + HK17*P[3][13] - HK7*P[13][22] + HK7*P[4][13]); // Kfusion[13] = -HK32*(-HK12*P[13][23] + HK12*P[5][13] - HK13*P[6][13] + HK14*P[1][13] + HK15*P[0][13] - HK16*P[2][13] + HK17*P[3][13] - HK7*P[13][22] + HK7*P[4][13]);
// Kfusion[14] = -HK32*(-HK12*P[14][23] + HK12*P[5][14] - HK13*P[6][14] + HK14*P[1][14] + HK15*P[0][14] - HK16*P[2][14] + HK17*P[3][14] - HK7*P[14][22] + HK7*P[4][14]); // Kfusion[14] = -HK32*(-HK12*P[14][23] + HK12*P[5][14] - HK13*P[6][14] + HK14*P[1][14] + HK15*P[0][14] - HK16*P[2][14] + HK17*P[3][14] - HK7*P[14][22] + HK7*P[4][14]);
// Kfusion[15] = -HK32*(-HK12*P[15][23] + HK12*P[5][15] - HK13*P[6][15] + HK14*P[1][15] + HK15*P[0][15] - HK16*P[2][15] + HK17*P[3][15] - HK7*P[15][22] + HK7*P[4][15]); // Kfusion[15] = -HK32*(-HK12*P[15][23] + HK12*P[5][15] - HK13*P[6][15] + HK14*P[1][15] + HK15*P[0][15] - HK16*P[2][15] + HK17*P[3][15] - HK7*P[15][22] + HK7*P[4][15]);
// Kfusion[16] = -HK32*(-HK12*P[16][23] + HK12*P[5][16] - HK13*P[6][16] + HK14*P[1][16] + HK15*P[0][16] - HK16*P[2][16] + HK17*P[3][16] - HK7*P[16][22] + HK7*P[4][16]); // Kfusion[16] = -HK32*(-HK12*P[16][23] + HK12*P[5][16] - HK13*P[6][16] + HK14*P[1][16] + HK15*P[0][16] - HK16*P[2][16] + HK17*P[3][16] - HK7*P[16][22] + HK7*P[4][16]);
// Kfusion[17] = -HK32*(-HK12*P[17][23] + HK12*P[5][17] - HK13*P[6][17] + HK14*P[1][17] + HK15*P[0][17] - HK16*P[2][17] + HK17*P[3][17] - HK7*P[17][22] + HK7*P[4][17]); // Kfusion[17] = -HK32*(-HK12*P[17][23] + HK12*P[5][17] - HK13*P[6][17] + HK14*P[1][17] + HK15*P[0][17] - HK16*P[2][17] + HK17*P[3][17] - HK7*P[17][22] + HK7*P[4][17]);
// Kfusion[18] = -HK32*(-HK12*P[18][23] + HK12*P[5][18] - HK13*P[6][18] + HK14*P[1][18] + HK15*P[0][18] - HK16*P[2][18] + HK17*P[3][18] - HK7*P[18][22] + HK7*P[4][18]); // Kfusion[18] = -HK32*(-HK12*P[18][23] + HK12*P[5][18] - HK13*P[6][18] + HK14*P[1][18] + HK15*P[0][18] - HK16*P[2][18] + HK17*P[3][18] - HK7*P[18][22] + HK7*P[4][18]);
// Kfusion[19] = -HK32*(-HK12*P[19][23] + HK12*P[5][19] - HK13*P[6][19] + HK14*P[1][19] + HK15*P[0][19] - HK16*P[2][19] + HK17*P[3][19] - HK7*P[19][22] + HK7*P[4][19]); // Kfusion[19] = -HK32*(-HK12*P[19][23] + HK12*P[5][19] - HK13*P[6][19] + HK14*P[1][19] + HK15*P[0][19] - HK16*P[2][19] + HK17*P[3][19] - HK7*P[19][22] + HK7*P[4][19]);
// Kfusion[20] = -HK32*(-HK12*P[20][23] + HK12*P[5][20] - HK13*P[6][20] + HK14*P[1][20] + HK15*P[0][20] - HK16*P[2][20] + HK17*P[3][20] - HK7*P[20][22] + HK7*P[4][20]); // Kfusion[20] = -HK32*(-HK12*P[20][23] + HK12*P[5][20] - HK13*P[6][20] + HK14*P[1][20] + HK15*P[0][20] - HK16*P[2][20] + HK17*P[3][20] - HK7*P[20][22] + HK7*P[4][20]);
// Kfusion[21] = -HK32*(-HK12*P[21][23] + HK12*P[5][21] - HK13*P[6][21] + HK14*P[1][21] + HK15*P[0][21] - HK16*P[2][21] + HK17*P[3][21] - HK7*P[21][22] + HK7*P[4][21]); // Kfusion[21] = -HK32*(-HK12*P[21][23] + HK12*P[5][21] - HK13*P[6][21] + HK14*P[1][21] + HK15*P[0][21] - HK16*P[2][21] + HK17*P[3][21] - HK7*P[21][22] + HK7*P[4][21]);
Kfusion[22] = -HK28*HK32; Kfusion[22] = -HK28*HK32;
Kfusion[23] = -HK20*HK32; Kfusion[23] = -HK20*HK32;
} else if (axis_index == 1) { } else if (axis_index == 1) {
// drag can be modelled as an arbitrary combination of bluff body drag that proportional to // drag can be modelled as an arbitrary combination of bluff body drag that proportional to
// speed squared, and rotor momentum drag that is proportional to speed. // speed squared, and rotor momentum drag that is proportional to speed.
float Kacc; // Derivative of specific force wrt airspeed float Kacc; // Derivative of specific force wrt airspeed
if (using_mcoef && using_bcoef_y) { if (using_mcoef && using_bcoef_y) {
// mixed bluff body and propeller momentum drag // mixed bluff body and propeller momentum drag
const float airSpd = (bcoef.y / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef.y) * fabsf(mea_acc))); const float airSpd = (bcoef_y / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef_y) * fabsf(mea_acc)));
Kacc = fmaxf(1e-1f, (rho / bcoef.y) * airSpd + mcoef * density_ratio); Kacc = fmaxf(1e-1f, (rho / bcoef_y) * airSpd + mcoef * density_ratio);
predAccel = (0.5f / bcoef[1]) * rho * sq(rel_wind_body[1]) * dragForceSign - rel_wind_body[1] * mcoef * density_ratio; predAccel = (0.5f / bcoef_y) * rho * sq(rel_wind_body[1]) * dragForceSign - rel_wind_body[1] * mcoef * density_ratio;
} else if (using_mcoef) { } else if (using_mcoef) {
// propeller momentum drag only // propeller momentum drag only
Kacc = fmaxf(1e-1f, mcoef * density_ratio); Kacc = fmaxf(1e-1f, mcoef * density_ratio);
predAccel = - rel_wind_body[1] * mcoef * density_ratio; predAccel = - rel_wind_body[1] * mcoef * density_ratio;
} else if (using_bcoef_y) { } else if (using_bcoef_y) {
// bluff body drag only // bluff body drag only
const float airSpd = sqrtf((2.0f * bcoef.y * fabsf(mea_acc)) / rho); const float airSpd = sqrtf((2.0f * bcoef_y * fabsf(mea_acc)) / rho);
Kacc = fmaxf(1e-1f, (rho / bcoef.y) * airSpd); Kacc = fmaxf(1e-1f, (rho / bcoef_y) * airSpd);
predAccel = (0.5f / bcoef[1]) * rho * sq(rel_wind_body[1]) * dragForceSign; predAccel = (0.5f / bcoef_y) * rho * sq(rel_wind_body[1]) * dragForceSign;
} else { } else {
// nothing more to do // nothing more to do
return; return;
} }
// intermediate variables // intermediate variables
const float HK0 = ve - vwe; const float HK0 = ve - vwe;
const float HK1 = vn - vwn; const float HK1 = vn - vwn;
const float HK2 = HK0*q0 - HK1*q3 + q1*vd; const float HK2 = HK0*q0 - HK1*q3 + q1*vd;
const float HK3 = 2*Kacc; const float HK3 = 2*Kacc;
const float HK4 = -HK0*q1 + HK1*q2 + q0*vd; const float HK4 = -HK0*q1 + HK1*q2 + q0*vd;
const float HK5 = HK0*q2 + HK1*q1 + q3*vd; const float HK5 = HK0*q2 + HK1*q1 + q3*vd;
const float HK6 = HK0*q3 + HK1*q0 - q2*vd; const float HK6 = HK0*q3 + HK1*q0 - q2*vd;
const float HK7 = q0*q3 - q1*q2; const float HK7 = q0*q3 - q1*q2;
const float HK8 = HK3*HK7; const float HK8 = HK3*HK7;
const float HK9 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2); const float HK9 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
const float HK10 = HK9*Kacc; const float HK10 = HK9*Kacc;
const float HK11 = q0*q1 + q2*q3; const float HK11 = q0*q1 + q2*q3;
const float HK12 = 2*HK11; const float HK12 = 2*HK11;
const float HK13 = 2*HK7; const float HK13 = 2*HK7;
const float HK14 = 2*HK5; const float HK14 = 2*HK5;
const float HK15 = 2*HK2; const float HK15 = 2*HK2;
const float HK16 = 2*HK4; const float HK16 = 2*HK4;
const float HK17 = 2*HK6; const float HK17 = 2*HK6;
const float HK18 = HK12*P[0][6] + HK13*P[0][22] - HK13*P[0][4] + HK14*P[0][2] + HK15*P[0][0] + HK16*P[0][1] - HK17*P[0][3] - HK9*P[0][23] + HK9*P[0][5]; const float HK18 = HK12*P[0][6] + HK13*P[0][22] - HK13*P[0][4] + HK14*P[0][2] + HK15*P[0][0] + HK16*P[0][1] - HK17*P[0][3] - HK9*P[0][23] + HK9*P[0][5];
const float HK19 = powf(Kacc, 2); const float HK19 = powf(Kacc, 2);
const float HK20 = HK12*P[6][6] - HK13*P[4][6] + HK13*P[6][22] + HK14*P[2][6] + HK15*P[0][6] + HK16*P[1][6] - HK17*P[3][6] + HK9*P[5][6] - HK9*P[6][23]; const float HK20 = HK12*P[6][6] - HK13*P[4][6] + HK13*P[6][22] + HK14*P[2][6] + HK15*P[0][6] + HK16*P[1][6] - HK17*P[3][6] + HK9*P[5][6] - HK9*P[6][23];
const float HK21 = HK13*P[4][22]; const float HK21 = HK13*P[4][22];
const float HK22 = HK12*P[6][22] + HK13*P[22][22] + HK14*P[2][22] + HK15*P[0][22] + HK16*P[1][22] - HK17*P[3][22] - HK21 - HK9*P[22][23] + HK9*P[5][22]; const float HK22 = HK12*P[6][22] + HK13*P[22][22] + HK14*P[2][22] + HK15*P[0][22] + HK16*P[1][22] - HK17*P[3][22] - HK21 - HK9*P[22][23] + HK9*P[5][22];
const float HK23 = HK13*HK19; const float HK23 = HK13*HK19;
const float HK24 = HK12*P[4][6] - HK13*P[4][4] + HK14*P[2][4] + HK15*P[0][4] + HK16*P[1][4] - HK17*P[3][4] + HK21 - HK9*P[4][23] + HK9*P[4][5]; const float HK24 = HK12*P[4][6] - HK13*P[4][4] + HK14*P[2][4] + HK15*P[0][4] + HK16*P[1][4] - HK17*P[3][4] + HK21 - HK9*P[4][23] + HK9*P[4][5];
const float HK25 = HK9*P[5][23]; const float HK25 = HK9*P[5][23];
const float HK26 = HK12*P[5][6] - HK13*P[4][5] + HK13*P[5][22] + HK14*P[2][5] + HK15*P[0][5] + HK16*P[1][5] - HK17*P[3][5] - HK25 + HK9*P[5][5]; const float HK26 = HK12*P[5][6] - HK13*P[4][5] + HK13*P[5][22] + HK14*P[2][5] + HK15*P[0][5] + HK16*P[1][5] - HK17*P[3][5] - HK25 + HK9*P[5][5];
const float HK27 = HK19*HK9; const float HK27 = HK19*HK9;
const float HK28 = HK12*P[6][23] + HK13*P[22][23] - HK13*P[4][23] + HK14*P[2][23] + HK15*P[0][23] + HK16*P[1][23] - HK17*P[3][23] + HK25 - HK9*P[23][23]; const float HK28 = HK12*P[6][23] + HK13*P[22][23] - HK13*P[4][23] + HK14*P[2][23] + HK15*P[0][23] + HK16*P[1][23] - HK17*P[3][23] + HK25 - HK9*P[23][23];
const float HK29 = HK12*P[2][6] + HK13*P[2][22] - HK13*P[2][4] + HK14*P[2][2] + HK15*P[0][2] + HK16*P[1][2] - HK17*P[2][3] - HK9*P[2][23] + HK9*P[2][5]; const float HK29 = HK12*P[2][6] + HK13*P[2][22] - HK13*P[2][4] + HK14*P[2][2] + HK15*P[0][2] + HK16*P[1][2] - HK17*P[2][3] - HK9*P[2][23] + HK9*P[2][5];
const float HK30 = HK12*P[1][6] + HK13*P[1][22] - HK13*P[1][4] + HK14*P[1][2] + HK15*P[0][1] + HK16*P[1][1] - HK17*P[1][3] - HK9*P[1][23] + HK9*P[1][5]; const float HK30 = HK12*P[1][6] + HK13*P[1][22] - HK13*P[1][4] + HK14*P[1][2] + HK15*P[0][1] + HK16*P[1][1] - HK17*P[1][3] - HK9*P[1][23] + HK9*P[1][5];
const float HK31 = HK12*P[3][6] + HK13*P[3][22] - HK13*P[3][4] + HK14*P[2][3] + HK15*P[0][3] + HK16*P[1][3] - HK17*P[3][3] - HK9*P[3][23] + HK9*P[3][5]; const float HK31 = HK12*P[3][6] + HK13*P[3][22] - HK13*P[3][4] + HK14*P[2][3] + HK15*P[0][3] + HK16*P[1][3] - HK17*P[3][3] - HK9*P[3][23] + HK9*P[3][5];
// const float HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC); // const float HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC);
innovDragVar.y = (HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC); innovDragVar.y = (HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC);
if (innovDragVar.y < R_ACC) { if (innovDragVar.y < R_ACC) {
// calculation is badly conditioned // calculation is badly conditioned
return; return;
} }
const float HK32 = Kacc / innovDragVar.y; const float HK32 = Kacc / innovDragVar.y;
// Observation Jacobians // Observation Jacobians
Hfusion[0] = -HK2*HK3; Hfusion[0] = -HK2*HK3;
Hfusion[1] = -HK3*HK4; Hfusion[1] = -HK3*HK4;
Hfusion[2] = -HK3*HK5; Hfusion[2] = -HK3*HK5;
Hfusion[3] = HK3*HK6; Hfusion[3] = HK3*HK6;
Hfusion[4] = HK8; Hfusion[4] = HK8;
Hfusion[5] = -HK10; Hfusion[5] = -HK10;
Hfusion[6] = -HK11*HK3; Hfusion[6] = -HK11*HK3;
Hfusion[22] = -HK8; Hfusion[22] = -HK8;
Hfusion[23] = HK10; Hfusion[23] = HK10;
// Kalman gains // Kalman gains
// Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate. // Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate.
// Kfusion[0] = -HK18*HK32; // Kfusion[0] = -HK18*HK32;
// Kfusion[1] = -HK30*HK32; // Kfusion[1] = -HK30*HK32;
// Kfusion[2] = -HK29*HK32; // Kfusion[2] = -HK29*HK32;
// Kfusion[3] = -HK31*HK32; // Kfusion[3] = -HK31*HK32;
// Kfusion[4] = -HK24*HK32; // Kfusion[4] = -HK24*HK32;
// Kfusion[5] = -HK26*HK32; // Kfusion[5] = -HK26*HK32;
// Kfusion[6] = -HK20*HK32; // Kfusion[6] = -HK20*HK32;
// Kfusion[7] = -HK32*(HK12*P[6][7] - HK13*P[4][7] + HK13*P[7][22] + HK14*P[2][7] + HK15*P[0][7] + HK16*P[1][7] - HK17*P[3][7] + HK9*P[5][7] - HK9*P[7][23]); // Kfusion[7] = -HK32*(HK12*P[6][7] - HK13*P[4][7] + HK13*P[7][22] + HK14*P[2][7] + HK15*P[0][7] + HK16*P[1][7] - HK17*P[3][7] + HK9*P[5][7] - HK9*P[7][23]);
// Kfusion[8] = -HK32*(HK12*P[6][8] - HK13*P[4][8] + HK13*P[8][22] + HK14*P[2][8] + HK15*P[0][8] + HK16*P[1][8] - HK17*P[3][8] + HK9*P[5][8] - HK9*P[8][23]); // Kfusion[8] = -HK32*(HK12*P[6][8] - HK13*P[4][8] + HK13*P[8][22] + HK14*P[2][8] + HK15*P[0][8] + HK16*P[1][8] - HK17*P[3][8] + HK9*P[5][8] - HK9*P[8][23]);
// Kfusion[9] = -HK32*(HK12*P[6][9] - HK13*P[4][9] + HK13*P[9][22] + HK14*P[2][9] + HK15*P[0][9] + HK16*P[1][9] - HK17*P[3][9] + HK9*P[5][9] - HK9*P[9][23]); // Kfusion[9] = -HK32*(HK12*P[6][9] - HK13*P[4][9] + HK13*P[9][22] + HK14*P[2][9] + HK15*P[0][9] + HK16*P[1][9] - HK17*P[3][9] + HK9*P[5][9] - HK9*P[9][23]);
// Kfusion[10] = -HK32*(HK12*P[6][10] + HK13*P[10][22] - HK13*P[4][10] + HK14*P[2][10] + HK15*P[0][10] + HK16*P[1][10] - HK17*P[3][10] - HK9*P[10][23] + HK9*P[5][10]); // Kfusion[10] = -HK32*(HK12*P[6][10] + HK13*P[10][22] - HK13*P[4][10] + HK14*P[2][10] + HK15*P[0][10] + HK16*P[1][10] - HK17*P[3][10] - HK9*P[10][23] + HK9*P[5][10]);
// Kfusion[11] = -HK32*(HK12*P[6][11] + HK13*P[11][22] - HK13*P[4][11] + HK14*P[2][11] + HK15*P[0][11] + HK16*P[1][11] - HK17*P[3][11] - HK9*P[11][23] + HK9*P[5][11]); // Kfusion[11] = -HK32*(HK12*P[6][11] + HK13*P[11][22] - HK13*P[4][11] + HK14*P[2][11] + HK15*P[0][11] + HK16*P[1][11] - HK17*P[3][11] - HK9*P[11][23] + HK9*P[5][11]);
// Kfusion[12] = -HK32*(HK12*P[6][12] + HK13*P[12][22] - HK13*P[4][12] + HK14*P[2][12] + HK15*P[0][12] + HK16*P[1][12] - HK17*P[3][12] - HK9*P[12][23] + HK9*P[5][12]); // Kfusion[12] = -HK32*(HK12*P[6][12] + HK13*P[12][22] - HK13*P[4][12] + HK14*P[2][12] + HK15*P[0][12] + HK16*P[1][12] - HK17*P[3][12] - HK9*P[12][23] + HK9*P[5][12]);
// Kfusion[13] = -HK32*(HK12*P[6][13] + HK13*P[13][22] - HK13*P[4][13] + HK14*P[2][13] + HK15*P[0][13] + HK16*P[1][13] - HK17*P[3][13] - HK9*P[13][23] + HK9*P[5][13]); // Kfusion[13] = -HK32*(HK12*P[6][13] + HK13*P[13][22] - HK13*P[4][13] + HK14*P[2][13] + HK15*P[0][13] + HK16*P[1][13] - HK17*P[3][13] - HK9*P[13][23] + HK9*P[5][13]);
// Kfusion[14] = -HK32*(HK12*P[6][14] + HK13*P[14][22] - HK13*P[4][14] + HK14*P[2][14] + HK15*P[0][14] + HK16*P[1][14] - HK17*P[3][14] - HK9*P[14][23] + HK9*P[5][14]); // Kfusion[14] = -HK32*(HK12*P[6][14] + HK13*P[14][22] - HK13*P[4][14] + HK14*P[2][14] + HK15*P[0][14] + HK16*P[1][14] - HK17*P[3][14] - HK9*P[14][23] + HK9*P[5][14]);
// Kfusion[15] = -HK32*(HK12*P[6][15] + HK13*P[15][22] - HK13*P[4][15] + HK14*P[2][15] + HK15*P[0][15] + HK16*P[1][15] - HK17*P[3][15] - HK9*P[15][23] + HK9*P[5][15]); // Kfusion[15] = -HK32*(HK12*P[6][15] + HK13*P[15][22] - HK13*P[4][15] + HK14*P[2][15] + HK15*P[0][15] + HK16*P[1][15] - HK17*P[3][15] - HK9*P[15][23] + HK9*P[5][15]);
// Kfusion[16] = -HK32*(HK12*P[6][16] + HK13*P[16][22] - HK13*P[4][16] + HK14*P[2][16] + HK15*P[0][16] + HK16*P[1][16] - HK17*P[3][16] - HK9*P[16][23] + HK9*P[5][16]); // Kfusion[16] = -HK32*(HK12*P[6][16] + HK13*P[16][22] - HK13*P[4][16] + HK14*P[2][16] + HK15*P[0][16] + HK16*P[1][16] - HK17*P[3][16] - HK9*P[16][23] + HK9*P[5][16]);
// Kfusion[17] = -HK32*(HK12*P[6][17] + HK13*P[17][22] - HK13*P[4][17] + HK14*P[2][17] + HK15*P[0][17] + HK16*P[1][17] - HK17*P[3][17] - HK9*P[17][23] + HK9*P[5][17]); // Kfusion[17] = -HK32*(HK12*P[6][17] + HK13*P[17][22] - HK13*P[4][17] + HK14*P[2][17] + HK15*P[0][17] + HK16*P[1][17] - HK17*P[3][17] - HK9*P[17][23] + HK9*P[5][17]);
// Kfusion[18] = -HK32*(HK12*P[6][18] + HK13*P[18][22] - HK13*P[4][18] + HK14*P[2][18] + HK15*P[0][18] + HK16*P[1][18] - HK17*P[3][18] - HK9*P[18][23] + HK9*P[5][18]); // Kfusion[18] = -HK32*(HK12*P[6][18] + HK13*P[18][22] - HK13*P[4][18] + HK14*P[2][18] + HK15*P[0][18] + HK16*P[1][18] - HK17*P[3][18] - HK9*P[18][23] + HK9*P[5][18]);
// Kfusion[19] = -HK32*(HK12*P[6][19] + HK13*P[19][22] - HK13*P[4][19] + HK14*P[2][19] + HK15*P[0][19] + HK16*P[1][19] - HK17*P[3][19] - HK9*P[19][23] + HK9*P[5][19]); // Kfusion[19] = -HK32*(HK12*P[6][19] + HK13*P[19][22] - HK13*P[4][19] + HK14*P[2][19] + HK15*P[0][19] + HK16*P[1][19] - HK17*P[3][19] - HK9*P[19][23] + HK9*P[5][19]);
// Kfusion[20] = -HK32*(HK12*P[6][20] + HK13*P[20][22] - HK13*P[4][20] + HK14*P[2][20] + HK15*P[0][20] + HK16*P[1][20] - HK17*P[3][20] - HK9*P[20][23] + HK9*P[5][20]); // Kfusion[20] = -HK32*(HK12*P[6][20] + HK13*P[20][22] - HK13*P[4][20] + HK14*P[2][20] + HK15*P[0][20] + HK16*P[1][20] - HK17*P[3][20] - HK9*P[20][23] + HK9*P[5][20]);
// Kfusion[21] = -HK32*(HK12*P[6][21] + HK13*P[21][22] - HK13*P[4][21] + HK14*P[2][21] + HK15*P[0][21] + HK16*P[1][21] - HK17*P[3][21] - HK9*P[21][23] + HK9*P[5][21]); // Kfusion[21] = -HK32*(HK12*P[6][21] + HK13*P[21][22] - HK13*P[4][21] + HK14*P[2][21] + HK15*P[0][21] + HK16*P[1][21] - HK17*P[3][21] - HK9*P[21][23] + HK9*P[5][21]);
Kfusion[22] = -HK22*HK32; Kfusion[22] = -HK22*HK32;
Kfusion[23] = -HK28*HK32; Kfusion[23] = -HK28*HK32;
} }
innovDrag[axis_index] = predAccel - mea_acc; innovDrag[axis_index] = predAccel - mea_acc;
dragTestRatio[axis_index] = sq(innovDrag[axis_index]) / (25.0f * innovDragVar[axis_index]); dragTestRatio[axis_index] = sq(innovDrag[axis_index]) / (25.0f * innovDragVar[axis_index]);
// if the innovation consistency check fails then don't fuse the sample // if the innovation consistency check fails then don't fuse the sample
if (dragTestRatio[axis_index] > 1.0f) { if (dragTestRatio[axis_index] > 1.0f) {
return; return;
} }
// correct the state vector // correct the state vector
for (uint8_t j= 0; j<=stateIndexLim; j++) { for (uint8_t j= 0; j<=stateIndexLim; j++) {
@ -757,7 +758,7 @@ void NavEKF3_core::FuseDragForces()
P[i][j] = P[i][j] - KHP[i][j]; P[i][j] = P[i][j] - KHP[i][j];
} }
} }
} }
} }
/******************************************************** /********************************************************

9
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
Unescape View File

@ -1362,10 +1362,11 @@ void NavEKF3_core::updateMovementCheck(void)
void NavEKF3_core::SampleDragData(const imu_elements &imu) void NavEKF3_core::SampleDragData(const imu_elements &imu)
{ {
// Average and down sample to 5Hz // Average and down sample to 5Hz
const Vector3f bcoef = frontend->_ballisticCoef.get(); const float bcoef_x = frontend->_ballisticCoef_x;
const float bcoef_y = frontend->_ballisticCoef_y;
const float mcoef = frontend->_momentumDragCoef.get(); const float mcoef = frontend->_momentumDragCoef.get();
const bool using_bcoef_x = bcoef.x > 1.0f; const bool using_bcoef_x = bcoef_x > 1.0f;
const bool using_bcoef_y = bcoef.y > 1.0f; const bool using_bcoef_y = bcoef_y > 1.0f;
const bool using_mcoef = mcoef > 0.001f; const bool using_mcoef = mcoef > 0.001f;
if (!using_bcoef_x && !using_bcoef_y && !using_mcoef) { if (!using_bcoef_x && !using_bcoef_y && !using_mcoef) {
// nothing to do // nothing to do
@ -1402,4 +1403,4 @@ void NavEKF3_core::SampleDragData(const imu_elements &imu)
dragDownSampled.time_ms = 0; dragDownSampled.time_ms = 0;
dragSampleTimeDelta = 0.0f; dragSampleTimeDelta = 0.0f;
} }
} }

Write Preview
Loading…
Cancel
Save