mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-25 00:31:36 +08:00
ekf2: EKFGSF_yaw minor cleanup (#20510)
- don't store unnecessary IMU copy in class, pass it through where required - remove custom pi constants - remove unused fields from ahrs_ekf_gsf (vel_NE, fuse_gps, accel_dt) - explicitly initialize everything in header - apply PX4 code style - set GPS velocity before yaw estimator update, not after
This commit is contained in:
Daniel Agar
GitHub
+123
-136
@@ -36,14 +36,7 @@
|
||||
|
||||
EKFGSF_yaw::EKFGSF_yaw()
|
||||
{
|
||||
// this flag must be false when we start
|
||||
_ahrs_ekf_gsf_tilt_aligned = false;
|
||||
|
||||
// these objects are initialised in initialise() before being used internally, but can be reported for logging before then
|
||||
memset(&_ahrs_ekf_gsf, 0, sizeof(_ahrs_ekf_gsf));
|
||||
memset(&_ekf_gsf, 0, sizeof(_ekf_gsf));
|
||||
_gsf_yaw = 0.0f;
|
||||
_ahrs_accel.zero();
|
||||
initialiseEKFGSF();
|
||||
}
|
||||
|
||||
void EKFGSF_yaw::update(const imuSample &imu_sample,
|
||||
@@ -52,16 +45,12 @@ void EKFGSF_yaw::update(const imuSample &imu_sample,
|
||||
const Vector3f &imu_gyro_bias) // estimated rate gyro bias (rad/sec)
|
||||
{
|
||||
// copy to class variables
|
||||
_delta_ang = imu_sample.delta_ang;
|
||||
_delta_vel = imu_sample.delta_vel;
|
||||
_delta_ang_dt = imu_sample.delta_ang_dt;
|
||||
_delta_vel_dt = imu_sample.delta_vel_dt;
|
||||
_true_airspeed = airspeed;
|
||||
|
||||
// to reduce effect of vibration, filter using an LPF whose time constant is 1/10 of the AHRS tilt correction time constant
|
||||
const float filter_coef = fminf(10.0f * _delta_vel_dt * _tilt_gain, 1.0f);
|
||||
const Vector3f accel = _delta_vel / fmaxf(_delta_vel_dt, 0.001f);
|
||||
_ahrs_accel = _ahrs_accel * (1.0f - filter_coef) + accel * filter_coef;
|
||||
const float filter_coef = fminf(10.f * imu_sample.delta_vel_dt * _tilt_gain, 1.f);
|
||||
const Vector3f accel = imu_sample.delta_vel / fmaxf(imu_sample.delta_vel_dt, 0.001f);
|
||||
_ahrs_accel = _ahrs_accel * (1.f - filter_coef) + accel * filter_coef;
|
||||
|
||||
// Initialise states first time
|
||||
if (!_ahrs_ekf_gsf_tilt_aligned) {
|
||||
@@ -73,8 +62,7 @@ void EKFGSF_yaw::update(const imuSample &imu_sample,
|
||||
const bool ok_to_align = (accel_norm_sq > sq(lower_accel_limit)) && (accel_norm_sq < sq(upper_accel_limit));
|
||||
|
||||
if (ok_to_align) {
|
||||
initialiseEKFGSF();
|
||||
ahrsAlignTilt();
|
||||
ahrsAlignTilt(imu_sample.delta_vel);
|
||||
_ahrs_ekf_gsf_tilt_aligned = true;
|
||||
}
|
||||
|
||||
@@ -88,20 +76,17 @@ void EKFGSF_yaw::update(const imuSample &imu_sample,
|
||||
_ahrs_accel_fusion_gain = ahrsCalcAccelGain();
|
||||
|
||||
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
|
||||
predictEKF(model_index);
|
||||
predictEKF(model_index, imu_sample.delta_ang, imu_sample.delta_ang_dt, imu_sample.delta_vel, imu_sample.delta_vel_dt);
|
||||
}
|
||||
|
||||
// The 3-state EKF models only run when flying to avoid corrupted estimates due to operator handling and GPS interference
|
||||
if (run_EKF && _vel_data_updated) {
|
||||
if (!_ekf_gsf_vel_fuse_started) {
|
||||
initialiseEKFGSF();
|
||||
ahrsAlignYaw();
|
||||
initialiseEKFGSF(_vel_NE, _vel_accuracy);
|
||||
|
||||
// Initialise to gyro bias estimate from main filter because there could be a large
|
||||
// uncorrected rate gyro bias error about the gravity vector
|
||||
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
|
||||
_ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias;
|
||||
}
|
||||
ahrsAlignYaw(imu_gyro_bias);
|
||||
|
||||
_ekf_gsf_vel_fuse_started = true;
|
||||
|
||||
@@ -110,7 +95,7 @@ void EKFGSF_yaw::update(const imuSample &imu_sample,
|
||||
|
||||
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
|
||||
// subsequent measurements are fused as direct state observations
|
||||
if (!updateEKF(model_index)) {
|
||||
if (!updateEKF(model_index, _vel_NE, _vel_accuracy)) {
|
||||
bad_update = true;
|
||||
}
|
||||
}
|
||||
@@ -138,7 +123,7 @@ void EKFGSF_yaw::update(const imuSample &imu_sample,
|
||||
|
||||
} else {
|
||||
// all weights have collapsed due to excessive innovation variances so reset filters
|
||||
initialiseEKFGSF();
|
||||
initialiseEKFGSF(_vel_NE, _vel_accuracy);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -173,11 +158,10 @@ void EKFGSF_yaw::update(const imuSample &imu_sample,
|
||||
_vel_data_updated = false;
|
||||
}
|
||||
|
||||
void EKFGSF_yaw::ahrsPredict(const uint8_t model_index)
|
||||
void EKFGSF_yaw::ahrsPredict(const uint8_t model_index, const Vector3f &delta_ang, const float delta_ang_dt)
|
||||
{
|
||||
// generate attitude solution using simple complementary filter for the selected model
|
||||
|
||||
const Vector3f ang_rate = _delta_ang / fmaxf(_delta_ang_dt, 0.001f) - _ahrs_ekf_gsf[model_index].gyro_bias;
|
||||
const Vector3f ang_rate = delta_ang / fmaxf(delta_ang_dt, 0.001f) - _ahrs_ekf_gsf[model_index].gyro_bias;
|
||||
|
||||
const Dcmf R_to_body = _ahrs_ekf_gsf[model_index].R.transpose();
|
||||
const Vector3f gravity_direction_bf = R_to_body.col(2);
|
||||
@@ -200,29 +184,27 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index)
|
||||
}
|
||||
|
||||
tilt_correction = (gravity_direction_bf % accel) * _ahrs_accel_fusion_gain / _ahrs_accel_norm;
|
||||
|
||||
}
|
||||
|
||||
// Gyro bias estimation
|
||||
constexpr float gyro_bias_limit = 0.05f;
|
||||
const float spinRate = ang_rate.length();
|
||||
const float spin_rate = ang_rate.length();
|
||||
|
||||
if (spinRate < 0.175f) {
|
||||
_ahrs_ekf_gsf[model_index].gyro_bias -= tilt_correction * (_gyro_bias_gain * _delta_ang_dt);
|
||||
if (spin_rate < math::radians(10.f)) {
|
||||
_ahrs_ekf_gsf[model_index].gyro_bias -= tilt_correction * (_gyro_bias_gain * delta_ang_dt);
|
||||
_ahrs_ekf_gsf[model_index].gyro_bias = matrix::constrain(_ahrs_ekf_gsf[model_index].gyro_bias, -gyro_bias_limit,
|
||||
gyro_bias_limit);
|
||||
}
|
||||
|
||||
// delta angle from previous to current frame
|
||||
const Vector3f delta_angle_corrected = _delta_ang + (tilt_correction - _ahrs_ekf_gsf[model_index].gyro_bias) *
|
||||
_delta_ang_dt;
|
||||
const Vector3f delta_angle_corrected = delta_ang + (tilt_correction - _ahrs_ekf_gsf[model_index].gyro_bias) *
|
||||
delta_ang_dt;
|
||||
|
||||
// Apply delta angle to rotation matrix
|
||||
_ahrs_ekf_gsf[model_index].R = ahrsPredictRotMat(_ahrs_ekf_gsf[model_index].R, delta_angle_corrected);
|
||||
|
||||
}
|
||||
|
||||
void EKFGSF_yaw::ahrsAlignTilt()
|
||||
void EKFGSF_yaw::ahrsAlignTilt(const Vector3f &delta_vel)
|
||||
{
|
||||
// Rotation matrix is constructed directly from acceleration measurement and will be the same for
|
||||
// all models so only need to calculate it once. Assumptions are:
|
||||
@@ -230,10 +212,10 @@ void EKFGSF_yaw::ahrsAlignTilt()
|
||||
// 2) The vehicle is not accelerating so all of the measured acceleration is due to gravity.
|
||||
|
||||
// Calculate earth frame Down axis unit vector rotated into body frame
|
||||
const Vector3f down_in_bf = -_delta_vel.normalized();
|
||||
const Vector3f down_in_bf = -delta_vel.normalized();
|
||||
|
||||
// Calculate earth frame North axis unit vector rotated into body frame, orthogonal to 'down_in_bf'
|
||||
const Vector3f i_vec_bf(1.0f, 0.0f, 0.0f);
|
||||
const Vector3f i_vec_bf(1.f, 0.f, 0.f);
|
||||
Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf.dot(down_in_bf));
|
||||
north_in_bf.normalize();
|
||||
|
||||
@@ -254,7 +236,7 @@ void EKFGSF_yaw::ahrsAlignTilt()
|
||||
}
|
||||
}
|
||||
|
||||
void EKFGSF_yaw::ahrsAlignYaw()
|
||||
void EKFGSF_yaw::ahrsAlignYaw(const Vector3f &imu_gyro_bias)
|
||||
{
|
||||
// Align yaw angle for each model
|
||||
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
|
||||
@@ -263,13 +245,16 @@ void EKFGSF_yaw::ahrsAlignYaw()
|
||||
R = updateYawInRotMat(yaw, R);
|
||||
|
||||
_ahrs_ekf_gsf[model_index].aligned = true;
|
||||
|
||||
_ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias;
|
||||
}
|
||||
}
|
||||
|
||||
void EKFGSF_yaw::predictEKF(const uint8_t model_index)
|
||||
void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang, const float delta_ang_dt,
|
||||
const Vector3f &delta_vel, const float delta_vel_dt)
|
||||
{
|
||||
// generate an attitude reference using IMU data
|
||||
ahrsPredict(model_index);
|
||||
ahrsPredict(model_index, delta_ang, delta_ang_dt);
|
||||
|
||||
// we don't start running the EKF part of the algorithm until there are regular velocity observations
|
||||
if (!_ekf_gsf_vel_fuse_started) {
|
||||
@@ -280,7 +265,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
|
||||
_ekf_gsf[model_index].X(2) = getEulerYaw(_ahrs_ekf_gsf[model_index].R);
|
||||
|
||||
// calculate delta velocity in a horizontal front-right frame
|
||||
const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel;
|
||||
const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * delta_vel;
|
||||
const float cos_yaw = cosf(_ekf_gsf[model_index].X(2));
|
||||
const float sin_yaw = sinf(_ekf_gsf[model_index].X(2));
|
||||
const float dvx = del_vel_NED(0) * cos_yaw + del_vel_NED(1) * sin_yaw;
|
||||
@@ -293,42 +278,42 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
|
||||
// predict covariance - equations generated using EKF/python/gsf_ekf_yaw_estimator/main.py
|
||||
|
||||
// Local short variable name copies required for readability
|
||||
const float P00 = _ekf_gsf[model_index].P(0,0);
|
||||
const float P01 = _ekf_gsf[model_index].P(0,1);
|
||||
const float P02 = _ekf_gsf[model_index].P(0,2);
|
||||
const float P11 = _ekf_gsf[model_index].P(1,1);
|
||||
const float P12 = _ekf_gsf[model_index].P(1,2);
|
||||
const float P22 = _ekf_gsf[model_index].P(2,2);
|
||||
const float P00 = _ekf_gsf[model_index].P(0, 0);
|
||||
const float P01 = _ekf_gsf[model_index].P(0, 1);
|
||||
const float P02 = _ekf_gsf[model_index].P(0, 2);
|
||||
const float P11 = _ekf_gsf[model_index].P(1, 1);
|
||||
const float P12 = _ekf_gsf[model_index].P(1, 2);
|
||||
const float P22 = _ekf_gsf[model_index].P(2, 2);
|
||||
const float psi = _ekf_gsf[model_index].X(2);
|
||||
|
||||
// Use fixed values for delta velocity and delta angle process noise variances
|
||||
const float dvxVar = sq(_accel_noise * _delta_vel_dt); // variance of forward delta velocity - (m/s)^2
|
||||
const float dvxVar = sq(_accel_noise * delta_vel_dt); // variance of forward delta velocity - (m/s)^2
|
||||
const float dvyVar = dvxVar; // variance of right delta velocity - (m/s)^2
|
||||
const float dazVar = sq(_gyro_noise * _delta_ang_dt); // variance of yaw delta angle - rad^2
|
||||
const float dazVar = sq(_gyro_noise * delta_ang_dt); // variance of yaw delta angle - rad^2
|
||||
|
||||
// optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py
|
||||
const float S0 = cosf(psi);
|
||||
const float S1 = ecl::powf(S0, 2);
|
||||
const float S2 = sinf(psi);
|
||||
const float S3 = ecl::powf(S2, 2);
|
||||
const float S4 = S0*dvy + S2*dvx;
|
||||
const float S5 = P02 - P22*S4;
|
||||
const float S6 = S0*dvx - S2*dvy;
|
||||
const float S7 = S0*S2;
|
||||
const float S8 = P01 + S7*dvxVar - S7*dvyVar;
|
||||
const float S9 = P12 + P22*S6;
|
||||
const float S4 = S0 * dvy + S2 * dvx;
|
||||
const float S5 = P02 - P22 * S4;
|
||||
const float S6 = S0 * dvx - S2 * dvy;
|
||||
const float S7 = S0 * S2;
|
||||
const float S8 = P01 + S7 * dvxVar - S7 * dvyVar;
|
||||
const float S9 = P12 + P22 * S6;
|
||||
|
||||
_ekf_gsf[model_index].P(0,0) = P00 - P02*S4 + S1*dvxVar + S3*dvyVar - S4*S5;
|
||||
_ekf_gsf[model_index].P(0,1) = -P12*S4 + S5*S6 + S8;
|
||||
_ekf_gsf[model_index].P(1,1) = P11 + P12*S6 + S1*dvyVar + S3*dvxVar + S6*S9;
|
||||
_ekf_gsf[model_index].P(0,2) = S5;
|
||||
_ekf_gsf[model_index].P(1,2) = S9;
|
||||
_ekf_gsf[model_index].P(2,2) = P22 + dazVar;
|
||||
_ekf_gsf[model_index].P(0, 0) = P00 - P02 * S4 + S1 * dvxVar + S3 * dvyVar - S4 * S5;
|
||||
_ekf_gsf[model_index].P(0, 1) = -P12 * S4 + S5 * S6 + S8;
|
||||
_ekf_gsf[model_index].P(1, 1) = P11 + P12 * S6 + S1 * dvyVar + S3 * dvxVar + S6 * S9;
|
||||
_ekf_gsf[model_index].P(0, 2) = S5;
|
||||
_ekf_gsf[model_index].P(1, 2) = S9;
|
||||
_ekf_gsf[model_index].P(2, 2) = P22 + dazVar;
|
||||
|
||||
// covariance matrix is symmetrical, so copy upper half to lower half
|
||||
_ekf_gsf[model_index].P(1,0) = _ekf_gsf[model_index].P(0,1);
|
||||
_ekf_gsf[model_index].P(2,0) = _ekf_gsf[model_index].P(0,2);
|
||||
_ekf_gsf[model_index].P(2,1) = _ekf_gsf[model_index].P(1,2);
|
||||
_ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1);
|
||||
_ekf_gsf[model_index].P(2, 0) = _ekf_gsf[model_index].P(0, 2);
|
||||
_ekf_gsf[model_index].P(2, 1) = _ekf_gsf[model_index].P(1, 2);
|
||||
|
||||
// constrain variances
|
||||
const float min_var = 1e-6f;
|
||||
@@ -339,87 +324,89 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
|
||||
}
|
||||
|
||||
// Update EKF states and covariance for specified model index using velocity measurement
|
||||
bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
|
||||
bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, const float vel_accuracy)
|
||||
{
|
||||
// set observation variance from accuracy estimate supplied by GPS and apply a sanity check minimum
|
||||
const float velObsVar = sq(fmaxf(_vel_accuracy, 0.01f));
|
||||
const float velObsVar = sq(fmaxf(vel_accuracy, 0.01f));
|
||||
|
||||
// calculate velocity observation innovations
|
||||
_ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - _vel_NE(0);
|
||||
_ekf_gsf[model_index].innov(1) = _ekf_gsf[model_index].X(1) - _vel_NE(1);
|
||||
_ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - vel_NE(0);
|
||||
_ekf_gsf[model_index].innov(1) = _ekf_gsf[model_index].X(1) - vel_NE(1);
|
||||
|
||||
// Use temporary variables for covariance elements to reduce verbosity of auto-code expressions
|
||||
const float P00 = _ekf_gsf[model_index].P(0,0);
|
||||
const float P01 = _ekf_gsf[model_index].P(0,1);
|
||||
const float P02 = _ekf_gsf[model_index].P(0,2);
|
||||
const float P11 = _ekf_gsf[model_index].P(1,1);
|
||||
const float P12 = _ekf_gsf[model_index].P(1,2);
|
||||
const float P22 = _ekf_gsf[model_index].P(2,2);
|
||||
const float P00 = _ekf_gsf[model_index].P(0, 0);
|
||||
const float P01 = _ekf_gsf[model_index].P(0, 1);
|
||||
const float P02 = _ekf_gsf[model_index].P(0, 2);
|
||||
const float P11 = _ekf_gsf[model_index].P(1, 1);
|
||||
const float P12 = _ekf_gsf[model_index].P(1, 2);
|
||||
const float P22 = _ekf_gsf[model_index].P(2, 2);
|
||||
|
||||
// optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py
|
||||
const float t0 = ecl::powf(P01, 2);
|
||||
const float t1 = -t0;
|
||||
const float t2 = P00*P11 + P00*velObsVar + P11*velObsVar + t1 + ecl::powf(velObsVar, 2);
|
||||
const float t2 = P00 * P11 + P00 * velObsVar + P11 * velObsVar + t1 + ecl::powf(velObsVar, 2);
|
||||
|
||||
if (fabsf(t2) < 1e-6f) {
|
||||
return false;
|
||||
}
|
||||
const float t3 = 1.0F/t2;
|
||||
|
||||
const float t3 = 1.0F / t2;
|
||||
const float t4 = P11 + velObsVar;
|
||||
const float t5 = P01*t3;
|
||||
const float t5 = P01 * t3;
|
||||
const float t6 = -t5;
|
||||
const float t7 = P00 + velObsVar;
|
||||
const float t8 = P00*t4 + t1;
|
||||
const float t9 = t5*velObsVar;
|
||||
const float t10 = P11*t7;
|
||||
const float t8 = P00 * t4 + t1;
|
||||
const float t9 = t5 * velObsVar;
|
||||
const float t10 = P11 * t7;
|
||||
const float t11 = t1 + t10;
|
||||
const float t12 = P01*P12;
|
||||
const float t13 = P02*t4;
|
||||
const float t14 = P01*P02;
|
||||
const float t15 = P12*t7;
|
||||
const float t16 = t0*velObsVar;
|
||||
const float t12 = P01 * P12;
|
||||
const float t13 = P02 * t4;
|
||||
const float t14 = P01 * P02;
|
||||
const float t15 = P12 * t7;
|
||||
const float t16 = t0 * velObsVar;
|
||||
const float t17 = ecl::powf(t2, -2);
|
||||
const float t18 = t4*velObsVar + t8;
|
||||
const float t19 = t17*t18;
|
||||
const float t20 = t17*(t16 + t7*t8);
|
||||
const float t18 = t4 * velObsVar + t8;
|
||||
const float t19 = t17 * t18;
|
||||
const float t20 = t17 * (t16 + t7 * t8);
|
||||
const float t21 = t0 - t10;
|
||||
const float t22 = t17*t21;
|
||||
const float t22 = t17 * t21;
|
||||
const float t23 = t14 - t15;
|
||||
const float t24 = P01*t23;
|
||||
const float t24 = P01 * t23;
|
||||
const float t25 = t12 - t13;
|
||||
const float t26 = t16 - t21*t4;
|
||||
const float t27 = t17*t26;
|
||||
const float t28 = t11 + t7*velObsVar;
|
||||
const float t30 = t17*t28;
|
||||
const float t31 = P01*t25;
|
||||
const float t32 = t23*t4 + t31;
|
||||
const float t33 = t17*t32;
|
||||
const float t35 = t24 + t25*t7;
|
||||
const float t36 = t17*t35;
|
||||
const float t26 = t16 - t21 * t4;
|
||||
const float t27 = t17 * t26;
|
||||
const float t28 = t11 + t7 * velObsVar;
|
||||
const float t30 = t17 * t28;
|
||||
const float t31 = P01 * t25;
|
||||
const float t32 = t23 * t4 + t31;
|
||||
const float t33 = t17 * t32;
|
||||
const float t35 = t24 + t25 * t7;
|
||||
const float t36 = t17 * t35;
|
||||
|
||||
_ekf_gsf[model_index].S_det_inverse = t3;
|
||||
|
||||
_ekf_gsf[model_index].S_inverse(0,0) = t3*t4;
|
||||
_ekf_gsf[model_index].S_inverse(0,1) = t6;
|
||||
_ekf_gsf[model_index].S_inverse(1,1) = t3*t7;
|
||||
_ekf_gsf[model_index].S_inverse(1,0) = _ekf_gsf[model_index].S_inverse(0,1);
|
||||
_ekf_gsf[model_index].S_inverse(0, 0) = t3 * t4;
|
||||
_ekf_gsf[model_index].S_inverse(0, 1) = t6;
|
||||
_ekf_gsf[model_index].S_inverse(1, 1) = t3 * t7;
|
||||
_ekf_gsf[model_index].S_inverse(1, 0) = _ekf_gsf[model_index].S_inverse(0, 1);
|
||||
|
||||
matrix::Matrix<float, 3, 2> K;
|
||||
K(0,0) = t3*t8;
|
||||
K(1,0) = t9;
|
||||
K(2,0) = t3*(-t12 + t13);
|
||||
K(0,1) = t9;
|
||||
K(1,1) = t11*t3;
|
||||
K(2,1) = t3*(-t14 + t15);
|
||||
K(0, 0) = t3 * t8;
|
||||
K(1, 0) = t9;
|
||||
K(2, 0) = t3 * (-t12 + t13);
|
||||
K(0, 1) = t9;
|
||||
K(1, 1) = t11 * t3;
|
||||
K(2, 1) = t3 * (-t14 + t15);
|
||||
|
||||
_ekf_gsf[model_index].P(0,0) = P00 - t16*t19 - t20*t8;
|
||||
_ekf_gsf[model_index].P(0,1) = P01*(t18*t22 - t20*velObsVar + 1);
|
||||
_ekf_gsf[model_index].P(1,1) = P11 - t16*t30 + t22*t26;
|
||||
_ekf_gsf[model_index].P(0,2) = P02 + t19*t24 + t20*t25;
|
||||
_ekf_gsf[model_index].P(1,2) = P12 + t23*t27 + t30*t31;
|
||||
_ekf_gsf[model_index].P(2,2) = P22 - t23*t33 - t25*t36;
|
||||
_ekf_gsf[model_index].P(1,0) = _ekf_gsf[model_index].P(0,1);
|
||||
_ekf_gsf[model_index].P(2,0) = _ekf_gsf[model_index].P(0,2);
|
||||
_ekf_gsf[model_index].P(2,1) = _ekf_gsf[model_index].P(1,2);
|
||||
_ekf_gsf[model_index].P(0, 0) = P00 - t16 * t19 - t20 * t8;
|
||||
_ekf_gsf[model_index].P(0, 1) = P01 * (t18 * t22 - t20 * velObsVar + 1);
|
||||
_ekf_gsf[model_index].P(1, 1) = P11 - t16 * t30 + t22 * t26;
|
||||
_ekf_gsf[model_index].P(0, 2) = P02 + t19 * t24 + t20 * t25;
|
||||
_ekf_gsf[model_index].P(1, 2) = P12 + t23 * t27 + t30 * t31;
|
||||
_ekf_gsf[model_index].P(2, 2) = P22 - t23 * t33 - t25 * t36;
|
||||
_ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1);
|
||||
_ekf_gsf[model_index].P(2, 0) = _ekf_gsf[model_index].P(0, 2);
|
||||
_ekf_gsf[model_index].P(2, 1) = _ekf_gsf[model_index].P(1, 2);
|
||||
|
||||
// constrain variances
|
||||
const float min_var = 1e-6f;
|
||||
@@ -462,24 +449,24 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
|
||||
return true;
|
||||
}
|
||||
|
||||
void EKFGSF_yaw::initialiseEKFGSF()
|
||||
void EKFGSF_yaw::initialiseEKFGSF(const Vector2f &vel_NE, const float vel_accuracy)
|
||||
{
|
||||
_gsf_yaw = 0.0f;
|
||||
_ekf_gsf_vel_fuse_started = false;
|
||||
_gsf_yaw_variance = _m_pi2 * _m_pi2;
|
||||
_gsf_yaw_variance = sq(M_PI_F / 2.f);
|
||||
_model_weights.setAll(1.0f / (float)N_MODELS_EKFGSF); // All filter models start with the same weight
|
||||
|
||||
memset(&_ekf_gsf, 0, sizeof(_ekf_gsf));
|
||||
const float yaw_increment = 2.0f * _m_pi / (float)N_MODELS_EKFGSF;
|
||||
const float yaw_increment = 2.0f * M_PI_F / (float)N_MODELS_EKFGSF;
|
||||
|
||||
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
|
||||
// evenly space initial yaw estimates in the region between +-Pi
|
||||
_ekf_gsf[model_index].X(2) = -_m_pi + (0.5f * yaw_increment) + ((float)model_index * yaw_increment);
|
||||
_ekf_gsf[model_index].X(2) = -M_PI_F + (0.5f * yaw_increment) + ((float)model_index * yaw_increment);
|
||||
|
||||
// take velocity states and corresponding variance from last measurement
|
||||
_ekf_gsf[model_index].X(0) = _vel_NE(0);
|
||||
_ekf_gsf[model_index].X(1) = _vel_NE(1);
|
||||
_ekf_gsf[model_index].P(0, 0) = sq(_vel_accuracy);
|
||||
_ekf_gsf[model_index].X(0) = vel_NE(0);
|
||||
_ekf_gsf[model_index].X(1) = vel_NE(1);
|
||||
_ekf_gsf[model_index].P(0, 0) = sq(fmaxf(vel_accuracy, 0.01f));
|
||||
_ekf_gsf[model_index].P(1, 1) = _ekf_gsf[model_index].P(0, 0);
|
||||
|
||||
// use half yaw interval for yaw uncertainty
|
||||
@@ -492,7 +479,7 @@ float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const
|
||||
// calculate transpose(innovation) * inv(S) * innovation
|
||||
const float normDist = _ekf_gsf[model_index].innov.dot(_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov);
|
||||
|
||||
return _m_2pi_inv * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist);
|
||||
return (1.f / (2.f * M_PI_F)) * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist);
|
||||
}
|
||||
|
||||
bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
|
||||
@@ -538,15 +525,15 @@ float EKFGSF_yaw::ahrsCalcAccelGain() const
|
||||
Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g)
|
||||
{
|
||||
Matrix3f ret = R;
|
||||
ret(0,0) += R(0,1) * g(2) - R(0,2) * g(1);
|
||||
ret(0,1) += R(0,2) * g(0) - R(0,0) * g(2);
|
||||
ret(0,2) += R(0,0) * g(1) - R(0,1) * g(0);
|
||||
ret(1,0) += R(1,1) * g(2) - R(1,2) * g(1);
|
||||
ret(1,1) += R(1,2) * g(0) - R(1,0) * g(2);
|
||||
ret(1,2) += R(1,0) * g(1) - R(1,1) * g(0);
|
||||
ret(2,0) += R(2,1) * g(2) - R(2,2) * g(1);
|
||||
ret(2,1) += R(2,2) * g(0) - R(2,0) * g(2);
|
||||
ret(2,2) += R(2,0) * g(1) - R(2,1) * g(0);
|
||||
ret(0, 0) += R(0, 1) * g(2) - R(0, 2) * g(1);
|
||||
ret(0, 1) += R(0, 2) * g(0) - R(0, 0) * g(2);
|
||||
ret(0, 2) += R(0, 0) * g(1) - R(0, 1) * g(0);
|
||||
ret(1, 0) += R(1, 1) * g(2) - R(1, 2) * g(1);
|
||||
ret(1, 1) += R(1, 2) * g(0) - R(1, 0) * g(2);
|
||||
ret(1, 2) += R(1, 0) * g(1) - R(1, 1) * g(0);
|
||||
ret(2, 0) += R(2, 1) * g(2) - R(2, 2) * g(1);
|
||||
ret(2, 1) += R(2, 2) * g(0) - R(2, 0) * g(2);
|
||||
ret(2, 2) += R(2, 0) * g(1) - R(2, 1) * g(0);
|
||||
|
||||
// Renormalise rows
|
||||
for (uint8_t r = 0; r < 3; r++) {
|
||||
|
||||
@@ -52,11 +52,6 @@ using matrix::wrap_pi;
|
||||
|
||||
static constexpr uint8_t N_MODELS_EKFGSF = 5;
|
||||
|
||||
// Required math constants
|
||||
static constexpr float _m_2pi_inv = 0.159154943f;
|
||||
static constexpr float _m_pi = 3.14159265f;
|
||||
static constexpr float _m_pi2 = 1.57079632f;
|
||||
|
||||
using namespace estimator;
|
||||
|
||||
class EKFGSF_yaw
|
||||
@@ -94,38 +89,30 @@ private:
|
||||
const float _gyro_bias_gain{0.04f}; // gain applied to integral of gyro correction for complementary filter (1/sec)
|
||||
|
||||
// Declarations used by the bank of N_MODELS_EKFGSF AHRS complementary filters
|
||||
|
||||
Vector3f _delta_ang{}; // IMU delta angle (rad)
|
||||
Vector3f _delta_vel{}; // IMU delta velocity (m/s)
|
||||
float _delta_ang_dt{}; // _delta_ang integration time interval (sec)
|
||||
float _delta_vel_dt{}; // _delta_vel integration time interval (sec)
|
||||
float _true_airspeed{}; // true airspeed used for centripetal accel compensation (m/s)
|
||||
|
||||
struct _ahrs_ekf_gsf_struct {
|
||||
Dcmf R; // matrix that rotates a vector from body to earth frame
|
||||
Vector3f gyro_bias; // gyro bias learned and used by the quaternion calculation
|
||||
bool aligned; // true when AHRS has been aligned
|
||||
float vel_NE[2]; // NE velocity vector from last GPS measurement (m/s)
|
||||
bool fuse_gps; // true when GPS should be fused on that frame
|
||||
float accel_dt; // time step used when generating _simple_accel_FR data (sec)
|
||||
struct {
|
||||
Dcmf R{matrix::eye<float, 3>()}; // matrix that rotates a vector from body to earth frame
|
||||
Vector3f gyro_bias{}; // gyro bias learned and used by the quaternion calculation
|
||||
bool aligned{false}; // true when AHRS has been aligned
|
||||
} _ahrs_ekf_gsf[N_MODELS_EKFGSF] {};
|
||||
|
||||
bool _ahrs_ekf_gsf_tilt_aligned{}; // true the initial tilt alignment has been calculated
|
||||
float _ahrs_accel_fusion_gain{}; // gain from accel vector tilt error to rate gyro correction used by AHRS calculation
|
||||
Vector3f _ahrs_accel{}; // low pass filtered body frame specific force vector used by AHRS calculation (m/s/s)
|
||||
float _ahrs_accel_norm{}; // length of _ahrs_accel specific force vector used by AHRS calculation (m/s/s)
|
||||
bool _ahrs_ekf_gsf_tilt_aligned{false}; // true the initial tilt alignment has been calculated
|
||||
float _ahrs_accel_fusion_gain{0.f}; // gain from accel vector tilt error to rate gyro correction used by AHRS calculation
|
||||
Vector3f _ahrs_accel{0.f, 0.f, 0.f}; // low pass filtered body frame specific force vector used by AHRS calculation (m/s/s)
|
||||
float _ahrs_accel_norm{0.f}; // length of _ahrs_accel specific force vector used by AHRS calculation (m/s/s)
|
||||
|
||||
// calculate the gain from gravity vector misalingment to tilt correction to be used by all AHRS filters
|
||||
float ahrsCalcAccelGain() const;
|
||||
|
||||
// update specified AHRS rotation matrix using IMU and optionally true airspeed data
|
||||
void ahrsPredict(const uint8_t model_index);
|
||||
void ahrsPredict(const uint8_t model_index, const Vector3f &delta_ang, const float delta_ang_dt);
|
||||
|
||||
// align all AHRS roll and pitch orientations using IMU delta velocity vector
|
||||
void ahrsAlignTilt();
|
||||
void ahrsAlignTilt(const Vector3f &delta_vel);
|
||||
|
||||
// align all AHRS yaw orientations to initial values
|
||||
void ahrsAlignYaw();
|
||||
void ahrsAlignYaw(const Vector3f &imu_gyro_bias = {});
|
||||
|
||||
// Efficient propagation of a delta angle in body frame applied to the body to earth frame rotation matrix
|
||||
Matrix3f ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g);
|
||||
@@ -133,11 +120,11 @@ private:
|
||||
// Declarations used by a bank of N_MODELS_EKFGSF EKFs
|
||||
|
||||
struct _ekf_gsf_struct {
|
||||
matrix::Vector3f X; // Vel North (m/s), Vel East (m/s), yaw (rad)s
|
||||
matrix::SquareMatrix<float, 3> P; // covariance matrix
|
||||
matrix::SquareMatrix<float, 2> S_inverse; // inverse of the innovation covariance matrix
|
||||
float S_det_inverse; // inverse of the innovation covariance matrix determinant
|
||||
matrix::Vector2f innov; // Velocity N,E innovation (m/s)
|
||||
matrix::Vector3f X{}; // Vel North (m/s), Vel East (m/s), yaw (rad)s
|
||||
matrix::SquareMatrix<float, 3> P{}; // covariance matrix
|
||||
matrix::SquareMatrix<float, 2> S_inverse{}; // inverse of the innovation covariance matrix
|
||||
float S_det_inverse{}; // inverse of the innovation covariance matrix determinant
|
||||
matrix::Vector2f innov{}; // Velocity N,E innovation (m/s)
|
||||
} _ekf_gsf[N_MODELS_EKFGSF] {};
|
||||
|
||||
bool _vel_data_updated{}; // true when velocity data has been updated
|
||||
@@ -146,14 +133,15 @@ private:
|
||||
bool _ekf_gsf_vel_fuse_started{}; // true when the EKF's have started fusing velocity data and the prediction and update processing is active
|
||||
|
||||
// initialise states and covariance data for the GSF and EKF filters
|
||||
void initialiseEKFGSF();
|
||||
void initialiseEKFGSF(const Vector2f &vel_NE = {}, const float vel_accuracy = 0.f);
|
||||
|
||||
// predict state and covariance for the specified EKF using inertial data
|
||||
void predictEKF(const uint8_t model_index);
|
||||
void predictEKF(const uint8_t model_index, const Vector3f &delta_ang, const float delta_ang_dt,
|
||||
const Vector3f &delta_vel, const float delta_vel_dt);
|
||||
|
||||
// update state and covariance for the specified EKF using a NE velocity measurement
|
||||
// return false if update failed
|
||||
bool updateEKF(const uint8_t model_index);
|
||||
bool updateEKF(const uint8_t model_index, const Vector2f &vel_NE, const float vel_accuracy);
|
||||
|
||||
inline float sq(float x) const { return x * x; };
|
||||
|
||||
|
||||
@@ -121,7 +121,12 @@ void Ekf::controlFusionModes()
|
||||
_gps_sample_delayed.pos -= pos_offset_earth.xy();
|
||||
_gps_sample_delayed.hgt += pos_offset_earth(2);
|
||||
|
||||
_gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise);
|
||||
// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
|
||||
if ((_gps_sample_delayed.sacc > 0.f) && (_gps_sample_delayed.sacc < _params.req_sacc)
|
||||
&& _gps_sample_delayed.vel.isAllFinite()
|
||||
) {
|
||||
_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), math::max(_gps_sample_delayed.sacc, _params.gps_vel_noise));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -117,7 +117,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
|
||||
// reset vertical velocity
|
||||
if (PX4_ISFINITE(gps_sample.vel(2)) && (_params.gnss_ctrl & GnssCtrl::VEL)) {
|
||||
// use 1.5 as a typical ratio of vacc/hacc
|
||||
resetVerticalVelocityTo(gps_sample.vel(2), sq(1.5f * gps_sample.sacc));
|
||||
resetVerticalVelocityTo(gps_sample.vel(2), sq(math::max(1.5f * gps_sample.sacc, _params.gps_vel_noise)));
|
||||
|
||||
} else {
|
||||
resetVerticalVelocityToZero();
|
||||
|
||||
@@ -87,11 +87,6 @@ void Ekf::controlGpsFusion()
|
||||
_aid_src_gnss_pos);
|
||||
_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
|
||||
|
||||
// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
|
||||
if (gps_checks_passing && !gps_checks_failing) {
|
||||
_yawEstimator.setVelocity(gps_sample.vel.xy(), gps_sample.sacc);
|
||||
}
|
||||
|
||||
// Determine if we should use GPS aiding for velocity and horizontal position
|
||||
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
|
||||
const bool mandatory_conditions_passing = ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))
|
||||
|
||||
Reference in New Issue
Block a user