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         if(!is_initialized(m_quat_driv.m_quat))
 231                 m_quat_driv = m_quat_aid;
 232
 233         quaternion<TYPE> quat_rot_inc(0, m_gyro.m_data.m_vec[0], m_gyro.m_data.m_vec[1],
 234                         m_gyro.m_data.m_vec[2]);
 235
 236         quat_diff = (m_quat_driv * quat_rot_inc) * (TYPE) 0.5;
 237
 238         m_quat_driv = m_quat_driv + (quat_diff * (TYPE) m_gyro_dt * (TYPE) PI);
 239         m_quat_driv.quat_normalize();
 240
 241         quat_output = phase_correction(m_quat_driv, m_quat_aid);
 242
 243         m_quat_9axis = quat_output;
 244
 245         orientation = quat2euler(quat_output);
 246
 247         m_orientation.m_ang.m_vec[0] = orientation.m_ang.m_vec[0] * m_pitch_phase_compensation;
 248         m_orientation.m_ang.m_vec[1] = orientation.m_ang.m_vec[1] * m_roll_phase_compensation;
 249         m_orientation.m_ang.m_vec[2] = orientation.m_ang.m_vec[2] * m_azimuth_phase_compensation;
 250
 251         m_rot_matrix = quat2rot_mat(m_quat_driv);
 252
 253         quat_error = m_quat_aid * m_quat_driv;
 254
 255         euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI;
 256
 257         quaternion<TYPE> quat_eu_er(1, euler_error.m_ang.m_vec[0], euler_error.m_ang.m_vec[1],
 258                         euler_error.m_ang.m_vec[2]);
 259
 260         m_quat_driv = (m_quat_driv * quat_eu_er) * (TYPE) PI;
 261         m_quat_driv.quat_normalize();
 262
 263         if (is_initialized(m_state_new))
 264         {
 265                 m_state_error.m_vec[0] = euler_error.m_ang.m_vec[0];
 266                 m_state_error.m_vec[1] = euler_error.m_ang.m_vec[1];
 267                 m_state_error.m_vec[2] = euler_error.m_ang.m_vec[2];
 268                 m_state_error.m_vec[3] = m_state_new.m_vec[3];
 269                 m_state_error.m_vec[4] = m_state_new.m_vec[4];
 270                 m_state_error.m_vec[5] = m_state_new.m_vec[5];
 271         }
 272 }
 273
 274 template <typename TYPE>
 275 inline void orientation_filter<TYPE>::time_update_gaming_rv()
 276 {
 277         quaternion<TYPE> quat_diff, quat_error, quat_output;
 278         euler_angles<TYPE> euler_error;
 279         euler_angles<TYPE> orientation;
 280         euler_angles<TYPE> euler_aid;
 281         euler_angles<TYPE> euler_driv;
 282
 283         m_tran_mat.m_mat[0][1] = m_gyro.m_data.m_vec[2];
 284         m_tran_mat.m_mat[0][2] = -m_gyro.m_data.m_vec[1];
 285         m_tran_mat.m_mat[1][0] = -m_gyro.m_data.m_vec[2];
 286         m_tran_mat.m_mat[1][2] = m_gyro.m_data.m_vec[0];
 287         m_tran_mat.m_mat[2][0] = m_gyro.m_data.m_vec[1];
 288         m_tran_mat.m_mat[2][1] = -m_gyro.m_data.m_vec[0];
 289         m_tran_mat.m_mat[3][3] = (TYPE) F_CONST;
 290         m_tran_mat.m_mat[4][4] = (TYPE) F_CONST;
 291         m_tran_mat.m_mat[5][5] = (TYPE) F_CONST;
 292
 293         m_measure_mat.m_mat[0][0] = 1;
 294         m_measure_mat.m_mat[1][1] = 1;
 295         m_measure_mat.m_mat[2][2] = 1;
 296
 297         if (is_initialized(m_state_old))
 298                 m_state_new = transpose(mul(m_tran_mat, transpose(m_state_old)));
 299
 300         m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov;
 301
 302         if(!is_initialized(m_quat_driv.m_quat))
 303                 m_quat_driv = m_quat_aid;
 304
 305         quaternion<TYPE> quat_rot_inc(0, m_gyro.m_data.m_vec[0], m_gyro.m_data.m_vec[1],
 306                         m_gyro.m_data.m_vec[2]);
 307
 308         quat_diff = (m_quat_driv * quat_rot_inc) * (TYPE) 0.5;
 309
 310         m_quat_driv = m_quat_driv + (quat_diff * (TYPE) m_gyro_dt * (TYPE) PI);
 311         m_quat_driv.quat_normalize();
 312
 313         quat_output = phase_correction(m_quat_driv, m_quat_aid);
 314
 315         quat_error = m_quat_aid * m_quat_driv;
 316
 317         euler_error = (quat2euler(quat_error)).m_ang / (TYPE) PI;
 318
 319         euler_aid = quat2euler(m_quat_aid);
 320         euler_driv = quat2euler(quat_output);
 321
 322         euler_angles<TYPE> euler_gaming_rv(euler_aid.m_ang.m_vec[0], euler_aid.m_ang.m_vec[1],
 323                         euler_driv.m_ang.m_vec[2]);
 324
 325         m_quat_gaming_rv = euler2quat(euler_gaming_rv);
 326
 327         if (is_initialized(m_state_new))
 328         {
 329                 m_state_error.m_vec[0] = euler_error.m_ang.m_vec[0];
 330                 m_state_error.m_vec[1] = euler_error.m_ang.m_vec[1];
 331                 m_state_error.m_vec[2] = euler_error.m_ang.m_vec[2];
 332                 m_state_error.m_vec[3] = m_state_new.m_vec[3];
 333                 m_state_error.m_vec[4] = m_state_new.m_vec[4];
 334                 m_state_error.m_vec[5] = m_state_new.m_vec[5];
 335         }
 336 }
 337
 338 template <typename TYPE>
 339 inline void orientation_filter<TYPE>::measurement_update()
 340 {
 341         matrix<TYPE, M6X6R, M6X6C> gain;
 342         TYPE iden = 0;
 343
 344         for (int j=0; j<M6X6C; ++j) {
 345                 for (int i=0; i<M6X6R; ++i)     {
 346                         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]);
 347
 348                         m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j];
 349
 350                         if (i == j)
 351                                 iden = 1;
 352                         else
 353                                 iden = 0;
 354
 355                         m_pred_cov.m_mat[j][i] = (iden - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i])) *
 356                                         m_pred_cov.m_mat[j][i];
 357
 358                         if (ABS(m_pred_cov.m_mat[j][i]) < NEGLIGIBLE_VAL)
 359                                 m_pred_cov.m_mat[j][i] = NEGLIGIBLE_VAL;
 360                 }
 361         }
 362
 363         m_state_old = m_state_new;
 364
 365         TYPE arr_bias[V1x3S] = {m_state_new.m_vec[3], m_state_new.m_vec[4], m_state_new.m_vec[5]};
 366         vect<TYPE, V1x3S> vec(arr_bias);
 367
 368         m_bias_correction = vec;
 369 }
 370
 371 template <typename TYPE>
 372 euler_angles<TYPE> orientation_filter<TYPE>::get_orientation(const sensor_data<TYPE> accel,
 373                 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
 374 {
 375         euler_angles<TYPE> cor_euler_ang;
 376
 377         init_accel_gyro_mag_data(accel, gyro, magnetic);
 378
 379         normalize(m_accel);
 380         m_gyro.m_data = m_gyro.m_data * (TYPE) PI;
 381         normalize(m_magnetic);
 382
 383         orientation_triad_algorithm();
 384
 385         compute_covariance();
 386
 387         time_update();
 388
 389         measurement_update();
 390
 391         return m_orientation;
 392 }
 393
 394 template <typename TYPE>
 395 rotation_matrix<TYPE> orientation_filter<TYPE>::get_rotation_matrix(const sensor_data<TYPE> accel,
 396                 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
 397 {
 398         get_orientation(accel, gyro, magnetic);
 399
 400         return m_rot_matrix;
 401 }
 402
 403 template <typename TYPE>
 404 quaternion<TYPE> orientation_filter<TYPE>::get_9axis_quaternion(const sensor_data<TYPE> accel,
 405                 const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
 406 {
 407
 408         get_orientation(accel, gyro, magnetic);
 409
 410         return m_quat_9axis;
 411 }
 412
 413 template <typename TYPE>
 414 quaternion<TYPE> orientation_filter<TYPE>::get_geomagnetic_quaternion(const sensor_data<TYPE> accel,
 415                 const sensor_data<TYPE> magnetic)
 416 {
 417         init_accel_mag_data(accel, magnetic);
 418
 419         normalize(m_accel);
 420         normalize(m_magnetic);
 421
 422         orientation_triad_algorithm();
 423
 424         return m_quat_aid;
 425 }
 426
 427 template <typename TYPE>
 428 quaternion<TYPE> orientation_filter<TYPE>::get_gaming_quaternion(const sensor_data<TYPE> accel,
 429                 const sensor_data<TYPE> gyro)
 430 {
 431         euler_angles<TYPE> cor_euler_ang;
 432
 433         init_accel_gyro_data(accel, gyro);
 434
 435         normalize(m_accel);
 436         m_gyro.m_data = m_gyro.m_data * (TYPE) PI;
 437
 438         compute_accel_orientation();
 439
 440         compute_covariance();
 441
 442         time_update_gaming_rv();
 443
 444         measurement_update();
 445
 446         return m_quat_gaming_rv;
 447 }
 448 #endif  //_ORIENTATION_FILTER_H_