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

AlphaFilter add constructor that takes alpha and use in EKF

Browse Source
This commit is contained in:
Daniel Agar
2020-12-16 13:55:59 -05:00
committed by GitHub
parent 37e84b763a
commit 1237087d70
3 changed files with 14 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
AlphaFilter/AlphaFilter.hpp
+11 -3
View File
@@ -45,9 +45,12 @@
#include <float.h> #include <float.h>
template <typename T> template <typename T>
class AlphaFilter { class AlphaFilter
{
public: public:
AlphaFilter() = default; AlphaFilter() = default;
explicit AlphaFilter(float alpha) : _alpha(alpha) {}
~AlphaFilter() = default; ~AlphaFilter() = default;
/** /**
@@ -58,7 +61,8 @@ public:
* @param sample_interval interval between two samples * @param sample_interval interval between two samples
* @param time_constant filter time constant determining convergence * @param time_constant filter time constant determining convergence
*/ */
void setParameters(float sample_interval, float time_constant) { void setParameters(float sample_interval, float time_constant)
{
const float denominator = time_constant + sample_interval; const float denominator = time_constant + sample_interval;
if (denominator > FLT_EPSILON) { if (denominator > FLT_EPSILON) {
@@ -85,7 +89,11 @@ public:
* *
* @return retrieve the filtered result * @return retrieve the filtered result
*/ */
void update(const T &sample) { _filter_state = updateCalculation(sample); } const T &update(const T &sample)
{
_filter_state = updateCalculation(sample);
return _filter_state;
}
const T &getState() const { return _filter_state; } const T &getState() const { return _filter_state; }
EKF/ekf.cpp
-3
View File
@@ -48,9 +48,6 @@ bool Ekf::init(uint64_t timestamp)
{ {
bool ret = initialise_interface(timestamp); bool ret = initialise_interface(timestamp);
reset(); reset();
_accel_lpf.setAlpha(.1f);
_gyro_lpf.setAlpha(.1f);
_mag_lpf.setAlpha(.1f);
return ret; return ret;
} }
EKF/ekf.h
+3 -3
View File
@@ -468,11 +468,11 @@ private:
bool _is_first_imu_sample{true}; bool _is_first_imu_sample{true};
uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation
uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation
AlphaFilter<Vector3f> _accel_lpf; ///< filtered accelerometer measurement used to align tilt (m/s/s) AlphaFilter<Vector3f> _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s)
AlphaFilter<Vector3f> _gyro_lpf; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) AlphaFilter<Vector3f> _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
// Variables used to perform in flight resets and switch between height sources // Variables used to perform in flight resets and switch between height sources
AlphaFilter<Vector3f> _mag_lpf; ///< filtered magnetometer measurement for instant reset (Gauss) AlphaFilter<Vector3f> _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss)
float _hgt_sensor_offset{0.0f}; ///< set as necessary if desired to maintain the same height after a height reset (m) float _hgt_sensor_offset{0.0f}; ///< set as necessary if desired to maintain the same height after a height reset (m)
float _baro_hgt_offset{0.0f}; ///< baro height reading at the local NED origin (m) float _baro_hgt_offset{0.0f}; ///< baro height reading at the local NED origin (m)