4 * Copyright (c) 2014 Samsung Electronics Co., Ltd.
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
10 * http://www.apache.org/licenses/LICENSE-2.0
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.
20 #ifndef _ORIENTATION_FILTER_H
21 #define _ORIENTATION_FILTER_H
25 #include "sensor_data.h"
26 #include "quaternion.h"
27 #include "euler_angles.h"
28 #include "rotation_matrix.h"
30 template <typename TYPE>
31 class orientation_filter {
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;
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;
56 int m_pitch_phase_compensation;
57 int m_roll_phase_compensation;
58 int m_yaw_phase_compensation;
59 int m_magnetic_alignment_factor;
62 ~orientation_filter();
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();
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);
77 #include "orientation_filter.cpp"
79 #endif /* _ORIENTATION_FILTER_H */