fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 06:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: update wind covariance initialization

Browse Source 
Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
This commit is contained in:
CarlOlsson
2020-01-15 09:29:10 +01:00
committed by Paul Riseborough
parent 37201fb071
commit dbf0bd43e8
2 changed files with 36 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/covariance.cpp
+16 -19
View File
@@ -894,29 +894,26 @@ void Ekf::zeroMagCov()
void Ekf::resetWindCovariance()
{
if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
// Use airspeed and zer sideslip assumption to set initial covariance values for wind states
// Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py
// TODO: explicitly include the sideslip angle in the derivation
Eulerf euler321(_state.quat_nominal);
const float euler_yaw = euler321(2);
const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
const float initial_sideslip_uncertainty = math::radians(15.0f);
const float initial_wind_var_body_y = sq(_airspeed_sample_delayed.true_airspeed * sinf(initial_sideslip_uncertainty));
const float R_yaw = sq(math::radians(10.0f));
// calculate the wind speed and bearing
float spd = sqrtf(sq(_state.wind_vel(0)) + sq(_state.wind_vel(1)));
float yaw = atan2f(_state.wind_vel(1), _state.wind_vel(0));
// rotate wind velocity into earth frame aligned with vehicle yaw
const float Wx = _state.wind_vel(0) * cosf(euler_yaw) + _state.wind_vel(1) * sinf(euler_yaw);
const float Wy = -_state.wind_vel(0) * sinf(euler_yaw) + _state.wind_vel(1) * cosf(euler_yaw);
// calculate the uncertainty in wind speed and direction using the uncertainty in airspeed and sideslip angle
// used to calculate the initial wind speed
float R_spd = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
float R_yaw = sq(math::radians(10.0f));
// calculate the variance and covariance terms for the wind states
float cos_yaw = cosf(yaw);
float sin_yaw = sinf(yaw);
float cos_yaw_2 = sq(cos_yaw);
float sin_yaw_2 = sq(sin_yaw);
float sin_cos_yaw = sin_yaw * cos_yaw;
float spd_2 = sq(spd);
// it is safer to remove all existing correlations to other states at this time
P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
P(22,22) = R_yaw * spd_2 * sin_yaw_2 + R_spd * cos_yaw_2;
P(22,23) = - R_yaw * sin_cos_yaw * spd_2 + R_spd * sin_cos_yaw;
P(22,22) = R_TAS*sq(cosf(euler_yaw)) + R_yaw*sq(-Wx*sinf(euler_yaw) - Wy*cosf(euler_yaw)) + initial_wind_var_body_y*sq(sinf(euler_yaw));
P(22,23) = R_TAS*sinf(euler_yaw)*cosf(euler_yaw) + R_yaw*(-Wx*sinf(euler_yaw) - Wy*cosf(euler_yaw))*(Wx*cosf(euler_yaw) - Wy*sinf(euler_yaw)) - initial_wind_var_body_y*sinf(euler_yaw)*cosf(euler_yaw);
P(23,22) = P(22,23);
P(23,23) = R_yaw * spd_2 * cos_yaw_2 + R_spd * sin_yaw_2;
P(23,23) = R_TAS*sq(sinf(euler_yaw)) + R_yaw*sq(Wx*cosf(euler_yaw) - Wy*sinf(euler_yaw)) + initial_wind_var_body_y*sq(cosf(euler_yaw));
// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
P(22,22) += P(4,4);