fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 03:30:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF2: Error-State Kalman Filter (#22262)

Browse Source 
* ekf derivation: change to error state formulation
* ekf2: update auto-generated code for error-state
* ekf2: adjust ekf2 code for error state formulation
* ekf2_tests: adjust unit tests for error-state EKF
* update change indicator for error-state EKF
* ekf2_derivation: allow disabling mag and wind states

---------

Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>
This commit is contained in:
Mathieu Bresciani
2023-10-31 15:02:18 +01:00
committed by GitHub
parent d7f388e590
commit 0d6c2c8ce9
51 changed files with 2667 additions and 4089 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+4 -3
View File
@@ -1862,7 +1862,7 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sa
_ekf.getPositionVariance().copyTo(odom.position_variance);
// orientation covariance
_ekf.calcRotVecVariances().copyTo(odom.orientation_variance);
_ekf.getQuaternionVariance().copyTo(odom.orientation_variance);
odom.reset_counter = _ekf.get_quat_reset_count()
+ _ekf.get_velNE_reset_count() + _ekf.get_velD_reset_count()
@@ -1949,8 +1949,9 @@ void EKF2::PublishStates(const hrt_abstime &timestamp)
// publish estimator states
estimator_states_s states;
states.timestamp_sample = _ekf.time_delayed_us();
states.n_states = Ekf::getNumberOfStates();
_ekf.getStateAtFusionHorizonAsVector().copyTo(states.states);
const auto state_vector = _ekf.state().vector();
state_vector.copyTo(states.states);
states.n_states = state_vector.size();
_ekf.covariances_diagonal().copyTo(states.covariances);
states.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_states_pub.publish(states);