fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 23:14:06 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
EKF/covariance.cpp
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);

20
EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py Normal file
View File

@ -0,0 +1,20 @@
import sympy as sp
Wx, Wy, yaw, R_TAS, initial_wind_var_body_y, R_yaw = sp.symbols('Wx Wy yaw R_TAS initial_wind_var_body_y R_yaw')
Wn = Wx * sp.cos(yaw) - Wy * sp.sin(yaw)
We = Wx * sp.sin(yaw) + Wy * sp.cos(yaw)
Wn_Wx = sp.diff(Wn, Wx)
Wn_Wy = sp.diff(Wn, Wy)
Wn_yaw = sp.diff(Wn, yaw)
We_Wx = sp.diff(We, Wx)
We_Wy = sp.diff(We, Wy)
We_yaw = sp.diff(We, yaw)
G = sp.Matrix([[Wn_Wx, Wn_Wy, Wn_yaw],[We_Wx, We_Wy, We_yaw]])
b_wind_cov = sp.Matrix([[R_TAS, 0.0, 0.0], [0.0,initial_wind_var_body_y, 0.0], [0.0, 0.0, R_yaw]])
i_wind_cov = G * b_wind_cov * G.T
print('P[22][22] = ' + str(i_wind_cov[0,0]))
print('P[22][23] = ' + str(i_wind_cov[0,1]))
print('P[23][22] = ' + str(i_wind_cov[1,0]))
print('P[23][23] = ' + str(i_wind_cov[1,1]))