src/sensor_fusion/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 template <typename TYPE>
  31 class orientation_filter {
  32 public:
  33         sensor_data<TYPE> m_filt_accel[2];
  34         sensor_data<TYPE> m_filt_gyro[2];
  35         sensor_data<TYPE> m_filt_magnetic[2];
  36         vect<TYPE> m_var_gyr_x;
  37         vect<TYPE> m_var_gyr_y;
  38         vect<TYPE> m_var_gyr_z;
  39         vect<TYPE> m_var_roll;
  40         vect<TYPE> m_var_pitch;
  41         vect<TYPE> m_var_yaw;
  42         matrix<TYPE> m_driv_cov;
  43         matrix<TYPE> m_aid_cov;
  44         matrix<TYPE> m_tran_mat;
  45         matrix<TYPE> m_measure_mat;
  46         matrix<TYPE> m_pred_cov;
  47         vect<TYPE> m_state_new;
  48         vect<TYPE> m_state_old;
  49         vect<TYPE> m_state_error;
  50         vect<TYPE> m_bias_correction;
  51         quaternion<TYPE> m_quat_aid;
  52         quaternion<TYPE> m_quat_driv;
  53         rotation_matrix<TYPE> m_rot_matrix;
  54         euler_angles<TYPE> m_orientation;
  55
  56         int m_pitch_phase_compensation;
  57         int m_roll_phase_compensation;
  58         int m_yaw_phase_compensation;
  59         int m_magnetic_alignment_factor;
  60
  61         orientation_filter();
  62         ~orientation_filter();
  63
  64         inline void filter_sensor_data(const sensor_data<TYPE> accel,
  65                         const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic);
  66         inline void orientation_triad_algorithm();
  67         inline void compute_covariance();
  68         inline void time_update();
  69         inline void measurement_update();
  70
  71         euler_angles<TYPE> get_orientation(const sensor_data<TYPE> accel,
  72                         const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic);
  73         rotation_matrix<TYPE> get_rotation_matrix(const sensor_data<TYPE> accel,
  74                         const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic);
  75 };
  76
  77 #include "orientation_filter.cpp"
  78
  79 #endif /* _ORIENTATION_FILTER_H */