src/sensor/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 #define ACCEL_THRESHOLD 0.2
  37 #define GYRO_THRESHOLD (0.01 * PI)
  38
  39 // constants for computation of covariance and transition matrices
  40 #define ZIGMA_W         (0.05 * DEG2RAD)
  41 #define TAU_W           1000
  42 #define QWB_CONST       ((2 * (ZIGMA_W * ZIGMA_W)) / TAU_W)
  43 #define F_CONST         (-1 / TAU_W)
  44 #define SQUARE(T)       (T * T)
  45
  46 #define NEGLIGIBLE_VAL 0.0000001
  47
  48 #define ABS(val) (((val) < 0) ? -(val) : (val))
  49
  50
  51 template <typename TYPE>
  52 orientation_filter<TYPE>::orientation_filter()
  53 {
  54         TYPE arr[MOVING_AVERAGE_WINDOW_LENGTH];
  55
  56         std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL);
  57
  58         vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> vec(arr);
  59
  60         m_var_gyr_x = vec;
  61         m_var_gyr_y = vec;
  62         m_var_gyr_z = vec;
  63         m_var_roll = vec;
  64         m_var_pitch = vec;
  65         m_var_azimuth = vec;
  66
  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>::initialize_sensor_data(const sensor_data<TYPE> *accel,
  79                 const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic)
  80 {
  81         m_accel.m_data = accel->m_data;
  82         m_accel.m_time_stamp = accel->m_time_stamp;
  83         normalize(m_accel);
  84
  85         if (gyro != NULL) {
  86                 unsigned long long sample_interval_gyro = SAMPLE_INTV;
  87
  88                 if (m_gyro.m_time_stamp != 0 && gyro->m_time_stamp != 0)
  89                         sample_interval_gyro = gyro->m_time_stamp - m_gyro.m_time_stamp;
  90
  91                 m_gyro_dt = sample_interval_gyro * US2S;
  92                 m_gyro.m_time_stamp = gyro->m_time_stamp;
  93
  94                 m_gyro.m_data = gyro->m_data * (TYPE) PI;
  95
  96                 m_gyro.m_data = m_gyro.m_data - m_bias_correction;
  97         }
  98
  99         if (magnetic != NULL) {
 100                 m_magnetic.m_data = magnetic->m_data;
 101                 m_magnetic.m_time_stamp = magnetic->m_time_stamp;
 102         }
 103 }
 104
 105 template <typename TYPE>
 106 inline void orientation_filter<TYPE>::orientation_triad_algorithm()
 107 {
 108         TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
 109         TYPE arr_mag_e[V1x3S] = {0.0, (TYPE) m_magnetic_alignment_factor, 0.0};
 110
 111         vect<TYPE, V1x3S> acc_e(arr_acc_e);
 112         vect<TYPE, V1x3S> mag_e(arr_mag_e);
 113
 114         vect<TYPE, SENSOR_DATA_SIZE> acc_b_x_mag_b = cross(m_accel.m_data, m_magnetic.m_data);
 115         vect<TYPE, V1x3S> acc_e_x_mag_e = cross(acc_e, mag_e);
 116
 117         vect<TYPE, SENSOR_DATA_SIZE> cross1 = cross(acc_b_x_mag_b, m_accel.m_data);
 118         vect<TYPE, V1x3S> cross2 = cross(acc_e_x_mag_e, acc_e);
 119
 120         matrix<TYPE, M3X3R, M3X3C> mat_b;
 121         matrix<TYPE, M3X3R, M3X3C> mat_e;
 122
 123         for(int i = 0; i < M3X3R; i++)
 124         {
 125                 mat_b.m_mat[i][0] = m_accel.m_data.m_vec[i];
 126                 mat_b.m_mat[i][1] = acc_b_x_mag_b.m_vec[i];
 127                 mat_b.m_mat[i][2] = cross1.m_vec[i];
 128                 mat_e.m_mat[i][0] = acc_e.m_vec[i];
 129                 mat_e.m_mat[i][1] = acc_e_x_mag_e.m_vec[i];
 130                 mat_e.m_mat[i][2] = cross2.m_vec[i];
 131         }
 132
 133         matrix<TYPE, M3X3R, M3X3C> mat_b_t = tran(mat_b);
 134         rotation_matrix<TYPE> rot_mat(mat_e * mat_b_t);
 135
 136         m_quat_aid = rot_mat2quat(rot_mat);
 137 }
 138
 139 template <typename TYPE>
 140 inline void orientation_filter<TYPE>::compute_accel_orientation()
 141 {
 142         TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
 143
 144         vect<TYPE, V1x3S> acc_e(arr_acc_e);
 145
 146         m_quat_aid = sensor_data2quat(m_accel, acc_e);
 147 }
 148
 149 template <typename TYPE>
 150 inline void orientation_filter<TYPE>::compute_covariance()
 151 {
 152         TYPE var_gyr_x, var_gyr_y, var_gyr_z;
 153         TYPE var_roll, var_pitch, var_azimuth;
 154         quaternion<TYPE> quat_diff, quat_error;
 155
 156         if(!is_initialized(m_quat_driv.m_quat))
 157                 m_quat_driv = m_quat_aid;
 158
 159         quaternion<TYPE> quat_rot_inc(0, m_gyro.m_data.m_vec[0], m_gyro.m_data.m_vec[1],
 160                         m_gyro.m_data.m_vec[2]);
 161
 162         quat_diff = (m_quat_driv * quat_rot_inc) * (TYPE) 0.5;
 163
 164         m_quat_driv = m_quat_driv + (quat_diff * (TYPE) m_gyro_dt * (TYPE) PI);
 165         m_quat_driv.quat_normalize();
 166
 167         m_quat_output = phase_correction(m_quat_driv, m_quat_aid);
 168
 169         m_orientation = quat2euler(m_quat_output);
 170
 171         quat_error = m_quat_aid * m_quat_driv;
 172
 173         m_euler_error = (quat2euler(quat_error)).m_ang;
 174
 175         m_gyro.m_data = m_gyro.m_data - m_euler_error.m_ang;
 176
 177         m_euler_error.m_ang = m_euler_error.m_ang / (TYPE) PI;
 178
 179         m_gyro_bias = m_euler_error.m_ang * (TYPE) PI;
 180
 181         insert_end(m_var_gyr_x, m_gyro.m_data.m_vec[0]);
 182         insert_end(m_var_gyr_y, m_gyro.m_data.m_vec[1]);
 183         insert_end(m_var_gyr_z, m_gyro.m_data.m_vec[2]);
 184         insert_end(m_var_roll, m_orientation.m_ang.m_vec[0]);
 185         insert_end(m_var_pitch, m_orientation.m_ang.m_vec[1]);
 186         insert_end(m_var_azimuth, m_orientation.m_ang.m_vec[2]);
 187
 188         var_gyr_x = var(m_var_gyr_x);
 189         var_gyr_y = var(m_var_gyr_y);
 190         var_gyr_z = var(m_var_gyr_z);
 191         var_roll = var(m_var_roll);
 192         var_pitch = var(m_var_pitch);
 193         var_azimuth = var(m_var_azimuth);
 194
 195         m_driv_cov.m_mat[0][0] = var_gyr_x;
 196         m_driv_cov.m_mat[1][1] = var_gyr_y;
 197         m_driv_cov.m_mat[2][2] = var_gyr_z;
 198         m_driv_cov.m_mat[3][3] = (TYPE) QWB_CONST;
 199         m_driv_cov.m_mat[4][4] = (TYPE) QWB_CONST;
 200         m_driv_cov.m_mat[5][5] = (TYPE) QWB_CONST;
 201
 202         m_aid_cov.m_mat[0][0] = var_roll;
 203         m_aid_cov.m_mat[1][1] = var_pitch;
 204         m_aid_cov.m_mat[2][2] = var_azimuth;
 205 }
 206
 207 template <typename TYPE>
 208 inline void orientation_filter<TYPE>::time_update()
 209 {
 210         euler_angles<TYPE> orientation;
 211
 212         m_tran_mat.m_mat[0][1] = m_gyro.m_data.m_vec[2];
 213         m_tran_mat.m_mat[0][2] = -m_gyro.m_data.m_vec[1];
 214         m_tran_mat.m_mat[1][0] = -m_gyro.m_data.m_vec[2];
 215         m_tran_mat.m_mat[1][2] = m_gyro.m_data.m_vec[0];
 216         m_tran_mat.m_mat[2][0] = m_gyro.m_data.m_vec[1];
 217         m_tran_mat.m_mat[2][1] = -m_gyro.m_data.m_vec[0];
 218         m_tran_mat.m_mat[3][3] = (TYPE) F_CONST;
 219         m_tran_mat.m_mat[4][4] = (TYPE) F_CONST;
 220         m_tran_mat.m_mat[5][5] = (TYPE) F_CONST;
 221
 222         m_measure_mat.m_mat[0][0] = 1;
 223         m_measure_mat.m_mat[1][1] = 1;
 224         m_measure_mat.m_mat[2][2] = 1;
 225
 226         if (is_initialized(m_state_old))
 227                 m_state_new = transpose(m_tran_mat * transpose(m_state_old));
 228
 229         m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov;
 230
 231         for (int j=0; j<M6X6C; ++j) {
 232                 for (int i=0; i<M6X6R; ++i)     {
 233                         if (ABS(m_pred_cov.m_mat[i][j]) < NEGLIGIBLE_VAL)
 234                                 m_pred_cov.m_mat[i][j] = NEGLIGIBLE_VAL;
 235                 }
 236                 if (ABS(m_state_new.m_vec[j]) < NEGLIGIBLE_VAL)
 237                         m_state_new.m_vec[j] = NEGLIGIBLE_VAL;
 238         }
 239
 240         m_quat_9axis = m_quat_output;
 241         m_quat_gaming_rv = m_quat_9axis;
 242
 243         m_rot_matrix = quat2rot_mat(m_quat_driv);
 244
 245         quaternion<TYPE> quat_eu_er(1, m_euler_error.m_ang.m_vec[0], m_euler_error.m_ang.m_vec[1],
 246                         m_euler_error.m_ang.m_vec[2]);
 247
 248         m_quat_driv = (m_quat_driv * quat_eu_er) * (TYPE) PI;
 249         m_quat_driv.quat_normalize();
 250
 251         if (is_initialized(m_state_new))
 252         {
 253                 m_state_error.m_vec[0] = m_euler_error.m_ang.m_vec[0];
 254                 m_state_error.m_vec[1] = m_euler_error.m_ang.m_vec[1];
 255                 m_state_error.m_vec[2] = m_euler_error.m_ang.m_vec[2];
 256                 m_state_error.m_vec[3] = m_state_new.m_vec[3];
 257                 m_state_error.m_vec[4] = m_state_new.m_vec[4];
 258                 m_state_error.m_vec[5] = m_state_new.m_vec[5];
 259         }
 260 }
 261
 262 template <typename TYPE>
 263 inline void orientation_filter<TYPE>::time_update_gaming_rv()
 264 {
 265         euler_angles<TYPE> orientation;
 266         euler_angles<TYPE> euler_aid;
 267         euler_angles<TYPE> euler_driv;
 268
 269         m_tran_mat.m_mat[0][1] = m_gyro.m_data.m_vec[2];
 270         m_tran_mat.m_mat[0][2] = -m_gyro.m_data.m_vec[1];
 271         m_tran_mat.m_mat[1][0] = -m_gyro.m_data.m_vec[2];
 272         m_tran_mat.m_mat[1][2] = m_gyro.m_data.m_vec[0];
 273         m_tran_mat.m_mat[2][0] = m_gyro.m_data.m_vec[1];
 274         m_tran_mat.m_mat[2][1] = -m_gyro.m_data.m_vec[0];
 275         m_tran_mat.m_mat[3][3] = (TYPE) F_CONST;
 276         m_tran_mat.m_mat[4][4] = (TYPE) F_CONST;
 277         m_tran_mat.m_mat[5][5] = (TYPE) F_CONST;
 278
 279         m_measure_mat.m_mat[0][0] = 1;
 280         m_measure_mat.m_mat[1][1] = 1;
 281         m_measure_mat.m_mat[2][2] = 1;
 282
 283         if (is_initialized(m_state_old))
 284                 m_state_new = transpose(m_tran_mat * transpose(m_state_old));
 285
 286         m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov;
 287
 288         euler_aid = quat2euler(m_quat_aid);
 289         euler_driv = quat2euler(m_quat_output);
 290
 291         if ((SQUARE(m_accel.m_data.m_vec[1]) < ACCEL_THRESHOLD) && (SQUARE(m_gyro.m_data.m_vec[0]) < GYRO_THRESHOLD))
 292         {
 293                 if ((SQUARE(m_accel.m_data.m_vec[0]) < ACCEL_THRESHOLD) && (SQUARE(m_gyro.m_data.m_vec[1]) < GYRO_THRESHOLD))
 294                 {
 295                         if (SQUARE(m_gyro.m_data.m_vec[2]) < GYRO_THRESHOLD)
 296                         {
 297                                 euler_angles<TYPE> euler_gaming_rv(euler_aid.m_ang.m_vec[0], euler_aid.m_ang.m_vec[1],
 298                                                 euler_driv.m_ang.m_vec[2]);
 299                                 m_quat_gaming_rv = euler2quat(euler_gaming_rv);
 300                         }
 301                 }
 302         }
 303
 304         if (is_initialized(m_state_new)) {
 305                 m_state_error.m_vec[0] = m_euler_error.m_ang.m_vec[0];
 306                 m_state_error.m_vec[1] = m_euler_error.m_ang.m_vec[1];
 307                 m_state_error.m_vec[2] = m_euler_error.m_ang.m_vec[2];
 308                 m_state_error.m_vec[3] = m_state_new.m_vec[3];
 309                 m_state_error.m_vec[4] = m_state_new.m_vec[4];
 310                 m_state_error.m_vec[5] = m_state_new.m_vec[5];
 311         }
 312 }
 313
 314 template <typename TYPE>
 315 inline void orientation_filter<TYPE>::measurement_update()
 316 {
 317         matrix<TYPE, M6X6R, M6X6C> gain;
 318         matrix<TYPE, M6X6R, M6X6C> iden;
 319         iden.m_mat[0][0] = iden.m_mat[1][1] = iden.m_mat[2][2] = 1;
 320         iden.m_mat[3][3] = iden.m_mat[4][4] = iden.m_mat[5][5] = 1;
 321
 322         for (int j=0; j<M6X6C; ++j) {
 323                 for (int i=0; i<M6X6R; ++i) {
 324                         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]);
 325                         m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j];
 326                 }
 327
 328                 matrix<TYPE, M6X6R, M6X6C> temp = iden;
 329
 330                 for (int i=0; i<M6X6R; ++i)
 331                         temp.m_mat[i][j] = iden.m_mat[i][j] - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i]);
 332
 333                 m_pred_cov = temp * m_pred_cov;
 334         }
 335
 336         for (int j=0; j<M6X6C; ++j) {
 337                 for (int i=0; i<M6X6R; ++i) {
 338                         if (ABS(m_pred_cov.m_mat[i][j]) < NEGLIGIBLE_VAL)
 339                                 m_pred_cov.m_mat[i][j] = NEGLIGIBLE_VAL;
 340                 }
 341         }
 342
 343         m_state_old = m_state_new;
 344
 345         TYPE arr_bias[V1x3S] = {m_state_new.m_vec[3], m_state_new.m_vec[4], m_state_new.m_vec[5]};
 346         vect<TYPE, V1x3S> vec(arr_bias);
 347
 348         m_bias_correction = vec;
 349
 350         m_gyro_bias = m_gyro_bias + vec;
 351 }
 352
 353 template <typename TYPE>
 354 void orientation_filter<TYPE>::get_device_orientation(const sensor_data<TYPE> *accel,
 355                 const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic)
 356 {
 357         initialize_sensor_data(accel, gyro, magnetic);
 358
 359         if (gyro != NULL && magnetic != NULL) {
 360
 361                 orientation_triad_algorithm();
 362
 363                 compute_covariance();
 364
 365                 time_update();
 366
 367                 measurement_update();
 368
 369                 m_quaternion = m_quat_9axis;
 370
 371         } else if (!gyro && !magnetic) {
 372
 373                 compute_accel_orientation();
 374
 375                 m_quaternion = m_quat_aid;
 376
 377         } else if (!gyro) {
 378
 379                 orientation_triad_algorithm();
 380
 381                 m_quaternion = m_quat_aid;
 382
 383         } else if (!magnetic) {
 384
 385                 compute_accel_orientation();
 386
 387                 compute_covariance();
 388
 389                 time_update_gaming_rv();
 390
 391                 measurement_update();
 392
 393                 m_quaternion = m_quat_gaming_rv;
 394         }
 395 }
 396
 397 #endif  //_ORIENTATION_FILTER_H_