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 MINIMUM_DELTA_T_S = 0.01f; // Minimum delta time for updates (10ms)
constexpr float MILLISECONDS_TO_SECONDS = 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 &altimeter);

    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 state_alt;
    float state_vel;
 
    // 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²).
    const float g = 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