src/sensor_fusion/standalone/util/orientation_filter.cpp

   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
  21 #ifdef _ORIENTATION_FILTER_H
  22
  23 #include "orientation_filter.h"
  24
  25 #define MOVING_AVERAGE_WINDOW_LENGTH    20
  26 #define GRAVITY         9.80665
  27 #define PI              3.141593
  28 #define NON_ZERO_VAL    0.1
  29 #define US2S    (1.0 / 1000000.0)
  30 #define SAMPLE_FREQ             100000
  31
  32 // Gyro Types
  33 // Systron-donner "Horizon"
  34 #define ZIGMA_W         (0.05 * DEG2RAD)//deg/s
  35 #define TAU_W           1000//secs
  36 // Crossbow DMU-6X
  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
  41
  42 #define QWB_CONST       ((2 * (ZIGMA_W * ZIGMA_W)) / TAU_W)
  43 #define F_CONST         (-1 / TAU_W)
  44
  45 #define BIAS_AX         0.098586
  46 #define BIAS_AY         0.18385
  47 #define BIAS_AZ         (10.084 - GRAVITY)
  48
  49 #define BIAS_GX         -5.3539
  50 #define BIAS_GY         0.24325
  51 #define BIAS_GZ         2.3391
  52
  53 #define DRIVING_SYSTEM_PHASE_COMPENSATION       -1
  54
  55 #define SCALE_GYRO              575
  56
  57 #define ENABLE_LPF              false
  58
  59 #define M3X3R   3
  60 #define M3X3C   3
  61
  62 #define M6X6R   6
  63 #define M6X6C   6
  64
  65 #define V1x3S   3
  66 #define V1x4S   4
  67 #define V1x6S   6
  68
  69 template <typename TYPE>
  70 orientation_filter<TYPE>::orientation_filter()
  71 {
  72         TYPE arr[MOVING_AVERAGE_WINDOW_LENGTH];
  73
  74         std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL);
  75
  76         vector<TYPE> vec(MOVING_AVERAGE_WINDOW_LENGTH, arr);
  77         vector<TYPE> vec1x3(V1x3S);
  78         vector<TYPE> vec1x6(V1x6S);
  79         matrix<TYPE> mat6x6(M6X6R, M6X6C);
  80
  81         m_var_gyr_x = vec;
  82         m_var_gyr_y = vec;
  83         m_var_gyr_z = vec;
  84         m_var_roll = vec;
  85         m_var_pitch = vec;
  86         m_var_yaw = vec;
  87
  88         m_tran_mat = mat6x6;
  89         m_measure_mat = mat6x6;
  90         m_pred_cov = mat6x6;
  91         m_driv_cov = mat6x6;
  92         m_aid_cov = mat6x6;
  93
  94         m_bias_correction = vec1x3;
  95         m_state_new = vec1x6;
  96         m_state_old = vec1x6;
  97         m_state_error = vec1x6;
  98 }
  99
 100 template <typename TYPE>
 101 orientation_filter<TYPE>::~orientation_filter()
 102 {
 103
 104 }
 105
 106 template <typename TYPE>
 107 inline void orientation_filter<TYPE>::filter_sensor_data(const sensor_data<TYPE> accel,
 108                 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
 109 {
 110         const TYPE iir_b[] = {0.98, 0};
 111         const TYPE iir_a[] = {1.0000000, 0.02};
 112
 113         TYPE a_bias[] = {BIAS_AX, BIAS_AY, BIAS_AZ};
 114         TYPE g_bias[] = {BIAS_GX, BIAS_GY, BIAS_GZ};
 115
 116         vector<TYPE> acc_data(V1x3S);
 117         vector<TYPE> gyr_data(V1x3S);
 118         vector<TYPE> acc_bias(V1x3S, a_bias);
 119         vector<TYPE> gyr_bias(V1x3S, g_bias);
 120
 121         acc_data = accel.m_data - acc_bias;
 122         gyr_data = (gyro.m_data - gyr_bias) / (TYPE) SCALE_GYRO;
 123
 124         m_filt_accel[0] = m_filt_accel[1];
 125         m_filt_gyro[0] = m_filt_gyro[1];
 126         m_filt_magnetic[0] = m_filt_magnetic[1];
 127
 128         if (ENABLE_LPF)
 129         {
 130                 m_filt_accel[1].m_data = acc_data * iir_b[0] - m_filt_accel[0].m_data * iir_a[1];
 131                 m_filt_gyro[1].m_data = gyr_data * iir_b[0] - m_filt_gyro[0].m_data * iir_a[1];
 132                 m_filt_magnetic[1].m_data = magnetic.m_data * iir_b[0] - m_filt_magnetic[0].m_data * iir_a[1];
 133         }
 134         else
 135         {
 136                 m_filt_accel[1].m_data = acc_data;
 137                 m_filt_gyro[1].m_data = gyr_data;
 138                 m_filt_magnetic[1].m_data = magnetic.m_data;
 139         }
 140
 141         m_filt_accel[1].m_time_stamp = accel.m_time_stamp;
 142         m_filt_gyro[1].m_time_stamp = accel.m_time_stamp;
 143         m_filt_magnetic[1].m_time_stamp = accel.m_time_stamp;
 144
 145         normalize(m_filt_accel[1]);
 146         normalize(m_filt_magnetic[1]);
 147
 148         m_filt_gyro[1].m_data = m_filt_gyro[1].m_data - m_bias_correction;
 149 }
 150
 151 template <typename TYPE>
 152 inline void orientation_filter<TYPE>::orientation_triad_algorithm()
 153 {
 154         TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
 155         TYPE arr_mag_e[V1x3S] = {0.0, -1.0, 0.0};
 156
 157         vector<TYPE> acc_e(V1x3S, arr_acc_e);
 158         vector<TYPE> mag_e(V1x3S, arr_mag_e);
 159
 160         vector<TYPE> acc_b_x_mag_b = cross(m_filt_accel[1].m_data, m_filt_magnetic[1].m_data);
 161         vector<TYPE> acc_e_x_mag_e = cross(acc_e, mag_e);
 162
 163         vector<TYPE> cross1 = cross(acc_b_x_mag_b, m_filt_accel[1].m_data);
 164         vector<TYPE> cross2 = cross(acc_e_x_mag_e, acc_e);
 165
 166         matrix<TYPE> mat_b(M3X3R, M3X3C);
 167         matrix<TYPE> mat_e(M3X3R, M3X3C);
 168
 169         for(int i = 0; i < M3X3R; i++)
 170         {
 171                 mat_b.m_mat[i][0] = m_filt_accel[1].m_data.m_vec[i];
 172                 mat_b.m_mat[i][1] = acc_b_x_mag_b.m_vec[i];
 173                 mat_b.m_mat[i][2] = cross1.m_vec[i];
 174                 mat_e.m_mat[i][0] = acc_e.m_vec[i];
 175                 mat_e.m_mat[i][1] = acc_e_x_mag_e.m_vec[i];
 176                 mat_e.m_mat[i][2] = cross2.m_vec[i];
 177         }
 178
 179         matrix<TYPE> mat_b_t = tran(mat_b);
 180         rotation_matrix<TYPE> rot_mat(mat_e * mat_b_t);
 181
 182         m_quat_aid = rot_mat2quat(rot_mat);
 183 }
 184
 185 template <typename TYPE>
 186 inline void orientation_filter<TYPE>::compute_covariance()
 187 {
 188         TYPE var_gyr_x, var_gyr_y, var_gyr_z;
 189         TYPE var_roll, var_pitch, var_yaw;
 190
 191         insert_end(m_var_gyr_x, m_filt_gyro[1].m_data.m_vec[0]);
 192         insert_end(m_var_gyr_y, m_filt_gyro[1].m_data.m_vec[1]);
 193         insert_end(m_var_gyr_z, m_filt_gyro[1].m_data.m_vec[2]);
 194         insert_end(m_var_roll, m_orientation.m_ang.m_vec[0]);
 195         insert_end(m_var_pitch, m_orientation.m_ang.m_vec[1]);
 196         insert_end(m_var_yaw, m_orientation.m_ang.m_vec[2]);
 197
 198         var_gyr_x = var(m_var_gyr_x);
 199         var_gyr_y = var(m_var_gyr_y);
 200         var_gyr_z = var(m_var_gyr_z);
 201         var_roll = var(m_var_roll);
 202         var_pitch = var(m_var_pitch);
 203         var_yaw = var(m_var_yaw);
 204
 205         m_driv_cov.m_mat[0][0] = var_gyr_x;
 206         m_driv_cov.m_mat[1][1] = var_gyr_y;
 207         m_driv_cov.m_mat[2][2] = var_gyr_z;
 208         m_driv_cov.m_mat[3][3] = (TYPE) QWB_CONST;
 209         m_driv_cov.m_mat[4][4] = (TYPE) QWB_CONST;
 210         m_driv_cov.m_mat[5][5] = (TYPE) QWB_CONST;
 211
 212         m_aid_cov.m_mat[0][0] = var_roll;
 213         m_aid_cov.m_mat[1][1] = var_pitch;
 214         m_aid_cov.m_mat[2][2] = var_yaw;
 215 }
 216
 217 template <typename TYPE>
 218 inline void orientation_filter<TYPE>::time_update()
 219 {
 220         quaternion<TYPE> quat_diff, quat_error;
 221         euler_angles<TYPE> euler_error;
 222         euler_angles<TYPE> orientation;
 223         unsigned long long sample_interval_gyro = SAMPLE_FREQ;
 224         TYPE dt = 0;
 225
 226         if (m_filt_gyro[1].m_time_stamp != 0 && m_filt_gyro[0].m_time_stamp != 0)
 227                 sample_interval_gyro =  m_filt_gyro[1].m_time_stamp - m_filt_gyro[0].m_time_stamp;
 228
 229         dt = sample_interval_gyro * US2S;
 230
 231         m_tran_mat.m_mat[0][1] = m_filt_gyro[1].m_data.m_vec[2];
 232         m_tran_mat.m_mat[0][2] = -m_filt_gyro[1].m_data.m_vec[1];
 233         m_tran_mat.m_mat[1][0] = -m_filt_gyro[1].m_data.m_vec[2];
 234         m_tran_mat.m_mat[1][2] = m_filt_gyro[1].m_data.m_vec[0];
 235         m_tran_mat.m_mat[2][0] = m_filt_gyro[1].m_data.m_vec[1];
 236         m_tran_mat.m_mat[2][1] = -m_filt_gyro[1].m_data.m_vec[0];
 237         m_tran_mat.m_mat[3][3] = (TYPE) F_CONST;
 238         m_tran_mat.m_mat[4][4] = (TYPE) F_CONST;
 239         m_tran_mat.m_mat[5][5] = (TYPE) F_CONST;
 240
 241         m_measure_mat.m_mat[0][0] = 1;
 242         m_measure_mat.m_mat[1][1] = 1;
 243         m_measure_mat.m_mat[2][2] = 1;
 244
 245         if (is_initialized(m_state_old))
 246                 m_state_new = transpose(mul(m_tran_mat, transpose(m_state_old)));
 247
 248         m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov;
 249
 250         if(!is_initialized(m_quat_driv.m_quat))
 251                 m_quat_driv = m_quat_aid;
 252
 253         quaternion<TYPE> quat_rot_inc(0, m_filt_gyro[1].m_data.m_vec[0], m_filt_gyro[1].m_data.m_vec[1],
 254                         m_filt_gyro[1].m_data.m_vec[2]);
 255
 256         quat_diff = (m_quat_driv * quat_rot_inc) * (TYPE) 0.5;
 257
 258         m_quat_driv = m_quat_driv + (quat_diff * (TYPE) dt * (TYPE) PI);
 259
 260         m_quat_driv.quat_normalize();
 261
 262         orientation = quat2euler(m_quat_driv);
 263
 264         m_orientation = orientation.m_ang * (TYPE) DRIVING_SYSTEM_PHASE_COMPENSATION;
 265
 266         m_rot_matrix = quat2rot_mat(m_quat_driv);
 267
 268         quat_error = m_quat_aid * m_quat_driv;
 269
 270         euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI;
 271
 272         quaternion<TYPE> quat_eu_er(1, euler_error.m_ang.m_vec[0], euler_error.m_ang.m_vec[1],
 273                         euler_error.m_ang.m_vec[2]);
 274
 275         m_quat_driv = (m_quat_driv * quat_eu_er) * (TYPE) PI;
 276         m_quat_driv.quat_normalize();
 277
 278         if (is_initialized(m_state_new))
 279         {
 280                 m_state_error.m_vec[0] = euler_error.m_ang.m_vec[0];
 281                 m_state_error.m_vec[1] = euler_error.m_ang.m_vec[1];
 282                 m_state_error.m_vec[2] = euler_error.m_ang.m_vec[2];
 283                 m_state_error.m_vec[3] = m_state_new.m_vec[3];
 284                 m_state_error.m_vec[4] = m_state_new.m_vec[4];
 285                 m_state_error.m_vec[5] = m_state_new.m_vec[5];
 286         }
 287 }
 288
 289 template <typename TYPE>
 290 inline void orientation_filter<TYPE>::measurement_update()
 291 {
 292         matrix<TYPE> gain(M6X6R, M6X6C);
 293         TYPE iden = 0;
 294
 295         for (int j = 0; j < M6X6C; j++)
 296         {
 297                 for (int i = 0; i < M6X6R; i++)
 298                 {
 299                         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]);
 300
 301                         m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j];
 302
 303                         if (i == j)
 304                                 iden = 1;
 305                         else
 306                                 iden = 0;
 307
 308                         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];
 309                 }
 310         }
 311
 312         m_state_old = m_state_new;
 313
 314         TYPE arr_bias[V1x3S] = {m_state_new.m_vec[3], m_state_new.m_vec[4], m_state_new.m_vec[5]};
 315         vector<TYPE> vec(V1x3S, arr_bias);
 316
 317         m_bias_correction = vec;
 318 }
 319
 320 template <typename TYPE>
 321 euler_angles<TYPE> orientation_filter<TYPE>::get_orientation(const sensor_data<TYPE> accel,
 322                 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
 323 {
 324         euler_angles<TYPE> cor_euler_ang;
 325
 326         filter_sensor_data(accel, gyro, magnetic);
 327
 328         normalize(m_filt_accel[1]);
 329         m_filt_gyro[1].m_data = m_filt_gyro[1].m_data * (TYPE) PI;
 330         normalize(m_filt_magnetic[1]);
 331
 332         orientation_triad_algorithm();
 333
 334         compute_covariance();
 335
 336         time_update();
 337
 338         measurement_update();
 339
 340         return m_orientation;
 341 }
 342
 343 template <typename TYPE>
 344 rotation_matrix<TYPE> orientation_filter<TYPE>::get_rotation_matrix(const sensor_data<TYPE> accel,
 345                 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
 346 {
 347         get_orientation(accel, gyro, magnetic);
 348
 349         return m_rot_matrix;
 350 }
 351
 352 #endif  //_ORIENTATION_FILTER_H