refactor: #pragma once

This commit is contained in:
2024-10-29 09:29:11 +07:00
parent 2648fc3598
commit a131cfdd9c
20 changed files with 55 additions and 38 deletions

View File

@@ -1,37 +1,38 @@
// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
#pragma once
#include <BasicLinearAlgebra.h>
class Kalman2DFilter {
public:
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 };
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 };
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 };
Acc = {AccZInertial};
S = F * S + G * Acc;
P = F * P * ~F + Q;
L = H * P * ~H + R;
K = P * ~H * Inverse(L);
M = { AltitudeBarometer };
M = {AltitudeBarometer};
S = S + K * (M - H * S);
AltitudeKalman = S(0, 0);
VelocityVerticalKalman = S(1, 0);
P = (I - K * H) * P;
}
private:
private:
BLA::Matrix<2, 2> F;
BLA::Matrix<2, 1> G;
BLA::Matrix<2, 2> P;
@@ -44,4 +45,4 @@ class Kalman2DFilter {
BLA::Matrix<1, 1> R;
BLA::Matrix<1, 1> L;
BLA::Matrix<1, 1> M;
};
};

View File

@@ -1,4 +1,5 @@
// упрощённый Калман для одномерного случая
#pragma once
#ifndef _GKalman_h
#define _GKalman_h

View File

@@ -1,4 +1,5 @@
// быстрый медианный фильтр 3-го порядка
#pragma once
#ifndef _GMedian3_h
#define _GMedian3_h

View File

@@ -1,4 +1,5 @@
// экспоненциальное бегущее среднее
#pragma once
#ifndef _GFilterRA_h
#define _GFilterRA_h