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

Last change on this file was 223, checked in by Sanahuja Guillaume, 4 years ago

add delta time to io_data

File size: 2.5 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::gui::DoubleSpinBox *Q_angle,*Q_gyro,*R_angle;
82 flair::core::Euler euler;
83};
84
85#endif // AHRSKALMAN_IMPL_H
Note: See TracBrowser for help on using the repository browser.