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::gui::DoubleSpinBox *Q_angle,*Q_gyro,*R_angle;
|
---|
82 | flair::core::Euler euler;
|
---|
83 | };
|
---|
84 |
|
---|
85 | #endif // AHRSKALMAN_IMPL_H
|
---|