src/fusion-sensor/rotation_vector/fusion_utils/orientation_filter.h

   1 /*
   2  * sensord
   3  *
   4  * Copyright (c) 2014 Samsung Electronics Co., Ltd.
   5  *
   6  * Licensed under the Apache License, Version 2.0 (the "License");
   7  * you may not use this file except in compliance with the License.
   8  * You may obtain a copy of the License at
   9  *
  10  * http://www.apache.org/licenses/LICENSE-2.0
  11  *
  12  * Unless required by applicable law or agreed to in writing, software
  13  * distributed under the License is distributed on an "AS IS" BASIS,
  14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  * See the License for the specific language governing permissions and
  16  * limitations under the License.
  17  *
  18  */
  19
  20 #ifndef _ORIENTATION_FILTER_H_
  21 #define _ORIENTATION_FILTER_H_
  22
  23 #include "matrix.h"
  24 #include "vector.h"
  25 #include "sensor_data.h"
  26 #include "quaternion.h"
  27 #include "euler_angles.h"
  28 #include "rotation_matrix.h"
  29
  30 #define MOVING_AVERAGE_WINDOW_LENGTH    20
  31
  32 #define M3X3R   3
  33 #define M3X3C   3
  34 #define M6X6R   6
  35 #define M6X6C   6
  36 #define V1x3S   3
  37 #define V1x4S   4
  38 #define V1x6S   6
  39 #define ACCEL_SCALE 1
  40 #define GYRO_SCALE 1146
  41 #define GEOMAGNETIC_SCALE 1
  42
  43 template <typename TYPE>
  44 class orientation_filter {
  45 public:
  46         sensor_data<TYPE> m_accel;
  47         sensor_data<TYPE> m_gyro;
  48         sensor_data<TYPE> m_magnetic;
  49         vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_gyr_x;
  50         vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_gyr_y;
  51         vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_gyr_z;
  52         vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_roll;
  53         vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_pitch;
  54         vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_azimuth;
  55         matrix<TYPE, M6X6R, M6X6C> m_driv_cov;
  56         matrix<TYPE, M6X6R, M6X6C> m_aid_cov;
  57         matrix<TYPE, M6X6R, M6X6C> m_tran_mat;
  58         matrix<TYPE, M6X6R, M6X6C> m_measure_mat;
  59         matrix<TYPE, M6X6R, M6X6C> m_pred_cov;
  60         vect<TYPE, V1x6S> m_state_new;
  61         vect<TYPE, V1x6S> m_state_old;
  62         vect<TYPE, V1x6S> m_state_error;
  63         vect<TYPE, V1x3S> m_bias_correction;
  64         vect<TYPE, V1x3S> m_gyro_bias;
  65         quaternion<TYPE> m_quat_aid;
  66         quaternion<TYPE> m_quat_driv;
  67         rotation_matrix<TYPE> m_rot_matrix;
  68         euler_angles<TYPE> m_orientation;
  69         quaternion<TYPE> m_quat_9axis;
  70         quaternion<TYPE> m_quat_gaming_rv;
  71         quaternion<TYPE> m_quaternion;
  72         quaternion<TYPE>  m_quat_output;
  73         euler_angles<TYPE> m_euler_error;
  74         TYPE m_gyro_dt;
  75
  76         orientation_filter(void);
  77         ~orientation_filter(void);
  78
  79         inline void initialize_sensor_data(const sensor_data<TYPE> *accel,
  80                         const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
  81         inline void orientation_triad_algorithm(void);
  82         inline void compute_accel_orientation(void);
  83         inline void compute_covariance(void);
  84         inline void time_update(void);
  85         inline void time_update_gaming_rv(void);
  86         inline void measurement_update(void);
  87
  88         void get_device_orientation(const sensor_data<TYPE> *accel,
  89                         const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
  90 };
  91
  92 #include "orientation_filter.cpp"
  93
  94 #endif /* _ORIENTATION_FILTER_H_ */