source: flair-src/trunk/lib/FlairFilter/src/unexported/AhrsKalman_impl.h @ 18

Last change on this file since 18 was 18, checked in by Sanahuja Guillaume, 5 years ago

ahrs kalman

File size: 2.6 KB
Line 
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
19namespace 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
33class 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
Note: See TracBrowser for help on using the repository browser.