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