src/sensor_fusion/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 //Windowing is used for buffering of previous samples for statistical analysis
  26 #define MOVING_AVERAGE_WINDOW_LENGTH    20
  27 //Earth's Gravity
  28 #define GRAVITY         9.80665
  29 #define PI              3.141593
  30 //Needed for non-zero initialization for statistical analysis
  31 #define NON_ZERO_VAL    0.1
  32 //microseconds to seconds
  33 #define US2S    (1.0 / 1000000.0)
  34 //Initialize sampling interval to 100000microseconds
  35 #define SAMPLE_INTV             100000
  36
  37 // constants for computation of covariance and transition matrices
  38 #define ZIGMA_W         (0.05 * DEG2RAD)
  39 #define TAU_W           1000
  40 #define QWB_CONST       ((2 * (ZIGMA_W * ZIGMA_W)) / TAU_W)
  41 #define F_CONST         (-1 / TAU_W)
  42
  43 #define NEGLIGIBLE_VAL 0.0000001
  44
  45 #define ABS(val) (((val) < 0) ? -(val) : (val))
  46
  47
  48 template <typename TYPE>
  49 orientation_filter<TYPE>::orientation_filter()
  50 {
  51         TYPE arr[MOVING_AVERAGE_WINDOW_LENGTH];
  52
  53         std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL);
  54
  55         vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> vec(arr);
  56
  57         m_var_gyr_x = vec;
  58         m_var_gyr_y = vec;
  59         m_var_gyr_z = vec;
  60         m_var_roll = vec;
  61         m_var_pitch = vec;
  62         m_var_azimuth = vec;
  63
  64         m_pitch_phase_compensation = 1;
  65         m_roll_phase_compensation = 1;
  66         m_azimuth_phase_compensation = 1;
  67         m_magnetic_alignment_factor = 1;
  68
  69         m_gyro.m_time_stamp = 0;
  70 }
  71
  72 template <typename TYPE>
  73 orientation_filter<TYPE>::~orientation_filter()
  74 {
  75 }
  76
  77 template <typename TYPE>
  78 inline void orientation_filter<TYPE>::init_accel_gyro_mag_data(const sensor_data<TYPE> accel,
  79                 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
  80 {
  81         unsigned long long sample_interval_gyro = SAMPLE_INTV;
  82
  83         m_accel.m_data = accel.m_data;
  84         m_magnetic.m_data = magnetic.m_data;
  85
  86         if (m_gyro.m_time_stamp != 0 && gyro.m_time_stamp != 0)
  87                 sample_interval_gyro =  gyro.m_time_stamp - m_gyro.m_time_stamp;
  88
  89         m_gyro_dt = sample_interval_gyro * US2S;
  90
  91         m_accel.m_time_stamp = accel.m_time_stamp;
  92         m_gyro.m_time_stamp = gyro.m_time_stamp;
  93         m_magnetic.m_time_stamp = magnetic.m_time_stamp;
  94
  95         m_gyro.m_data = gyro.m_data - m_bias_correction;
  96 }
  97
  98 template <typename TYPE>
  99 inline void orientation_filter<TYPE>::init_accel_mag_data(const sensor_data<TYPE> accel,
 100                 const sensor_data<TYPE> magnetic)
 101 {
 102         m_accel.m_data = accel.m_data;
 103         m_magnetic.m_data = magnetic.m_data;
 104
 105         m_accel.m_time_stamp = accel.m_time_stamp;
 106         m_magnetic.m_time_stamp = magnetic.m_time_stamp;
 107 }
 108
 109 template <typename TYPE>
 110 inline void orientation_filter<TYPE>::init_accel_gyro_data(const sensor_data<TYPE> accel,
 111                 const sensor_data<TYPE> gyro)
 112 {
 113         unsigned long long sample_interval_gyro = SAMPLE_INTV;
 114
 115         m_accel.m_data = accel.m_data;
 116
 117         if (m_gyro.m_time_stamp != 0 && gyro.m_time_stamp != 0)
 118                 sample_interval_gyro = gyro.m_time_stamp - m_gyro.m_time_stamp;
 119
 120         m_gyro_dt = sample_interval_gyro * US2S;
 121
 122         m_accel.m_time_stamp = accel.m_time_stamp;
 123         m_gyro.m_time_stamp = gyro.m_time_stamp;
 124
 125         m_gyro.m_data = gyro.m_data - m_bias_correction;
 126 }
 127
 128 template <typename TYPE>
 129 inline void orientation_filter<TYPE>::orientation_triad_algorithm()
 130 {
 131         TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
 132         TYPE arr_mag_e[V1x3S] = {0.0, (TYPE) m_magnetic_alignment_factor, 0.0};
 133
 134         vect<TYPE, V1x3S> acc_e(arr_acc_e);
 135         vect<TYPE, V1x3S> mag_e(arr_mag_e);
 136
 137         vect<TYPE, SENSOR_DATA_SIZE> acc_b_x_mag_b = cross(m_accel.m_data, m_magnetic.m_data);
 138         vect<TYPE, V1x3S> acc_e_x_mag_e = cross(acc_e, mag_e);
 139
 140         vect<TYPE, SENSOR_DATA_SIZE> cross1 = cross(acc_b_x_mag_b, m_accel.m_data);
 141         vect<TYPE, V1x3S> cross2 = cross(acc_e_x_mag_e, acc_e);
 142
 143         matrix<TYPE, M3X3R, M3X3C> mat_b;
 144         matrix<TYPE, M3X3R, M3X3C> mat_e;
 145
 146         for(int i = 0; i < M3X3R; i++)
 147         {
 148                 mat_b.m_mat[i][0] = m_accel.m_data.m_vec[i];
 149                 mat_b.m_mat[i][1] = acc_b_x_mag_b.m_vec[i];
 150                 mat_b.m_mat[i][2] = cross1.m_vec[i];
 151                 mat_e.m_mat[i][0] = acc_e.m_vec[i];
 152                 mat_e.m_mat[i][1] = acc_e_x_mag_e.m_vec[i];
 153                 mat_e.m_mat[i][2] = cross2.m_vec[i];
 154         }
 155
 156         matrix<TYPE, M3X3R, M3X3C> mat_b_t = tran(mat_b);
 157         rotation_matrix<TYPE> rot_mat(mat_e * mat_b_t);
 158
 159         m_quat_aid = rot_mat2quat(rot_mat);
 160 }
 161
 162 template <typename TYPE>
 163 inline void orientation_filter<TYPE>::compute_accel_orientation()
 164 {
 165         TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
 166
 167         vect<TYPE, V1x3S> acc_e(arr_acc_e);
 168
 169         m_quat_aid = sensor_data2quat(m_accel, acc_e);
 170 }
 171
 172 template <typename TYPE>
 173 inline void orientation_filter<TYPE>::compute_covariance()
 174 {
 175         TYPE var_gyr_x, var_gyr_y, var_gyr_z;
 176         TYPE var_roll, var_pitch, var_azimuth;
 177
 178         insert_end(m_var_gyr_x, m_gyro.m_data.m_vec[0]);
 179         insert_end(m_var_gyr_y, m_gyro.m_data.m_vec[1]);
 180         insert_end(m_var_gyr_z, m_gyro.m_data.m_vec[2]);
 181         insert_end(m_var_roll, m_orientation.m_ang.m_vec[0]);
 182         insert_end(m_var_pitch, m_orientation.m_ang.m_vec[1]);
 183         insert_end(m_var_azimuth, m_orientation.m_ang.m_vec[2]);
 184
 185         var_gyr_x = var(m_var_gyr_x);
 186         var_gyr_y = var(m_var_gyr_y);
 187         var_gyr_z = var(m_var_gyr_z);
 188         var_roll = var(m_var_roll);
 189         var_pitch = var(m_var_pitch);
 190         var_azimuth = var(m_var_azimuth);
 191
 192         m_driv_cov.m_mat[0][0] = var_gyr_x;
 193         m_driv_cov.m_mat[1][1] = var_gyr_y;
 194         m_driv_cov.m_mat[2][2] = var_gyr_z;
 195         m_driv_cov.m_mat[3][3] = (TYPE) QWB_CONST;
 196         m_driv_cov.m_mat[4][4] = (TYPE) QWB_CONST;
 197         m_driv_cov.m_mat[5][5] = (TYPE) QWB_CONST;
 198
 199         m_aid_cov.m_mat[0][0] = var_roll;
 200         m_aid_cov.m_mat[1][1] = var_pitch;
 201         m_aid_cov.m_mat[2][2] = var_azimuth;
 202 }
 203
 204 template <typename TYPE>
 205 inline void orientation_filter<TYPE>::time_update()
 206 {
 207         quaternion<TYPE> quat_diff, quat_error, quat_output;
 208         euler_angles<TYPE> euler_error;
 209         euler_angles<TYPE> orientation;
 210
 211         m_tran_mat.m_mat[0][1] = m_gyro.m_data.m_vec[2];
 212         m_tran_mat.m_mat[0][2] = -m_gyro.m_data.m_vec[1];
 213         m_tran_mat.m_mat[1][0] = -m_gyro.m_data.m_vec[2];
 214         m_tran_mat.m_mat[1][2] = m_gyro.m_data.m_vec[0];
 215         m_tran_mat.m_mat[2][0] = m_gyro.m_data.m_vec[1];
 216         m_tran_mat.m_mat[2][1] = -m_gyro.m_data.m_vec[0];
 217         m_tran_mat.m_mat[3][3] = (TYPE) F_CONST;
 218         m_tran_mat.m_mat[4][4] = (TYPE) F_CONST;
 219         m_tran_mat.m_mat[5][5] = (TYPE) F_CONST;
 220
 221         m_measure_mat.m_mat[0][0] = 1;
 222         m_measure_mat.m_mat[1][1] = 1;
 223         m_measure_mat.m_mat[2][2] = 1;
 224
 225         if (is_initialized(m_state_old))
 226                 m_state_new = transpose(m_tran_mat * transpose(m_state_old));
 227
 228         m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov;
 229
 230         for (int j=0; j<M6X6C; ++j) {
 231                 for (int i=0; i<M6X6R; ++i)     {
 232                         if (ABS(m_pred_cov.m_mat[i][j]) < NEGLIGIBLE_VAL)
 233                                 m_pred_cov.m_mat[i][j] = NEGLIGIBLE_VAL;
 234                 }
 235                 if (ABS(m_state_new.m_vec[j]) < NEGLIGIBLE_VAL)
 236                         m_state_new.m_vec[j] = NEGLIGIBLE_VAL;
 237         }
 238
 239         if(!is_initialized(m_quat_driv.m_quat))
 240                 m_quat_driv = m_quat_aid;
 241
 242         quaternion<TYPE> quat_rot_inc(0, m_gyro.m_data.m_vec[0], m_gyro.m_data.m_vec[1],
 243                         m_gyro.m_data.m_vec[2]);
 244
 245         quat_diff = (m_quat_driv * quat_rot_inc) * (TYPE) 0.5;
 246
 247         m_quat_driv = m_quat_driv + (quat_diff * (TYPE) m_gyro_dt * (TYPE) PI);
 248         m_quat_driv.quat_normalize();
 249
 250         quat_output = phase_correction(m_quat_driv, m_quat_aid);
 251
 252         m_quat_9axis = quat_output;
 253
 254         orientation = quat2euler(quat_output);
 255
 256         m_orientation.m_ang.m_vec[0] = orientation.m_ang.m_vec[0] * m_pitch_phase_compensation;
 257         m_orientation.m_ang.m_vec[1] = orientation.m_ang.m_vec[1] * m_roll_phase_compensation;
 258         m_orientation.m_ang.m_vec[2] = orientation.m_ang.m_vec[2] * m_azimuth_phase_compensation;
 259
 260         m_rot_matrix = quat2rot_mat(m_quat_driv);
 261
 262         quat_error = m_quat_aid * m_quat_driv;
 263
 264         euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI;
 265
 266         quaternion<TYPE> quat_eu_er(1, euler_error.m_ang.m_vec[0], euler_error.m_ang.m_vec[1],
 267                         euler_error.m_ang.m_vec[2]);
 268
 269         m_quat_driv = (m_quat_driv * quat_eu_er) * (TYPE) PI;
 270         m_quat_driv.quat_normalize();
 271
 272         if (is_initialized(m_state_new))
 273         {
 274                 m_state_error.m_vec[0] = euler_error.m_ang.m_vec[0];
 275                 m_state_error.m_vec[1] = euler_error.m_ang.m_vec[1];
 276                 m_state_error.m_vec[2] = euler_error.m_ang.m_vec[2];
 277                 m_state_error.m_vec[3] = m_state_new.m_vec[3];
 278                 m_state_error.m_vec[4] = m_state_new.m_vec[4];
 279                 m_state_error.m_vec[5] = m_state_new.m_vec[5];
 280         }
 281 }
 282
 283 template <typename TYPE>
 284 inline void orientation_filter<TYPE>::time_update_gaming_rv()
 285 {
 286         quaternion<TYPE> quat_diff, quat_error, quat_output;
 287         euler_angles<TYPE> euler_error;
 288         euler_angles<TYPE> orientation;
 289         euler_angles<TYPE> euler_aid;
 290         euler_angles<TYPE> euler_driv;
 291
 292         m_tran_mat.m_mat[0][1] = m_gyro.m_data.m_vec[2];
 293         m_tran_mat.m_mat[0][2] = -m_gyro.m_data.m_vec[1];
 294         m_tran_mat.m_mat[1][0] = -m_gyro.m_data.m_vec[2];
 295         m_tran_mat.m_mat[1][2] = m_gyro.m_data.m_vec[0];
 296         m_tran_mat.m_mat[2][0] = m_gyro.m_data.m_vec[1];
 297         m_tran_mat.m_mat[2][1] = -m_gyro.m_data.m_vec[0];
 298         m_tran_mat.m_mat[3][3] = (TYPE) F_CONST;
 299         m_tran_mat.m_mat[4][4] = (TYPE) F_CONST;
 300         m_tran_mat.m_mat[5][5] = (TYPE) F_CONST;
 301
 302         m_measure_mat.m_mat[0][0] = 1;
 303         m_measure_mat.m_mat[1][1] = 1;
 304         m_measure_mat.m_mat[2][2] = 1;
 305
 306         if (is_initialized(m_state_old))
 307                 m_state_new = transpose(mul(m_tran_mat, transpose(m_state_old)));
 308
 309         m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov;
 310
 311         if(!is_initialized(m_quat_driv.m_quat))
 312                 m_quat_driv = m_quat_aid;
 313
 314         quaternion<TYPE> quat_rot_inc(0, m_gyro.m_data.m_vec[0], m_gyro.m_data.m_vec[1],
 315                         m_gyro.m_data.m_vec[2]);
 316
 317         quat_diff = (m_quat_driv * quat_rot_inc) * (TYPE) 0.5;
 318
 319         m_quat_driv = m_quat_driv + (quat_diff * (TYPE) m_gyro_dt * (TYPE) PI);
 320         m_quat_driv.quat_normalize();
 321
 322         quat_output = phase_correction(m_quat_driv, m_quat_aid);
 323
 324         quat_error = m_quat_aid * m_quat_driv;
 325
 326         euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI;
 327
 328         euler_aid = quat2euler(m_quat_aid);
 329         euler_driv = quat2euler(quat_output);
 330
 331         euler_angles<TYPE> euler_gaming_rv(euler_aid.m_ang.m_vec[0], euler_aid.m_ang.m_vec[1],
 332                         euler_driv.m_ang.m_vec[2]);
 333
 334         m_quat_gaming_rv = euler2quat(euler_gaming_rv);
 335
 336         if (is_initialized(m_state_new))
 337         {
 338                 m_state_error.m_vec[0] = euler_error.m_ang.m_vec[0];
 339                 m_state_error.m_vec[1] = euler_error.m_ang.m_vec[1];
 340                 m_state_error.m_vec[2] = euler_error.m_ang.m_vec[2];
 341                 m_state_error.m_vec[3] = m_state_new.m_vec[3];
 342                 m_state_error.m_vec[4] = m_state_new.m_vec[4];
 343                 m_state_error.m_vec[5] = m_state_new.m_vec[5];
 344         }
 345 }
 346
 347 template <typename TYPE>
 348 inline void orientation_filter<TYPE>::measurement_update()
 349 {
 350         matrix<TYPE, M6X6R, M6X6C> gain;
 351         matrix<TYPE, M6X6R, M6X6C> iden;
 352         iden.m_mat[0][0] = iden.m_mat[1][1] = iden.m_mat[2][2] = 1;
 353         iden.m_mat[3][3] = iden.m_mat[4][4] = iden.m_mat[5][5] = 1;
 354
 355         for (int j=0; j<M6X6C; ++j) {
 356                 for (int i=0; i<M6X6R; ++i) {
 357                         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]);
 358                         m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j];
 359                 }
 360
 361                 matrix<TYPE, M6X6R, M6X6C> temp = iden;
 362
 363                 for (int i=0; i<M6X6R; ++i)
 364                         temp.m_mat[i][j] = iden.m_mat[i][j] - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i]);
 365
 366                 m_pred_cov = temp * m_pred_cov;
 367         }
 368
 369         for (int j=0; j<M6X6C; ++j) {
 370                 for (int i=0; i<M6X6R; ++i) {
 371                         if (ABS(m_pred_cov.m_mat[i][j]) < NEGLIGIBLE_VAL)
 372                                 m_pred_cov.m_mat[i][j] = NEGLIGIBLE_VAL;
 373                 }
 374         }
 375
 376         m_state_old = m_state_new;
 377
 378         TYPE arr_bias[V1x3S] = {m_state_new.m_vec[3], m_state_new.m_vec[4], m_state_new.m_vec[5]};
 379         vect<TYPE, V1x3S> vec(arr_bias);
 380
 381         m_bias_correction = vec;
 382 }
 383
 384 template <typename TYPE>
 385 euler_angles<TYPE> orientation_filter<TYPE>::get_orientation(const sensor_data<TYPE> accel,
 386                 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
 387 {
 388         euler_angles<TYPE> cor_euler_ang;
 389
 390         init_accel_gyro_mag_data(accel, gyro, magnetic);
 391
 392         normalize(m_accel);
 393         m_gyro.m_data = m_gyro.m_data * (TYPE) PI;
 394         normalize(m_magnetic);
 395
 396         orientation_triad_algorithm();
 397
 398         compute_covariance();
 399
 400         time_update();
 401
 402         measurement_update();
 403
 404         return m_orientation;
 405 }
 406
 407 template <typename TYPE>
 408 rotation_matrix<TYPE> orientation_filter<TYPE>::get_rotation_matrix(const sensor_data<TYPE> accel,
 409                 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
 410 {
 411         get_orientation(accel, gyro, magnetic);
 412
 413         return m_rot_matrix;
 414 }
 415
 416 template <typename TYPE>
 417 quaternion<TYPE> orientation_filter<TYPE>::get_9axis_quaternion(const sensor_data<TYPE> accel,
 418                 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
 419 {
 420
 421         get_orientation(accel, gyro, magnetic);
 422
 423         return m_quat_9axis;
 424 }
 425
 426 template <typename TYPE>
 427 quaternion<TYPE> orientation_filter<TYPE>::get_geomagnetic_quaternion(const sensor_data<TYPE> accel,
 428                 const sensor_data<TYPE> magnetic)
 429 {
 430         init_accel_mag_data(accel, magnetic);
 431
 432         normalize(m_accel);
 433         normalize(m_magnetic);
 434
 435         orientation_triad_algorithm();
 436
 437         return m_quat_aid;
 438 }
 439
 440 template <typename TYPE>
 441 quaternion<TYPE> orientation_filter<TYPE>::get_gaming_quaternion(const sensor_data<TYPE> accel,
 442                 const sensor_data<TYPE> gyro)
 443 {
 444         euler_angles<TYPE> cor_euler_ang;
 445
 446         init_accel_gyro_data(accel, gyro);
 447
 448         normalize(m_accel);
 449         m_gyro.m_data = m_gyro.m_data * (TYPE) PI;
 450
 451         compute_accel_orientation();
 452
 453         compute_covariance();
 454
 455         time_update_gaming_rv();
 456
 457         measurement_update();
 458
 459         return m_quat_gaming_rv;
 460 }
 461 #endif  //_ORIENTATION_FILTER_H_