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

ekf: Extract resetState from main reset and reinitialize

Browse Source 
the covariances in the init function
This commit is contained in:
bresch 2019-10-24 17:39:26 +02:00 committed by Mathieu Bresciani
parent bf48004fb9
commit a6840655e8
3 changed files with 30 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
EKF/ekf.cpp
View File

@ -54,20 +54,7 @@ bool Ekf::init(uint64_t timestamp)
void Ekf::reset(uint64_t timestamp)
{
_state.vel.setZero();
_state.pos.setZero();
_state.gyro_bias.setZero();
_state.accel_bias.setZero();
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
_state.quat_nominal.setZero();
_state.quat_nominal(0) = 1.0f;
_output_new.vel.setZero();
_output_new.pos.setZero();
_output_new.quat_nominal.setZero();
_output_new.quat_nominal(0) = 1.0f;
resetStatesAndCovariances();
_delta_angle_corr.setZero();
_imu_down_sampled.delta_ang.setZero();
@ -103,6 +90,30 @@ void Ekf::reset(uint64_t timestamp)
_prev_dvel_bias_var.zero();
}
void Ekf::resetStatesAndCovariances()
{
resetStates();
initialiseCovariance();
}
void Ekf::resetStates()
{
_state.vel.setZero();
_state.pos.setZero();
_state.gyro_bias.setZero();
_state.accel_bias.setZero();
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
_state.quat_nominal.setZero();
_state.quat_nominal(0) = 1.0f;
_output_new.vel.setZero();
_output_new.pos.setZero();
_output_new.quat_nominal.setZero();
_output_new.quat_nominal(0) = 1.0f;
}
bool Ekf::update()
{
bool updated = false;

3
EKF/ekf.h
View File

@ -57,6 +57,9 @@ public:
// set the internal states and status to their default value
void reset(uint64_t timestamp) override;
void resetStatesAndCovariances() override;
void resetStates() override;
// should be called every time new data is pushed into the filter
bool update() override;

2
EKF/estimator_interface.h
View File

@ -60,6 +60,8 @@ public:
virtual bool init(uint64_t timestamp) = 0;
virtual void reset(uint64_t timestamp) = 0;
virtual void resetStates() = 0;
virtual void resetStatesAndCovariances() = 0;
virtual bool update() = 0;
// gets the innovations of velocity and position measurements