blob: 1ca6cb0b55392770cee54daa381aa7f07e73ed4d (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
|
/*
altitude.h: Altitude estimation via barometer/accelerometer fusion
*/
# pragma once
#include "filters.h"
#include "algebra.h"
#include "pico/time.h"
#include "pico/types.h"
class AltitudeEstimator {
private:
// required parameters for the filters used for the estimations
// sensor's standard deviations
float sigmaAccel;
float sigmaGyro;
float sigmaBaro;
// Acceleration markov chain model state transition constant
float ca;
// Zero-velocity update acceleration threshold
float accelThreshold;
// gravity
float g = 9.81;
// For computing the sampling period
absolute_time_t prevTime = get_absolute_time();
uint32_t previousTime = to_us_since_boot(prevTime);
// required filters for altitude and vertical velocity estimation
KalmanFilter kalman;
ComplementaryFilter complementary;
// Estimated past vertical acceleration
float pastVerticalAccel = 0;
float pastVerticalVelocity = 0;
float pastAltitude = 0;
float pastGyro[3] = {0, 0, 0};
float pastAccel[3] = {0, 0, 0};
// estimated altitude and vertical velocity
float estimatedAltitude = 0;
float estimatedVelocity = 0;
public:
AltitudeEstimator(float sigmaAccel, float sigmaGyro, float sigmaBaro,
float ca, float accelThreshold);
void estimate(float accel[3], float gyro[3], float baroHeight, uint32_t timestamp);
float getAltitude();
float getVerticalVelocity();
float getVerticalAcceleration();
}; // class AltitudeEstimator
|
