mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 03:04:07 +08:00
CleanUp of the EKFGSF (#769)
* Add const modifiers * Pass imu data as sampe * Remove emergency reset request counter * Improve matrix library usage * Indentation fix * Do not store innovation matrix for each model * Make weights a separate vector * EKF: Add missing alignment transfer to AHRS solutions * EKF: Replace #define constants with static constexpr * EKF: Move declaration for weights into GSF section * EKF: Fix documentation error Co-authored-by: Paul Riseborough <p_riseborough@live.com.au>
This commit is contained in:
kritz
GitHub
parent
4669aa6312
commit
bcd3869ca2
@ -13,18 +13,15 @@ EKFGSF_yaw::EKFGSF_yaw()
|
||||
_ahrs_accel.zero();
|
||||
}
|
||||
|
||||
void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vector meassured in body frame (rad)
|
||||
const Vector3f del_vel, // IMU delta velocity vector meassured in body frame (m/s)
|
||||
const float del_ang_dt, // time interval that del_ang was integrated over (sec)
|
||||
const float del_vel_dt, // time interval that del_vel was integrated over (sec)
|
||||
void EKFGSF_yaw::update(const imuSample& imu_sample,
|
||||
bool run_EKF, // set to true when flying or movement is suitable for yaw estimation
|
||||
float airspeed) // true airspeed used for centripetal accel compensation - set to 0 when not required.
|
||||
{
|
||||
// copy to class variables
|
||||
_delta_ang = del_ang;
|
||||
_delta_vel = del_vel;
|
||||
_delta_ang_dt = del_ang_dt;
|
||||
_delta_vel_dt = del_vel_dt;
|
||||
_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;
|
||||
_run_ekf_gsf = run_EKF;
|
||||
_true_airspeed = airspeed;
|
||||
|
||||
@ -62,8 +59,6 @@ void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vect
|
||||
ahrsAlignYaw();
|
||||
_ekf_gsf_vel_fuse_started = true;
|
||||
} else {
|
||||
float total_w = 0.0f;
|
||||
float newWeight[N_MODELS_EKFGSF];
|
||||
bool bad_update = false;
|
||||
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
|
||||
// subsequent measurements are fused as direct state observations
|
||||
@ -73,18 +68,16 @@ void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vect
|
||||
}
|
||||
|
||||
if (!bad_update) {
|
||||
float total_weight = 0.0f;
|
||||
// calculate weighting for each model assuming a normal distribution
|
||||
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
|
||||
newWeight[model_index] = fmaxf(gaussianDensity(model_index) * _ekf_gsf[model_index].W, 0.0f);
|
||||
total_w += newWeight[model_index];
|
||||
_model_weights(model_index) = fmaxf(gaussianDensity(model_index) * _model_weights(model_index), 0.0f);
|
||||
total_weight += _model_weights(model_index);
|
||||
}
|
||||
|
||||
// normalise the weighting function
|
||||
if (_ekf_gsf_vel_fuse_started && total_w > 1e-15f && !bad_update) {
|
||||
float total_w_inv = 1.0f / total_w;
|
||||
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
|
||||
_ekf_gsf[model_index].W = newWeight[model_index] * total_w_inv;
|
||||
}
|
||||
if (total_weight > 1e-15f) {
|
||||
_model_weights /= total_weight;
|
||||
}
|
||||
|
||||
// Enforce a minimum weighting value. This was added during initial development but has not been needed
|
||||
@ -95,12 +88,12 @@ void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vect
|
||||
bool change_mask[N_MODELS_EKFGSF] = {}; // true when the weighting for that model has been increased
|
||||
float unmodified_weights_sum = 0.0f; // sum of unmodified weights
|
||||
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
|
||||
if (_ekf_gsf[model_index].W < _weight_min) {
|
||||
correction_sum += _weight_min - _ekf_gsf[model_index].W;
|
||||
_ekf_gsf[model_index].W = _weight_min;
|
||||
if (_model_weights(model_index) < _weight_min) {
|
||||
correction_sum += _weight_min - _model_weights(model_index);
|
||||
_model_weights(model_index) = _weight_min;
|
||||
change_mask[model_index] = true;
|
||||
} else {
|
||||
unmodified_weights_sum += _ekf_gsf[model_index].W;
|
||||
unmodified_weights_sum += _model_weights(model_index);
|
||||
}
|
||||
}
|
||||
|
||||
@ -108,7 +101,7 @@ void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vect
|
||||
const float scale_factor = (unmodified_weights_sum - correction_sum - _weight_min) / (unmodified_weights_sum - _weight_min);
|
||||
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
|
||||
if (!change_mask[model_index]) {
|
||||
_ekf_gsf[model_index].W = _weight_min + scale_factor * (_ekf_gsf[model_index].W - _weight_min);
|
||||
_model_weights(model_index) = _weight_min + scale_factor * (_model_weights(model_index) - _weight_min);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -122,10 +115,10 @@ void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vect
|
||||
// Calculate a composite yaw vector as a weighted average of the states for each model.
|
||||
// To avoid issues with angle wrapping, the yaw state is converted to a vector with legnth
|
||||
// equal to the weighting value before it is summed.
|
||||
Vector2f yaw_vector = {};
|
||||
Vector2f yaw_vector;
|
||||
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
|
||||
yaw_vector(0) += _ekf_gsf[model_index].W * cosf(_ekf_gsf[model_index].X(2));
|
||||
yaw_vector(1) += _ekf_gsf[model_index].W * sinf(_ekf_gsf[model_index].X(2));
|
||||
yaw_vector(0) += _model_weights(model_index) * cosf(_ekf_gsf[model_index].X(2));
|
||||
yaw_vector(1) += _model_weights(model_index) * sinf(_ekf_gsf[model_index].X(2));
|
||||
}
|
||||
_gsf_yaw = atan2f(yaw_vector(1),yaw_vector(0));
|
||||
|
||||
@ -133,8 +126,8 @@ void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vect
|
||||
// models with larger innovations are weighted less
|
||||
_gsf_yaw_variance = 0.0f;
|
||||
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
|
||||
float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw);
|
||||
_gsf_yaw_variance += _ekf_gsf[model_index].W * (_ekf_gsf[model_index].P(2,2) + yaw_delta * yaw_delta);
|
||||
const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw);
|
||||
_gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2,2) + yaw_delta * yaw_delta);
|
||||
}
|
||||
|
||||
// prevent the same velocity data being used more than once
|
||||
@ -145,33 +138,28 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index)
|
||||
{
|
||||
// generate attitude solution using simple complementary filter for the selected model
|
||||
|
||||
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;
|
||||
|
||||
// Accelerometer correction
|
||||
// Project 'k' unit vector of earth frame to body frame
|
||||
// Vector3f k = quaterion.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f));
|
||||
// Optimized version with dropped zeros
|
||||
const Vector3f k(_ahrs_ekf_gsf[model_index].R(2,0), _ahrs_ekf_gsf[model_index].R(2,1), _ahrs_ekf_gsf[model_index].R(2,2));
|
||||
const Dcmf R_to_body = _ahrs_ekf_gsf[model_index].R.transpose();
|
||||
const Vector3f gravity_direction_bf = R_to_body.col(2);
|
||||
|
||||
// Perform angular rate correction using accel data and reduce correction as accel magnitude moves away from 1 g (reduces drift when vehicle picked up and moved).
|
||||
// During fixed wing flight, compensate for centripetal acceleration assuming coordinated turns and X axis forward
|
||||
Vector3f tilt_correction = {};
|
||||
Vector3f tilt_correction;
|
||||
if (_ahrs_accel_fusion_gain > 0.0f) {
|
||||
|
||||
Vector3f accel = _ahrs_accel;
|
||||
|
||||
if (_true_airspeed > FLT_EPSILON) {
|
||||
// turn rate is component of gyro rate about vertical (down) axis
|
||||
const float turn_rate = _ahrs_ekf_gsf[model_index].R(2,0) * ang_rate(0)
|
||||
+ _ahrs_ekf_gsf[model_index].R(2,1) * ang_rate(1)
|
||||
+ _ahrs_ekf_gsf[model_index].R(2,2) * ang_rate(2);
|
||||
const float turn_rate = gravity_direction_bf.dot(ang_rate);
|
||||
|
||||
// use measured airspeed to calculate centripetal acceleration if available
|
||||
float centripetal_accel = _true_airspeed * turn_rate;
|
||||
const float centripetal_accel = _true_airspeed * turn_rate;
|
||||
|
||||
// project Y body axis onto horizontal and multiply by centripetal acceleration to give estimated
|
||||
// centripetal acceleration vector in earth frame due to coordinated turn
|
||||
Vector3f centripetal_accel_vec_ef = {_ahrs_ekf_gsf[model_index].R(0,1), _ahrs_ekf_gsf[model_index].R(1,1), 0.0f};
|
||||
Vector3f centripetal_accel_vec_ef(_ahrs_ekf_gsf[model_index].R(0,1), _ahrs_ekf_gsf[model_index].R(1,1), 0.0f);
|
||||
if (_ahrs_ekf_gsf[model_index].R(2,2) > 0.0f) {
|
||||
// vehicle is upright
|
||||
centripetal_accel_vec_ef *= centripetal_accel;
|
||||
@ -181,29 +169,26 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index)
|
||||
}
|
||||
|
||||
// rotate into body frame
|
||||
Vector3f centripetal_accel_vec_bf = _ahrs_ekf_gsf[model_index].R.transpose() * centripetal_accel_vec_ef;
|
||||
Vector3f centripetal_accel_vec_bf = R_to_body * centripetal_accel_vec_ef;
|
||||
|
||||
// correct measured accel for centripetal acceleration
|
||||
accel -= centripetal_accel_vec_bf;
|
||||
}
|
||||
|
||||
tilt_correction = (k % accel) * _ahrs_accel_fusion_gain / _ahrs_accel_norm;
|
||||
tilt_correction = (gravity_direction_bf % accel) * _ahrs_accel_fusion_gain / _ahrs_accel_norm;
|
||||
|
||||
}
|
||||
|
||||
// Gyro bias estimation
|
||||
const float gyro_bias_limit = 0.05f;
|
||||
constexpr float gyro_bias_limit = 0.05f;
|
||||
const float spinRate = ang_rate.length();
|
||||
if (spinRate < 0.175f) {
|
||||
_ahrs_ekf_gsf[model_index].gyro_bias -= tilt_correction * (_gyro_bias_gain * _delta_ang_dt);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_ahrs_ekf_gsf[model_index].gyro_bias(i) = math::constrain(_ahrs_ekf_gsf[model_index].gyro_bias(i), -gyro_bias_limit, gyro_bias_limit);
|
||||
}
|
||||
_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
|
||||
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);
|
||||
@ -218,17 +203,14 @@ 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
|
||||
Vector3f down_in_bf = -_delta_vel;
|
||||
down_in_bf.normalize();
|
||||
const Vector3f down_in_bf = -_delta_vel.normalized();
|
||||
|
||||
// Calculate earth frame North axis unit vector rotated into body frame, orthogonal to 'down_in_bf'
|
||||
// * operator is overloaded to provide a dot product
|
||||
const Vector3f i_vec_bf(1.0f,0.0f,0.0f);
|
||||
Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf * down_in_bf);
|
||||
Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf.dot(down_in_bf));
|
||||
north_in_bf.normalize();
|
||||
|
||||
// Calculate earth frame East axis unit vector rotated into body frame, orthogonal to 'down_in_bf' and 'north_in_bf'
|
||||
// % operator is overloaded to provide a cross product
|
||||
const Vector3f east_in_bf = down_in_bf % north_in_bf;
|
||||
|
||||
// Each column in a rotation matrix from earth frame to body frame represents the projection of the
|
||||
@ -240,6 +222,9 @@ void EKFGSF_yaw::ahrsAlignTilt()
|
||||
R.setRow(1, east_in_bf);
|
||||
R.setRow(2, down_in_bf);
|
||||
|
||||
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
|
||||
_ahrs_ekf_gsf[model_index].R = R;
|
||||
}
|
||||
}
|
||||
|
||||
void EKFGSF_yaw::ahrsAlignYaw()
|
||||
@ -371,13 +356,12 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
|
||||
const float &P22 = _ekf_gsf[model_index].P(2,2);
|
||||
|
||||
// calculate innovation variance
|
||||
_ekf_gsf[model_index].S(0,0) = P00 + velObsVar;
|
||||
_ekf_gsf[model_index].S(1,1) = P11 + velObsVar;
|
||||
_ekf_gsf[model_index].S(0,1) = P01;
|
||||
_ekf_gsf[model_index].S(1,0) = P10;
|
||||
matrix::SquareMatrix<float, 2> S = _ekf_gsf[model_index].P.slice<2, 2>(0, 0);
|
||||
S(0, 0) += velObsVar;
|
||||
S(1, 1) += velObsVar;
|
||||
|
||||
// Update the inverse of the innovation covariance matrix S_inverse
|
||||
updateInnovCovMatInv(model_index);
|
||||
updateInnovCovMatInv(model_index, S);
|
||||
|
||||
// Perform a chi-square innovation consistency test and calculate a compression scale factor that limits the magnitude of innovations to 5-sigma
|
||||
float innov_comp_scale_factor = 1.0f;
|
||||
@ -470,8 +454,10 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
|
||||
|
||||
// Correct the state vector and capture the change in yaw angle
|
||||
const float oldYaw = _ekf_gsf[model_index].X(2);
|
||||
|
||||
_ekf_gsf[model_index].X -= (K * _ekf_gsf[model_index].innov) * innov_comp_scale_factor;
|
||||
float yawDelta = _ekf_gsf[model_index].X(2) - oldYaw;
|
||||
|
||||
const float yawDelta = _ekf_gsf[model_index].X(2) - oldYaw;
|
||||
|
||||
// apply the change in yaw angle to the AHRS
|
||||
// take advantage of sparseness in the yaw rotation matrix
|
||||
@ -493,15 +479,14 @@ void EKFGSF_yaw::initialiseEKFGSF()
|
||||
{
|
||||
_gsf_yaw = 0.0f;
|
||||
_ekf_gsf_vel_fuse_started = false;
|
||||
_gsf_yaw_variance = M_PI_2_F * M_PI_2_F;
|
||||
_gsf_yaw_variance = _m_pi2 * _m_pi2;
|
||||
_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_F / (float)N_MODELS_EKFGSF;
|
||||
const float yaw_increment = 2.0f * _m_pi / (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_F + (0.5f * yaw_increment) + ((float)model_index * yaw_increment);
|
||||
|
||||
// All filter models start with the same weight
|
||||
_ekf_gsf[model_index].W = 1.0f / (float)N_MODELS_EKFGSF;
|
||||
_ekf_gsf[model_index].X(2) = -_m_pi + (0.5f * yaw_increment) + ((float)model_index * yaw_increment);
|
||||
|
||||
// take velocity states and corresponding variance from last meaurement
|
||||
_ekf_gsf[model_index].X(0) = _vel_NE(0);
|
||||
@ -516,31 +501,27 @@ void EKFGSF_yaw::initialiseEKFGSF()
|
||||
|
||||
float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const
|
||||
{
|
||||
// calculate inv(S) * innovation
|
||||
const matrix::Vector2f tempVec = _ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov;
|
||||
|
||||
// calculate transpose(innovation) * inv(S) * innovation
|
||||
// * operator is overloaded to provide a dot product
|
||||
const float normDist = _ekf_gsf[model_index].innov * tempVec;
|
||||
const float normDist = _ekf_gsf[model_index].innov.dot(_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov);
|
||||
|
||||
return M_TWOPI_INV * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist);
|
||||
return _m_2pi_inv * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist);
|
||||
}
|
||||
|
||||
void EKFGSF_yaw::updateInnovCovMatInv(const uint8_t model_index)
|
||||
void EKFGSF_yaw::updateInnovCovMatInv(const uint8_t model_index, const matrix::SquareMatrix<float, 2> &S)
|
||||
{
|
||||
// calculate determinant for innovation covariance matrix
|
||||
const float t2 = _ekf_gsf[model_index].S(0,0) * _ekf_gsf[model_index].S(1,1);
|
||||
const float t5 = _ekf_gsf[model_index].S(0,1) * _ekf_gsf[model_index].S(1,0);
|
||||
const float t2 = S(0,0) * S(1,1);
|
||||
const float t5 = S(0,1) * S(1,0);
|
||||
const float t3 = t2 - t5;
|
||||
|
||||
// calculate determinant inverse and protect against badly conditioned matrix
|
||||
_ekf_gsf[model_index].S_det_inverse = 1.0f / fmaxf(t3 , 1e-12f);
|
||||
|
||||
// calculate inv(S)
|
||||
_ekf_gsf[model_index].S_inverse(0,0) = _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(1,1);
|
||||
_ekf_gsf[model_index].S_inverse(1,1) = _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(0,0);
|
||||
_ekf_gsf[model_index].S_inverse(0,1) = - _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(0,1);
|
||||
_ekf_gsf[model_index].S_inverse(1,0) = - _ekf_gsf[model_index].S_det_inverse * _ekf_gsf[model_index].S(1,0);
|
||||
_ekf_gsf[model_index].S_inverse(0,0) = _ekf_gsf[model_index].S_det_inverse * S(1,1);
|
||||
_ekf_gsf[model_index].S_inverse(1,1) = _ekf_gsf[model_index].S_det_inverse * S(0,0);
|
||||
_ekf_gsf[model_index].S_inverse(0,1) = - _ekf_gsf[model_index].S_det_inverse * S(0,1);
|
||||
_ekf_gsf[model_index].S_inverse(1,0) = - _ekf_gsf[model_index].S_det_inverse * S(1,0);
|
||||
|
||||
}
|
||||
|
||||
@ -553,7 +534,7 @@ bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw
|
||||
yaw[model_index] = _ekf_gsf[model_index].X(2);
|
||||
innov_VN[model_index] = _ekf_gsf[model_index].innov(0);
|
||||
innov_VE[model_index] = _ekf_gsf[model_index].innov(1);
|
||||
weight[model_index] = _ekf_gsf[model_index].W;
|
||||
weight[model_index] = _model_weights(model_index);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
@ -562,26 +543,26 @@ bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw
|
||||
|
||||
Dcmf EKFGSF_yaw::taitBryan312ToRotMat(const Vector3f &rot312)
|
||||
{
|
||||
// Calculate the frame2 to frame 1 rotation matrix from a 312 rotation sequence
|
||||
const float c2 = cosf(rot312(2));
|
||||
const float s2 = sinf(rot312(2));
|
||||
const float s1 = sinf(rot312(1));
|
||||
const float c1 = cosf(rot312(1));
|
||||
const float s0 = sinf(rot312(0));
|
||||
const float c0 = cosf(rot312(0));
|
||||
// Calculate the frame2 to frame 1 rotation matrix from a 312 rotation sequence
|
||||
const float c2 = cosf(rot312(2));
|
||||
const float s2 = sinf(rot312(2));
|
||||
const float s1 = sinf(rot312(1));
|
||||
const float c1 = cosf(rot312(1));
|
||||
const float s0 = sinf(rot312(0));
|
||||
const float c0 = cosf(rot312(0));
|
||||
|
||||
Dcmf R;
|
||||
R(0, 0) = c0 * c2 - s0 * s1 * s2;
|
||||
R(1, 1) = c0 * c1;
|
||||
R(2, 2) = c2 * c1;
|
||||
R(0, 1) = -c1 * s0;
|
||||
R(0, 2) = s2 * c0 + c2 * s1 * s0;
|
||||
R(1, 0) = c2 * s0 + s2 * s1 * c0;
|
||||
R(1, 2) = s0 * s2 - s1 * c0 * c2;
|
||||
R(2, 0) = -s2 * c1;
|
||||
R(2, 1) = s1;
|
||||
Dcmf R;
|
||||
R(0, 0) = c0 * c2 - s0 * s1 * s2;
|
||||
R(1, 1) = c0 * c1;
|
||||
R(2, 2) = c2 * c1;
|
||||
R(0, 1) = -c1 * s0;
|
||||
R(0, 2) = s2 * c0 + c2 * s1 * s0;
|
||||
R(1, 0) = c2 * s0 + s2 * s1 * c0;
|
||||
R(1, 2) = s0 * s2 - s1 * c0 * c2;
|
||||
R(2, 0) = -s2 * c1;
|
||||
R(2, 1) = s1;
|
||||
|
||||
return R;
|
||||
return R;
|
||||
}
|
||||
|
||||
void EKFGSF_yaw::ahrsCalcAccelGain()
|
||||
@ -593,7 +574,7 @@ void EKFGSF_yaw::ahrsCalcAccelGain()
|
||||
// at the min and mas g value. Allow for more acceleration when flying as a fixed wing vehicle using centripetal
|
||||
// acceleration correction as higher and more sustained g will be experienced.
|
||||
// Use a quadratic finstead of linear unction to prevent vibration around 1g reducing the tilt correction effectiveness.
|
||||
float accel_g = _ahrs_accel_norm / CONSTANTS_ONE_G;
|
||||
const float accel_g = _ahrs_accel_norm / CONSTANTS_ONE_G;
|
||||
if (accel_g > 1.0f) {
|
||||
if (_true_airspeed > FLT_EPSILON && accel_g < 2.0f) {
|
||||
_ahrs_accel_fusion_gain = _tilt_gain * sq(2.0f - accel_g);
|
||||
@ -609,7 +590,7 @@ void EKFGSF_yaw::ahrsCalcAccelGain()
|
||||
}
|
||||
}
|
||||
|
||||
Matrix3f EKFGSF_yaw::ahrsPredictRotMat(Matrix3f &R, Vector3f &g)
|
||||
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);
|
||||
@ -648,7 +629,7 @@ bool EKFGSF_yaw::getYawData(float *yaw, float *yaw_variance)
|
||||
return false;
|
||||
}
|
||||
|
||||
void EKFGSF_yaw::setVelocity(Vector2f velocity, float accuracy)
|
||||
void EKFGSF_yaw::setVelocity(const Vector2f &velocity, float accuracy)
|
||||
{
|
||||
_vel_NE = velocity;
|
||||
_vel_accuracy = accuracy;
|
||||
|
||||
@ -4,6 +4,8 @@
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
#include "common.h"
|
||||
|
||||
using matrix::AxisAnglef;
|
||||
using matrix::Dcmf;
|
||||
using matrix::Eulerf;
|
||||
@ -13,35 +15,26 @@ using matrix::Vector2f;
|
||||
using matrix::Vector3f;
|
||||
using matrix::wrap_pi;
|
||||
|
||||
#define N_MODELS_EKFGSF 5
|
||||
static constexpr uint8_t N_MODELS_EKFGSF = 5;
|
||||
|
||||
#ifndef M_PI_F
|
||||
#define M_PI_F 3.14159265f
|
||||
#endif
|
||||
// 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;
|
||||
|
||||
#ifndef M_PI_2_F
|
||||
#define M_PI_2_F 1.57079632f
|
||||
#endif
|
||||
|
||||
#ifndef M_TWOPI_INV
|
||||
#define M_TWOPI_INV 0.159154943f
|
||||
#endif
|
||||
using namespace estimator;
|
||||
|
||||
class EKFGSF_yaw
|
||||
{
|
||||
public:
|
||||
// Constructor
|
||||
EKFGSF_yaw();
|
||||
|
||||
// Update Filter States - this should be called whenever new IMU data is available
|
||||
void update(const Vector3f del_ang, // IMU delta angle rotation vector meassured in body frame (rad)
|
||||
const Vector3f del_vel, // IMU delta velocity vector meassured in body frame (m/s)
|
||||
const float del_ang_dt, // time interval that del_ang was integrated over (sec)
|
||||
const float del_vel_dt, // time interval that del_vel was integrated over (sec)
|
||||
void update(const imuSample &imu_sample,
|
||||
bool run_EKF, // set to true when flying or movement is suitable for yaw estimation
|
||||
float airspeed); // true airspeed used for centripetal accel compensation - set to 0 when not required.
|
||||
|
||||
void setVelocity(Vector2f velocity, // NE velocity measurement (m/s)
|
||||
void setVelocity(const Vector2f &velocity, // NE velocity measurement (m/s)
|
||||
float accuracy); // 1-sigma accuracy of velocity measurement (m/s)
|
||||
|
||||
// get solution data for logging
|
||||
@ -57,6 +50,7 @@ public:
|
||||
bool getYawData(float *yaw, float *yaw_variance);
|
||||
|
||||
private:
|
||||
|
||||
// Parameters - these could be made tuneable
|
||||
const float _gyro_noise{1.0e-1f}; // yaw rate noise used for covariance prediction (rad/sec)
|
||||
const float _accel_noise{2.0f}; // horizontal accel noise used for covariance prediction (m/sec**2)
|
||||
@ -99,15 +93,13 @@ private:
|
||||
void ahrsAlignYaw();
|
||||
|
||||
// Efficient propagation of a delta angle in body frame applied to the body to earth frame rotation matrix
|
||||
Matrix3f ahrsPredictRotMat(Matrix3f &R, Vector3f &g);
|
||||
Matrix3f ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g);
|
||||
|
||||
// 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
|
||||
float W = 0.0f; // weighting
|
||||
matrix::SquareMatrix<float, 2> S; // innovation 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)
|
||||
@ -141,6 +133,7 @@ private:
|
||||
|
||||
// Declarations used by the Gaussian Sum Filter (GSF) that combines the individual EKF yaw estimates
|
||||
|
||||
matrix::Vector<float, N_MODELS_EKFGSF> _model_weights;
|
||||
float _gsf_yaw; // yaw estimate (rad)
|
||||
float _gsf_yaw_variance; // variance of yaw estimate (rad^2)
|
||||
|
||||
@ -148,6 +141,6 @@ private:
|
||||
float gaussianDensity(const uint8_t model_index) const;
|
||||
|
||||
// update the inverse of the innovation covariance matrix
|
||||
void updateInnovCovMatInv(const uint8_t model_index);
|
||||
void updateInnovCovMatInv(const uint8_t model_index, const matrix::SquareMatrix<float, 2> &S);
|
||||
|
||||
};
|
||||
|
||||
@ -290,10 +290,9 @@ public:
|
||||
|
||||
// Request the EKF reset the yaw to the estimate from the internal EKF-GSF filter
|
||||
// and reset the velocity and position states to the GPS. This will cause the EKF
|
||||
// to ignore the magnetomer for the remainder of flight.
|
||||
// to ignore the magnetometer for the remainder of flight.
|
||||
// This should only be used as a last resort before activating a loss of navigation failsafe
|
||||
// The counter must be incremented for each new reset request
|
||||
void requestEmergencyNavReset(uint8_t counter) override;
|
||||
void requestEmergencyNavReset() override;
|
||||
|
||||
private:
|
||||
|
||||
@ -846,7 +845,6 @@ private:
|
||||
|
||||
int64_t _emergency_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec)
|
||||
uint64_t _time_last_on_ground_us{0}; ///< last tine we were on the ground (uSec)
|
||||
uint8_t _yaw_extreset_counter{0}; // number of external emergency yaw reset requests
|
||||
bool _do_emergency_yaw_reset{false}; // true when an emergency yaw reset has been requested
|
||||
|
||||
// Call once per _imu_sample_delayed update after all main EKF data fusion oeprations have been completed
|
||||
|
||||
@ -1785,12 +1785,9 @@ bool Ekf::resetYawToEKFGSF()
|
||||
|
||||
}
|
||||
|
||||
void Ekf::requestEmergencyNavReset(uint8_t counter)
|
||||
void Ekf::requestEmergencyNavReset()
|
||||
{
|
||||
if (counter > _yaw_extreset_counter) {
|
||||
_yaw_extreset_counter = counter;
|
||||
_do_emergency_yaw_reset = true;
|
||||
}
|
||||
_do_emergency_yaw_reset = true;
|
||||
}
|
||||
|
||||
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
|
||||
@ -1830,11 +1827,11 @@ void Ekf::runYawEKFGSF()
|
||||
} else {
|
||||
TAS = _airspeed_sample_delayed.true_airspeed;
|
||||
}
|
||||
yawEstimator.update(_imu_sample_delayed.delta_ang, _imu_sample_delayed.delta_vel, _imu_sample_delayed.delta_ang_dt, _imu_sample_delayed.delta_vel_dt, _control_status.flags.in_air, TAS);
|
||||
|
||||
yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS);
|
||||
|
||||
// basic sanity check on GPS velocity data
|
||||
if (_gps_data_ready && _gps_sample_delayed.vacc > FLT_EPSILON && isfinite(_gps_sample_delayed.vel(0)) && isfinite(_gps_sample_delayed.vel(1))) {
|
||||
Vector2f vel_NE = Vector2f(_gps_sample_delayed.vel(0),_gps_sample_delayed.vel(1));
|
||||
yawEstimator.setVelocity(vel_NE, _gps_sample_delayed.vacc);
|
||||
yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc);
|
||||
}
|
||||
}
|
||||
|
||||
@ -417,7 +417,7 @@ public:
|
||||
|
||||
// request the EKF reset the yaw to the estimate from the internal EKF-GSF filter
|
||||
// argment should be incremented only when a new reset is required
|
||||
virtual void requestEmergencyNavReset(uint8_t counter) = 0;
|
||||
virtual void requestEmergencyNavReset() = 0;
|
||||
|
||||
// get ekf-gsf debug data
|
||||
virtual bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) = 0;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user