mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 13:53:34 +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
+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++) {
|
||||
|
||||
Reference in New Issue
Block a user