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.
21 #ifdef _ORIENTATION_FILTER_H
23 #include "orientation_filter.h"
25 #define MOVING_AVERAGE_WINDOW_LENGTH 20
26 #define GRAVITY 9.80665
28 #define NON_ZERO_VAL 0.1
29 #define US2S (1.0 / 1000000.0)
30 #define SAMPLE_FREQ 100000
33 // Systron-donner "Horizon"
34 #define ZIGMA_W (0.05 * DEG2RAD)//deg/s
35 #define TAU_W 1000//secs
37 //#define ZIGMA_W 0.05 * DEG2RAD //deg/s
38 //#define TAU_W 300 //secs
39 // FOGs (KVH Autogyro and Crossbow DMU-FOG)
40 //#define ZIGMA_W 0 //deg/s
42 #define QWB_CONST ((2 * (ZIGMA_W * ZIGMA_W)) / TAU_W)
43 #define F_CONST (-1 / TAU_W)
45 #define ENABLE_LPF false
57 template <typename TYPE>
58 orientation_filter<TYPE>::orientation_filter()
60 TYPE arr[MOVING_AVERAGE_WINDOW_LENGTH];
62 std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL);
64 vect<TYPE> vec(MOVING_AVERAGE_WINDOW_LENGTH, arr);
65 vect<TYPE> vec1x3(V1x3S);
66 vect<TYPE> vec1x6(V1x6S);
67 matrix<TYPE> mat6x6(M6X6R, M6X6C);
77 m_measure_mat = mat6x6;
82 m_bias_correction = vec1x3;
85 m_state_error = vec1x6;
87 m_pitch_phase_compensation = 1;
88 m_roll_phase_compensation = 1;
89 m_yaw_phase_compensation = 1;
90 m_magnetic_alignment_factor = 1;
93 template <typename TYPE>
94 orientation_filter<TYPE>::~orientation_filter()
99 template <typename TYPE>
100 inline void orientation_filter<TYPE>::filter_sensor_data(const sensor_data<TYPE> accel,
101 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
103 const TYPE iir_b[] = {0.98, 0};
104 const TYPE iir_a[] = {1.0000000, 0.02};
106 vect<TYPE> acc_data(V1x3S);
107 vect<TYPE> gyr_data(V1x3S);
109 m_filt_accel[0] = m_filt_accel[1];
110 m_filt_gyro[0] = m_filt_gyro[1];
111 m_filt_magnetic[0] = m_filt_magnetic[1];
115 m_filt_accel[1].m_data = accel.m_data * iir_b[0] - m_filt_accel[0].m_data * iir_a[1];
116 m_filt_gyro[1].m_data = gyro.m_data * iir_b[0] - m_filt_gyro[0].m_data * iir_a[1];
117 m_filt_magnetic[1].m_data = magnetic.m_data * iir_b[0] - m_filt_magnetic[0].m_data * iir_a[1];
121 m_filt_accel[1].m_data = accel.m_data;
122 m_filt_gyro[1].m_data = gyro.m_data;
123 m_filt_magnetic[1].m_data = magnetic.m_data;
126 m_filt_accel[1].m_time_stamp = accel.m_time_stamp;
127 m_filt_gyro[1].m_time_stamp = accel.m_time_stamp;
128 m_filt_magnetic[1].m_time_stamp = accel.m_time_stamp;
130 m_filt_gyro[1].m_data = m_filt_gyro[1].m_data - m_bias_correction;
133 template <typename TYPE>
134 inline void orientation_filter<TYPE>::orientation_triad_algorithm()
136 TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
137 TYPE arr_mag_e[V1x3S] = {0.0, (TYPE) m_magnetic_alignment_factor, 0.0};
139 vect<TYPE> acc_e(V1x3S, arr_acc_e);
140 vect<TYPE> mag_e(V1x3S, arr_mag_e);
142 vect<TYPE> acc_b_x_mag_b = cross(m_filt_accel[1].m_data, m_filt_magnetic[1].m_data);
143 vect<TYPE> acc_e_x_mag_e = cross(acc_e, mag_e);
145 vect<TYPE> cross1 = cross(acc_b_x_mag_b, m_filt_accel[1].m_data);
146 vect<TYPE> cross2 = cross(acc_e_x_mag_e, acc_e);
148 matrix<TYPE> mat_b(M3X3R, M3X3C);
149 matrix<TYPE> mat_e(M3X3R, M3X3C);
151 for(int i = 0; i < M3X3R; i++)
153 mat_b.m_mat[i][0] = m_filt_accel[1].m_data.m_vec[i];
154 mat_b.m_mat[i][1] = acc_b_x_mag_b.m_vec[i];
155 mat_b.m_mat[i][2] = cross1.m_vec[i];
156 mat_e.m_mat[i][0] = acc_e.m_vec[i];
157 mat_e.m_mat[i][1] = acc_e_x_mag_e.m_vec[i];
158 mat_e.m_mat[i][2] = cross2.m_vec[i];
161 matrix<TYPE> mat_b_t = tran(mat_b);
162 rotation_matrix<TYPE> rot_mat(mat_e * mat_b_t);
164 m_quat_aid = rot_mat2quat(rot_mat);
167 template <typename TYPE>
168 inline void orientation_filter<TYPE>::compute_covariance()
170 TYPE var_gyr_x, var_gyr_y, var_gyr_z;
171 TYPE var_roll, var_pitch, var_yaw;
173 insert_end(m_var_gyr_x, m_filt_gyro[1].m_data.m_vec[0]);
174 insert_end(m_var_gyr_y, m_filt_gyro[1].m_data.m_vec[1]);
175 insert_end(m_var_gyr_z, m_filt_gyro[1].m_data.m_vec[2]);
176 insert_end(m_var_roll, m_orientation.m_ang.m_vec[0]);
177 insert_end(m_var_pitch, m_orientation.m_ang.m_vec[1]);
178 insert_end(m_var_yaw, m_orientation.m_ang.m_vec[2]);
180 var_gyr_x = var(m_var_gyr_x);
181 var_gyr_y = var(m_var_gyr_y);
182 var_gyr_z = var(m_var_gyr_z);
183 var_roll = var(m_var_roll);
184 var_pitch = var(m_var_pitch);
185 var_yaw = var(m_var_yaw);
187 m_driv_cov.m_mat[0][0] = var_gyr_x;
188 m_driv_cov.m_mat[1][1] = var_gyr_y;
189 m_driv_cov.m_mat[2][2] = var_gyr_z;
190 m_driv_cov.m_mat[3][3] = (TYPE) QWB_CONST;
191 m_driv_cov.m_mat[4][4] = (TYPE) QWB_CONST;
192 m_driv_cov.m_mat[5][5] = (TYPE) QWB_CONST;
194 m_aid_cov.m_mat[0][0] = var_roll;
195 m_aid_cov.m_mat[1][1] = var_pitch;
196 m_aid_cov.m_mat[2][2] = var_yaw;
199 template <typename TYPE>
200 inline void orientation_filter<TYPE>::time_update()
202 quaternion<TYPE> quat_diff, quat_error;
203 euler_angles<TYPE> euler_error;
204 euler_angles<TYPE> orientation;
205 unsigned long long sample_interval_gyro = SAMPLE_FREQ;
208 if (m_filt_gyro[1].m_time_stamp != 0 && m_filt_gyro[0].m_time_stamp != 0)
209 sample_interval_gyro = m_filt_gyro[1].m_time_stamp - m_filt_gyro[0].m_time_stamp;
211 dt = sample_interval_gyro * US2S;
213 m_tran_mat.m_mat[0][1] = m_filt_gyro[1].m_data.m_vec[2];
214 m_tran_mat.m_mat[0][2] = -m_filt_gyro[1].m_data.m_vec[1];
215 m_tran_mat.m_mat[1][0] = -m_filt_gyro[1].m_data.m_vec[2];
216 m_tran_mat.m_mat[1][2] = m_filt_gyro[1].m_data.m_vec[0];
217 m_tran_mat.m_mat[2][0] = m_filt_gyro[1].m_data.m_vec[1];
218 m_tran_mat.m_mat[2][1] = -m_filt_gyro[1].m_data.m_vec[0];
219 m_tran_mat.m_mat[3][3] = (TYPE) F_CONST;
220 m_tran_mat.m_mat[4][4] = (TYPE) F_CONST;
221 m_tran_mat.m_mat[5][5] = (TYPE) F_CONST;
223 m_measure_mat.m_mat[0][0] = 1;
224 m_measure_mat.m_mat[1][1] = 1;
225 m_measure_mat.m_mat[2][2] = 1;
227 if (is_initialized(m_state_old))
228 m_state_new = transpose(mul(m_tran_mat, transpose(m_state_old)));
230 m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov;
232 if(!is_initialized(m_quat_driv.m_quat))
233 m_quat_driv = m_quat_aid;
235 quaternion<TYPE> quat_rot_inc(0, m_filt_gyro[1].m_data.m_vec[0], m_filt_gyro[1].m_data.m_vec[1],
236 m_filt_gyro[1].m_data.m_vec[2]);
238 quat_diff = (m_quat_driv * quat_rot_inc) * (TYPE) 0.5;
240 m_quat_driv = m_quat_driv + (quat_diff * (TYPE) dt * (TYPE) PI);
242 m_quat_driv.quat_normalize();
244 orientation = quat2euler(m_quat_driv);
246 m_orientation.m_ang.m_vec[0] = orientation.m_ang.m_vec[0] * m_roll_phase_compensation;
247 m_orientation.m_ang.m_vec[1] = orientation.m_ang.m_vec[1] * m_pitch_phase_compensation;
248 m_orientation.m_ang.m_vec[2] = orientation.m_ang.m_vec[2] * m_yaw_phase_compensation;
250 m_rot_matrix = quat2rot_mat(m_quat_driv);
252 quat_error = m_quat_aid * m_quat_driv;
254 euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI;
256 quaternion<TYPE> quat_eu_er(1, euler_error.m_ang.m_vec[0], euler_error.m_ang.m_vec[1],
257 euler_error.m_ang.m_vec[2]);
259 m_quat_driv = (m_quat_driv * quat_eu_er) * (TYPE) PI;
260 m_quat_driv.quat_normalize();
262 if (is_initialized(m_state_new))
264 m_state_error.m_vec[0] = euler_error.m_ang.m_vec[0];
265 m_state_error.m_vec[1] = euler_error.m_ang.m_vec[1];
266 m_state_error.m_vec[2] = euler_error.m_ang.m_vec[2];
267 m_state_error.m_vec[3] = m_state_new.m_vec[3];
268 m_state_error.m_vec[4] = m_state_new.m_vec[4];
269 m_state_error.m_vec[5] = m_state_new.m_vec[5];
273 template <typename TYPE>
274 inline void orientation_filter<TYPE>::measurement_update()
276 matrix<TYPE> gain(M6X6R, M6X6C);
279 for (int j = 0; j < M6X6C; j++)
281 for (int i = 0; i < M6X6R; i++)
283 gain.m_mat[i][j] = m_pred_cov.m_mat[j][i] / (m_pred_cov.m_mat[j][j] + m_aid_cov.m_mat[j][j]);
285 m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j];
292 m_pred_cov.m_mat[i][j] = (iden - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i])) * m_pred_cov.m_mat[i][j];
296 m_state_old = m_state_new;
298 TYPE arr_bias[V1x3S] = {m_state_new.m_vec[3], m_state_new.m_vec[4], m_state_new.m_vec[5]};
299 vect<TYPE> vec(V1x3S, arr_bias);
301 m_bias_correction = vec;
304 template <typename TYPE>
305 euler_angles<TYPE> orientation_filter<TYPE>::get_orientation(const sensor_data<TYPE> accel,
306 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
308 euler_angles<TYPE> cor_euler_ang;
310 filter_sensor_data(accel, gyro, magnetic);
312 normalize(m_filt_accel[1]);
313 m_filt_gyro[1].m_data = m_filt_gyro[1].m_data * (TYPE) PI;
314 normalize(m_filt_magnetic[1]);
316 orientation_triad_algorithm();
318 compute_covariance();
322 measurement_update();
324 return m_orientation;
327 template <typename TYPE>
328 rotation_matrix<TYPE> orientation_filter<TYPE>::get_rotation_matrix(const sensor_data<TYPE> accel,
329 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
331 get_orientation(accel, gyro, magnetic);
336 #endif //_ORIENTATION_FILTER_H