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

Remove resetStates(AndCovariances) function

Browse Source
This commit is contained in:
kamilritz 2019-12-13 16:26:42 +01:00 committed by Mathieu Bresciani
parent 64652f523b
commit 85e0e6e89c
3 changed files with 17 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
EKF/ekf.cpp
View File

@ -54,7 +54,20 @@ bool Ekf::init(uint64_t timestamp)
void Ekf::reset(uint64_t timestamp)
{
resetStatesAndCovariances();
_state.vel.setZero();
_state.pos.setZero();
_state.delta_ang_bias.setZero();
_state.delta_vel_bias.setZero();
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
_state.quat_nominal.setIdentity();
_output_new.vel.setZero();
_output_new.pos.setZero();
_output_new.quat_nominal.setIdentity();
initialiseCovariance();
_delta_angle_corr.setZero();
_imu_down_sampled.delta_ang.setZero();
@ -63,10 +76,7 @@ void Ekf::reset(uint64_t timestamp)
_imu_down_sampled.delta_vel_dt = 0.0f;
_imu_down_sampled.time_us = timestamp;
_q_down_sampled(0) = 1.0f;
_q_down_sampled(1) = 0.0f;
_q_down_sampled(2) = 0.0f;
_q_down_sampled(3) = 0.0f;
_q_down_sampled.setIdentity();
_imu_updated = false;
_NED_origin_initialised = false;
@ -90,30 +100,6 @@ 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.delta_ang_bias.setZero();
_state.delta_vel_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;
@ -153,7 +139,7 @@ bool Ekf::initialiseFilter()
{
// Keep accumulating measurements until we have a minimum of 10 samples for the required sensors
// Sum the IMU delta angle measurements
// Sum the IMU delta velocity measurements
const imuSample &imu_init = _imu_buffer.get_newest();
_delVel_sum += imu_init.delta_vel;

3
EKF/ekf.h
View File

@ -57,9 +57,6 @@ 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;

3
EKF/estimator_interface.h
View File

@ -63,8 +63,7 @@ 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;
virtual void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0;