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

EKF2: centralized auto-generated state (#22183)

Browse Source 
* ekf2_derivation: use single source of state definition

The state is defined as an ordered dictionary of group elements and
everything else is generated using that state definition

* ekf2: generated state sample add const reference getter

---------

Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>
Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Mathieu Bresciani
2023-10-06 10:28:21 -04:00
committed by GitHub
co-authored by bresch Daniel Agar
parent 2e1d5687f9
commit 05fd8c5976
15 changed files with 279 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+2 -2
View File
@@ -230,7 +230,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
SquareMatrixState nextP;
// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P,
sym::PredictCovariance(_state.vector(), P,
imu_delayed.delta_vel, imu_delayed.delta_vel_dt, d_vel_var,
imu_delayed.delta_ang, imu_delayed.delta_ang_dt, d_ang_var,
&nextP);
@@ -542,7 +542,7 @@ void Ekf::resetQuatCov(const float yaw_noise)
void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
{
matrix::SquareMatrix<float, State::quat_nominal.dof> q_cov;
sym::RotVarNedToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), rot_var_ned, &q_cov);
sym::RotVarNedToLowerTriangularQuatCov(_state.vector(), rot_var_ned, &q_cov);
q_cov.copyLowerToUpperTriangle();
resetStateCovariance<State::quat_nominal>(q_cov);
}