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