1 | // %flair:license{ |
---|
2 | // This file is part of the Flair framework distributed under the |
---|
3 | // CECILL-C License, Version 1.0. |
---|
4 | // %flair:license} |
---|
5 | /*! |
---|
6 | * \file AhrsKalman_impl.h |
---|
7 | * \brief Class defining an Ahrs Kalman filter |
---|
8 | * \author Tom Pycke, API changes by Guillaume Sanahuja to fit the Flair framework |
---|
9 | * \date 2007/10/17, 2014/01/15 |
---|
10 | * \version |
---|
11 | */ |
---|
12 | |
---|
13 | #ifndef AHRSKALMAN_IMPL_H |
---|
14 | #define AHRSKALMAN_IMPL_H |
---|
15 | |
---|
16 | #include <Ahrs.h> |
---|
17 | #include <Euler.h> |
---|
18 | |
---|
19 | namespace flair { |
---|
20 | namespace core { |
---|
21 | class AhrsData; |
---|
22 | } |
---|
23 | namespace gui { |
---|
24 | class Layout; |
---|
25 | class DoubleSpinBox; |
---|
26 | } |
---|
27 | } |
---|
28 | |
---|
29 | /*! \class AhrsKalman_impl |
---|
30 | * \brief Class defining an Ahrs Kalman filter |
---|
31 | */ |
---|
32 | |
---|
33 | class AhrsKalman_impl { |
---|
34 | public: |
---|
35 | AhrsKalman_impl(const flair::gui::Layout *layout,flair::core::AhrsData *ahrsData); |
---|
36 | ~AhrsKalman_impl(); |
---|
37 | void UpdateFrom(const flair::core::io_data *data); |
---|
38 | |
---|
39 | private: |
---|
40 | flair::core::AhrsData *ahrsData; |
---|
41 | typedef struct ars_Gyro1DKalman |
---|
42 | { |
---|
43 | /* These variables represent our state matrix x */ |
---|
44 | double x_angle, |
---|
45 | x_bias; |
---|
46 | |
---|
47 | /* Our error covariance matrix */ |
---|
48 | double P_00, |
---|
49 | P_01, |
---|
50 | P_10, |
---|
51 | P_11; |
---|
52 | |
---|
53 | /* |
---|
54 | * Q is a 2x2 matrix of the covariance. Because we |
---|
55 | * assume the gyro and accelero noise to be independend |
---|
56 | * of eachother, the covariances on the / diagonal are 0. |
---|
57 | * |
---|
58 | * Covariance Q, the process noise, from the assumption |
---|
59 | * x = F x + B u + w |
---|
60 | * with w having a normal distribution with covariance Q. |
---|
61 | * (covariance = E[ (X - E[X])*(X - E[X])' ] |
---|
62 | * We assume is linair with dt |
---|
63 | */ |
---|
64 | double Q_angle, Q_gyro; |
---|
65 | /* |
---|
66 | * Covariance R, our observation noise (from the accelerometer) |
---|
67 | * Also assumed to be linair with dt |
---|
68 | */ |
---|
69 | double R_angle; |
---|
70 | } ars_Gyro1DKalman; |
---|
71 | |
---|
72 | // Initializing the struct |
---|
73 | void ars_Init(ars_Gyro1DKalman *filterdata, double Q_angle, double Q_gyro, double R_angle); |
---|
74 | // Kalman predict |
---|
75 | void ars_predict(ars_Gyro1DKalman *filterdata, const double gyro, const double dt); |
---|
76 | // Kalman update |
---|
77 | void ars_update(ars_Gyro1DKalman *filterdata, const double angle_m); |
---|
78 | |
---|
79 | ars_Gyro1DKalman ars_roll; |
---|
80 | ars_Gyro1DKalman ars_pitch; |
---|
81 | flair::core::Time previous_time; |
---|
82 | bool is_init; |
---|
83 | flair::gui::DoubleSpinBox *Q_angle,*Q_gyro,*R_angle; |
---|
84 | flair::core::Euler euler; |
---|
85 | }; |
---|
86 | |
---|
87 | #endif // AHRSKALMAN_IMPL_H |
---|