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 #define MOVING_AVERAGE_WINDOW_LENGTH 20
40 #define GYRO_SCALE 1146
41 #define GEOMAGNETIC_SCALE 1
43 template <typename TYPE>
44 class orientation_filter {
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;
76 int m_magnetic_alignment_factor;
79 ~orientation_filter();
81 inline void initialize_sensor_data(const sensor_data<TYPE> *accel,
82 const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
83 inline void orientation_triad_algorithm();
84 inline void compute_accel_orientation();
85 inline void compute_covariance();
86 inline void time_update();
87 inline void time_update_gaming_rv();
88 inline void measurement_update();
90 void get_device_orientation(const sensor_data<TYPE> *accel,
91 const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
94 #include "orientation_filter.cpp"
96 #endif /* _ORIENTATION_FILTER_H_ */