[324] | 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
|
---|