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

WindEstimator: fix state covariance prediction

Browse Source 
All the states are stationary so the discrete-time process noise Qk can
be directly added to the state covariance matrix P
This commit is contained in:
bresch 2022-05-04 14:24:26 +02:00 committed by Mathieu Bresciani
parent 14a2fdfe55
commit c890e8bf99
2 changed files with 8 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/lib/wind_estimator/WindEstimator.cpp
View File

@ -113,27 +113,17 @@ WindEstimator::update(uint64_t time_now)
}
float dt = (float)(time_now - _time_last_update) * 1e-6f;
float dt2 = dt * dt;
_time_last_update = time_now;
float q_w = _wind_p_var;
float q_k_tas = _tas_scale_p_var;
float SPP0 = dt * dt;
float SPP1 = SPP0 * q_w;
float SPP2 = SPP1 + _P(0, 1);
matrix::Matrix3f P_next;
P_next(0, 0) = SPP1 + _P(0, 0);
P_next(0, 1) = SPP2;
P_next(0, 2) = _P(0, 2);
P_next(1, 0) = SPP2;
P_next(1, 1) = SPP1 + _P(1, 1);
P_next(1, 2) = _P(1, 2);
P_next(2, 0) = _P(0, 2);
P_next(2, 1) = _P(1, 2);
P_next(2, 2) = SPP0 * q_k_tas + _P(2, 2);
_P = P_next;
matrix::Matrix3f Qk;
Qk(0, 0) = q_w * dt2;
Qk(1, 1) = Qk(0, 0);
Qk(2, 2) = q_k_tas * dt2;
_P += Qk;
}
void

6
src/lib/wind_estimator/python/wind_est_derivation.py
View File

@ -100,10 +100,8 @@ dt = Symbol("dt", real=True)
# process model. We only need to provide formula for covariance prediction
# create process noise matrix for covariance prediction
state_new = state + Matrix([q_w, q_w, q_k_tas]) * dt
Q = diag(q_w, q_k_tas)
L = state_new.jacobian([q_w, q_k_tas])
Q = L * Q * Transpose(L)
state_new = state
Q = diag(q_w, q_w, q_k_tas) * dt**2
# define symbolic covariance matrix
p00 = Symbol('_P(0,0)', real=True)