dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: remove sparse vector optimization 

The sparse vector template requires to know which states are non-zero in
the observation jacobian. This complicates the modularity of the code
when the state vector or the derivation is changed.
The computation cost difference is almost negligible for this size.
This commit is contained in:
bresch 2023-09-29 14:38:04 +02:00 committed by Daniel Agar
parent abfd00aeb9
commit 10db6b6eda
4 changed files with 4 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/ekf2/EKF/ekf.h
View File

@ -63,9 +63,6 @@ public:
typedef matrix::Vector<float, State::size> VectorState;
typedef matrix::SquareMatrix<float, State::size> SquareMatrixState;
typedef matrix::SquareMatrix<float, 2> Matrix2f;
template<int ... Idxs>
using SparseVectorState = matrix::SparseVectorf<State::size, Idxs...>;
Ekf()
{

4
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
View File

@ -100,8 +100,6 @@ void Ekf::fuseGpsYaw()
sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
}
const SparseVectorState<0,1,2,3> Hfusion(H);
// check if the innovation variance calculation is badly conditioned
if (gnss_yaw.innovation_variance < gnss_yaw.observation_variance) {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
@ -129,7 +127,7 @@ void Ekf::fuseGpsYaw()
// calculate the Kalman gains
// only calculate gains for states we are using
VectorState Kfusion = P * Hfusion / gnss_yaw.innovation_variance;
VectorState Kfusion = P * H / gnss_yaw.innovation_variance;
const bool is_fused = measurementUpdate(Kfusion, gnss_yaw.innovation_variance, gnss_yaw.innovation);
_fault_status.flags.bad_hdg = !is_fused;

10
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -62,11 +62,9 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
Vector3f innov_var;
// Observation jacobian and Kalman gain vectors
SparseVectorState<0,1,2,3,16,17,18,19,20,21> Hfusion;
VectorState H;
const VectorState state_vector = getStateAtFusionHorizonAsVector();
sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
Hfusion = H;
innov_var.copyTo(aid_src_mag.innovation_variance);
@ -162,7 +160,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
} else if (index == 1) {
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H);
Hfusion = H;
// recalculate innovation using the updated state
aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
@ -191,7 +188,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H);
Hfusion = H;
// recalculate innovation using the updated state
aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
@ -212,7 +208,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
}
}
VectorState Kfusion = P * Hfusion / aid_src_mag.innovation_variance[index];
VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index];
if (!update_all_states) {
for (unsigned row = 0; row <= 15; row++) {
@ -275,10 +271,8 @@ bool Ekf::fuseDeclination(float decl_sigma)
return false;
}
SparseVectorState<16,17> Hfusion(H);
// Calculate the Kalman gains
VectorState Kfusion = P * Hfusion / innovation_variance;
VectorState Kfusion = P * H / innovation_variance;
const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation);

3
src/modules/ekf2/EKF/optflow_fusion.cpp
View File

@ -146,8 +146,7 @@ void Ekf::fuseOptFlow()
}
}
SparseVectorState<0,1,2,3,4,5,6> Hfusion(H);
VectorState Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[index];
VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index];
if (measurementUpdate(Kfusion, _aid_src_optical_flow.innovation_variance[index], _aid_src_optical_flow.innovation[index])) {
fused[index] = true;