fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 01:50:35 +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 16:28:21 +02:00
committed by GitHub
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/ekf_helper.cpp
+1 -16
View File
@@ -369,21 +369,6 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
}
#endif // CONFIG_EKF2_AUXVEL
// get the state vector at the delayed time horizon
matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
{
matrix::Vector<float, 24> state;
state.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) = _state.quat_nominal;
state.slice<State::vel.dof, 1>(State::vel.idx, 0) = _state.vel;
state.slice<State::pos.dof, 1>(State::pos.idx, 0) = _state.pos;
state.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx, 0) = _state.gyro_bias;
state.slice<State::accel_bias.dof, 1>(State::accel_bias.idx, 0) = _state.accel_bias;
state.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) = _state.mag_I;
state.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = _state.mag_B;
state.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = _state.wind_vel;
return state;
}
bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const
{
origin_time = _pos_ref.getProjectionReferenceTimestamp();
@@ -907,7 +892,7 @@ void Ekf::updateVerticalDeadReckoningStatus()
Vector3f Ekf::calcRotVecVariances() const
{
Vector3f rot_var;
sym::QuatVarToRotVar(getStateAtFusionHorizonAsVector(), P, FLT_EPSILON, &rot_var);
sym::QuatVarToRotVar(_state.vector(), P, FLT_EPSILON, &rot_var);
return rot_var;
}