mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: rework parameter handling
and fixed indentation
This commit is contained in:
Andrew Tridgell
parent
1c1c067dee
commit
be4d10a95e
|
|
@ -188,25 +188,9 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
|
|||
// @Units: m
|
||||
AP_GROUPINFO("GLITCH_RAD", 7, NavEKF3, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
|
||||
|
||||
// bluff body drag fusion
|
||||
|
||||
// @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
|
||||
|
||||
// @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),
|
||||
// 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
|
||||
// specified by the GPS_DELAY and GPS_DELAY2 parameters.
|
||||
|
||||
// Height measurement parameters
|
||||
|
||||
|
|
@ -372,14 +356,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
|
|||
// @Units: rad/s/s
|
||||
AP_GROUPINFO("GBIAS_P_NSE", 26, NavEKF3, _gyroBiasProcessNoise, GBIAS_P_NSE_DEFAULT),
|
||||
|
||||
// @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", 27, NavEKF3, _dragObsNoise, 0.5f),
|
||||
// 27 previously used for EK2_GSCL_P_NSE parameter that has been removed
|
||||
|
||||
// @Param: ABIAS_P_NSE
|
||||
// @DisplayName: Accelerometer bias stability (m/s^3)
|
||||
|
|
@ -389,13 +366,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
|
|||
// @Units: m/s/s/s
|
||||
AP_GROUPINFO("ABIAS_P_NSE", 28, NavEKF3, _accelBiasProcessNoise, ABIAS_P_NSE_DEFAULT),
|
||||
|
||||
// @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", 29, NavEKF3, _momentumDragCoef, 0.0f),
|
||||
// 29 previously used for EK2_MAG_P_NSE parameter that has been replaced with EK3_MAGE_P_NSE and EK3_MAGB_P_NSE
|
||||
|
||||
// @Param: WIND_P_NSE
|
||||
// @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
|
||||
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
|
||||
};
|
||||
|
||||
|
|
@ -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
|
||||
void NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
|
||||
{
|
||||
|
|
|
|||
|
|
@ -181,9 +181,6 @@ public:
|
|||
// 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;
|
||||
|
||||
// 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
|
||||
// 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;
|
||||
|
|
@ -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_Int32 _affinity; // bitmask of sensor affinity options
|
||||
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
|
||||
|
||||
// Possible values for _flowUse
|
||||
|
|
|
|||
|
|
@ -456,41 +456,42 @@ void NavEKF3_core::FuseSideslip()
|
|||
*/
|
||||
void NavEKF3_core::FuseDragForces()
|
||||
{
|
||||
// drag model parameters
|
||||
const Vector3f bcoef = frontend->_ballisticCoef.get();
|
||||
// drag model parameters
|
||||
const float bcoef_x = frontend->_ballisticCoef_x;
|
||||
const float bcoef_y = frontend->_ballisticCoef_x;
|
||||
const float mcoef = frontend->_momentumDragCoef.get();
|
||||
const bool using_bcoef_x = bcoef.x > 1.0f;
|
||||
const bool using_bcoef_y = bcoef.y > 1.0f;
|
||||
const bool using_bcoef_x = bcoef_x > 1.0f;
|
||||
const bool using_bcoef_y = bcoef_y > 1.0f;
|
||||
const bool using_mcoef = mcoef > 0.001f;
|
||||
|
||||
memset (&Kfusion, 0, sizeof(Kfusion));
|
||||
Vector24 Hfusion; // Observation Jacobians
|
||||
const float R_ACC = sq(fmaxf(frontend->_dragObsNoise, 0.5f));
|
||||
const float density_ratio = sqrtf(dal.get_EAS2TAS());
|
||||
const float rho = fmaxf(1.225f * density_ratio, 0.1f); // air density
|
||||
Vector24 Hfusion; // Observation Jacobians
|
||||
const float R_ACC = sq(fmaxf(frontend->_dragObsNoise, 0.5f));
|
||||
const float density_ratio = sqrtf(dal.get_EAS2TAS());
|
||||
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 &q1 = stateStruct.quat[1];
|
||||
const float &q2 = stateStruct.quat[2];
|
||||
const float &q3 = stateStruct.quat[3];
|
||||
|
||||
// get latest velocity in earth frame
|
||||
const float &vn = stateStruct.velocity.x;
|
||||
const float &ve = stateStruct.velocity.y;
|
||||
const float &vd = stateStruct.velocity.z;
|
||||
// get latest velocity in earth frame
|
||||
const float &vn = stateStruct.velocity.x;
|
||||
const float &ve = stateStruct.velocity.y;
|
||||
const float &vd = stateStruct.velocity.z;
|
||||
|
||||
// get latest wind velocity in earth frame
|
||||
const float &vwn = stateStruct.wind_vel.x;
|
||||
const float &vwe = stateStruct.wind_vel.y;
|
||||
// get latest wind velocity in earth frame
|
||||
const float &vwn = stateStruct.wind_vel.x;
|
||||
const float &vwe = stateStruct.wind_vel.y;
|
||||
|
||||
// predicted specific forces
|
||||
// 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_body = prevTnb * rel_wind_earth;
|
||||
// predicted specific forces
|
||||
// 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_body = prevTnb * rel_wind_earth;
|
||||
|
||||
// perform sequential fusion of XY specific forces
|
||||
for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
|
||||
// perform sequential fusion of XY specific forces
|
||||
for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
|
||||
// correct accel data for bias
|
||||
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
|
||||
float predAccel = mea_acc;
|
||||
|
||||
// predicted sign of drag force
|
||||
const float dragForceSign = is_positive(rel_wind_body[axis_index]) ? -1.0f : 1.0f;
|
||||
// predicted sign of drag force
|
||||
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
|
||||
// speed squared, and rotor momentum drag that is proportional to speed.
|
||||
float Kacc; // Derivative of specific force wrt airspeed
|
||||
if (using_mcoef && using_bcoef_x) {
|
||||
float Kacc; // Derivative of specific force wrt airspeed
|
||||
if (using_mcoef && using_bcoef_x) {
|
||||
// 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)));
|
||||
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;
|
||||
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);
|
||||
predAccel = (0.5f / bcoef_x) * rho * sq(rel_wind_body[0]) * dragForceSign - rel_wind_body[0] * mcoef * density_ratio;
|
||||
} else if (using_mcoef) {
|
||||
// propeller momentum drag only
|
||||
Kacc = fmaxf(1e-1f, mcoef * density_ratio);
|
||||
predAccel = - rel_wind_body[0] * mcoef * density_ratio;
|
||||
} else if (using_bcoef_x) {
|
||||
// bluff body drag only
|
||||
const float airSpd = sqrtf((2.0f * bcoef.x * fabsf(mea_acc)) / rho);
|
||||
Kacc = fmaxf(1e-1f, (rho / bcoef.x) * airSpd);
|
||||
predAccel = (0.5f / bcoef[0]) * rho * sq(rel_wind_body[0]) * dragForceSign;
|
||||
const float airSpd = sqrtf((2.0f * bcoef_x * fabsf(mea_acc)) / rho);
|
||||
Kacc = fmaxf(1e-1f, (rho / bcoef_x) * airSpd);
|
||||
predAccel = (0.5f / bcoef_x) * rho * sq(rel_wind_body[0]) * dragForceSign;
|
||||
} else {
|
||||
// skip this axis
|
||||
continue;
|
||||
}
|
||||
|
||||
// intermediate variables
|
||||
const float HK0 = vn - vwn;
|
||||
const float HK1 = ve - vwe;
|
||||
const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
|
||||
const float HK3 = 2*Kacc;
|
||||
const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
|
||||
const float HK5 = HK0*q2 - HK1*q1 + q0*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 HK8 = HK7*Kacc;
|
||||
const float HK9 = q0*q3 + q1*q2;
|
||||
const float HK10 = HK3*HK9;
|
||||
const float HK11 = q0*q2 - q1*q3;
|
||||
const float HK12 = 2*HK9;
|
||||
const float HK13 = 2*HK11;
|
||||
const float HK14 = 2*HK4;
|
||||
const float HK15 = 2*HK2;
|
||||
const float HK16 = 2*HK5;
|
||||
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 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 HK21 = powf(Kacc, 2);
|
||||
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 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 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 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 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 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);
|
||||
// intermediate variables
|
||||
const float HK0 = vn - vwn;
|
||||
const float HK1 = ve - vwe;
|
||||
const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
|
||||
const float HK3 = 2*Kacc;
|
||||
const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
|
||||
const float HK5 = HK0*q2 - HK1*q1 + q0*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 HK8 = HK7*Kacc;
|
||||
const float HK9 = q0*q3 + q1*q2;
|
||||
const float HK10 = HK3*HK9;
|
||||
const float HK11 = q0*q2 - q1*q3;
|
||||
const float HK12 = 2*HK9;
|
||||
const float HK13 = 2*HK11;
|
||||
const float HK14 = 2*HK4;
|
||||
const float HK15 = 2*HK2;
|
||||
const float HK16 = 2*HK5;
|
||||
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 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 HK21 = powf(Kacc, 2);
|
||||
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 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 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 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 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 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
|
||||
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) {
|
||||
return;
|
||||
}
|
||||
const float HK32 = Kacc / innovDragVar.x;
|
||||
// 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);
|
||||
if (innovDragVar.x < R_ACC) {
|
||||
return;
|
||||
}
|
||||
const float HK32 = Kacc / innovDragVar.x;
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion[0] = -HK2*HK3;
|
||||
Hfusion[1] = -HK3*HK4;
|
||||
Hfusion[2] = HK3*HK5;
|
||||
Hfusion[3] = -HK3*HK6;
|
||||
Hfusion[4] = -HK8;
|
||||
Hfusion[5] = -HK10;
|
||||
Hfusion[6] = HK11*HK3;
|
||||
Hfusion[22] = HK8;
|
||||
Hfusion[23] = HK10;
|
||||
// Observation Jacobians
|
||||
Hfusion[0] = -HK2*HK3;
|
||||
Hfusion[1] = -HK3*HK4;
|
||||
Hfusion[2] = HK3*HK5;
|
||||
Hfusion[3] = -HK3*HK6;
|
||||
Hfusion[4] = -HK8;
|
||||
Hfusion[5] = -HK10;
|
||||
Hfusion[6] = HK11*HK3;
|
||||
Hfusion[22] = HK8;
|
||||
Hfusion[23] = HK10;
|
||||
|
||||
// Kalman gains
|
||||
// Don't allow modification of any states other than wind velocity - we only need a wind estimate.
|
||||
// Kfusion[0] = -HK18*HK32;
|
||||
// Kfusion[1] = -HK29*HK32;
|
||||
// Kfusion[2] = -HK30*HK32;
|
||||
// Kfusion[3] = -HK31*HK32;
|
||||
// Kfusion[4] = -HK26*HK32;
|
||||
// Kfusion[5] = -HK23*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[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[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[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[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[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[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[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[22] = -HK28*HK32;
|
||||
Kfusion[23] = -HK20*HK32;
|
||||
// Kalman gains
|
||||
// Don't allow modification of any states other than wind velocity - we only need a wind estimate.
|
||||
// Kfusion[0] = -HK18*HK32;
|
||||
// Kfusion[1] = -HK29*HK32;
|
||||
// Kfusion[2] = -HK30*HK32;
|
||||
// Kfusion[3] = -HK31*HK32;
|
||||
// Kfusion[4] = -HK26*HK32;
|
||||
// Kfusion[5] = -HK23*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[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[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[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[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[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[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[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[22] = -HK28*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
|
||||
// speed squared, and rotor momentum drag that is proportional to speed.
|
||||
float Kacc; // Derivative of specific force wrt airspeed
|
||||
if (using_mcoef && using_bcoef_y) {
|
||||
// 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)));
|
||||
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;
|
||||
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);
|
||||
predAccel = (0.5f / bcoef_y) * rho * sq(rel_wind_body[1]) * dragForceSign - rel_wind_body[1] * mcoef * density_ratio;
|
||||
} else if (using_mcoef) {
|
||||
// propeller momentum drag only
|
||||
Kacc = fmaxf(1e-1f, mcoef * density_ratio);
|
||||
predAccel = - rel_wind_body[1] * mcoef * density_ratio;
|
||||
} else if (using_bcoef_y) {
|
||||
// bluff body drag only
|
||||
const float airSpd = sqrtf((2.0f * bcoef.y * fabsf(mea_acc)) / rho);
|
||||
Kacc = fmaxf(1e-1f, (rho / bcoef.y) * airSpd);
|
||||
predAccel = (0.5f / bcoef[1]) * rho * sq(rel_wind_body[1]) * dragForceSign;
|
||||
const float airSpd = sqrtf((2.0f * bcoef_y * fabsf(mea_acc)) / rho);
|
||||
Kacc = fmaxf(1e-1f, (rho / bcoef_y) * airSpd);
|
||||
predAccel = (0.5f / bcoef_y) * rho * sq(rel_wind_body[1]) * dragForceSign;
|
||||
} else {
|
||||
// nothing more to do
|
||||
return;
|
||||
}
|
||||
|
||||
// intermediate variables
|
||||
const float HK0 = ve - vwe;
|
||||
const float HK1 = vn - vwn;
|
||||
const float HK2 = HK0*q0 - HK1*q3 + q1*vd;
|
||||
const float HK3 = 2*Kacc;
|
||||
const float HK4 = -HK0*q1 + HK1*q2 + q0*vd;
|
||||
const float HK5 = HK0*q2 + HK1*q1 + q3*vd;
|
||||
const float HK6 = HK0*q3 + HK1*q0 - q2*vd;
|
||||
const float HK7 = q0*q3 - q1*q2;
|
||||
const float HK8 = HK3*HK7;
|
||||
const float HK9 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
|
||||
const float HK10 = HK9*Kacc;
|
||||
const float HK11 = q0*q1 + q2*q3;
|
||||
const float HK12 = 2*HK11;
|
||||
const float HK13 = 2*HK7;
|
||||
const float HK14 = 2*HK5;
|
||||
const float HK15 = 2*HK2;
|
||||
const float HK16 = 2*HK4;
|
||||
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 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 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 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 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 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 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 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);
|
||||
// intermediate variables
|
||||
const float HK0 = ve - vwe;
|
||||
const float HK1 = vn - vwn;
|
||||
const float HK2 = HK0*q0 - HK1*q3 + q1*vd;
|
||||
const float HK3 = 2*Kacc;
|
||||
const float HK4 = -HK0*q1 + HK1*q2 + q0*vd;
|
||||
const float HK5 = HK0*q2 + HK1*q1 + q3*vd;
|
||||
const float HK6 = HK0*q3 + HK1*q0 - q2*vd;
|
||||
const float HK7 = q0*q3 - q1*q2;
|
||||
const float HK8 = HK3*HK7;
|
||||
const float HK9 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
|
||||
const float HK10 = HK9*Kacc;
|
||||
const float HK11 = q0*q1 + q2*q3;
|
||||
const float HK12 = 2*HK11;
|
||||
const float HK13 = 2*HK7;
|
||||
const float HK14 = 2*HK5;
|
||||
const float HK15 = 2*HK2;
|
||||
const float HK16 = 2*HK4;
|
||||
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 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 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 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 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 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 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 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);
|
||||
|
||||
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) {
|
||||
// calculation is badly conditioned
|
||||
return;
|
||||
}
|
||||
const float HK32 = Kacc / innovDragVar.y;
|
||||
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) {
|
||||
// calculation is badly conditioned
|
||||
return;
|
||||
}
|
||||
const float HK32 = Kacc / innovDragVar.y;
|
||||
|
||||
// Observation Jacobians
|
||||
Hfusion[0] = -HK2*HK3;
|
||||
Hfusion[1] = -HK3*HK4;
|
||||
Hfusion[2] = -HK3*HK5;
|
||||
Hfusion[3] = HK3*HK6;
|
||||
Hfusion[4] = HK8;
|
||||
Hfusion[5] = -HK10;
|
||||
Hfusion[6] = -HK11*HK3;
|
||||
Hfusion[22] = -HK8;
|
||||
Hfusion[23] = HK10;
|
||||
// Observation Jacobians
|
||||
Hfusion[0] = -HK2*HK3;
|
||||
Hfusion[1] = -HK3*HK4;
|
||||
Hfusion[2] = -HK3*HK5;
|
||||
Hfusion[3] = HK3*HK6;
|
||||
Hfusion[4] = HK8;
|
||||
Hfusion[5] = -HK10;
|
||||
Hfusion[6] = -HK11*HK3;
|
||||
Hfusion[22] = -HK8;
|
||||
Hfusion[23] = HK10;
|
||||
|
||||
// Kalman gains
|
||||
// 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[1] = -HK30*HK32;
|
||||
// Kfusion[2] = -HK29*HK32;
|
||||
// Kfusion[3] = -HK31*HK32;
|
||||
// Kfusion[4] = -HK24*HK32;
|
||||
// Kfusion[5] = -HK26*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[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[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[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[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[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[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[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[22] = -HK22*HK32;
|
||||
Kfusion[23] = -HK28*HK32;
|
||||
}
|
||||
// Kalman gains
|
||||
// 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[1] = -HK30*HK32;
|
||||
// Kfusion[2] = -HK29*HK32;
|
||||
// Kfusion[3] = -HK31*HK32;
|
||||
// Kfusion[4] = -HK24*HK32;
|
||||
// Kfusion[5] = -HK26*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[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[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[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[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[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[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[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[22] = -HK22*HK32;
|
||||
Kfusion[23] = -HK28*HK32;
|
||||
}
|
||||
|
||||
innovDrag[axis_index] = predAccel - mea_acc;
|
||||
dragTestRatio[axis_index] = sq(innovDrag[axis_index]) / (25.0f * innovDragVar[axis_index]);
|
||||
innovDrag[axis_index] = predAccel - mea_acc;
|
||||
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 (dragTestRatio[axis_index] > 1.0f) {
|
||||
return;
|
||||
}
|
||||
// if the innovation consistency check fails then don't fuse the sample
|
||||
if (dragTestRatio[axis_index] > 1.0f) {
|
||||
return;
|
||||
}
|
||||
|
||||
// correct the state vector
|
||||
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];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/********************************************************
|
||||
|
|
|
|||
|
|
@ -1362,10 +1362,11 @@ void NavEKF3_core::updateMovementCheck(void)
|
|||
void NavEKF3_core::SampleDragData(const imu_elements &imu)
|
||||
{
|
||||
// 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 bool using_bcoef_x = bcoef.x > 1.0f;
|
||||
const bool using_bcoef_y = bcoef.y > 1.0f;
|
||||
const bool using_bcoef_x = bcoef_x > 1.0f;
|
||||
const bool using_bcoef_y = bcoef_y > 1.0f;
|
||||
const bool using_mcoef = mcoef > 0.001f;
|
||||
if (!using_bcoef_x && !using_bcoef_y && !using_mcoef) {
|
||||
// nothing to do
|
||||
|
|
@ -1402,4 +1403,4 @@ void NavEKF3_core::SampleDragData(const imu_elements &imu)
|
|||
dragDownSampled.time_ms = 0;
|
||||
dragSampleTimeDelta = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue