VerticalVelocityEstimator.h

Go to the documentation of this file.

#ifndef VELOCITY_ESTIMATOR_H
#define VELOCITY_ESTIMATOR_H
 
#include <array>
#include <cstdint>
 
#include "data_handling/DataPoint.h"
#include "state_estimation/StateEstimationTypes.h"
 
constexpr float kMillisecondsToSeconds = 0.001f; // Conversion factor from milliseconds to seconds
 
struct alignas(8) NoiseVariances {
    float accelNoiseVar;
    float altimeterNoiseVar;
};
 
struct InitialState {
    float initialAltitude;
    uint32_t initialTimestamp;
};
 
 

class VerticalVelocityEstimator {
public:
    VerticalVelocityEstimator(NoiseVariances noise = {1.0f, 0.1f});

    void init(InitialState initialState);

    void update(const AccelerationTriplet& accel,
                const DataPoint &altitude);

    virtual float getEstimatedAltitude() const;

    virtual float getEstimatedVelocity() const;

    virtual uint32_t getTimestamp() const;

    virtual float getInertialVerticalAcceleration() const;

    virtual int8_t getVerticalAxis() const;

    virtual int8_t getVerticalDirection() const;
 
private:
    void determineVerticalAxis(const std::array<float, 3>& rawAcl);
 
private:
    // Kalman filter state: altitude (m), vertical velocity (m/s).
    float stateAltitude_m_;
    float stateVelocity_mps_;
 
    // Covariance matrix (2x2).
    float P_[2][2] = {{}, {}};
 
    // Time of last update (milliseconds).
    uint32_t lastTimestamp_ms_;
 
    // True after init() has been called.
    bool initialized_;
 
    // Noise parameters.
    float accelNoiseVariance_;      // Acceleration noise variance (process noise).
    float altimeterNoiseVariance_;  // Altimeter noise variance (measurement noise).
 
    // Gravity constant (m/s²).
    static constexpr float kGravity_mps2 = 9.81f;
 
    // Which axis is vertical, and in what direction?
    int8_t verticalAxis_;  
    int8_t verticalDirection_;  
    bool verticalAxisDetermined_;
 
    // Latest computed inertial acceleration along the vertical axis.
    float inertialVerticalAcceleration_;
};
 
#endif // VELOCITY_ESTIMATOR_H