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 vector<TYPE> m_var_gyr_x;
37 vector<TYPE> m_var_gyr_y;
38 vector<TYPE> m_var_gyr_z;
39 vector<TYPE> m_var_roll;
40 vector<TYPE> m_var_pitch;
41 vector<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 vector<TYPE> m_state_new;
48 vector<TYPE> m_state_old;
49 vector<TYPE> m_state_error;
50 vector<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;
57 ~orientation_filter();
59 inline void filter_sensor_data(const sensor_data<TYPE> accel,
60 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic);
61 inline void orientation_triad_algorithm();
62 inline void compute_covariance();
63 inline void time_update();
64 inline void measurement_update();
66 euler_angles<TYPE> get_orientation(const sensor_data<TYPE> accel,
67 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic);
68 rotation_matrix<TYPE> get_rotation_matrix(const sensor_data<TYPE> accel,
69 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic);
72 #include "orientation_filter.cpp"
74 #endif /* _ORIENTATION_FILTER_H */