fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 13:34:08 +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
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 14 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
AlphaFilter/AlphaFilter.hpp
View File

@ -45,9 +45,12 @@
#include <float.h>
template <typename T>
class AlphaFilter {
class AlphaFilter
{
public:
AlphaFilter() = default;
explicit AlphaFilter(float alpha) : _alpha(alpha) {}
~AlphaFilter() = default;
/**
@ -58,7 +61,8 @@ public:
* @param sample_interval interval between two samples
* @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;
if (denominator > FLT_EPSILON) {
@ -85,7 +89,11 @@ public:
*
* @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; }

3
EKF/ekf.cpp
View File

@ -48,9 +48,6 @@ bool Ekf::init(uint64_t timestamp)
{
bool ret = initialise_interface(timestamp);
reset();
_accel_lpf.setAlpha(.1f);
_gyro_lpf.setAlpha(.1f);
_mag_lpf.setAlpha(.1f);
return ret;
}

6
EKF/ekf.h
View File

@ -468,11 +468,11 @@ private:
bool _is_first_imu_sample{true};
uint32_t _baro_counter{0}; ///< number of baro 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> _gyro_lpf; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
AlphaFilter<Vector3f> _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s)
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
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 _baro_hgt_offset{0.0f}; ///< baro height reading at the local NED origin (m)