51 lines
1.3 KiB
C++
51 lines
1.3 KiB
C++
#include <BasicLinearAlgebra.h>
|
|
|
|
class Kalman2DFilter {
|
|
public:
|
|
Kalman2DFilter(float dt = 4.f, float accelUncertainty = 10.f, float barometerUncertainty = 100.f) {
|
|
dt /= 1000.f;
|
|
F = { 1, dt,
|
|
0, 1 };
|
|
G = { 0.5f * dt * dt,
|
|
dt };
|
|
H = { 1, 0 };
|
|
I = { 1, 0,
|
|
0, 1 };
|
|
Q = G * ~G * accelUncertainty * accelUncertainty;
|
|
R = { barometerUncertainty * barometerUncertainty };
|
|
P = { 0, 0,
|
|
0, 0 };
|
|
S = { 0,
|
|
0 };
|
|
}
|
|
|
|
void filter(const float &AccZInertial,
|
|
const float &AltitudeBarometer,
|
|
float &AltitudeKalman,
|
|
float &VelocityVerticalKalman) {
|
|
Acc = { AccZInertial };
|
|
S = F * S + G * Acc;
|
|
P = F * P * ~F + Q;
|
|
L = H * P * ~H + R;
|
|
K = P * ~H * Inverse(L);
|
|
M = { AltitudeBarometer };
|
|
S = S + K * (M - H * S);
|
|
AltitudeKalman = S(0, 0);
|
|
VelocityVerticalKalman = S(1, 0);
|
|
P = (I - K * H) * P;
|
|
}
|
|
|
|
private:
|
|
BLA::Matrix<2, 2> F;
|
|
BLA::Matrix<2, 1> G;
|
|
BLA::Matrix<2, 2> P;
|
|
BLA::Matrix<2, 2> Q;
|
|
BLA::Matrix<2, 1> S;
|
|
BLA::Matrix<1, 2> H;
|
|
BLA::Matrix<2, 2> I;
|
|
BLA::Matrix<1, 1> Acc;
|
|
BLA::Matrix<2, 1> K;
|
|
BLA::Matrix<1, 1> R;
|
|
BLA::Matrix<1, 1> L;
|
|
BLA::Matrix<1, 1> M;
|
|
}; |