dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

added LP filters (10Hz) on attitude rates in estimator

This commit is contained in:
Youssef Demitri 2015-06-24 14:37:58 +02:00
parent ad1158b548
commit da29b88a04
2 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -66,6 +66,8 @@
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <geo/geo.h>
#include <systemlib/perf_counter.h>
@ -258,6 +260,11 @@ private:
AttPosEKF *_ekf;
/* Low pass filter for attitude rates */
math::LowPassFilter2p _LP_att_P;
math::LowPassFilter2p _LP_att_Q;
math::LowPassFilter2p _LP_att_R;
private:
/**
* Update our local parameter cache.

16
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -200,9 +200,13 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_newRangeData(false),
_mavlink_fd(-1),
_parameters {},
_parameter_handles {},
_ekf(nullptr)
_parameters{},
_parameter_handles{},
_ekf(nullptr),
_LP_att_P(100.0f, 10.0f),
_LP_att_Q(100.0f, 10.0f),
_LP_att_R(100.0f, 10.0f)
{
_last_run = hrt_absolute_time();
@ -819,9 +823,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt;
_att.rollspeed = _LP_att_P.apply(_ekf->angRate.x) - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _LP_att_Q.apply(_ekf->angRate.y) - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _LP_att_R.apply(_ekf->angRate.z) - _ekf->states[12] / _ekf->dtIMUfilt;
// gyro offsets
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;