forked from Archive/PX4-Autopilot
welford mean: convert to matrix only template
This commit is contained in:
parent
d4e5876d4c
commit
d3849c0265
|
@ -42,17 +42,17 @@
|
||||||
namespace math
|
namespace math
|
||||||
{
|
{
|
||||||
|
|
||||||
template<typename T>
|
template <typename Type, size_t N>
|
||||||
class WelfordMean
|
class WelfordMean
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
// For a new value, compute the new count, new mean, the new M2.
|
// For a new value, compute the new count, new mean, the new M2.
|
||||||
void update(const T &new_value)
|
void update(const matrix::Vector<Type, N> &new_value)
|
||||||
{
|
{
|
||||||
_count++;
|
_count++;
|
||||||
|
|
||||||
// mean accumulates the mean of the entire dataset
|
// mean accumulates the mean of the entire dataset
|
||||||
const T delta{new_value - _mean};
|
const matrix::Vector<Type, N> delta{new_value - _mean};
|
||||||
_mean += delta / _count;
|
_mean += delta / _count;
|
||||||
|
|
||||||
// M2 aggregates the squared distance from the mean
|
// M2 aggregates the squared distance from the mean
|
||||||
|
@ -71,12 +71,12 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
// Retrieve the mean, variance and sample variance
|
// Retrieve the mean, variance and sample variance
|
||||||
T mean() const { return _mean; }
|
matrix::Vector<Type, N> mean() const { return _mean; }
|
||||||
T variance() const { return _M2 / _count; }
|
matrix::Vector<Type, N> variance() const { return _M2 / _count; }
|
||||||
T sample_variance() const { return _M2 / (_count - 1); }
|
matrix::Vector<Type, N> sample_variance() const { return _M2 / (_count - 1); }
|
||||||
private:
|
private:
|
||||||
T _mean{};
|
matrix::Vector<Type, N> _mean{};
|
||||||
T _M2{};
|
matrix::Vector<Type, N> _M2{};
|
||||||
unsigned _count{0};
|
unsigned _count{0};
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -45,7 +45,7 @@ TEST(WelfordMeanTest, NoisySignal)
|
||||||
std::normal_distribution<float> standard_normal_distribution{0.f, std_dev};
|
std::normal_distribution<float> standard_normal_distribution{0.f, std_dev};
|
||||||
std::default_random_engine random_generator{}; // Pseudo-random generator with constant seed
|
std::default_random_engine random_generator{}; // Pseudo-random generator with constant seed
|
||||||
random_generator.seed(42);
|
random_generator.seed(42);
|
||||||
WelfordMean<Vector3f> welford{};
|
WelfordMean<float, 3> welford{};
|
||||||
|
|
||||||
for (int i = 0; i < 50; i++) {
|
for (int i = 0; i < 50; i++) {
|
||||||
const float noisy_value = standard_normal_distribution(random_generator);
|
const float noisy_value = standard_normal_distribution(random_generator);
|
||||||
|
|
|
@ -93,7 +93,7 @@ private:
|
||||||
uORB::SubscriptionMultiArray<sensor_gyro_s, MAX_SENSORS> _sensor_gyro_subs{ORB_ID::sensor_gyro};
|
uORB::SubscriptionMultiArray<sensor_gyro_s, MAX_SENSORS> _sensor_gyro_subs{ORB_ID::sensor_gyro};
|
||||||
|
|
||||||
calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {};
|
calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {};
|
||||||
math::WelfordMean<matrix::Vector3f> _gyro_mean[MAX_SENSORS] {};
|
math::WelfordMean<float, 3> _gyro_mean[MAX_SENSORS] {};
|
||||||
float _temperature[MAX_SENSORS] {};
|
float _temperature[MAX_SENSORS] {};
|
||||||
hrt_abstime _gyro_last_update[MAX_SENSORS] {};
|
hrt_abstime _gyro_last_update[MAX_SENSORS] {};
|
||||||
|
|
||||||
|
|
|
@ -119,13 +119,13 @@ private:
|
||||||
hrt_abstime _gyro_timestamp_sample_last{0};
|
hrt_abstime _gyro_timestamp_sample_last{0};
|
||||||
hrt_abstime _gyro_timestamp_last{0};
|
hrt_abstime _gyro_timestamp_last{0};
|
||||||
|
|
||||||
math::WelfordMean<matrix::Vector3f> _raw_accel_mean{};
|
math::WelfordMean<float, 3> _raw_accel_mean{};
|
||||||
math::WelfordMean<matrix::Vector3f> _raw_gyro_mean{};
|
math::WelfordMean<float, 3> _raw_gyro_mean{};
|
||||||
|
|
||||||
math::WelfordMean<matrix::Vector2f> _accel_interval_mean{};
|
math::WelfordMean<float, 2> _accel_interval_mean{};
|
||||||
math::WelfordMean<matrix::Vector2f> _gyro_interval_mean{};
|
math::WelfordMean<float, 2> _gyro_interval_mean{};
|
||||||
|
|
||||||
math::WelfordMean<matrix::Vector2f> _gyro_update_latency_mean{};
|
math::WelfordMean<float, 2> _gyro_update_latency_mean{};
|
||||||
|
|
||||||
float _accel_interval_best_variance{(float)INFINITY};
|
float _accel_interval_best_variance{(float)INFINITY};
|
||||||
float _gyro_interval_best_variance{(float)INFINITY};
|
float _gyro_interval_best_variance{(float)INFINITY};
|
||||||
|
|
Loading…
Reference in New Issue